Working with current ramping

diff --git a/src/config.h b/src/config.h
index d9345b67f5fd3162add5596a4cad7d4ca455368b..2e062dc7012c27af9585169c7fee7ebcfdd9dc12 100644 (file)
--- a/src/config.h
+++ b/src/config.h
@@ -103,7 +103,7 @@ typedef enum {
 #define JOG_ACCELERATION         500000 // mm/min^2
 
 // Axis settings
-#define VELOCITY_MAX             15000  // mm/min
+#define VELOCITY_MAX             13000  // mm/min
 #define FEEDRATE_MAX             VELOCITY_MAX
 
 #define X_AXIS_MODE              AXIS_STANDARD // See canonical_machine.h
@@ -133,7 +133,7 @@ typedef enum {
 #define Y_ZERO_BACKOFF           1
 
 #define Z_AXIS_MODE              AXIS_STANDARD
-#define Z_VELOCITY_MAX           VELOCITY_MAX
+#define Z_VELOCITY_MAX           2000 //VELOCITY_MAX
 #define Z_FEEDRATE_MAX           FEEDRATE_MAX
 #define Z_TRAVEL_MIN             0
 #define Z_TRAVEL_MAX             75
@@ -335,7 +335,7 @@ typedef enum {
 #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_STABILIZE_TIME 0.001 // sec.  Must be at least 1ms
+#define TMC2660_STABILIZE_TIME 0.01 // sec.  Must be at least 1ms
 
 
 // PWM settings

diff --git a/src/motor.c b/src/motor.c
index 6b48da612ee8e596386831506296f10ad3ea4859..26a87adf1463fa8d97ec201553f898e604501c13 100644 (file)
--- a/src/motor.c
+++ b/src/motor.c
@@ -195,7 +195,7 @@ void motor_init() {
 
 
 void motor_enable(int motor, bool enable) {
-  if (enable) motors[motor].port->OUTCLR = MOTOR_ENABLE_BIT_bm;
+  if (enable) motors[motor].port->OUTCLR = MOTOR_ENABLE_BIT_bm; // Active low
   else {
     motors[motor].port->OUTSET = MOTOR_ENABLE_BIT_bm;
     motors[motor].power_state = MOTOR_IDLE;
@@ -315,6 +315,56 @@ void motor_error_callback(int motor, cmMotorFlags_t errors) {
 }
 
 
+void motor_load_move(int motor) {
+  motor_t *m = &motors[motor];
+
+  // Get actual step count from DMA channel
+  uint16_t steps = 0xffff - m->dma->TRFCNT;
+  m->dma->TRFCNT = 0xffff; // Reset DMA channel counter
+  m->dma->CTRLB = DMA_CH_CHBUSY_bm | DMA_CH_CHPEND_bm;
+  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->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
+  if (m->direction == DIRECTION_CW) m->port->OUTCLR = DIRECTION_BIT_bm;
+  else m->port->OUTSET = DIRECTION_BIT_bm;
+
+  // Accumulate encoder
+  // TODO we currently accumulate the x-axis here
+  if (!motor) {
+    steps = m->steps;
+    m->steps = 0;
+  }
+  m->encoder += m->positive ? steps : -(int32_t)steps;
+}
+
+
+void motor_end_move(int motor) {
+  motors[motor].dma->CTRLA &= ~DMA_CH_ENABLE_bm;
+  motors[motor].timer->CTRLA = 0; // Stop clock
+}
+
+
 void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps,
                      float error) {
   motor_t *m = &motors[motor];
@@ -378,61 +428,23 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps,
   m->timer_period = ticks_per_step;
   m->positive = 0 <= travel_steps;
 
+  // Sanity check steps
+  if (m->timer_clock) {
+    uint32_t clocks = seg_clocks >> (m->timer_clock - 1); // Motor timer clocks
+    float steps = (float)clocks / m->timer_period;
+    float diff = fabs(fabs(travel_steps) - steps);
+    if (10 < diff)
+      printf_P(PSTR("clock=%u period=%u expected=%f actual=%f diff=%f\n"),
+               m->timer_clock, m->timer_period, fabs(travel_steps), steps,
+               diff);
+  }
+
   // Setup the direction, compensating for polarity.
   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];
-
-  // Get actual step count from DMA channel
-  uint16_t steps = 0xffff - m->dma->TRFCNT;
-  m->dma->TRFCNT = 0xffff;
-  m->dma->CTRLB = DMA_CH_CHBUSY_bm | DMA_CH_CHPEND_bm;
-  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->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
-  if (m->direction == DIRECTION_CW) m->port->OUTCLR = DIRECTION_BIT_bm;
-  else m->port->OUTSET = DIRECTION_BIT_bm;
-
-  // Accumulate encoder
-  if (!motor) {
-    steps = m->steps;
-    m->steps = 0;
-  }
-  m->encoder += m->positive ? steps : -(int32_t)steps;
-}
-
-
-void motor_end_move(int motor) {
-  motors[motor].dma->CTRLA &= ~DMA_CH_ENABLE_bm;
-  motors[motor].timer->CTRLA = 0; // Stop clock
-}
-
-
 // Var callbacks
 float get_step_angle(int motor) {
   return motors[motor].step_angle;
@@ -542,6 +554,12 @@ void print_status_flags(uint8_t flags) {
     first = false;
   }
 
+  if (MOTOR_FLAG_OPEN_LOAD_bm & flags) {
+    if (!first) printf_P(PSTR(", "));
+    printf_P(PSTR("open"));
+    first = false;
+  }
+
   putchar('"');
 }
 

diff --git a/src/motor.h b/src/motor.h
index 714bb60eddb2e98ec5d91474023fd97af06afc82..dbeac21e0b8cf9b61b46b3ab5ba1b7d7b6506cf9 100644 (file)
--- a/src/motor.h
+++ b/src/motor.h
@@ -39,6 +39,7 @@ typedef enum {
   MOTOR_FLAG_OVERTEMP_WARN_bm = 1 << 2,
   MOTOR_FLAG_OVERTEMP_bm      = 1 << 3,
   MOTOR_FLAG_SHORTED_bm       = 1 << 4,
+  MOTOR_FLAG_OPEN_LOAD_bm     = 1 << 5,
   MOTOR_FLAG_ERROR_bm         = (//MOTOR_FLAG_STALLED_bm | TODO revisit this
                                  MOTOR_FLAG_OVERTEMP_WARN_bm |
                                  MOTOR_FLAG_OVERTEMP_bm |
@@ -79,7 +80,7 @@ void motor_driver_callback(int motor);
 stat_t motor_power_callback();
 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);
+void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps,
+                     float error);

diff --git a/src/tmc2660.c b/src/tmc2660.c
index a697c048697461e4aaf1ff399813e33d31704b27..dcd0e8abd08dde336d12ebd2a2f2b1ecfd63785b 100644 (file)
--- a/src/tmc2660.c
+++ b/src/tmc2660.c
@@ -48,15 +48,16 @@
 typedef struct {
   bool wrote_data;
   bool configured;
-  bool reset;
   uint32_t next_cmd;
   uint32_t stabilizing;
 
   uint16_t sguard;
   uint8_t flags;
-  uint32_t last_regs[REGS];
+  uint8_t reg_valid;
   uint32_t regs[REGS];
 
+  float current;
+  float target_current;
   float idle_current;
   float drive_current;
 
@@ -114,6 +115,9 @@ static void _report_error_flags(int driver) {
 
   if (TMC2660_DRVSTATUS_OVERTEMP & dflags) mflags |= MOTOR_FLAG_OVERTEMP_bm;
 
+  if ((TMC2660_DRVSTATUS_OPEN_LOAD_A | TMC2660_DRVSTATUS_OPEN_LOAD_A) & dflags)
+    mflags |= MOTOR_FLAG_OPEN_LOAD_bm;
+
   if (drv->port->IN & FAULT_BIT_bm) mflags |= MOTOR_FLAG_STALLED_bm;
 
   if (mflags) motor_error_callback(driver, mflags);
@@ -158,6 +162,9 @@ static void _driver_write(int driver) {
 }
 
 
+static void _set_current(int motor, float value);
+
+
 // Returns true if the current driver has more data to send
 static bool _driver_read(int driver) {
   tmc2660_driver_t *drv = &drivers[driver];
@@ -182,19 +189,22 @@ static bool _driver_read(int driver) {
     _report_error_flags(driver);
   }
 
-  // Handle reset
-  if (drv->reset) {
-    drv->reset = false;
-    for (int i = 0; i < REGS; i++) drv->last_regs[i] = -1;
+  // Update current
+  if (drv->target_current != drv->current) {
+    if (!drv->current) drv->current = 0.05;
+    else drv->current *= 1.05;
+    if (drv->target_current < drv->current) drv->current = drv->target_current;
+
+    _set_current(driver, drv->current);
   }
 
-  // Check if regs have changed (skipping DRVCTRL)
-  for (int i = 1; i < REGS; i++)
-    if (drv->last_regs[i] != drv->regs[i]) {
+  // Check if regs have changed
+  for (int i = 0; i < REGS; i++)
+    if (!(drv->reg_valid & (1 << i))) {
       // Reg changed, update driver
-      drv->last_regs[i] = drv->regs[i];
+      drv->reg_valid |= 1 << i;
       drv->next_cmd = reg_addrs[i] | drv->regs[i];
-      drv->stabilizing = rtc_get_time() + TMC2660_STABILIZE_TIME * 1000;
+      //drv->stabilizing = rtc_get_time() + TMC2660_STABILIZE_TIME * 1000;
       drv->configured = false;
 
       return true;
@@ -220,6 +230,8 @@ static bool _driver_read(int driver) {
 static void _spi_next() {
   bool hasMore = _driver_read(spi.driver);
 
+  //if (!hasMore) drivers[spi.driver].reg_valid = 0;
+
   if (!hasMore && ++spi.driver == MOTORS) {
     spi.driver = 0;
     TMC2660_TIMER.CTRLA = TMC2660_TIMER_ENABLE;
@@ -256,9 +268,6 @@ ISR(PORT_4_FAULT_ISR_vect) {_fault_isr(3);}
 void tmc2660_init() {
   // Configure motors
   for (int i = 0; i < MOTORS; i++) {
-    for (int j = 0; j < REGS; j++)
-      drivers[i].last_regs[j] = -1; // Force config
-
     drivers[i].idle_current = MOTOR_IDLE_CURRENT;
     drivers[i].drive_current = MOTOR_CURRENT;
 
@@ -348,6 +357,7 @@ static void _set_reg(int motor, int reg, uint32_t value) {
   if (drv->regs[reg] == value) return;
 
   drv->regs[reg] = value;
+  drv->reg_valid &= ~(1 << reg);
   drv->configured = false;
 }
 
@@ -377,7 +387,7 @@ uint8_t tmc2660_flags(int motor) {
 
 
 void tmc2660_reset(int driver) {
-  drivers[driver].reset = true;
+  drivers[driver].reg_valid = 0;
 }
 
 
@@ -397,20 +407,27 @@ stat_t tmc2660_sync() {
 void tmc2660_enable(int driver) {
   printf("Enable %d\n", driver);
   tmc2660_reset(driver);
-  _set_current(driver, drivers[driver].drive_current);
+  cli();
+  drivers[driver].target_current = drivers[driver].drive_current;
+  sei();
+  //_set_current(driver, drivers[driver].drive_current);
 }
 
 
 void tmc2660_disable(int driver) {
   printf("Disable %d\n", driver);
-  _set_current(driver, drivers[driver].idle_current);
+  cli();
+  drivers[driver].target_current = drivers[driver].idle_current;
+  sei();
+  //_set_current(driver, drivers[driver].idle_current);
 }
 
 
 void tmc2660_set_stallguard_threshold(int driver, int8_t threshold) {
-  drivers[driver].regs[TMC2660_SGCSCONF] =
-    (drivers[driver].regs[TMC2660_SGCSCONF] & ~TMC2660_SGCSCONF_THRESH_bm) |
-    TMC2660_SGCSCONF_THRESH(threshold);
+  uint32_t value =
+    _get_reg(driver, TMC2660_SGCSCONF) & ~TMC2660_SGCSCONF_THRESH_bm;
+  value |= TMC2660_SGCSCONF_THRESH(threshold);
+  _set_reg(driver, TMC2660_SGCSCONF, value);
 }
 
 

diff --git a/tmc2660_decode.py b/tmc2660_decode.py
index 4056d6b2433d5cc9eaf79ca53cac9abee4bd2389..1b5adb3f7991c3ffd4eb607217bcf59202730698 100755 (executable)
--- a/tmc2660_decode.py
+++ b/tmc2660_decode.py
@@ -14,6 +14,7 @@ def twos_comp(val, bits):
 
 
 def tmc2660_decode_response(x, rdsel = 1):
+    x >>= 4 # Shift right 4 bits
     d = {'_hex': '0x%05x' % x}
 
     if rdsel == 0:
@@ -27,14 +28,14 @@ def tmc2660_decode_response(x, rdsel = 1):
         d['se'] = (x >> 10) & 0x1f
 
     flags = []
-    if x & (1 << 7): flags += ['Standstill']
-    if x & (1 << 6): flags += ['Open B']
-    if x & (1 << 5): flags += ['Open A']
-    if x & (1 << 4): flags += ['Short B']
-    if x & (1 << 3): flags += ['Short A']
-    if x & (1 << 2): flags += ['Temp warn']
-    if x & (1 << 1): flags += ['Overtemp']
-    if x & (1 << 0): flags += ['Stall']
+    if x & (1 << 7): flags += ['stand']
+    if x & (1 << 6): flags += ['open B']
+    if x & (1 << 5): flags += ['open A']
+    if x & (1 << 4): flags += ['short B']
+    if x & (1 << 3): flags += ['short A']
+    if x & (1 << 2): flags += ['temp warn']
+    if x & (1 << 1): flags += ['overtemp']
+    if x & (1 << 0): flags += ['stall']
 
     d['flags'] = flags
 
@@ -78,11 +79,12 @@ def tmc2660_decode_cmd(x, r):
 
     elif addr == 5:
         cmd = 'SMARTEN'
-        d['SEIMIN'] = '1/2 CS' if x & (1 << 15) else '1/4 CS'
+        d['SEIMIN'] = '1/4 CS' if x & (1 << 15) else '1/2 CS'
         d['SEDN'] = (32, 8, 2, 1)[(x >> 13) & 3]
         d['SEMAX'] = (x >> 8) & 0xf
         d['SEUP'] = (1, 2, 4, 8)[(x >> 5) & 3]
-        d['SEMIN'] = x & 0xf
+        semin = x & 0xf
+        d['SEMIN'] = semin if semin else 'disabled'
 
     elif addr == 6:
         cmd = 'SGCSCONF'
@@ -117,10 +119,9 @@ def tmc2660_decode(path):
                 first = False
                 continue
 
-            mosi = int(mosi, 16)
-            miso = int(miso, 16)
-
-            cmd = tmc2660_decode_cmd(mosi, miso)
+            cmd = tmc2660_decode_cmd(int(mosi, 0), int(miso, 0))
+            cmd['id'] = int(packet)
+            cmd['ts'] = float(time)
 
             print(json.dumps(cmd, sort_keys = True))