New motor driver code working well

diff --git a/avr/src/motor.c b/avr/src/motor.c
index 4246bb519e908ac922d056325062db6cc285cd17..e46aef370c47f8dcceb056a7a82e7aa19d88f8a8 100644 (file)
--- a/avr/src/motor.c
+++ b/avr/src/motor.c
@@ -348,32 +348,26 @@ void motor_error_callback(int motor, motor_flags_t errors) {
 static void _load_move(int motor) {
   motor_t *m = &motors[motor];
 
-  // Stop clock & DMA
+  // Stop clock
   m->timer->CTRLA = 0;
-  m->dma->CTRLA &= ~DMA_CH_ENABLE_bm;
-  m->dma->CTRLB |= DMA_CH_TRNIF_bm | DMA_CH_ERRIF_bm; // Clear interrupt flags
+
+  // Clear DMA interrupt flags
+  m->dma->CTRLB |= DMA_CH_TRNIF_bm | DMA_CH_ERRIF_bm;
+
+  m->active = m->timer_period && m->timer_clock;
+  if (!m->active) return;
 
   // Set direction
   if (m->clockwise) OUTCLR_PIN(m->dir_pin);
   else OUTSET_PIN(m->dir_pin);
 
-  // Set power
-  drv8711_set_power(motor, m->power);
-
-  // Get clocks remaining in segment
-  //uint32_t clocks =
-  //  (uint32_t)(SEGMENT_PERIOD - TIMER_STEP.CNT) * STEP_TIMER_DIV;
-
   // Count steps with phony DMA transfer
   m->dma->TRFCNT = m->steps;
   m->dma->CTRLA |= DMA_CH_ENABLE_bm;
 
   // Set clock and period
-  if ((m->active = m->timer_period && m->timer_clock)) {
-    m->timer->CNT = 0;
-    m->timer->CCA = m->timer_period;     // Set step pulse period
-    m->timer->CTRLA = m->timer_clock;    // Set clock rate
-  }
+  m->timer->CCA = m->timer_period;     // Set step pulse period
+  m->timer->CTRLA = m->timer_clock;    // Set clock rate
 }
 
 
@@ -402,26 +396,28 @@ stat_t motor_prep_move(int motor, int32_t target) {
 
   // Set direction, compensating for polarity
   bool negative = steps < 0;
-  m->clockwise = !(negative ^ m->reverse);
+  bool clockwise = !(negative ^ m->reverse);
 
   // Positive steps from here on
   if (negative) steps = -steps;
-  m->steps = steps;
 
   // Find the fastest clock rate that will fit the required number of steps
-  uint32_t ticks_per_step = SEGMENT_CLOCKS / m->steps;
-  if (ticks_per_step <= 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc;
-  else if (ticks_per_step <= 0x1ffff) m->timer_clock = TC_CLKSEL_DIV2_gc;
-  else if (ticks_per_step <= 0x3ffff) m->timer_clock = TC_CLKSEL_DIV4_gc;
-  else if (ticks_per_step <= 0x7ffff) m->timer_clock = TC_CLKSEL_DIV8_gc;
-  else m->timer_clock = 0; // Clock off, too slow
+  // Note, clock toggles step line so we need two clocks per step
+  uint32_t ticks_per_step = SEGMENT_CLOCKS / 2 / steps;
+  uint8_t timer_clock;
+  if (ticks_per_step <= 0xffff) timer_clock = TC_CLKSEL_DIV1_gc;
+  else if (ticks_per_step <= 0x1ffff) timer_clock = TC_CLKSEL_DIV2_gc;
+  else if (ticks_per_step <= 0x3ffff) timer_clock = TC_CLKSEL_DIV4_gc;
+  else if (ticks_per_step <= 0x7ffff) timer_clock = TC_CLKSEL_DIV8_gc;
+  else timer_clock = 0; // Clock off, too slow
 
   // Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc
   // equal 1, 2, 3 & 4 respectively.
-  m->timer_period = ticks_per_step >> (m->timer_clock - 1);
+  uint16_t timer_period = ticks_per_step >> (timer_clock - 1);
 
   // Compute power from axis velocity
-  m->power = steps / (_get_max_velocity(motor) * SEGMENT_TIME);
+  float power = steps / (_get_max_velocity(motor) * SEGMENT_TIME);
+  drv8711_set_power(motor, m->power);
 
   // Power motor
   switch (m->power_mode) {
@@ -438,6 +434,15 @@ stat_t motor_prep_move(int motor, int32_t target) {
   }
   _update_power(motor);
 
+  // Queue move
+  sei();
+  m->clockwise = clockwise;
+  m->steps = steps;
+  m->timer_clock = timer_clock;
+  m->timer_period = timer_period;
+  m->power = power;
+  cli();
+
   return STAT_OK;
 }