Stepper driver tweaks. Seems to run well.

diff --git a/src/config.h b/src/config.h
index 374462b45d3bd147e4b14e1b1181fd2c013e60bd..174171e4393101e849ce3555121ddedaae8d3833 100644 (file)
--- a/src/config.h
+++ b/src/config.h
@@ -31,7 +31,7 @@
 
 
 // Compile-time settings
-#define __STEP_CORRECTION
+//#define __STEP_CORRECTION
 //#define __JERK_EXEC            // Use computed jerk (vs. forward difference)
 //#define __KAHAN                // Use Kahan summation in aline exec functions
 #define __CLOCK_EXTERNAL_16MHZ   // uses PLL to provide 32 MHz system clock
@@ -61,14 +61,14 @@ typedef enum {
 
 #define M1_MOTOR_MAP             AXIS_X
 #define M1_STEP_ANGLE            1.8
-#define M1_TRAVEL_PER_REV        3.175
+#define M1_TRAVEL_PER_REV        6.35
 #define M1_MICROSTEPS            MOTOR_MICROSTEPS
 #define M1_POLARITY              MOTOR_POLARITY_NORMAL
 #define M1_POWER_MODE            MOTOR_POWER_MODE
 
 #define M2_MOTOR_MAP             AXIS_Y
 #define M2_STEP_ANGLE            1.8
-#define M2_TRAVEL_PER_REV        3.175
+#define M2_TRAVEL_PER_REV        6.35
 #define M2_MICROSTEPS            MOTOR_MICROSTEPS
 #define M2_POLARITY              MOTOR_POLARITY_NORMAL
 #define M2_POWER_MODE            MOTOR_POWER_MODE
@@ -82,7 +82,7 @@ typedef enum {
 
 #define M4_MOTOR_MAP             AXIS_Z
 #define M4_STEP_ANGLE            1.8
-#define M4_TRAVEL_PER_REV        3.175
+#define M4_TRAVEL_PER_REV        (25.4 / 6.0)
 #define M4_MICROSTEPS            MOTOR_MICROSTEPS
 #define M4_POLARITY              MOTOR_POLARITY_NORMAL
 #define M4_POWER_MODE            MOTOR_POWER_MODE
@@ -352,7 +352,7 @@ typedef enum {
 #define TMC2660_SPI_MOSI_PIN   7
 #define TMC2660_TIMER          TCC1
 #define TMC2660_TIMER_ENABLE   TC_CLKSEL_DIV64_gc
-#define TMC2660_POLL_RATE      0.001 // sec.  Must be in (0, 1]
+#define TMC2660_POLL_RATE      0.01 // sec.  Must be in (0, 1]
 #define TMC2660_STABILIZE_TIME 0.001 // sec.  Must be at least 1ms
 
 

diff --git a/src/motor.c b/src/motor.c
index 6d9438010986f48c8c6da79e42950f8395b3132c..4a45be771e60cdced8d7d3b4fe1decd875466be0 100644 (file)
--- a/src/motor.c
+++ b/src/motor.c
@@ -70,11 +70,12 @@ typedef struct {
   uint32_t timeout;
   cmMotorFlags_t flags;
   int32_t encoder;
+  uint8_t last_clock;
 
   // Move prep
   uint8_t timer_clock;           // clock divisor setting or zero for off
   uint16_t timer_period;         // clock period counter
-  int32_t steps;                 // expected steps, for encoder
+  bool positive;                 // step sign
   cmDirection_t direction;       // travel direction corrected for polarity
 
   // Step error correction
@@ -141,6 +142,7 @@ ISR(TCE1_CCA_vect) {
 }
 
 
+#if 0
 ISR(DMA_CH0_vect) {
   M1_TIMER.CTRLA = 0; // Top motor clock
   M1_DMA_CH.CTRLB |= DMA_CH_TRNIF_bm; // Clear interrupt flag
@@ -163,6 +165,7 @@ ISR(DMA_CH3_vect) {
   M4_TIMER.CTRLA = 0; // Top motor clock
   M4_DMA_CH.CTRLB |= DMA_CH_TRNIF_bm; // Clear interrupt flag
 }
+#endif
 
 
 void motor_init() {
@@ -282,8 +285,7 @@ stat_t motor_power_callback() { // called by controller
   for (int motor = 0; motor < MOTORS; motor++)
     // Deenergize motor if disabled, in error or after timeout when not holding
     if (motors[motor].power_mode == MOTOR_DISABLED || _error(motor) ||
-        (cm_get_combined_state() != COMBINED_HOLD &&
-         motors[motor].timeout < rtc_get_time()))
+        motors[motor].timeout < rtc_get_time())
       _deenergize(motor);
 
   return STAT_OK;
@@ -310,21 +312,6 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps,
                      float error) {
   motor_t *m = &motors[motor];
 
-  // Power motor
-  switch (motors[motor].power_mode) {
-  case MOTOR_DISABLED: return;
-
-  case MOTOR_POWERED_ONLY_WHEN_MOVING:
-    if (fp_ZERO(travel_steps)) return; // Not moving
-    // Fall through
-
-  case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE:
-    _energize(motor);
-    break;
-
-  case MOTOR_POWER_MODE_MAX_VALUE: break; // Shouldn't get here
-  }
-
 #ifdef __STEP_CORRECTION
   // 'Nudge' correction strategy. Inject a single, scaled correction value
   // then hold off
@@ -344,25 +331,35 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps,
   }
 #endif
 
+  // Power motor
+  switch (motors[motor].power_mode) {
+  case MOTOR_DISABLED: return;
+
+  case MOTOR_POWERED_ONLY_WHEN_MOVING:
+    if (fp_ZERO(travel_steps)) return; // Not moving
+    // Fall through
+
+  case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE:
+    _energize(motor);
+    break;
+
+  case MOTOR_POWER_MODE_MAX_VALUE: break; // Shouldn't get here
+  }
+
   // Compute motor timer clock and period. Rounding is performed to eliminate
   // a negative bias in the uint32_t conversion that results in long-term
   // negative drift.
-  uint16_t steps = round(fabs(travel_steps / 2));
-  // TODO why do we need to multiply by 2 here?
-  uint32_t ticks_per_step = seg_clocks / steps;
+  uint32_t ticks_per_step = round(fabs(seg_clocks / travel_steps));
 
   // Find the clock rate that will fit the required number of steps
   if (ticks_per_step & 0xffff0000UL) {
     ticks_per_step /= 2;
-    seg_clocks /= 2;
 
     if (ticks_per_step & 0xffff0000UL) {
       ticks_per_step /= 2;
-      seg_clocks /= 2;
 
       if (ticks_per_step & 0xffff0000UL) {
         ticks_per_step /= 2;
-        seg_clocks /= 2;
 
         if (ticks_per_step & 0xffff0000UL) m->timer_clock = 0; // Off, too slow
         else m->timer_clock = TC_CLKSEL_DIV8_gc;
@@ -370,28 +367,43 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps,
     } else m->timer_clock = TC_CLKSEL_DIV2_gc;
   } else m->timer_clock = TC_CLKSEL_DIV1_gc;
 
+  if (!ticks_per_step || fabs(travel_steps) < 0.001) m->timer_clock = 0;
   m->timer_period = ticks_per_step;
-  m->steps = steps;
+  m->positive = 0 <= travel_steps;
 
   // Setup the direction, compensating for polarity.
-  if (0 <= travel_steps) m->direction = DIRECTION_CW ^ m->polarity;
-  else {
-    m->direction = DIRECTION_CCW ^ m->polarity;
-    m->steps *= -1;
-  }
+  if (m->positive) m->direction = DIRECTION_CW ^ m->polarity;
+  else m->direction = DIRECTION_CCW ^ m->polarity;
 }
 
 
 void motor_load_move(int motor) {
   motor_t *m = &motors[motor];
 
-  // Setup DMA count
-  m->dma->TRFCNT = m->steps < 0 ? -m->steps : m->steps;
+  // Get actual step count from DMA channel
+  uint16_t steps = 0xffff - m->dma->TRFCNT;
+  m->dma->TRFCNT = 0xffff;
+  m->dma->CTRLA |= DMA_CH_ENABLE_bm;
+
+  // Adjust clock count
+  if (m->last_clock) {
+    uint32_t count = m->timer->CNT;
+    int8_t freq_change = m->last_clock - m->timer_clock;
+
+    count <<= freq_change; // Adjust count
+
+    if (m->timer_period < count) count -= m->timer_period;
+    if (m->timer_period < count) count -= m->timer_period;
+    if (m->timer_period < count) count = m->timer_period / 2;
+
+    m->timer->CNT = count;
+
+  } else m->timer->CNT = m->timer_period / 2;
 
   // Set or zero runtime clock and period
-  m->timer->CNT = 0;                   // Start at zero
   m->timer->CCA = m->timer_period;     // Set frequency
   m->timer->CTRLA = m->timer_clock;    // Start or stop
+  m->last_clock = m->timer_clock;
   m->timer_clock = 0;                  // Clear clock
 
   // Set direction
@@ -399,8 +411,13 @@ void motor_load_move(int motor) {
   else m->port->OUTSET = DIRECTION_BIT_bm;
 
   // Accumulate encoder
-  m->encoder += m->steps;
-  m->steps = 0;
+  m->encoder += m->positive ? steps : -steps;
+}
+
+
+void motor_end_move(int motor) {
+  motors[motor].dma->CTRLA &= ~DMA_CH_ENABLE_bm;
+  motors[motor].timer->CTRLA = 0; // Stop clock
 }
 
 

diff --git a/src/motor.h b/src/motor.h
index bbf149a6212f8749290d404d84293ced9ead3df3..ab431f7389cc7301846e29291f9f7cbbe4bee710 100644 (file)
--- a/src/motor.h
+++ b/src/motor.h
@@ -79,3 +79,4 @@ void motor_error_callback(int motor, cmMotorFlags_t errors);
 void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps,
                      float error);
 void motor_load_move(int motor);
+void motor_end_move(int motor);

diff --git a/src/stepper.c b/src/stepper.c
index 91f3bb35dc024e3979c800bccc6727f0e552cc03..409abff7b15c1345caa079afa8094548d8e2fcc8 100644 (file)
--- a/src/stepper.c
+++ b/src/stepper.c
@@ -44,6 +44,7 @@
 typedef struct {
   // Runtime
   bool busy;
+  bool requesting;
   uint16_t dwell;
 
   // Move prep
@@ -81,10 +82,14 @@ uint8_t st_runtime_isbusy() {return st.busy;}
 /// ADC channel 0 triggered by load ISR as a "software" interrupt.
 ISR(ADCB_CH0_vect) {
   mp_exec_move();
+  st.requesting = false;
 }
 
 
 static void _request_exec_move() {
+  if (st.requesting) return;
+  st.requesting = true;
+
   // Use ADC as a "software" interrupt to trigger next move exec
   ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // LO level interrupt
   ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm;
@@ -101,9 +106,12 @@ ISR(STEP_TIMER_ISR) {
   st.busy = false;
   TIMER_STEP.PER = STEP_TIMER_POLL;
 
-  // If there are no more moves try to load one
+  for (int motor = 0; motor < MOTORS; motor++)
+    motor_end_move(motor);
+
+  // If the next move is not ready try to load it
   if (!st.move_ready) {
-    mp_exec_move();
+    _request_exec_move();
     return;
   }
 

diff --git a/src/tmc2660.c b/src/tmc2660.c
index ae0ec007dff050ce2298086ad7f32598a1d85b4d..b2d1bdb9c65a434559d0d2860b6d7e0ac6000f1d 100644 (file)
--- a/src/tmc2660.c
+++ b/src/tmc2660.c
@@ -71,7 +71,7 @@ static const uint32_t reg_addrs[] = {
   TMC2660_CHOPCONF_ADDR,
   TMC2660_SMARTEN_ADDR,
   TMC2660_SGCSCONF_ADDR,
-  TMC2660_DRVCONF_ADDR
+  TMC2660_DRVCONF_ADDR,
 };
 
 
@@ -160,6 +160,7 @@ static void _driver_write(int driver) {
     break;
 
   case TMC2660_STATE_RECONFIGURE:
+    // Disable MOSFETs during configuration
     spi.out = TMC2660_CHOPCONF_ADDR | (drv->regs[TMC2660_CHOPCONF] & 0xffff0);
     break;
   }
@@ -281,21 +282,25 @@ void tmc2660_init() {
 
     drivers[i].regs[TMC2660_DRVCTRL] = TMC2660_DRVCTRL_DEDGE | mstep |
       (MOTOR_MICROSTEPS == 16 ? TMC2660_DRVCTRL_INTPOL : 0);
+
     drivers[i].regs[TMC2660_CHOPCONF] = TMC2660_CHOPCONF_TBL_16 |
       TMC2660_CHOPCONF_HEND(3) | TMC2660_CHOPCONF_HSTART(7) |
       TMC2660_CHOPCONF_TOFF(4);
     //drivers[i].regs[TMC2660_CHOPCONF] = TMC2660_CHOPCONF_TBL_36 |
     //  TMC2660_CHOPCONF_CHM | TMC2660_CHOPCONF_HEND(7) |
     //  TMC2660_CHOPCONF_FASTD(6) | TMC2660_CHOPCONF_TOFF(7);
+
     drivers[i].regs[TMC2660_SMARTEN] = TMC2660_SMARTEN_SEIMIN |
       TMC2660_SMARTEN_SE(350, 450);
     drivers[i].regs[TMC2660_SMARTEN] = 0; // Disable CoolStep
+
     drivers[i].regs[TMC2660_SGCSCONF] = TMC2660_SGCSCONF_SFILT |
       TMC2660_SGCSCONF_THRESH(63);
+
     drivers[i].regs[TMC2660_DRVCONF] = TMC2660_DRVCONF_RDSEL_SG;
 
     set_power_level(i, MOTOR_IDLE_CURRENT);
-    drivers[i].reconfigure = false; // No need to reconfigure
+    drivers[i].reconfigure = false; // No need to reconfigure after init
   }
 
   // Setup pins
@@ -342,13 +347,21 @@ void tmc2660_init() {
 }
 
 
-uint8_t tmc2660_flags(int motor) {
-  return motor < MOTORS ? drivers[motor].flags : 0;
+static void _set_reg(int motor, int reg, uint32_t value) {
+  if (drivers[motor].regs[reg] == value) return;
+
+  drivers[motor].regs[reg] = value;
+  drivers[motor].reconfigure = true;
 }
 
 
-void tmc2660_reconfigure(int motor) {
-  if (motor < MOTORS) drivers[motor].reconfigure = true;
+static uint32_t _get_reg(int motor, int reg) {
+  return drivers[motor].regs[reg];
+}
+
+
+uint8_t tmc2660_flags(int motor) {
+  return motor < MOTORS ? drivers[motor].flags : 0;
 }
 
 
@@ -366,31 +379,28 @@ stat_t tmc2660_sync() {
 
 
 void tmc2660_enable(int driver) {
+  // TODO MOTOR_CURRENT should be configurable
   set_power_level(driver, MOTOR_CURRENT);
 }
 
 
 void tmc2660_disable(int driver) {
+  // TODO MOTOR_IDLE_CURRENT should be configurable
   set_power_level(driver, MOTOR_IDLE_CURRENT);
 }
 
 
 float get_power_level(int motor) {
-  uint8_t x = drivers[motor].regs[TMC2660_SGCSCONF] & 31;
-  return (x + 1) / 32.0;
+  return (_get_reg(motor, TMC2660_SGCSCONF) & 31) / 31.0;
 }
 
 
 void set_power_level(int motor, float value) {
   if (value < 0 || 1 < value) return;
 
-  uint8_t x = value ? value * 32.0 - 1 : 0;
-  if (x < 0) x = 0;
-
-  tmc2660_driver_t *d = &drivers[motor];
-  d->regs[TMC2660_SGCSCONF] = (d->regs[TMC2660_SGCSCONF] & ~31) | x;
-
-  tmc2660_reconfigure(motor);
+  uint32_t reg =
+    (_get_reg(motor, TMC2660_SGCSCONF) & ~31) | (uint8_t)(value * 31.0);
+  _set_reg(motor, TMC2660_SGCSCONF, reg);
 }
 
 
@@ -400,7 +410,7 @@ uint16_t get_sg_value(int motor) {
 
 
 int8_t get_stallguard(int motor) {
-  uint8_t x = (drivers[motor].regs[TMC2660_SGCSCONF] & 0x7f00) >> 8;
+  uint8_t x = (_get_reg(motor, TMC2660_SGCSCONF) & 0x7f00) >> 8;
   return (x & (1 << 6)) ? (x & 0xc0) : x;
 }
 
@@ -408,9 +418,7 @@ int8_t get_stallguard(int motor) {
 void set_stallguard(int motor, int8_t value) {
   if (value < -64 || 63 < value) return;
 
-  tmc2660_driver_t *d = &drivers[motor];
-  d->regs[TMC2660_SGCSCONF] = (d->regs[TMC2660_SGCSCONF] & ~0x7f00) |
-    TMC2660_SGCSCONF_THRESH(value);
-
-  tmc2660_reconfigure(motor);
+  _set_reg(motor, TMC2660_SGCSCONF,
+           (_get_reg(motor, TMC2660_SGCSCONF) & ~0x7f00) |
+           TMC2660_SGCSCONF_THRESH(value));
 }