From 031f0499a5a491fc9c5e3644f7cc0a367294f652 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 18 Sep 2016 13:17:58 -0700 Subject: [PATCH] Use encoder counts to correct step position exactly, Reduced forward diff calc from 42 multiples and 2 divides to 23 and 1 --- src/motor.c | 136 +++++++++++++++---------------------------- src/motor.h | 3 +- src/plan/calibrate.c | 3 +- src/plan/exec.c | 58 +++++++++--------- src/plan/runtime.c | 73 +++-------------------- src/stepper.c | 12 ++-- src/stepper.h | 4 +- 7 files changed, 96 insertions(+), 193 deletions(-) diff --git a/src/motor.c b/src/motor.c index 2dda5f7..6d9d2ed 100644 --- a/src/motor.c +++ b/src/motor.c @@ -76,18 +76,18 @@ typedef struct { uint32_t timeout; motor_flags_t flags; int32_t encoder; - uint16_t steps; + int32_t commanded; + uint16_t steps; // Currently used by the x-axis only uint8_t last_clock; + bool wasEnabled; // Move prep uint8_t timer_clock; // clock divisor setting or zero for off uint16_t timer_period; // clock period counter bool positive; // step sign direction_t direction; // travel direction corrected for polarity - - // Step error correction - int32_t correction_holdoff; // count down segments between corrections - float corrected_steps; // accumulated for cycle (diagnostic) + int32_t position; + int32_t error; } motor_t; @@ -159,9 +159,6 @@ ISR(TCE1_CCA_vect) { void motor_init() { - // Reset position - mp_runtime_set_steps_from_position(); - // Enable DMA DMA.CTRL = DMA_RESET_bm; DMA.CTRL = DMA_ENABLE_bm; @@ -215,9 +212,7 @@ void motor_enable(int motor, bool enable) { } -int motor_get_axis(int motor) { - return motors[motor].motor_map; -} +int motor_get_axis(int motor) {return motors[motor].motor_map;} float motor_get_steps_per_unit(int motor) { @@ -232,7 +227,9 @@ int32_t motor_get_encoder(int motor) { void motor_set_encoder(int motor, float encoder) { - motors[motor].encoder = round(encoder); + motor_t *m = &motors[motor]; + m->encoder = m->position = m->commanded = round(encoder); + m->error = 0; } @@ -309,9 +306,6 @@ stat_t motor_rtc_callback() { // called by controller } -void print_status_flags(uint8_t flags); - - void motor_error_callback(int motor, motor_flags_t errors) { if (motors[motor].power_state != MOTOR_ACTIVE) return; @@ -362,45 +356,35 @@ void motor_load_move(int motor) { steps = m->steps; m->steps = 0; } + if (!m->wasEnabled) steps = 0; m->encoder += m->positive ? steps : -(int32_t)steps; + + // Compute error + m->error = m->commanded - m->encoder; + m->commanded = m->position; } void motor_end_move(int motor) { - motors[motor].dma->CTRLA &= ~DMA_CH_ENABLE_bm; - motors[motor].timer->CTRLA = 0; // Stop clock + motor_t *m = &motors[motor]; + m->wasEnabled = m->dma->CTRLA & DMA_CH_ENABLE_bm; + m->dma->CTRLA &= ~DMA_CH_ENABLE_bm; + m->timer->CTRLA = 0; // Stop clock } -void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, - float error) { +void motor_prep_move(int motor, int32_t seg_clocks, float target) { motor_t *m = &motors[motor]; -#ifdef __STEP_CORRECTION - // 'Nudge' correction strategy. Inject a single, scaled correction value - // then hold off - if (--m->correction_holdoff < 0 && - STEP_CORRECTION_THRESHOLD < fabs(error)) { - - m->correction_holdoff = STEP_CORRECTION_HOLDOFF; - float correction = error * STEP_CORRECTION_FACTOR; - - if (0 < correction) - correction = min3(correction, fabs(travel_steps), STEP_CORRECTION_MAX); - else correction = - max3(correction, -fabs(travel_steps), -STEP_CORRECTION_MAX); - - m->corrected_steps += correction; - travel_steps -= correction; - } -#endif + int32_t travel = round(target) - m->position + m->error; + m->error = 0; // Power motor - switch (motors[motor].power_mode) { + switch (m->power_mode) { case MOTOR_DISABLED: return; case MOTOR_POWERED_ONLY_WHEN_MOVING: - if (fp_ZERO(travel_steps)) return; // Not moving + if (!travel) return; // Not moving // Fall through case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: @@ -413,7 +397,7 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, // 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. - uint32_t ticks_per_step = round(fabs(seg_clocks / travel_steps)); + int32_t ticks_per_step = labs(seg_clocks / travel); // Find the clock rate that will fit the required number of steps if (ticks_per_step & 0xffff0000UL) { @@ -431,48 +415,35 @@ 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; + if (!ticks_per_step) m->timer_clock = 0; 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) ALARM(STAT_STEP_CHECK_FAILED); - } + m->positive = 0 <= travel; // Setup the direction, compensating for polarity. if (m->positive) m->direction = DIRECTION_CW ^ m->polarity; else m->direction = DIRECTION_CCW ^ m->polarity; -} - -// Var callbacks -float get_step_angle(int motor) { - return motors[motor].step_angle; -} - - -void set_step_angle(int motor, float value) { - motors[motor].step_angle = value; -} - - -float get_travel(int motor) { - return motors[motor].travel_rev; -} + // Compute actual steps + if (m->timer_clock) { + int32_t clocks = seg_clocks >> (m->timer_clock - 1); // Motor timer clocks + int32_t steps = clocks / m->timer_period; + // Update position + m->position += m->positive ? steps : -steps; -void set_travel(int motor, float value) { - motors[motor].travel_rev = value; + // Sanity check + int32_t diff = labs(labs(travel) - steps); + if (16 < diff) ALARM(STAT_STEP_CHECK_FAILED); + } } -uint16_t get_microstep(int motor) { - return motors[motor].microsteps; -} +// Var callbacks +float get_step_angle(int motor) {return motors[motor].step_angle;} +void set_step_angle(int motor, float value) {motors[motor].step_angle = value;} +float get_travel(int motor) {return motors[motor].travel_rev;} +void set_travel(int motor, float value) {motors[motor].travel_rev = value;} +uint16_t get_microstep(int motor) {return motors[motor].microsteps;} void set_microstep(int motor, uint16_t value) { @@ -492,14 +463,8 @@ uint8_t get_polarity(int motor) { } -void set_polarity(int motor, uint8_t value) { - motors[motor].polarity = value; -} - - -uint8_t get_motor_map(int motor) { - return motors[motor].motor_map; -} +void set_polarity(int motor, uint8_t value) {motors[motor].polarity = value;} +uint8_t get_motor_map(int motor) {return motors[motor].motor_map;} void set_motor_map(int motor, uint16_t value) { @@ -507,9 +472,7 @@ void set_motor_map(int motor, uint16_t value) { } -uint8_t get_power_mode(int motor) { - return motors[motor].power_mode; -} +uint8_t get_power_mode(int motor) {return motors[motor].power_mode;} void set_power_mode(int motor, uint16_t value) { @@ -518,10 +481,7 @@ void set_power_mode(int motor, uint16_t value) { } - -uint8_t get_status_flags(int motor) { - return motors[motor].flags; -} +uint8_t get_status_flags(int motor) {return motors[motor].flags;} void print_status_flags(uint8_t flags) { @@ -568,9 +528,7 @@ void print_status_flags(uint8_t flags) { } -uint8_t get_status_strings(int motor) { - return get_status_flags(motor); -} +uint8_t get_status_strings(int motor) {return get_status_flags(motor);} // Command callback diff --git a/src/motor.h b/src/motor.h index 123289e..c1e7007 100644 --- a/src/motor.h +++ b/src/motor.h @@ -82,5 +82,4 @@ void motor_error_callback(int motor, motor_flags_t errors); 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); +void motor_prep_move(int motor, int32_t seg_clocks, float target); diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index a341d00..03ab1b4 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -137,8 +137,7 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { mp_kinematics(travel, steps); // Queue segment - float error[MOTORS] = {0}; - st_prep_line(steps, error, time); + st_prep_line(steps, time); return STAT_EAGAIN; } diff --git a/src/plan/exec.c b/src/plan/exec.c index f097a75..533b31a 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -208,33 +208,37 @@ static stat_t _exec_aline_segment() { /// F_1 = 120Ah^5 /// /// Note that with our current control points, D and E are actually 0. -static float _init_forward_diffs(float Vi, float Vt, float segments) { - float A = -6.0 * Vi + 6.0 * Vt; - float B = 15.0 * Vi - 15.0 * Vt; - float C = -10.0 * Vi + 10.0 * Vt; - // D = 0 - // E = 0 - // F = Vi - - float h = 1 / segments; - - float Ah_5 = A * h * h * h * h * h; - float Bh_4 = B * h * h * h * h; - float Ch_3 = C * h * h * h; - - ex.forward_diff[4] = 121.0 / 16.0 * Ah_5 + 5.0 * Bh_4 + 13.0 / 4.0 * Ch_3; - ex.forward_diff[3] = 165.0 / 2.0 * Ah_5 + 29.0 * Bh_4 + 9.0 * Ch_3; - ex.forward_diff[2] = 255.0 * Ah_5 + 48.0 * Bh_4 + 6.0 * Ch_3; - ex.forward_diff[1] = 300.0 * Ah_5 + 24.0 * Bh_4; - ex.forward_diff[0] = 120.0 * Ah_5; - - // Calculate the initial velocity by calculating V(h / 2) - float half_h = h / 2.0; - float half_Ch_3 = C * half_h * half_h * half_h; - float half_Bh_4 = B * half_h * half_h * half_h * half_h; - float half_Ah_5 = C * half_h * half_h * half_h * half_h * half_h; - - return half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi; +/// +/// This can be simplified even further by subsituting Ah, Bh & Ch back in and +/// reducing to: +/// +/// F_5 = (32.5 * s^2 - 75 * s + 45.375)(Vt - Vi) * h^5 +/// F_4 = (90.0 * s^2 - 435 * s + 495.0 )(Vt - Vi) * h^5 +/// F_3 = (60.0 * s^2 - 720 * s + 1530.0 )(Vt - Vi) * h^5 +/// F_2 = ( - 360 * s + 1800.0 )(Vt - Vi) * h^5 +/// F_1 = ( 720.0 )(Vt - Vi) * h^5 +/// +static float _init_forward_diffs(float Vi, float Vt, float s) { + const float h = 1 / s; + const float s2 = square(s); + const float Vdxh5 = (Vt - Vi) * h * h * h * h * h; + + ex.forward_diff[4] = (32.5 * s2 - 75.0 * s + 45.375) * Vdxh5; + ex.forward_diff[3] = (90.0 * s2 - 435.0 * s + 495.0 ) * Vdxh5; + ex.forward_diff[2] = (60.0 * s2 - 720.0 * s + 1530.0 ) * Vdxh5; + ex.forward_diff[1] = ( - 360.0 * s + 1800.0 ) * Vdxh5; + ex.forward_diff[0] = ( 720.0 ) * Vdxh5; + + // Calculate the initial velocity by calculating: + // + // V(h / 2) = + // + // ( -6Vi + 6Vt) / (2^5 * s^8) + + // ( 15Vi - 15Vt) / (2^4 * s^8) + + // (-10Vi + 10Vt) / (2^3 * s^8) + Vi = + // + // (Vt - Vi) * 1/2 * h^8 + Vi + return (Vt - Vi) * 0.5 * square(square(square(h))) + Vi; } diff --git a/src/plan/runtime.c b/src/plan/runtime.c index 5040173..cd13c42 100644 --- a/src/plan/runtime.c +++ b/src/plan/runtime.c @@ -42,21 +42,11 @@ #include -typedef struct { - float target[MOTORS]; // Current absolute target in steps - float position[MOTORS]; // Current position, target from previous segment - float commanded[MOTORS]; // Should align with next encoder sample (2nd prev) - float encoder[MOTORS]; // Encoder steps, ideally the same as commanded steps - float error[MOTORS]; // Difference between encoder & commanded steps -} mp_steps_t; - - typedef struct { bool busy; // True if a move is running float position[AXES]; // Current move position float work_offset[AXES]; // Current move work offset float velocity; // Current move velocity - mp_steps_t steps; int32_t line; // Current move GCode line number uint8_t tool; // Active tool @@ -99,19 +89,12 @@ void mp_runtime_set_velocity(float velocity) { /// Set encoder counts to the runtime position void mp_runtime_set_steps_from_position() { // Convert lengths to steps in floating point - float step_position[MOTORS]; - mp_kinematics(rt.position, step_position); - - for (int motor = 0; motor < MOTORS; motor++) { - rt.steps.target[motor] = rt.steps.position[motor] = - rt.steps.commanded[motor] = step_position[motor]; + float steps[MOTORS]; + mp_kinematics(rt.position, steps); + for (int motor = 0; motor < MOTORS; motor++) // Write steps to encoder register - motor_set_encoder(motor, step_position[motor]); - - // Must be zero - rt.steps.error[motor] = 0; - } + motor_set_encoder(motor, steps[motor]); } @@ -167,54 +150,16 @@ void mp_runtime_set_work_offsets(float offset[]) { } -/*** Segment runner - * - * Notes on step error correction: - * - * The commanded_steps are the target_steps delayed by one more - * segment. This lines them up in time with the encoder readings so a - * following error can be generated - * - * The rt.steps.error term is positive if the encoder reading is - * greater than (ahead of) the commanded steps, and negative (behind) - * if the encoder reading is less than the commanded steps. The - * following error is not affected by the direction of movement - it's - * purely a statement of relative position. Examples: - * - * Encoder Commanded Following Err - * 100 90 +10 encoder is 10 steps ahead of commanded steps - * -90 -100 +10 encoder is 10 steps ahead of commanded steps - * 90 100 -10 encoder is 10 steps behind commanded steps - * -100 -90 -10 encoder is 10 steps behind commanded steps - */ +/// Segment runner stat_t mp_runtime_move_to_target(float target[], float time) { - // Bucket-brigade old target down the chain before getting new target - for (int motor = 0; motor < MOTORS; motor++) { - // previous segment's position, delayed by 1 segment - rt.steps.commanded[motor] = rt.steps.position[motor]; - // previous segment's target becomes position - rt.steps.position[motor] = rt.steps.target[motor]; - // current encoder position (aligns to commanded_steps) - rt.steps.encoder[motor] = motor_get_encoder(motor); - rt.steps.error[motor] = rt.steps.encoder[motor] - rt.steps.commanded[motor]; - } - // Convert target position to steps. - mp_kinematics(target, rt.steps.target); - - // Compute distances in steps to be traveled. - // - // The direct manipulation of steps to compute travel_steps only - // works for Cartesian kinematics. Other kinematics may require - // transforming travel distance as opposed to simply subtracting steps. - float travel_steps[MOTORS]; - for (int motor = 0; motor < MOTORS; motor++) - travel_steps[motor] = rt.steps.target[motor] - rt.steps.position[motor]; + float steps[MOTORS]; + mp_kinematics(target, steps); // Call the stepper prep function - RITORNO(st_prep_line(travel_steps, rt.steps.error, time)); + RITORNO(st_prep_line(steps, time)); - // Update position + // Update positions mp_runtime_set_position(target); return STAT_OK; diff --git a/src/stepper.c b/src/stepper.c index 026a435..58404f1 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -184,16 +184,14 @@ ISR(STEP_TIMER_ISR) { * to integer values. * * Args: - * @param travel_steps signed relative motion in steps for each motor. + * @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 error is a vector of measured step errors used for correction. - * * @param seg_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 travel_steps[], float error[], float seg_time) { +stat_t st_prep_line(float target[], float seg_time) { // Trap conditions that would prevent queueing the line if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR); if (isinf(seg_time)) @@ -204,13 +202,13 @@ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { // Setup segment parameters st.move_type = MOVE_TYPE_ALINE; st.seg_period = seg_time * 60 * STEP_TIMER_FREQ; // Must fit 16-bit - uint32_t seg_clocks = (uint32_t)st.seg_period * STEP_TIMER_DIV; + int32_t seg_clocks = (int32_t)st.seg_period * STEP_TIMER_DIV; // Prepare motor moves for (int motor = 0; motor < MOTORS; motor++) - motor_prep_move(motor, seg_clocks, travel_steps[motor], error[motor]); + motor_prep_move(motor, seg_clocks, target[motor]); - st.move_ready = true; // signal prep buffer ready(do this last) + st.move_ready = true; // signal prep buffer ready (do this last) return STAT_OK; } diff --git a/src/stepper.h b/src/stepper.h index 6ee709a..7cf752d 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -32,11 +32,11 @@ #include "status.h" #include +#include void stepper_init(); void st_shutdown(); bool st_is_busy(); -stat_t st_prep_line(float travel_steps[], float following_error[], - float segment_time); +stat_t st_prep_line(float target[], float segment_time); void st_prep_dwell(float seconds); -- 2.27.0