From f42864db758eb4165948b0c536c75e15567172f6 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 27 Mar 2016 15:51:51 -0700 Subject: [PATCH] Stepper driver tweaks. Seems to run well. --- src/config.h | 10 +++--- src/motor.c | 87 ++++++++++++++++++++++++++++++--------------------- src/motor.h | 1 + src/stepper.c | 12 +++++-- src/tmc2660.c | 50 ++++++++++++++++------------- 5 files changed, 97 insertions(+), 63 deletions(-) diff --git a/src/config.h b/src/config.h index 374462b..174171e 100644 --- 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 6d94380..4a45be7 100644 --- 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 bbf149a..ab431f7 100644 --- 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 91f3bb3..409abff 100644 --- 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 ae0ec00..b2d1bdb 100644 --- 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)); } -- 2.27.0