From 81ec8a5386422c9d5e23b1d93ab40e056ed753e9 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 29 Mar 2017 04:20:26 -0700 Subject: [PATCH] New motor driver code working somewhat --- avr/MoveLifecycleCalls.md | 10 +- avr/src/config.h | 14 ++- avr/src/main.c | 3 - avr/src/motor.c | 194 ++++++++++++++------------------------ avr/src/motor.h | 8 +- avr/src/plan/calibrate.c | 10 +- avr/src/plan/exec.c | 11 +-- avr/src/plan/jog.c | 8 +- avr/src/plan/line.c | 2 +- avr/src/plan/planner.c | 20 ++-- avr/src/plan/planner.h | 6 -- avr/src/plan/runtime.c | 58 +----------- avr/src/plan/runtime.h | 2 +- avr/src/stepper.c | 45 ++++----- avr/src/stepper.h | 2 +- 15 files changed, 131 insertions(+), 262 deletions(-) diff --git a/avr/MoveLifecycleCalls.md b/avr/MoveLifecycleCalls.md index 8c8feb9..2fca19b 100644 --- a/avr/MoveLifecycleCalls.md +++ b/avr/MoveLifecycleCalls.md @@ -27,12 +27,10 @@ * _exec_aline_init() * _exec_aline_head() || _exec_aline_body() || _exec_aline_tail() * _exec_aline_section() - * mp_init_forward_dif() || mp_next_forward_dif() - * _exec_aline_segment() - * mp_runtime_move_to_target() - * mp_kinematics() - Converts target mm to steps and maps motors - * st_prep_line() - * motor_prep_move() + * mp_runtime_move_to_target() + * mp_kinematics() - Converts target mm to steps and maps motors + * st_prep_line() + * motor_prep_move() # Step Output * STEP_TIMER_ISR diff --git a/avr/src/config.h b/avr/src/config.h index daf0a96..d2db1bd 100644 --- a/avr/src/config.h +++ b/avr/src/config.h @@ -141,6 +141,11 @@ enum { #define M3_DMA_CH DMA.CH2 #define M4_DMA_CH DMA.CH3 +#define M1_DMA_VECT DMA_CH0_vect +#define M2_DMA_VECT DMA_CH1_vect +#define M3_DMA_VECT DMA_CH2_vect +#define M4_DMA_VECT DMA_CH3_vect + #define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc #define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc #define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc @@ -157,10 +162,11 @@ enum { #define STEP_TIMER_ISR TCC0_OVF_vect #define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc -#define MAX_SEGMENT_TIME ((float)0xffff / 60.0 / STEP_TIMER_FREQ) -#define NOM_SEGMENT_USEC 5000.0 // nominal segment time -#define MIN_SEGMENT_USEC 2500.0 // minimum segment time - +#define SEGMENT_USEC 5000.0 // segment time +#define SEGMENT_SEC (SEGMENT_USEC / 1000000.0) +#define SEGMENT_TIME (SEGMENT_SEC / 60.0) +#define SEGMENT_CLOCKS (F_CPU * SEGMENT_SEC) +#define SEGMENT_PERIOD (STEP_TIMER_FREQ * SEGMENT_SEC) // Huanyang settings #define HUANYANG_PORT USARTD1 diff --git a/avr/src/main.c b/avr/src/main.c index 4e8476a..4c79e69 100644 --- a/avr/src/main.c +++ b/avr/src/main.c @@ -80,9 +80,6 @@ int main() { fprintf_P(stdout, PSTR("\n{\"firmware\": \"Buildbotics AVR\", " "\"version\": \"" VERSION "\"}\n")); - // Nominal segment time cannot be longer than maximum - if (MAX_SEGMENT_TIME < NOM_SEGMENT_TIME) ALARM(STAT_INTERNAL_ERROR); - // Main loop while (true) { hw_reset_handler(); // handle hard reset requests diff --git a/avr/src/motor.c b/avr/src/motor.c index ad2141a..2c5e1a6 100644 --- a/avr/src/motor.c +++ b/avr/src/motor.c @@ -72,23 +72,14 @@ typedef struct { // Runtime state motor_power_state_t power_state; // state machine for managing motor power - uint32_t timeout; + uint32_t power_timeout; motor_flags_t flags; - int32_t encoder; - int32_t commanded; - int32_t steps; // Currently used by the x-axis only - uint8_t last_clock; - bool wasEnabled; - int32_t error; - bool reading; - bool last_clockwise; + bool active; // Move prep - uint8_t timer_clock; // clock divisor setting or zero for off - uint16_t timer_period; // clock period counter - bool clockwise; // travel direction corrected for polarity + uint16_t steps; + bool clockwise; int32_t position; - bool round_up; // toggle rounding direction float power; } motor_t; @@ -154,7 +145,6 @@ void motor_init() { // Setup DMA channel as timer event counter m->dma->ADDRCTRL = DMA_CH_SRCDIR_FIXED_gc | DMA_CH_DESTDIR_FIXED_gc; m->dma->TRIGSRC = m->dma_trigger; - m->dma->REPCNT = 0; // Note, the DMA transfer must read CCA to clear the trigger m->dma->SRCADDR0 = (((uintptr_t)&m->timer->CCA) >> 0) & 0xff; @@ -165,10 +155,9 @@ void motor_init() { m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff; m->dma->DESTADDR2 = 0; - m->dma->CTRLB = 0; - m->dma->CTRLA = - DMA_CH_REPEAT_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc; - m->dma->CTRLA |= DMA_CH_ENABLE_bm; + m->dma->REPCNT = 0; + m->dma->CTRLB = DMA_CH_TRNINTLVL_HI_gc; + m->dma->CTRLA = DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc; drv8711_set_microsteps(motor, m->microsteps); } @@ -241,27 +230,10 @@ void motor_set_microsteps(int motor, uint16_t microsteps) { } -int32_t motor_get_encoder(int motor) {return motors[motor].encoder;} - - -void motor_set_encoder(int motor, float encoder) { +void motor_set_position(int motor, int32_t position) { //if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO - motor_t *m = &motors[motor]; - //m->encoder = m->position = m->commanded = round(encoder); - m->position = m->commanded = round(encoder); - m->error = 0; -} - - -int32_t motor_get_error(int motor) { - motor_t *m = &motors[motor]; - - m->reading = true; - int32_t error = motors[motor].error; - m->reading = false; - - return error; + motors[motor].position = position; } @@ -276,14 +248,8 @@ bool motor_error(int m) { } -bool motor_stalled(int m) { - return motors[m].flags & MOTOR_FLAG_STALLED_bm; -} - - -void motor_reset(int m) { - motors[m].flags = 0; -} +bool motor_stalled(int m) {return motors[m].flags & MOTOR_FLAG_STALLED_bm;} +void motor_reset(int m) {motors[m].flags = 0;} static void _set_power_state(int motor, motor_power_state_t state) { @@ -298,7 +264,7 @@ static void _update_power(int motor) { switch (m->power_mode) { case MOTOR_POWERED_ONLY_WHEN_MOVING: case MOTOR_POWERED_IN_CYCLE: - if (rtc_expired(m->timeout)) { + if (rtc_expired(m->power_timeout)) { if (m->power_state == MOTOR_ACTIVE) { _set_power_state(motor, MOTOR_IDLE); drv8711_set_state(motor, DRV8711_IDLE); @@ -377,118 +343,100 @@ void motor_error_callback(int motor, motor_flags_t errors) { } -void motor_load_move(int motor) { +static void _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; + // Stop clock & DMA + 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 - // Set or zero runtime clock and period - m->timer->CCA = m->timer_period; // Set step pulse period - m->timer->CTRLA = m->timer_clock; // Clock rate or stop - m->last_clock = m->timer_clock; - m->timer_clock = 0; // Clear clock + if (!m->steps) { + m->active = false; + return; + } // Set direction if (m->clockwise) OUTCLR_PIN(m->dir_pin); else OUTSET_PIN(m->dir_pin); - // Compute error - if (!m->reading) m->error = m->commanded - m->encoder; - m->commanded = m->position; - // 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; + + // Find the fastest clock rate that will fit the required number of steps + uint32_t ticks_per_step = clocks / m->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. + uint16_t timer_period = ticks_per_step >> (timer_clock - 1); + + // Set clock and period + if ((m->active = timer_period && timer_clock)) { + m->timer->CNT = 0; + m->timer->CCA = timer_period; // Set step pulse period + m->timer->CTRLA = timer_clock; // Set clock rate + m->steps = 0; + } } -void motor_end_move(int motor) { - motor_t *m = &motors[motor]; - m->dma->CTRLA &= ~DMA_CH_ENABLE_bm; - m->timer->CTRLA = 0; // Stop clock +ISR(M1_DMA_VECT) {_load_move(0);} +ISR(M2_DMA_VECT) {_load_move(1);} +ISR(M3_DMA_VECT) {_load_move(2);} +ISR(M4_DMA_VECT) {_load_move(3);} + + +void motor_load_move(int motor) { + if (!motors[motor].active) _load_move(motor); } -stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error, - float time) { +stat_t motor_prep_move(int motor, int32_t target) { motor_t *m = &motors[motor]; // Validate input if (motor < 0 || MOTORS < motor) return ALARM(STAT_INTERNAL_ERROR); - if (clocks < 0) return ALARM(STAT_INTERNAL_ERROR); if (isinf(target)) return ALARM(STAT_MOVE_TARGET_INFINITE); if (isnan(target)) return ALARM(STAT_MOVE_TARGET_NAN); - // 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. - int32_t travel = round(target) - m->position + error; - m->position = round(target); + // Compute travel in steps + int32_t steps = target - m->position; + m->position = target; - // Setup the direction, compensating for polarity. - bool negative = travel < 0; + // Set direction, compensating for polarity + bool negative = steps < 0; m->clockwise = !(negative ^ m->reverse); - // Use positive travel from here on - if (negative) travel = -travel; - - // Find the fastest clock rate that will fit the required number of steps - uint32_t ticks_per_step = travel ? clocks / 2 / travel : 0; - 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, 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); - - // Round up if DIV4 or DIV8 and the error is high enough - if (0xffff < ticks_per_step && m->timer_period < 0xffff) { - int8_t step_error = ticks_per_step & ((1 << (m->timer_clock - 1)) - 1); - int8_t half_error = 1 << (m->timer_clock - 2); - - if (step_error == half_error) { - if (m->round_up) m->timer_period++; - m->round_up = !m->round_up; - - } else if (half_error < step_error) m->timer_period++; - } - - if (!m->timer_period) m->timer_clock = 0; - if (!m->timer_clock) m->timer_period = 0; + // Positive steps from here on + if (negative) steps = -steps; + m->steps = steps; // Compute power from axis velocity - m->power = travel / (_get_max_velocity(motor) * time); + m->power = steps / (_get_max_velocity(motor) * SEGMENT_TIME); // Power motor switch (m->power_mode) { case MOTOR_POWERED_ONLY_WHEN_MOVING: - if (!m->timer_clock) break; // Not moving + if (!steps) break; // Not moving // Fall through case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: // Reset timeout - m->timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000; + m->power_timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000; break; default: break; @@ -597,7 +545,7 @@ void print_status_flags(uint8_t flags) { uint8_t get_status_strings(int m) {return get_status_flags(m);} -int32_t get_encoder(int m) {return motor_get_encoder(m);} +int32_t get_encoder(int m) {return 0;} // Command callback diff --git a/avr/src/motor.h b/avr/src/motor.h index 313b268..2bf71c1 100644 --- a/avr/src/motor.h +++ b/avr/src/motor.h @@ -72,9 +72,7 @@ float motor_get_steps_per_unit(int motor); float motor_get_units_per_step(int motor); uint16_t motor_get_microsteps(int motor); void motor_set_microsteps(int motor, uint16_t microsteps); -int32_t motor_get_encoder(int motor); -void motor_set_encoder(int motor, float encoder); -int32_t motor_get_error(int motor); +void motor_set_position(int motor, int32_t position); int32_t motor_get_position(int motor); bool motor_error(int motor); @@ -88,6 +86,4 @@ stat_t motor_rtc_callback(); void motor_error_callback(int motor, motor_flags_t errors); void motor_load_move(int motor); -void motor_end_move(int motor); -stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error, - float time); +stat_t motor_prep_move(int motor, int32_t target); diff --git a/avr/src/plan/calibrate.c b/avr/src/plan/calibrate.c index 67b368e..e075d90 100644 --- a/avr/src/plan/calibrate.c +++ b/avr/src/plan/calibrate.c @@ -77,7 +77,7 @@ static calibrate_t cal = {0}; static stat_t _exec_calibrate(mp_buffer_t *bf) { - const float time = MIN_SEGMENT_TIME; // In minutes + const float time = SEGMENT_TIME; // In minutes const float max_delta_v = CAL_ACCELERATION * time; do { @@ -105,7 +105,7 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { if (cal.stalled) { if (cal.reverse) { - int32_t steps = -motor_get_encoder(cal.motor); + int32_t steps = -motor_get_position(cal.motor); float mm = (float)steps / motor_get_steps_per_unit(cal.motor); STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm); @@ -116,7 +116,7 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { return STAT_NOOP; // Done, no move queued } else { - motor_set_encoder(cal.motor, 0); + motor_set_position(cal.motor, 0); cal.reverse = true; cal.velocity = 0; @@ -137,7 +137,7 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { mp_kinematics(travel, steps); // Queue segment - st_prep_line(time, steps, 0); + st_prep_line(steps); return STAT_EAGAIN; } @@ -153,7 +153,7 @@ void calibrate_set_stallguard(int motor, uint16_t sg) { int16_t delta = sg - cal.stallguard; if (!sg || CAL_MAX_DELTA_SG < abs(delta)) { cal.stalled = true; - motor_end_move(cal.motor); + //motor_end_move(cal.motor); } } diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c index 867d334..17ea41a 100644 --- a/avr/src/plan/exec.c +++ b/avr/src/plan/exec.c @@ -61,7 +61,6 @@ typedef struct { uint24_t segment_count; // count of running segments uint24_t segment; // current segment float segment_velocity; // computed velocity for segment - float segment_time; // actual time increment per segment float segment_start[AXES]; float segment_delta; float segment_dist; @@ -87,8 +86,7 @@ static stat_t _exec_aline_section(float length, float Vi, float Vt) { // len / avg. velocity float move_time = 2 * length / (Vi + Vt); // in mins - float segments = ceil(move_time / NOM_SEGMENT_TIME); - ex.segment_time = move_time / segments; + float segments = ceil(move_time / SEGMENT_TIME); ex.segment_count = round(segments); ex.segment = 0; ex.segment_dist = 0; @@ -102,9 +100,6 @@ static stat_t _exec_aline_section(float length, float Vi, float Vt) { } else ex.segment_delta = 1 / (segments + 1); - if (ex.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position - ex.section_new = false; } @@ -124,7 +119,7 @@ static stat_t _exec_aline_section(float length, float Vi, float Vt) { // Compute quintic Bezier curve ex.segment_velocity = velocity_curve(Vi, Vt, ex.segment * ex.segment_delta); - ex.segment_dist += ex.segment_velocity * ex.segment_time; + ex.segment_dist += ex.segment_velocity * SEGMENT_TIME; } for (int axis = 0; axis < AXES; axis++) @@ -132,7 +127,7 @@ static stat_t _exec_aline_section(float length, float Vi, float Vt) { } mp_runtime_set_velocity(ex.segment_velocity); - RITORNO(mp_runtime_move_to_target(target, ex.segment_time)); + RITORNO(mp_runtime_move_to_target(target)); // Return EAGAIN to continue or OK if this segment is done return ex.segment < ex.segment_count ? STAT_EAGAIN : STAT_OK; diff --git a/avr/src/plan/jog.c b/avr/src/plan/jog.c index ee4528b..930126a 100644 --- a/avr/src/plan/jog.c +++ b/avr/src/plan/jog.c @@ -108,14 +108,14 @@ static stat_t _exec_jog(mp_buffer_t *bf) { // Compute move time float move_time = 2 * length / (V + Vt); - if (move_time < MIN_SEGMENT_TIME) { + if (move_time < SEGMENT_TIME) { V = Vt; jr.velocity_t[axis] = 1; } else { jr.initial_velocity[axis] = V; jr.velocity_t[axis] = jr.velocity_delta[axis] = - NOM_SEGMENT_TIME / move_time; + SEGMENT_TIME / move_time; } } } @@ -148,11 +148,11 @@ static stat_t _exec_jog(mp_buffer_t *bf) { float target[AXES]; for (int axis = 0; axis < AXES; axis++) target[axis] = mp_runtime_get_axis_position(axis) + - jr.velocity[axis] * NOM_SEGMENT_TIME; + jr.velocity[axis] * SEGMENT_TIME; // Set velocity and target mp_runtime_set_velocity(sqrt(velocity_sqr)); - stat_t status = mp_runtime_move_to_target(target, NOM_SEGMENT_TIME); + stat_t status = mp_runtime_move_to_target(target); if (status != STAT_OK) return status; return STAT_EAGAIN; diff --git a/avr/src/plan/line.c b/avr/src/plan/line.c index a58f876..8cb4565 100644 --- a/avr/src/plan/line.c +++ b/avr/src/plan/line.c @@ -342,7 +342,7 @@ static float _calc_move_time(const float axis_length[], if (max_time < time) max_time = time; } - return max_time < MIN_SEGMENT_TIME ? MIN_SEGMENT_TIME : max_time; + return max_time < SEGMENT_TIME ? SEGMENT_TIME : max_time; } diff --git a/avr/src/plan/planner.c b/avr/src/plan/planner.c index 6a256c5..4d9b1a6 100644 --- a/avr/src/plan/planner.c +++ b/avr/src/plan/planner.c @@ -127,11 +127,11 @@ void mp_kinematics(const float travel[], float steps[]) { // expressions evaluate to the minimum lengths for the current velocity // settings. Note: The head and tail lengths are 2 minimum segments, the body // is 1 min segment. -#define MIN_HEAD_LENGTH \ - (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity)) -#define MIN_TAIL_LENGTH \ - (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->exit_velocity)) -#define MIN_BODY_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * bf->cruise_velocity) +#define MIN_HEAD_LENGTH \ + (SEGMENT_TIME * (bf->cruise_velocity + bf->entry_velocity)) +#define MIN_TAIL_LENGTH \ + (SEGMENT_TIME * (bf->cruise_velocity + bf->exit_velocity)) +#define MIN_BODY_LENGTH (SEGMENT_TIME * bf->cruise_velocity) /*** Calculate move acceleration / deceleration @@ -241,8 +241,8 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { float naive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average - if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { - bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN; + if (naive_move_time < SEGMENT_TIME) { + bf->cruise_velocity = bf->length / SEGMENT_TIME; bf->exit_velocity = max(0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); bf->body_length = bf->length; @@ -254,7 +254,7 @@ void mp_calculate_trapezoid(mp_buffer_t *bf) { } // B" case: Block is short, but fits into a single body segment - if (naive_move_time <= NOM_SEGMENT_TIME) { + if (naive_move_time <= SEGMENT_TIME) { bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity; if (!fp_ZERO(bf->entry_velocity)) { @@ -486,10 +486,6 @@ void mp_print_queue(mp_buffer_t *bf) { * bl (function arg) - end of block list (last block in time) * bl->replannable - start of block list set by last FALSE value * [Note 1] - * bl->move_type - typically MOVE_TYPE_ALINE. Other move_types should - * be set to length=0, entry_vmax=0 and exit_vmax=0 - * and are treated as a momentary stop (plan to zero - * and from zero). * bl->length - provides block length * bl->entry_vmax - used during forward planning to set entry velocity * bl->cruise_vmax - used during forward planning to set cruise velocity diff --git a/avr/src/plan/planner.h b/avr/src/plan/planner.h index a8e4c58..d94de9a 100644 --- a/avr/src/plan/planner.h +++ b/avr/src/plan/planner.h @@ -43,12 +43,6 @@ #define JERK_MULTIPLIER 1000000.0 #define JERK_MATCH_PRECISION 1000.0 // jerk precision to be considered same -#define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) -#define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) - -#define MIN_SEGMENT_TIME_PLUS_MARGIN \ - ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE) - /// Error percentage for iteration convergence. As percent - 0.01 = 1% #define TRAPEZOID_ITERATION_ERROR_PERCENT 0.1 diff --git a/avr/src/plan/runtime.c b/avr/src/plan/runtime.c index cf848a7..d9d3e60 100644 --- a/avr/src/plan/runtime.c +++ b/avr/src/plan/runtime.c @@ -96,7 +96,7 @@ void mp_runtime_set_steps_from_position() { for (int motor = 0; motor < MOTORS; motor++) // Write steps to encoder register - motor_set_encoder(motor, steps[motor]); + motor_set_position(motor, steps[motor]); } @@ -152,65 +152,17 @@ void mp_runtime_set_work_offsets(float offset[]) { } -static void _step_correction(const float steps[], float time, int32_t error[]) { -#ifdef STEP_CORRECTION - float travel[MOTORS]; - float new_length_sqr = 0; - float old_length_sqr = 0; - - for (int motor = 0; motor < MOTORS; motor++) { - error[motor] = motor_get_error(motor); - travel[motor] = steps[motor] - motor_get_position(motor); - - if (fp_ZERO(travel[motor])) { - motor[travel] = 0; - motor[error] = 0; - } - - error[motor] = 0.5 * (error[motor] - rt.previous_error[motor]); - rt.previous_error[motor] = error[motor]; - - if (error[motor] < -MAX_STEP_CORRECTION) - error[motor] = -MAX_STEP_CORRECTION; - else if (MAX_STEP_CORRECTION < error[motor]) - error[motor] = MAX_STEP_CORRECTION; - - old_length_sqr += square(travel[motor]); - new_length_sqr += square(travel[motor] - error[motor]); - } - - bool use_error = false; - if (!fp_ZERO(new_length_sqr)) { - float new_time = time * invsqrt(old_length_sqr / new_length_sqr); - - if (!isnan(new_time) && !isinf(new_time) && - EPSILON <= new_time && new_time <= MAX_SEGMENT_TIME) { - time = new_time; - use_error = true; - } - } - - if (!use_error) - for (int motor = 0; motor < MOTORS; motor++) - error[motor] = 0; - -#endif // STEP_CORRECTION -} - - /// Segment runner -stat_t mp_runtime_move_to_target(float target[], float time) { - ASSERT(isfinite(time)); +stat_t mp_runtime_move_to_target(float target[]) { + ASSERT(isfinite(target[0]) && isfinite(target[1]) && + isfinite(target[2]) && isfinite(target[3])); // Convert target position to steps. float steps[MOTORS]; mp_kinematics(target, steps); - int32_t error[MOTORS] = {0}; - _step_correction(steps, time, error); - // Call the stepper prep function - RITORNO(st_prep_line(time, steps, error)); + RITORNO(st_prep_line(steps)); // Update positions mp_runtime_set_position(target); diff --git a/avr/src/plan/runtime.h b/avr/src/plan/runtime.h index 4ae6cbb..ca835a9 100644 --- a/avr/src/plan/runtime.h +++ b/avr/src/plan/runtime.h @@ -55,4 +55,4 @@ void mp_runtime_set_position(float position[]); float mp_runtime_get_work_position(uint8_t axis); void mp_runtime_set_work_offsets(float offset[]); -stat_t mp_runtime_move_to_target(float target[], float time); +stat_t mp_runtime_move_to_target(float target[]); diff --git a/avr/src/stepper.c b/avr/src/stepper.c index e29bd77..6181043 100644 --- a/avr/src/stepper.c +++ b/avr/src/stepper.c @@ -45,7 +45,7 @@ typedef enum { MOVE_TYPE_NULL, // null move - does a no-op - MOVE_TYPE_ALINE, // acceleration planned line + MOVE_TYPE_LINE, // acceleration planned line MOVE_TYPE_DWELL, // delay with no movement } move_type_t; @@ -54,14 +54,13 @@ typedef struct { // Runtime bool busy; bool requesting; - uint16_t dwell; + float dwell; // Move prep bool move_ready; // prepped move ready for loader bool move_queued; // prepped move queued move_type_t move_type; - uint16_t seg_period; - uint32_t prep_dwell; + float prep_dwell; } stepper_t; @@ -131,15 +130,14 @@ static void _request_exec_move() { /// Dequeue move and load into stepper struct ISR(STEP_TIMER_ISR) { // Dwell - if (st.dwell && --st.dwell) return; + if (0 < st.dwell) { + st.dwell -= SEGMENT_SEC; + return; + } else st.dwell = 0; - // End last move + // Default clock rate TIMER_STEP.PER = STEP_TIMER_POLL; - DMA.INTFLAGS = 0xff; // clear all interrups - for (int motor = 0; motor < MOTORS; motor++) - motor_end_move(motor); - if (estop_triggered()) { st.move_type = MOVE_TYPE_NULL; return; @@ -155,11 +153,12 @@ ISR(STEP_TIMER_ISR) { if (motor_energizing()) return; // Start move - if (st.seg_period) { + if (st.move_type == MOVE_TYPE_LINE) for (int motor = 0; motor < MOTORS; motor++) motor_load_move(motor); - TIMER_STEP.PER = st.seg_period; + if (st.move_type != MOVE_TYPE_NULL) { + TIMER_STEP.PER = SEGMENT_PERIOD; st.busy = true; // Start dwell @@ -168,7 +167,6 @@ ISR(STEP_TIMER_ISR) { // We are done with this move st.move_type = MOVE_TYPE_NULL; - st.seg_period = 0; // clear timer st.prep_dwell = 0; // clear dwell st.move_ready = false; // flip the flag back @@ -189,27 +187,17 @@ ISR(STEP_TIMER_ISR) { * @param target signed position in steps for each motor. * Steps are fractional. Their sign indicates direction. Motors not in the * move have 0 steps. - * - * @param time is segment run time in minutes. If timing is not 100% - * accurate this will affect the move velocity but not travel distance. */ -stat_t st_prep_line(float time, const float target[], const int32_t error[]) { +stat_t st_prep_line(const float target[]) { // Trap conditions that would prevent queueing the line - if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR); - if (isinf(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_INFINITE); - if (isnan(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_NAN); - if (time < EPSILON) return ALARM(STAT_MINIMUM_TIME_MOVE); - if (MAX_SEGMENT_TIME < time) return ALARM(STAT_MAXIMUM_TIME_MOVE); + if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR); // Setup segment parameters - st.move_type = MOVE_TYPE_ALINE; - st.seg_period = round(time * 60 * STEP_TIMER_FREQ); // Must fit 16-bit - int32_t seg_clocks = (int32_t)st.seg_period * STEP_TIMER_DIV; + st.move_type = MOVE_TYPE_LINE; // Prepare motor moves for (int motor = 0; motor < MOTORS; motor++) - RITORNO - (motor_prep_move(motor, seg_clocks, target[motor], error[motor], time)); + RITORNO(motor_prep_move(motor, round(target[motor]))); st.move_queued = true; // signal prep buffer ready (do this last) @@ -221,7 +209,6 @@ stat_t st_prep_line(float time, const float target[], const int32_t error[]) { void st_prep_dwell(float seconds) { if (st.move_ready) ALARM(STAT_INTERNAL_ERROR); st.move_type = MOVE_TYPE_DWELL; - st.seg_period = STEP_TIMER_FREQ * 0.001; // 1 ms - st.prep_dwell = seconds * 1000; // convert to ms + st.prep_dwell = seconds; st.move_queued = true; // signal prep buffer ready } diff --git a/avr/src/stepper.h b/avr/src/stepper.h index 7c67a91..0f0a0ff 100644 --- a/avr/src/stepper.h +++ b/avr/src/stepper.h @@ -38,5 +38,5 @@ void stepper_init(); void st_shutdown(); bool st_is_busy(); -stat_t st_prep_line(float time, const float target[], const int32_t error[]); +stat_t st_prep_line(const float target[]); void st_prep_dwell(float seconds); -- 2.27.0