From 647a7e32a97564655acdd927c9a8570f63914339 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 21 Sep 2016 06:42:42 -0700 Subject: [PATCH] Much improved step correction using PID loop --- src/config.h | 3 +- src/motor.c | 20 ++++-------- src/motor.h | 2 +- src/plan/calibrate.c | 2 +- src/plan/exec.c | 75 +++++--------------------------------------- src/plan/runtime.c | 28 ++++++++++++----- src/stepper.c | 27 +++++++++------- src/stepper.h | 2 +- 8 files changed, 54 insertions(+), 105 deletions(-) diff --git a/src/config.h b/src/config.h index 31d2d2d..4e810cf 100644 --- a/src/config.h +++ b/src/config.h @@ -156,7 +156,8 @@ typedef enum { // Machine settings -#define MAX_STEP_CORRECTION 8 // In steps per segment +#define STEP_CORRECTION // Enable step correction +#define MAX_STEP_CORRECTION 4 // In steps per segment #define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs #define JERK_MAX 50 // yes, that's km/min^3 #define JUNCTION_DEVIATION 0.05 // default value, in mm diff --git a/src/motor.c b/src/motor.c index 7dd157d..6271530 100644 --- a/src/motor.c +++ b/src/motor.c @@ -394,12 +394,12 @@ void motor_end_move(int motor) { } -stat_t motor_prep_move(int motor, int32_t seg_clocks, float target) { +stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error) { motor_t *m = &motors[motor]; // Validate input if (motor < 0 || MOTORS < motor) return ALARM(STAT_INTERNAL_ERROR); - if (seg_clocks < 0) return ALARM(STAT_INTERNAL_ERROR); + if (clocks < 0) return ALARM(STAT_INTERNAL_ERROR); if (isinf(target)) return ALARM(STAT_MOVE_TARGET_IS_INFINITE); if (isnan(target)) return ALARM(STAT_MOVE_TARGET_IS_NAN); @@ -414,18 +414,15 @@ stat_t motor_prep_move(int motor, int32_t seg_clocks, float target) { if (100 < labs(actual_error)) { STATUS_DEBUG("Motor %d error is %ld", motor, actual_error); - ALARM(STAT_EXCESSIVE_MOVE_ERROR); - return; + return ALARM(STAT_EXCESSIVE_MOVE_ERROR); } -#else - int32_t error = 0; #endif // 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; - uint32_t ticks_per_step = travel ? labs(seg_clocks / travel) : 0; + uint32_t ticks_per_step = travel ? labs(clocks / travel) : 0; m->position = round(target); // Setup the direction, compensating for polarity. @@ -448,17 +445,12 @@ stat_t motor_prep_move(int motor, int32_t seg_clocks, float target) { 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); - bool rounded = false; if (step_error == half_error) { - if (m->round_up) {m->timer_period++; rounded = true;} + if (m->round_up) m->timer_period++; m->round_up = !m->round_up; - } else if (half_error < step_error) {m->timer_period++; rounded = true;} - - if (rounded) step_error = -(((1 << (m->timer_clock - 1)) - 1) - step_error); - - m->position += m->negative ? step_error : -step_error; + } else if (half_error < step_error) m->timer_period++; } if (!m->timer_period) m->timer_clock = 0; diff --git a/src/motor.h b/src/motor.h index 80bbcf8..93dc0ad 100644 --- a/src/motor.h +++ b/src/motor.h @@ -85,4 +85,4 @@ 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 seg_clocks, float target); +stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error); diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index 389560a..05df0fd 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -137,7 +137,7 @@ static stat_t _exec_calibrate(mp_buffer_t *bf) { mp_kinematics(travel, steps); // Queue segment - st_prep_line(steps, time); + st_prep_line(time, steps, 0); return STAT_EAGAIN; } diff --git a/src/plan/exec.c b/src/plan/exec.c index 0c8cc53..50a5af5 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -48,7 +48,6 @@ typedef struct { float unit[AXES]; // unit vector for axis scaling & planning float final_target[AXES]; // final target, used to correct rounding errors float waypoint[3][AXES]; // head/body/tail endpoints for correction - float position[AXES]; // copies of bf variables of same name float head_length; @@ -86,15 +85,13 @@ static stat_t _exec_aline_segment() { float segment_length = ex.segment_velocity * ex.segment_time; for (int axis = 0; axis < AXES; axis++) - target[axis] = ex.position[axis] + ex.unit[axis] * segment_length; + target[axis] = + mp_runtime_get_axis_position(axis) + ex.unit[axis] * segment_length; } mp_runtime_set_velocity(ex.segment_velocity); RITORNO(mp_runtime_move_to_target(target, ex.segment_time)); - for (int axis = 0; axis < AXES; axis++) - ex.position[axis] = target[axis]; - // Return EAGAIN to continue or OK if this segment is done return ex.segment_count ? STAT_EAGAIN : STAT_OK; } @@ -372,7 +369,8 @@ static void _plan_hold() { if (!bf) return; // Oops! nothing's running // Examine and process current buffer and compute length left for decel - float available_length = get_axis_vector_length(ex.final_target, ex.position); + float available_length = + get_axis_vector_length(ex.final_target, mp_runtime_get_position()); // Compute next_segment velocity, velocity left to shed to brake to zero float braking_velocity = _compute_next_segment_velocity(); // Distance to brake to zero from braking_velocity, bf is OK to use here @@ -437,73 +435,14 @@ static stat_t _exec_aline_init(mp_buffer_t *bf) { ex.section_new = true; ex.hold_planned = false; - for (int axis = 0; axis < AXES; axis++) - ex.position[axis] = mp_runtime_get_axis_position(axis); - -#if 0 - // Adjust for error - int32_t step_error[MOTORS]; - st_get_error(step_error); - - float position[AXES]; - float axis_length[AXES]; - float length_square = 0; - bool correct = true; //false; - - for (int motor = 0; motor < MOTORS; motor++) { - int axis = motor_get_axis(motor); - float error = (float)step_error[motor] * motor_get_units_per_step(motor); - error = 0; - - if (fp_ZERO(ex.unit[axis]) || fp_ZERO(error)) error = 0; - -#if 0 - if (error < -MAX_STEP_CORRECTION) error = -MAX_STEP_CORRECTION; - else if (MAX_STEP_CORRECTION < error) error = MAX_STEP_CORRECTION; -#endif - - position[axis] = ex.position[axis] - error; - axis_length[axis] = ex.final_target[axis] - position[axis]; - length_square += square(axis_length[axis]); - - if (error) correct = true; - } - - if (correct && !fp_ZERO(length_square)) { - float length = sqrt(length_square); - - - - if (!fp_ZERO(length)) { - // Compute unit vector & adjust position - for (int axis = 0; axis < AXES; axis++) { - ex.unit[axis] = axis_length[axis] / length; - ex.position[axis] = position[axis]; - } - - // Adjust section lengths - float scale = length / bf->length; - ex.head_length *= scale; - ex.body_length *= scale; - ex.tail_length *= scale; - } - } - - copy_vector(ex.unit, bf->unit); - ex.head_length = bf->head_length; - ex.body_length = bf->body_length; - ex.tail_length = bf->tail_length; -#endif - // Generate waypoints for position correction at section ends. This helps // negate floating point errors in the forward differencing code. for (int axis = 0; axis < AXES; axis++) { - ex.waypoint[SECTION_HEAD][axis] = - ex.position[axis] + ex.unit[axis] * ex.head_length; + float position = mp_runtime_get_axis_position(axis); - ex.waypoint[SECTION_BODY][axis] = ex.position[axis] + + ex.waypoint[SECTION_HEAD][axis] = position + ex.unit[axis] * ex.head_length; + ex.waypoint[SECTION_BODY][axis] = position + ex.unit[axis] * (ex.head_length + ex.body_length); - ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis]; } diff --git a/src/plan/runtime.c b/src/plan/runtime.c index 272beb6..8220a6e 100644 --- a/src/plan/runtime.c +++ b/src/plan/runtime.c @@ -63,6 +63,9 @@ typedef struct { path_mode_t path_mode; distance_mode_t distance_mode; distance_mode_t arc_distance_mode; + + float previousP[MOTORS]; + float previousI[MOTORS]; } mp_runtime_t; static mp_runtime_t rt = {0}; @@ -156,7 +159,7 @@ stat_t mp_runtime_move_to_target(float target[], float time) { float steps[MOTORS]; mp_kinematics(target, steps); -#if 0 +#ifdef STEP_CORRECTION int32_t error[MOTORS]; st_get_error(error); @@ -172,7 +175,16 @@ stat_t mp_runtime_move_to_target(float target[], float time) { motor[error] = 0; } -#if 1 + float P = error[motor] - rt.previousI[motor]; + float I = error[motor]; + float D = P - rt.previousP[motor]; + + rt.previousP[motor] = P; + rt.previousI[motor] = I; + + error[motor] = 0.5 * P + 0.5 * I + 0.15 * D; + +#if 0 if (error[motor] < -MAX_STEP_CORRECTION) error[motor] = -MAX_STEP_CORRECTION; else if (MAX_STEP_CORRECTION < error[motor]) @@ -183,20 +195,22 @@ stat_t mp_runtime_move_to_target(float target[], float time) { new_length_sqr += square(travel[motor] - error[motor]); } + bool use_error = false; if (!fp_ZERO(old_length_sqr)) { float new_time = time * sqrt(new_length_sqr / old_length_sqr); - if (false && SEG_MIN_TIME <= new_time && new_time <= SEG_MAX_TIME) { + if (SEG_MIN_TIME <= new_time && new_time <= SEG_MAX_TIME) { time = new_time; - - for (int motor = 0; motor < MOTORS; motor++) - steps[motor] -= error[motor]; + use_error = true; } } + + if (!use_error) + for (int motor = 0; motor < MOTORS; motor++) error[motor] = 0; #endif // Call the stepper prep function - RITORNO(st_prep_line(steps, time)); + RITORNO(st_prep_line(time, steps, error)); // Update positions mp_runtime_set_position(target); diff --git a/src/stepper.c b/src/stepper.c index 4f8cd73..da31d7e 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -57,7 +57,8 @@ typedef struct { uint16_t dwell; // Move prep - bool move_ready; // prepped move ready for loader + 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; @@ -100,7 +101,9 @@ ISR(ADCB_CH0_vect) { case STAT_EAGAIN: continue; // No command executed, try again case STAT_OK: // Move executed - if (!st.move_ready) ALARM(STAT_EXPECTED_MOVE); // No move was queued + if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued + st.move_queued = false; + st.move_ready = true; break; default: ALARM(status); break; @@ -188,26 +191,26 @@ ISR(STEP_TIMER_ISR) { * Steps are fractional. Their sign indicates direction. Motors not in the * move have 0 steps. * - * @param seg_time is segment run time in minutes. If timing is not 100% + * @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 target[], float seg_time) { +stat_t st_prep_line(float time, const float target[], const int32_t error[]) { // Trap conditions that would prevent queueing the line - if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR); - if (isinf(seg_time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); - if (isnan(seg_time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_IS_NAN); - if (seg_time < EPSILON) return ALARM(STAT_MINIMUM_TIME_MOVE); + if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR); + if (isinf(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); + if (isnan(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_IS_NAN); + if (time < EPSILON) return ALARM(STAT_MINIMUM_TIME_MOVE); // Setup segment parameters st.move_type = MOVE_TYPE_ALINE; - st.seg_period = seg_time * 60 * STEP_TIMER_FREQ; // Must fit 16-bit + st.seg_period = time * 60 * STEP_TIMER_FREQ; // Must fit 16-bit int32_t seg_clocks = (int32_t)st.seg_period * STEP_TIMER_DIV; // Prepare motor moves for (int motor = 0; motor < MOTORS; motor++) - RITORNO(motor_prep_move(motor, seg_clocks, target[motor])); + RITORNO(motor_prep_move(motor, seg_clocks, target[motor], error[motor])); - st.move_ready = true; // signal prep buffer ready (do this last) + st.move_queued = true; // signal prep buffer ready (do this last) return STAT_OK; } @@ -219,7 +222,7 @@ void st_prep_dwell(float seconds) { 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.move_ready = true; // signal prep buffer ready + st.move_queued = true; // signal prep buffer ready } diff --git a/src/stepper.h b/src/stepper.h index 839f440..3aa57b9 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -38,6 +38,6 @@ void stepper_init(); void st_shutdown(); bool st_is_busy(); -stat_t st_prep_line(float target[], float segment_time); +stat_t st_prep_line(float time, const float target[], const int32_t error[]); void st_prep_dwell(float seconds); void st_get_error(int32_t error[]); -- 2.27.0