From 5024990b8ae46b7a4165830a09cb52ca6fcbdc2f Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Mon, 20 Jun 2016 03:28:51 -0700 Subject: [PATCH] Working with current ramping --- src/config.h | 6 +-- src/motor.c | 118 ++++++++++++++++++++++++++-------------------- src/motor.h | 5 +- src/tmc2660.c | 57 ++++++++++++++-------- tmc2660_decode.py | 29 ++++++------ 5 files changed, 126 insertions(+), 89 deletions(-) diff --git a/src/config.h b/src/config.h index d9345b6..2e062dc 100644 --- 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 6b48da6..26a87ad 100644 --- 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 714bb60..dbeac21 100644 --- 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 a697c04..dcd0e8a 100644 --- 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 4056d6b..1b5adb3 100755 --- 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)) -- 2.27.0