Sfoglia il codice sorgente

Tweak STM32F7 TMC2660 class

Scott Lahteine 6 anni fa
parent
commit
b682a1961a
2 ha cambiato i file con 137 aggiunte e 139 eliminazioni
  1. 90
    90
      Marlin/src/HAL/HAL_STM32F7/TMC2660.cpp
  2. 47
    49
      Marlin/src/HAL/HAL_STM32F7/TMC2660.h

+ 90
- 90
Marlin/src/HAL/HAL_STM32F7/TMC2660.cpp Vedi File

@@ -87,7 +87,7 @@
87 87
 #define SE_CURRENT_STEP_WIDTH_PATTERN 0x60ul
88 88
 #define SE_MIN_PATTERN 0xFul
89 89
 
90
-//definitions for stall guard2 current register
90
+//definitions for StallGuard2 current register
91 91
 #define STALL_GUARD_FILTER_ENABLED 0x10000ul
92 92
 #define STALL_GUARD_TRESHHOLD_VALUE_PATTERN 0x17F00ul
93 93
 #define CURRENT_SCALING_PATTERN 0x1Ful
@@ -118,7 +118,7 @@ SPIClass SPI_6(SPI6, SPI6_MOSI_PIN, SPI6_MISO_PIN, SPI6_SCK_PIN);
118 118
 
119 119
 //#define TMC_DEBUG1
120 120
 
121
-unsigned char current_scaling = 0;
121
+uint8_t current_scaling = 0;
122 122
 
123 123
 /**
124 124
  * Constructor
@@ -127,7 +127,7 @@ unsigned char current_scaling = 0;
127 127
  * dir_pin - the pin where the direction pin is connected
128 128
  * step_pin - the pin where the step pin is connected
129 129
  */
130
-TMC26XStepper::TMC26XStepper(int number_of_steps, int cs_pin, int dir_pin, int step_pin, unsigned int current, unsigned int resistor) {
130
+TMC26XStepper::TMC26XStepper(int16_t number_of_steps, int16_t cs_pin, int16_t dir_pin, int16_t step_pin, uint16_t current, uint16_t resistor) {
131 131
   // We are not started yet
132 132
   started = false;
133 133
 
@@ -155,7 +155,7 @@ TMC26XStepper::TMC26XStepper(int number_of_steps, int cs_pin, int dir_pin, int s
155 155
   microsteps = _BV(INITIAL_MICROSTEPPING);
156 156
   chopper_config_register = CHOPPER_CONFIG_REGISTER;
157 157
   cool_step_register_value = COOL_STEP_REGISTER;
158
-  stall_guard2_current_register_value = STALL_GUARD2_LOAD_MEASURE_REGISTER;
158
+  stallguard2_current_register_value = STALL_GUARD2_LOAD_MEASURE_REGISTER;
159 159
   driver_configuration_register_value = DRIVER_CONFIG_REGISTER | READ_STALL_GUARD_READING;
160 160
 
161 161
   // Set the current
@@ -202,7 +202,7 @@ void TMC26XStepper::start() {
202 202
   send262(driver_control_register_value);
203 203
   send262(chopper_config_register);
204 204
   send262(cool_step_register_value);
205
-  send262(stall_guard2_current_register_value);
205
+  send262(stallguard2_current_register_value);
206 206
   send262(driver_configuration_register_value);
207 207
 
208 208
   //save that we are in running mode
@@ -218,9 +218,9 @@ void TMC26XStepper::un_start() { started = false; }
218 218
 /**
219 219
  * Sets the speed in revs per minute
220 220
  */
221
-void TMC26XStepper::setSpeed(unsigned int whatSpeed) {
221
+void TMC26XStepper::setSpeed(uint16_t whatSpeed) {
222 222
   this->speed = whatSpeed;
223
-  this->step_delay = 60UL * sq(1000UL) / ((unsigned long)this->number_of_steps * (unsigned long)whatSpeed * (unsigned long)this->microsteps);
223
+  this->step_delay = 60UL * sq(1000UL) / ((uint32_t)this->number_of_steps * (uint32_t)whatSpeed * (uint32_t)this->microsteps);
224 224
   #ifdef TMC_DEBUG0 // crashes
225 225
     //SERIAL_PRINTF("Step delay in micros: ");
226 226
     SERIAL_ECHOPAIR("\nStep delay in micros: ", this->step_delay);
@@ -229,13 +229,13 @@ void TMC26XStepper::setSpeed(unsigned int whatSpeed) {
229 229
   this->next_step_time = this->last_step_time + this->step_delay;
230 230
 }
231 231
 
232
-unsigned int TMC26XStepper::getSpeed(void) { return this->speed; }
232
+uint16_t TMC26XStepper::getSpeed(void) { return this->speed; }
233 233
 
234 234
 /**
235 235
  * Moves the motor steps_to_move steps.
236 236
  * Negative indicates the reverse direction.
237 237
  */
238
-char TMC26XStepper::step(int steps_to_move) {
238
+char TMC26XStepper::step(int16_t steps_to_move) {
239 239
   if (this->steps_left == 0) {
240 240
     this->steps_left = ABS(steps_to_move);  // how many steps to take
241 241
 
@@ -252,7 +252,7 @@ char TMC26XStepper::step(int steps_to_move) {
252 252
 char TMC26XStepper::move(void) {
253 253
   // decrement the number of steps, moving one step each time:
254 254
   if (this->steps_left > 0) {
255
-    unsigned long time = micros();
255
+    uint32_t time = micros();
256 256
     // move only if the appropriate delay has passed:
257 257
 
258 258
     // rem if (time >= this->next_step_time) {
@@ -282,7 +282,7 @@ char TMC26XStepper::move(void) {
282 282
 
283 283
 char TMC26XStepper::isMoving(void) { return this->steps_left > 0; }
284 284
 
285
-unsigned int TMC26XStepper::getStepsLeft(void) { return this->steps_left; }
285
+uint16_t TMC26XStepper::getStepsLeft(void) { return this->steps_left; }
286 286
 
287 287
 char TMC26XStepper::stop(void) {
288 288
   //note to self if the motor is currently moving
@@ -294,8 +294,8 @@ char TMC26XStepper::stop(void) {
294 294
   return state;
295 295
 }
296 296
 
297
-void TMC26XStepper::setCurrent(unsigned int current) {
298
-  unsigned char current_scaling = 0;
297
+void TMC26XStepper::setCurrent(uint16_t current) {
298
+  uint8_t current_scaling = 0;
299 299
   //calculate the current scaling from the max current setting (in mA)
300 300
   float mASetting = (float)current,
301 301
          resistor_value = (float)this->resistor;
@@ -327,49 +327,49 @@ void TMC26XStepper::setCurrent(unsigned int current) {
327 327
   NOMORE(current_scaling, 31);
328 328
 
329 329
   // delete the old value
330
-  stall_guard2_current_register_value &= ~(CURRENT_SCALING_PATTERN);
330
+  stallguard2_current_register_value &= ~(CURRENT_SCALING_PATTERN);
331 331
   // set the new current scaling
332
-  stall_guard2_current_register_value |= current_scaling;
332
+  stallguard2_current_register_value |= current_scaling;
333 333
   // if started we directly send it to the motor
334 334
   if (started) {
335 335
     send262(driver_configuration_register_value);
336
-    send262(stall_guard2_current_register_value);
336
+    send262(stallguard2_current_register_value);
337 337
   }
338 338
 }
339 339
 
340
-unsigned int TMC26XStepper::getCurrent(void) {
340
+uint16_t TMC26XStepper::getCurrent(void) {
341 341
   // Calculate the current according to the datasheet to be on the safe side.
342 342
   // This is not the fastest but the most accurate and illustrative way.
343
-  float result = (float)(stall_guard2_current_register_value & CURRENT_SCALING_PATTERN),
343
+  float result = (float)(stallguard2_current_register_value & CURRENT_SCALING_PATTERN),
344 344
          resistor_value = (float)this->resistor,
345 345
          voltage = (driver_configuration_register_value & VSENSE) ? 0.165 : 0.31;
346 346
   result = (result + 1.0) / 32.0 * voltage / resistor_value * sq(1000.0);
347
-  return (unsigned int)result;
347
+  return (uint16_t)result;
348 348
 }
349 349
 
350
-void TMC26XStepper::setStallGuardThreshold(char stall_guard_threshold, char stall_guard_filter_enabled) {
350
+void TMC26XStepper::setStallGuardThreshold(char stallguard_threshold, char stallguard_filter_enabled) {
351 351
   // We just have 5 bits
352
-  LIMIT(stall_guard_threshold, -64, 63);
352
+  LIMIT(stallguard_threshold, -64, 63);
353 353
 
354 354
   // Add trim down to 7 bits
355
-  stall_guard_threshold &= 0x7F;
356
-  // Delete old stall guard settings
357
-  stall_guard2_current_register_value &= ~(STALL_GUARD_CONFIG_PATTERN);
358
-  if (stall_guard_filter_enabled)
359
-    stall_guard2_current_register_value |= STALL_GUARD_FILTER_ENABLED;
360
-
361
-  // Set the new stall guard threshold
362
-  stall_guard2_current_register_value |= (((unsigned long)stall_guard_threshold << 8) & STALL_GUARD_CONFIG_PATTERN);
355
+  stallguard_threshold &= 0x7F;
356
+  // Delete old StallGuard settings
357
+  stallguard2_current_register_value &= ~(STALL_GUARD_CONFIG_PATTERN);
358
+  if (stallguard_filter_enabled)
359
+    stallguard2_current_register_value |= STALL_GUARD_FILTER_ENABLED;
360
+
361
+  // Set the new StallGuard threshold
362
+  stallguard2_current_register_value |= (((uint32_t)stallguard_threshold << 8) & STALL_GUARD_CONFIG_PATTERN);
363 363
   // If started we directly send it to the motor
364
-  if (started) send262(stall_guard2_current_register_value);
364
+  if (started) send262(stallguard2_current_register_value);
365 365
 }
366 366
 
367 367
 char TMC26XStepper::getStallGuardThreshold(void) {
368
-  unsigned long stall_guard_threshold = stall_guard2_current_register_value & STALL_GUARD_VALUE_PATTERN;
368
+  uint32_t stallguard_threshold = stallguard2_current_register_value & STALL_GUARD_VALUE_PATTERN;
369 369
   //shift it down to bit 0
370
-  stall_guard_threshold >>= 8;
371
-  //convert the value to an int to correctly handle the negative numbers
372
-  char result = stall_guard_threshold;
370
+  stallguard_threshold >>= 8;
371
+  //convert the value to an int16_t to correctly handle the negative numbers
372
+  char result = stallguard_threshold;
373 373
   //check if it is negative and fill it up with leading 1 for proper negative number representation
374 374
   //rem if (result & _BV(6)) {
375 375
 
@@ -378,7 +378,7 @@ char TMC26XStepper::getStallGuardThreshold(void) {
378 378
 }
379 379
 
380 380
 char TMC26XStepper::getStallGuardFilter(void) {
381
-  if (stall_guard2_current_register_value & STALL_GUARD_FILTER_ENABLED)
381
+  if (stallguard2_current_register_value & STALL_GUARD_FILTER_ENABLED)
382 382
     return -1;
383 383
   return 0;
384 384
 }
@@ -389,7 +389,7 @@ char TMC26XStepper::getStallGuardFilter(void) {
389 389
  * any value in between will be mapped to the next smaller value
390 390
  * 0 and 1 set the motor in full step mode
391 391
  */
392
-void TMC26XStepper::setMicrosteps(int number_of_steps) {
392
+void TMC26XStepper::setMicrosteps(int16_t number_of_steps) {
393 393
   long setting_pattern;
394 394
   //poor mans log
395 395
   if (number_of_steps >= 256) {
@@ -448,7 +448,7 @@ void TMC26XStepper::setMicrosteps(int number_of_steps) {
448 448
 /**
449 449
  * returns the effective number of microsteps at the moment
450 450
  */
451
-int TMC26XStepper::getMicrosteps(void) { return microsteps; }
451
+int16_t TMC26XStepper::getMicrosteps(void) { return microsteps; }
452 452
 
453 453
 /**
454 454
  * constant_off_time: The off time setting controls the minimum chopper frequency.
@@ -473,7 +473,7 @@ int TMC26XStepper::getMicrosteps(void) { return microsteps; }
473 473
  *    1: enable comparator termination of fast decay cycle
474 474
  *    0: end by time only
475 475
  */
476
-void TMC26XStepper::setConstantOffTimeChopper(char constant_off_time, char blank_time, char fast_decay_time_setting, char sine_wave_offset, unsigned char use_current_comparator) {
476
+void TMC26XStepper::setConstantOffTimeChopper(char constant_off_time, char blank_time, char fast_decay_time_setting, char sine_wave_offset, uint8_t use_current_comparator) {
477 477
   // Perform some sanity checks
478 478
   LIMIT(constant_off_time, 2, 15);
479 479
 
@@ -497,16 +497,16 @@ void TMC26XStepper::setConstantOffTimeChopper(char constant_off_time, char blank
497 497
   // Set the constant off pattern
498 498
   chopper_config_register |= CHOPPER_MODE_T_OFF_FAST_DECAY;
499 499
   // Set the blank timing value
500
-  chopper_config_register |= ((unsigned long)blank_value) << BLANK_TIMING_SHIFT;
500
+  chopper_config_register |= ((uint32_t)blank_value) << BLANK_TIMING_SHIFT;
501 501
   // Setting the constant off time
502 502
   chopper_config_register |= constant_off_time;
503 503
   // Set the fast decay time
504 504
   // Set msb
505
-  chopper_config_register |= (((unsigned long)(fast_decay_time_setting & 0x8)) << HYSTERESIS_DECREMENT_SHIFT);
505
+  chopper_config_register |= (((uint32_t)(fast_decay_time_setting & 0x8)) << HYSTERESIS_DECREMENT_SHIFT);
506 506
   // Other bits
507
-  chopper_config_register |= (((unsigned long)(fast_decay_time_setting & 0x7)) << HYSTERESIS_START_VALUE_SHIFT);
507
+  chopper_config_register |= (((uint32_t)(fast_decay_time_setting & 0x7)) << HYSTERESIS_START_VALUE_SHIFT);
508 508
   // Set the sine wave offset
509
-  chopper_config_register |= (unsigned long)sine_wave_offset << HYSTERESIS_LOW_SHIFT;
509
+  chopper_config_register |= (uint32_t)sine_wave_offset << HYSTERESIS_LOW_SHIFT;
510 510
   // Using the current comparator?
511 511
   if (!use_current_comparator)
512 512
     chopper_config_register |= _BV(12);
@@ -564,15 +564,15 @@ void TMC26XStepper::setSpreadCycleChopper(char constant_off_time, char blank_tim
564 564
   chopper_config_register &= ~(CHOPPER_MODE_T_OFF_FAST_DECAY | BLANK_TIMING_PATTERN | HYSTERESIS_DECREMENT_PATTERN | HYSTERESIS_LOW_VALUE_PATTERN | HYSTERESIS_START_VALUE_PATTERN | T_OFF_TIMING_PATERN);
565 565
 
566 566
   //set the blank timing value
567
-  chopper_config_register |= ((unsigned long)blank_value) << BLANK_TIMING_SHIFT;
567
+  chopper_config_register |= ((uint32_t)blank_value) << BLANK_TIMING_SHIFT;
568 568
   //setting the constant off time
569 569
   chopper_config_register |= constant_off_time;
570 570
   //set the hysteresis_start
571
-  chopper_config_register |= ((unsigned long)hysteresis_start) << HYSTERESIS_START_VALUE_SHIFT;
571
+  chopper_config_register |= ((uint32_t)hysteresis_start) << HYSTERESIS_START_VALUE_SHIFT;
572 572
   //set the hysteresis end
573
-  chopper_config_register |= ((unsigned long)hysteresis_end) << HYSTERESIS_LOW_SHIFT;
573
+  chopper_config_register |= ((uint32_t)hysteresis_end) << HYSTERESIS_LOW_SHIFT;
574 574
   //set the hystereis decrement
575
-  chopper_config_register |= ((unsigned long)blank_value) << BLANK_TIMING_SHIFT;
575
+  chopper_config_register |= ((uint32_t)blank_value) << BLANK_TIMING_SHIFT;
576 576
   //if started we directly send it to the motor
577 577
   if (started) {
578 578
     //rem send262(driver_control_register_value);
@@ -605,12 +605,12 @@ void TMC26XStepper::setRandomOffTime(char value) {
605 605
 }
606 606
 
607 607
 void TMC26XStepper::setCoolStepConfiguration(
608
-  unsigned int lower_SG_threshold,
609
-  unsigned int SG_hysteresis,
610
-  unsigned char current_decrement_step_size,
611
-  unsigned char current_increment_step_size,
612
-  unsigned char lower_current_limit)
613
-{
608
+  uint16_t lower_SG_threshold,
609
+  uint16_t SG_hysteresis,
610
+  uint8_t current_decrement_step_size,
611
+  uint8_t current_increment_step_size,
612
+  uint8_t lower_current_limit
613
+) {
614 614
   // Sanitize the input values
615 615
   NOMORE(lower_SG_threshold, 480);
616 616
   // Divide by 32
@@ -628,11 +628,11 @@ void TMC26XStepper::setCoolStepConfiguration(
628 628
   if (!this->cool_step_enabled) lower_SG_threshold = 0;
629 629
   // The good news is that we can start with a complete new cool step register value
630 630
   // And simply set the values in the register
631
-  cool_step_register_value = ((unsigned long)lower_SG_threshold)
632
-                          | (((unsigned long)SG_hysteresis) << 8)
633
-                          | (((unsigned long)current_decrement_step_size) << 5)
634
-                          | (((unsigned long)current_increment_step_size) << 13)
635
-                          | (((unsigned long)lower_current_limit) << 15)
631
+  cool_step_register_value = ((uint32_t)lower_SG_threshold)
632
+                          | (((uint32_t)SG_hysteresis) << 8)
633
+                          | (((uint32_t)current_decrement_step_size) << 5)
634
+                          | (((uint32_t)current_increment_step_size) << 13)
635
+                          | (((uint32_t)lower_current_limit) << 15)
636 636
                           | COOL_STEP_REGISTER; // Register signature
637 637
 
638 638
   //SERIAL_PRINTFln(cool_step_register_value,HEX);
@@ -653,25 +653,25 @@ void TMC26XStepper::setCoolStepEnabled(boolean enabled) {
653 653
 
654 654
 boolean TMC26XStepper::isCoolStepEnabled(void) { return this->cool_step_enabled; }
655 655
 
656
-unsigned int TMC26XStepper::getCoolStepLowerSgThreshold() {
656
+uint16_t TMC26XStepper::getCoolStepLowerSgThreshold() {
657 657
   // We return our internally stored value - in order to provide the correct setting even if cool step is not enabled
658 658
   return this->cool_step_lower_threshold<<5;
659 659
 }
660 660
 
661
-unsigned int TMC26XStepper::getCoolStepUpperSgThreshold() {
662
-  return (unsigned char)((cool_step_register_value & SE_MAX_PATTERN) >> 8) << 5;
661
+uint16_t TMC26XStepper::getCoolStepUpperSgThreshold() {
662
+  return (uint8_t)((cool_step_register_value & SE_MAX_PATTERN) >> 8) << 5;
663 663
 }
664 664
 
665
-unsigned char TMC26XStepper::getCoolStepCurrentIncrementSize() {
666
-  return (unsigned char)((cool_step_register_value & CURRENT_DOWN_STEP_SPEED_PATTERN) >> 13);
665
+uint8_t TMC26XStepper::getCoolStepCurrentIncrementSize() {
666
+  return (uint8_t)((cool_step_register_value & CURRENT_DOWN_STEP_SPEED_PATTERN) >> 13);
667 667
 }
668 668
 
669
-unsigned char TMC26XStepper::getCoolStepNumberOfSGReadings() {
670
-  return (unsigned char)((cool_step_register_value & SE_CURRENT_STEP_WIDTH_PATTERN) >> 5);
669
+uint8_t TMC26XStepper::getCoolStepNumberOfSGReadings() {
670
+  return (uint8_t)((cool_step_register_value & SE_CURRENT_STEP_WIDTH_PATTERN) >> 5);
671 671
 }
672 672
 
673
-unsigned char TMC26XStepper::getCoolStepLowerCurrentLimit() {
674
-  return (unsigned char)((cool_step_register_value & MINIMUM_CURRENT_FOURTH) >> 15);
673
+uint8_t TMC26XStepper::getCoolStepLowerCurrentLimit() {
674
+  return (uint8_t)((cool_step_register_value & MINIMUM_CURRENT_FOURTH) >> 15);
675 675
 }
676 676
 
677 677
 void TMC26XStepper::setEnabled(boolean enabled) {
@@ -693,7 +693,7 @@ boolean TMC26XStepper::isEnabled() { return !!(chopper_config_register & T_OFF_P
693 693
  *
694 694
  */
695 695
 void TMC26XStepper::readStatus(char read_value) {
696
-  unsigned long old_driver_configuration_register_value = driver_configuration_register_value;
696
+  uint32_t old_driver_configuration_register_value = driver_configuration_register_value;
697 697
   //reset the readout configuration
698 698
   driver_configuration_register_value &= ~(READ_SELECTION_PATTERN);
699 699
   //this now equals TMC26X_READOUT_POSITION - so we just have to check the other two options
@@ -712,42 +712,42 @@ void TMC26XStepper::readStatus(char read_value) {
712 712
   send262(driver_configuration_register_value);
713 713
 }
714 714
 
715
-int TMC26XStepper::getMotorPosition(void) {
715
+int16_t TMC26XStepper::getMotorPosition(void) {
716 716
   //we read it out even if we are not started yet - perhaps it is useful information for somebody
717 717
   readStatus(TMC26X_READOUT_POSITION);
718 718
   return getReadoutValue();
719 719
 }
720 720
 
721
-//reads the stall guard setting from last status
722
-//returns -1 if stallguard information is not present
723
-int TMC26XStepper::getCurrentStallGuardReading(void) {
724
-  //if we don't yet started there cannot be a stall guard value
721
+//reads the StallGuard setting from last status
722
+//returns -1 if StallGuard information is not present
723
+int16_t TMC26XStepper::getCurrentStallGuardReading(void) {
724
+  //if we don't yet started there cannot be a StallGuard value
725 725
   if (!started) return -1;
726 726
   //not time optimal, but solution optiomal:
727
-  //first read out the stall guard value
727
+  //first read out the StallGuard value
728 728
   readStatus(TMC26X_READOUT_STALLGUARD);
729 729
   return getReadoutValue();
730 730
 }
731 731
 
732
-unsigned char TMC26XStepper::getCurrentCSReading(void) {
733
-  //if we don't yet started there cannot be a stall guard value
732
+uint8_t TMC26XStepper::getCurrentCSReading(void) {
733
+  //if we don't yet started there cannot be a StallGuard value
734 734
   if (!started) return 0;
735 735
   //not time optimal, but solution optiomal:
736
-  //first read out the stall guard value
736
+  //first read out the StallGuard value
737 737
   readStatus(TMC26X_READOUT_CURRENT);
738 738
   return (getReadoutValue() & 0x1F);
739 739
 }
740 740
 
741
-unsigned int TMC26XStepper::getCurrentCurrent(void) {
741
+uint16_t TMC26XStepper::getCurrentCurrent(void) {
742 742
     float result = (float)getCurrentCSReading(),
743 743
            resistor_value = (float)this->resistor,
744 744
            voltage = (driver_configuration_register_value & VSENSE)? 0.165 : 0.31;
745 745
     result = (result + 1.0) / 32.0 * voltage / resistor_value * sq(1000.0);
746
-    return (unsigned int)result;
746
+    return (uint16_t)result;
747 747
 }
748 748
 
749 749
 /**
750
- * Return true if the stallguard threshold has been reached
750
+ * Return true if the StallGuard threshold has been reached
751 751
  */
752 752
 boolean TMC26XStepper::isStallGuardOverThreshold(void) {
753 753
   if (!this->started) return false;
@@ -808,13 +808,13 @@ boolean TMC26XStepper::isStallGuardReached(void) {
808 808
   return (driver_status_result & STATUS_STALL_GUARD_STATUS);
809 809
 }
810 810
 
811
-//reads the stall guard setting from last status
812
-//returns -1 if stallguard inforamtion is not present
813
-int TMC26XStepper::getReadoutValue(void) {
811
+//reads the StallGuard setting from last status
812
+//returns -1 if StallGuard information is not present
813
+int16_t TMC26XStepper::getReadoutValue(void) {
814 814
   return (int)(driver_status_result >> 10);
815 815
 }
816 816
 
817
-int TMC26XStepper::getResistor() { return this->resistor; }
817
+int16_t TMC26XStepper::getResistor() { return this->resistor; }
818 818
 
819 819
 boolean TMC26XStepper::isCurrentScalingHalfed() {
820 820
   return !!(this->driver_configuration_register_value & VSENSE);
@@ -822,7 +822,7 @@ boolean TMC26XStepper::isCurrentScalingHalfed() {
822 822
 /**
823 823
  * version() returns the version of the library:
824 824
  */
825
-int TMC26XStepper::version(void) { return 1; }
825
+int16_t TMC26XStepper::version(void) { return 1; }
826 826
 
827 827
 void TMC26XStepper::debugLastStatus() {
828 828
   #ifdef TMC_DEBUG1
@@ -850,8 +850,8 @@ void TMC26XStepper::debugLastStatus() {
850 850
       if (this->isStandStill())
851 851
         SERIAL_ECHOLNPGM("\n  INFO: Motor is standing still.");
852 852
 
853
-      unsigned long readout_config = driver_configuration_register_value & READ_SELECTION_PATTERN;
854
-      const int value = getReadoutValue();
853
+      uint32_t readout_config = driver_configuration_register_value & READ_SELECTION_PATTERN;
854
+      const int16_t value = getReadoutValue();
855 855
       if (readout_config == READ_MICROSTEP_POSTION) {
856 856
         //SERIAL_PRINTF("Microstep postion phase A: ");
857 857
         SERIAL_ECHOPAIR("\n  Microstep postion phase A: ", value);
@@ -861,7 +861,7 @@ void TMC26XStepper::debugLastStatus() {
861 861
         SERIAL_ECHOPAIR("\n  Stall Guard value:", value);
862 862
       }
863 863
       else if (readout_config == READ_STALL_GUARD_AND_COOL_STEP) {
864
-        int stallGuard = value & 0xF, current = value & 0x1F0;
864
+        int16_t stallGuard = value & 0xF, current = value & 0x1F0;
865 865
         //SERIAL_PRINTF("Approx Stall Guard: ");
866 866
         SERIAL_ECHOPAIR("\n  Approx Stall Guard: ", stallGuard);
867 867
         //SERIAL_PRINTF("Current level");
@@ -875,11 +875,11 @@ void TMC26XStepper::debugLastStatus() {
875 875
  * send register settings to the stepper driver via SPI
876 876
  * returns the current status
877 877
  */
878
-inline void TMC26XStepper::send262(unsigned long datagram) {
879
-  unsigned long i_datagram;
878
+inline void TMC26XStepper::send262(uint32_t datagram) {
879
+  uint32_t i_datagram;
880 880
 
881 881
   //preserver the previous spi mode
882
-  //unsigned char oldMode =  SPCR & SPI_MODE_MASK;
882
+  //uint8_t oldMode =  SPCR & SPI_MODE_MASK;
883 883
 
884 884
   //if the mode is not correct set it to mode 3
885 885
   //if (oldMode != SPI_MODE3) {

+ 47
- 49
Marlin/src/HAL/HAL_STM32F7/TMC2660.h Vedi File

@@ -83,7 +83,7 @@
83 83
  * \code
84 84
  * TMC26XStepper stepper = TMC26XStepper(200,1,2,3,500);
85 85
  * \endcode
86
- * see TMC26XStepper(int number_of_steps, int cs_pin, int dir_pin, int step_pin, unsigned int rms_current)
86
+ * see TMC26XStepper(int16_t number_of_steps, int16_t cs_pin, int16_t dir_pin, int16_t step_pin, uint16_t rms_current)
87 87
  *
88 88
  * Keep in mind that you need to start the driver with start() in order to get the TMC26X configured.
89 89
  *
@@ -122,7 +122,7 @@ class TMC26XStepper {
122 122
      * You can select a different stepping with setMicrosteps() to aa different value.
123 123
      * \sa start(), setMicrosteps()
124 124
      */
125
-    TMC26XStepper(int number_of_steps, int cs_pin, int dir_pin, int step_pin, unsigned int current, unsigned int resistor=100); //resistor=150
125
+    TMC26XStepper(int16_t number_of_steps, int16_t cs_pin, int16_t dir_pin, int16_t step_pin, uint16_t current, uint16_t resistor=100); //resistor=150
126 126
 
127 127
     /*!
128 128
      * \brief configures and starts the TMC26X stepper driver. Before you called this function the stepper driver is in nonfunctional mode.
@@ -150,13 +150,13 @@ class TMC26XStepper {
150 150
      * \brief Sets the rotation speed in revolutions per minute.
151 151
      * \param whatSpeed the desired speed in rotations per minute.
152 152
      */
153
-    void setSpeed(unsigned int whatSpeed);
153
+    void setSpeed(uint16_t whatSpeed);
154 154
 
155 155
     /*!
156 156
      * \brief reads out the currently selected speed in revolutions per minute.
157 157
      * \sa setSpeed()
158 158
      */
159
-    unsigned int getSpeed(void);
159
+    uint16_t getSpeed(void);
160 160
 
161 161
     /*!
162 162
      * \brief Set the number of microsteps in 2^i values (rounded) up to 256
@@ -166,7 +166,7 @@ class TMC26XStepper {
166 166
      * If you give any other value it will be rounded to the next smaller number (3 would give a microstepping of 2).
167 167
      * You can always check the current microstepping with getMicrosteps().
168 168
      */
169
-    void setMicrosteps(int number_of_steps);
169
+    void setMicrosteps(int16_t number_of_steps);
170 170
 
171 171
     /*!
172 172
      * \brief returns the effective current number of microsteps selected.
@@ -176,7 +176,7 @@ class TMC26XStepper {
176 176
      *
177 177
      * \sa setMicrosteps()
178 178
      */
179
-    int getMicrosteps(void);
179
+    int16_t getMicrosteps(void);
180 180
 
181 181
     /*!
182 182
      * \brief Initiate a movement for the given number of steps. Positive numbers move in one, negative numbers in the other direction.
@@ -193,7 +193,7 @@ class TMC26XStepper {
193 193
      * You can always verify with isMoving() or even use stop() to stop the motor before giving it new step directions.
194 194
      * \sa isMoving(), getStepsLeft(), stop()
195 195
      */
196
-    char step(int number_of_steps);
196
+    char step(int16_t number_of_steps);
197 197
 
198 198
     /*!
199 199
      * \brief Central movement method, must be called as often as possible in the lopp function and is very fast.
@@ -228,7 +228,7 @@ class TMC26XStepper {
228 228
      * \brief Get the number of steps left in the current movement.
229 229
      * \return The number of steps left in the movement. This number is always positive.
230 230
      */
231
-    unsigned int getStepsLeft(void);
231
+    uint16_t getStepsLeft(void);
232 232
 
233 233
     /*!
234 234
      * \brief Stops the motor regardless if it moves or not.
@@ -262,7 +262,7 @@ class TMC26XStepper {
262 262
      * \sa setSpreadCycleChoper() for other alternatives.
263 263
      * \sa setRandomOffTime() for spreading the noise over a wider spectrum
264 264
      */
265
-    void setConstantOffTimeChopper(char constant_off_time, char blank_time, char fast_decay_time_setting, char sine_wave_offset, unsigned char use_current_comparator);
265
+    void setConstantOffTimeChopper(char constant_off_time, char blank_time, char fast_decay_time_setting, char sine_wave_offset, uint8_t use_current_comparator);
266 266
 
267 267
     /*!
268 268
      * \brief Sets and configures with spread cycle chopper.
@@ -310,7 +310,7 @@ class TMC26XStepper {
310 310
      * \param current the maximum motor current in mA
311 311
      * \sa getCurrent(), getCurrentCurrent()
312 312
      */
313
-    void setCurrent(unsigned int current);
313
+    void setCurrent(uint16_t current);
314 314
 
315 315
     /*!
316 316
      * \brief readout the motor maximum current in mA (1000 is an Amp)
@@ -318,12 +318,12 @@ class TMC26XStepper {
318 318
      * \return the maximum motor current in milli amps
319 319
      * \sa getCurrentCurrent()
320 320
      */
321
-    unsigned int getCurrent(void);
321
+    uint16_t getCurrent(void);
322 322
 
323 323
     /*!
324 324
      * \brief set the StallGuard threshold in order to get sensible StallGuard readings.
325
-     * \param stall_guard_threshold -64 … 63 the StallGuard threshold
326
-     * \param stall_guard_filter_enabled 0 if the filter is disabled, -1 if it is enabled
325
+     * \param stallguard_threshold -64 … 63 the StallGuard threshold
326
+     * \param stallguard_filter_enabled 0 if the filter is disabled, -1 if it is enabled
327 327
      *
328 328
      * The StallGuard threshold is used to optimize the StallGuard reading to sensible values. It should be at 0 at
329 329
      * the maximum allowable load on the otor (but not before). = is a good starting point (and the default)
@@ -335,7 +335,7 @@ class TMC26XStepper {
335 335
      *
336 336
      * \sa getCurrentStallGuardReading() to read out the current value.
337 337
      */
338
-    void setStallGuardThreshold(char stall_guard_threshold, char stall_guard_filter_enabled);
338
+    void setStallGuardThreshold(char stallguard_threshold, char stallguard_filter_enabled);
339 339
 
340 340
     /*!
341 341
      * \brief reads out the StallGuard threshold
@@ -366,8 +366,8 @@ class TMC26XStepper {
366 366
      * (1/2 or 1/4th otf the configured current).
367 367
      * \sa COOL_STEP_HALF_CS_LIMIT, COOL_STEP_QUARTER_CS_LIMIT
368 368
      */
369
-    void setCoolStepConfiguration(unsigned int lower_SG_threshold, unsigned int SG_hysteresis, unsigned char current_decrement_step_size,
370
-                                  unsigned char current_increment_step_size, unsigned char lower_current_limit);
369
+    void setCoolStepConfiguration(uint16_t lower_SG_threshold, uint16_t SG_hysteresis, uint8_t current_decrement_step_size,
370
+                                  uint8_t current_increment_step_size, uint8_t lower_current_limit);
371 371
 
372 372
     /*!
373 373
      * \brief enables or disables the CoolStep smart energy operation feature. It must be configured before enabling it.
@@ -387,32 +387,32 @@ class TMC26XStepper {
387 387
      * \brief returns the lower StallGuard threshold for the CoolStep operation
388 388
      * \sa setCoolStepConfiguration()
389 389
      */
390
-    unsigned int getCoolStepLowerSgThreshold();
390
+    uint16_t getCoolStepLowerSgThreshold();
391 391
 
392 392
     /*!
393 393
      * \brief returns the upper StallGuard threshold for the CoolStep operation
394 394
      * \sa setCoolStepConfiguration()
395 395
      */
396
-    unsigned int getCoolStepUpperSgThreshold();
396
+    uint16_t getCoolStepUpperSgThreshold();
397 397
 
398 398
     /*!
399 399
      * \brief returns the number of StallGuard readings befor CoolStep adjusts the motor current.
400 400
      * \sa setCoolStepConfiguration()
401 401
      */
402
-    unsigned char getCoolStepNumberOfSGReadings();
402
+    uint8_t getCoolStepNumberOfSGReadings();
403 403
 
404 404
     /*!
405 405
      * \brief returns the increment steps for the current for the CoolStep operation
406 406
      * \sa setCoolStepConfiguration()
407 407
      */
408
-    unsigned char getCoolStepCurrentIncrementSize();
408
+    uint8_t getCoolStepCurrentIncrementSize();
409 409
 
410 410
     /*!
411 411
      * \brief returns the absolut minium current for the CoolStep operation
412 412
      * \sa setCoolStepConfiguration()
413 413
      * \sa COOL_STEP_HALF_CS_LIMIT, COOL_STEP_QUARTER_CS_LIMIT
414 414
      */
415
-    unsigned char getCoolStepLowerCurrentLimit();
415
+    uint8_t getCoolStepLowerCurrentLimit();
416 416
 
417 417
     /*!
418 418
      * \brief Get the current microstep position for phase A
@@ -420,7 +420,7 @@ class TMC26XStepper {
420 420
      *
421 421
      * Keep in mind that this routine reads and writes a value via SPI - so this may take a bit time.
422 422
      */
423
-    int getMotorPosition(void);
423
+    int16_t getMotorPosition(void);
424 424
 
425 425
     /*!
426 426
      * \brief Reads the current StallGuard value.
@@ -428,14 +428,14 @@ class TMC26XStepper {
428 428
      * Keep in mind that this routine reads and writes a value via SPI - so this may take a bit time.
429 429
      * \sa setStallGuardThreshold() for tuning the readout to sensible ranges.
430 430
      */
431
-    int getCurrentStallGuardReading(void);
431
+    int16_t getCurrentStallGuardReading(void);
432 432
 
433 433
     /*!
434 434
      * \brief Reads the current current setting value as fraction of the maximum current
435 435
      * Returns values between 0 and 31, representing 1/32 to 32/32 (=1)
436 436
      * \sa setCoolStepConfiguration()
437 437
      */
438
-    unsigned char getCurrentCSReading(void);
438
+    uint8_t getCurrentCSReading(void);
439 439
 
440 440
 
441 441
     /*!
@@ -451,7 +451,7 @@ class TMC26XStepper {
451 451
      * may not be the fastest.
452 452
      * \sa getCurrentCSReading(), getResistor(), isCurrentScalingHalfed(), getCurrent()
453 453
      */
454
-    unsigned int getCurrentCurrent(void);
454
+    uint16_t getCurrentCurrent(void);
455 455
 
456 456
     /*!
457 457
      * \brief checks if there is a StallGuard warning in the last status
@@ -552,56 +552,54 @@ class TMC26XStepper {
552 552
      * \brief Returns the current sense resistor value in milliohm.
553 553
      * The default value of ,15 Ohm will return 150.
554 554
      */
555
-    int getResistor();
555
+    int16_t getResistor();
556 556
 
557 557
     /*!
558 558
      * \brief Prints out all the information that can be found in the last status read out - it does not force a status readout.
559 559
      * The result is printed via Serial
560 560
      */
561 561
     void debugLastStatus(void);
562
+
562 563
     /*!
563 564
      * \brief library version
564 565
      * \return the version number as int.
565 566
      */
566
-    int version(void);
567
+    int16_t version(void);
567 568
 
568 569
   private:
569
-    unsigned int steps_left;    // The steps the motor has to do to complete the movement
570
-    int direction;              // Direction of rotation
571
-    unsigned long step_delay;   // Delay between steps, in ms, based on speed
572
-    int number_of_steps;        // Total number of steps this motor can take
573
-    unsigned int speed;         // Store the current speed in order to change the speed after changing microstepping
574
-    unsigned int resistor;      // Current sense resitor value in milliohm
570
+    uint16_t steps_left;      // The steps the motor has to do to complete the movement
571
+    int16_t direction;        // Direction of rotation
572
+    uint32_t step_delay;      // Delay between steps, in ms, based on speed
573
+    int16_t number_of_steps;  // Total number of steps this motor can take
574
+    uint16_t speed;           // Store the current speed in order to change the speed after changing microstepping
575
+    uint16_t resistor;        // Current sense resitor value in milliohm
575 576
 
576
-    unsigned long last_step_time;   // Time stamp in ms of when the last step was taken
577
-    unsigned long next_step_time;   // Time stamp in ms of when the last step was taken
577
+    uint32_t last_step_time,  // Timestamp (ms) of the last step
578
+             next_step_time;  // Timestamp (ms) of the next step
578 579
 
579 580
     // Driver control register copies to easily set & modify the registers
580
-    unsigned long driver_control_register_value;
581
-    unsigned long chopper_config_register;
582
-    unsigned long cool_step_register_value;
583
-    unsigned long stall_guard2_current_register_value;
584
-    unsigned long driver_configuration_register_value;
585
-    // The driver status result
586
-    unsigned long driver_status_result;
581
+    uint32_t driver_control_register_value,
582
+             chopper_config_register,
583
+             cool_step_register_value,
584
+             stallguard2_current_register_value,
585
+             driver_configuration_register_value,
586
+             driver_status_result; // The driver status result
587 587
 
588 588
     // Helper routione to get the top 10 bit of the readout
589
-    inline int getReadoutValue();
589
+    inline int16_t getReadoutValue();
590 590
 
591 591
     // The pins for the stepper driver
592
-    unsigned char cs_pin;
593
-    unsigned char step_pin;
594
-    unsigned char dir_pin;
592
+    uint8_t cs_pin, step_pin, dir_pin;
595 593
 
596 594
     // Status values
597 595
     boolean started; // If the stepper has been started yet
598
-    int microsteps; // The current number of micro steps
596
+    int16_t microsteps; // The current number of micro steps
599 597
     char constant_off_time; // We need to remember this value in order to enable and disable the motor
600
-    unsigned char cool_step_lower_threshold; //  we need to remember the threshold to enable and disable the CoolStep feature
598
+    uint8_t cool_step_lower_threshold; //  we need to remember the threshold to enable and disable the CoolStep feature
601 599
     boolean cool_step_enabled; // We need to remember this to configure the coolstep if it si enabled
602 600
 
603 601
     // SPI sender
604
-    inline void send262(unsigned long datagram);
602
+    inline void send262(uint32_t datagram);
605 603
 };
606 604
 
607 605
 #endif // _TMC26XSTEPPER_H_

Loading…
Annulla
Salva