From d8a9b20f6de3e725e86113f74ba199516db8b162 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 21 Sep 2016 04:18:53 -0700 Subject: [PATCH] Working on step correction --- src/config.h | 27 +++------------- src/motor.c | 70 ++++++++++++++++++++++++++---------------- src/motor.h | 5 ++- src/plan/exec.c | 76 +++++++++++++++++++++++++++++++++++++++++----- src/plan/runtime.c | 39 ++++++++++++++++++++++++ src/stepper.c | 17 +++++++---- src/stepper.h | 1 + 7 files changed, 172 insertions(+), 63 deletions(-) diff --git a/src/config.h b/src/config.h index 401e788..31d2d2d 100644 --- a/src/config.h +++ b/src/config.h @@ -156,7 +156,7 @@ typedef enum { // Machine settings -#define MAX_STEP_CORRECTION 4 // In steps per segment +#define MAX_STEP_CORRECTION 8 // 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 @@ -324,27 +324,8 @@ typedef enum { #define STEP_TIMER_ISR TCC0_OVF_vect #define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc - -/* Step correction settings - * - * Step correction settings determine how the encoder error is fed - * back to correct position errors. Since the following_error is - * running 2 segments behind the current segment you have to be - * careful not to overcompensate. The threshold determines if a - * correction should be applied, and the factor is how much. The - * holdoff is how many segments before applying another correction. If - * threshold is too small and/or amount too large and/or holdoff is - * too small you may get a runaway correction and error will grow - * instead of shrink (or oscillate). - */ -/// magnitude of forwarding error (in steps) -#define STEP_CORRECTION_THRESHOLD 2.00 -/// apply to step correction for a single segment -#define STEP_CORRECTION_FACTOR 0.25 -/// max step correction allowed in a single segment -#define STEP_CORRECTION_MAX 0.60 -/// minimum wait between error correction -#define STEP_CORRECTION_HOLDOFF 5 +#define SEG_MIN_TIME EPSILON +#define SEG_MAX_TIME ((float)0xffff / 60.0 / STEP_TIMER_FREQ) // TMC2660 driver settings @@ -391,7 +372,7 @@ typedef enum { /// planning in the case of very short lines or arc segments. Suggest no less /// than 12. Maximum is 255 with out also changing the type of mb.space. Must /// leave headroom for stack. -#define PLANNER_BUFFER_POOL_SIZE 48 +#define PLANNER_BUFFER_POOL_SIZE 32 /// Buffers to reserve in planner before processing new input line #define PLANNER_BUFFER_HEADROOM 4 diff --git a/src/motor.c b/src/motor.c index de38139..7dd157d 100644 --- a/src/motor.c +++ b/src/motor.c @@ -58,7 +58,7 @@ typedef enum { typedef struct { // Config - uint8_t motor_map; // map motor to axis + uint8_t axis; // map motor to axis uint16_t microsteps; // microsteps per full step motor_polarity_t polarity; motor_power_mode_t power_mode; @@ -95,7 +95,7 @@ typedef struct { static motor_t motors[MOTORS] = { { - .motor_map = M1_MOTOR_MAP, + .axis = M1_MOTOR_MAP, .step_angle = M1_STEP_ANGLE, .travel_rev = M1_TRAVEL_PER_REV, .microsteps = M1_MICROSTEPS, @@ -108,7 +108,7 @@ static motor_t motors[MOTORS] = { .dma = &M1_DMA_CH, .dma_trigger = M1_DMA_TRIGGER, }, { - .motor_map = M2_MOTOR_MAP, + .axis = M2_MOTOR_MAP, .step_angle = M2_STEP_ANGLE, .travel_rev = M2_TRAVEL_PER_REV, .microsteps = M2_MICROSTEPS, @@ -121,7 +121,7 @@ static motor_t motors[MOTORS] = { .dma = &M2_DMA_CH, .dma_trigger = M2_DMA_TRIGGER, }, { - .motor_map = M3_MOTOR_MAP, + .axis = M3_MOTOR_MAP, .step_angle = M3_STEP_ANGLE, .travel_rev = M3_TRAVEL_PER_REV, .microsteps = M3_MICROSTEPS, @@ -134,7 +134,7 @@ static motor_t motors[MOTORS] = { .dma = &M3_DMA_CH, .dma_trigger = M3_DMA_TRIGGER, }, { - .motor_map = M4_MOTOR_MAP, + .axis = M4_MOTOR_MAP, .step_angle = M4_STEP_ANGLE, .travel_rev = M4_TRAVEL_PER_REV, .microsteps = M4_MICROSTEPS, @@ -214,7 +214,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].axis;} float motor_get_steps_per_unit(int motor) { @@ -223,11 +223,15 @@ float motor_get_steps_per_unit(int motor) { } -int32_t motor_get_encoder(int motor) { - return motors[motor].encoder; +float motor_get_units_per_step(int motor) { + return motors[motor].travel_rev * motors[motor].step_angle / + motors[motor].microsteps / 360; } +int32_t motor_get_encoder(int motor) {return motors[motor].encoder;} + + void motor_set_encoder(int motor, float encoder) { motor_t *m = &motors[motor]; cli(); @@ -237,6 +241,10 @@ void motor_set_encoder(int motor, float encoder) { } +int32_t motor_get_error(int motor) {return motors[motor].error;} +int32_t motor_get_position(int motor) {return motors[motor].position;} + + /// returns true if motor is in an error state bool motor_error(int motor) { uint8_t flags = motors[motor].flags; @@ -386,16 +394,17 @@ void motor_end_move(int motor) { } -void motor_prep_move(int motor, int32_t seg_clocks, float target) { +stat_t motor_prep_move(int motor, int32_t seg_clocks, float target) { motor_t *m = &motors[motor]; // Validate input - if (motor < 0 || MOTORS < motor) {ALARM(STAT_INTERNAL_ERROR); return;} - if (seg_clocks < 0) {ALARM(STAT_INTERNAL_ERROR); return;} - if (isinf(target)) {ALARM(STAT_MOVE_TARGET_IS_INFINITE); return;} - if (isnan(target)) {ALARM(STAT_MOVE_TARGET_IS_NAN); return;} + if (motor < 0 || MOTORS < motor) return ALARM(STAT_INTERNAL_ERROR); + if (seg_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); // Compute error correction +#if 0 cli(); int32_t error = m->error; int32_t actual_error = error; @@ -408,12 +417,21 @@ void motor_prep_move(int motor, int32_t seg_clocks, float target) { ALARM(STAT_EXCESSIVE_MOVE_ERROR); return; } +#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; + m->position = round(target); + + // Setup the direction, compensating for polarity. + m->negative = travel < 0; + if (m->negative) m->direction = DIRECTION_CCW ^ m->polarity; + else m->direction = DIRECTION_CW ^ m->polarity; // Find the clock rate that will fit the required number of steps if (ticks_per_step <= 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc; @@ -428,26 +446,24 @@ void motor_prep_move(int motor, int32_t seg_clocks, float target) { // Round up if DIV4 or DIV8 and the error is high enough if (0xffff < ticks_per_step && m->timer_period < 0xffff) { - uint8_t step_error = ticks_per_step & ((1 << (m->timer_clock - 1)) - 1); - uint8_t half_error = 1 << (m->timer_clock - 2); + 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++; + if (m->round_up) {m->timer_period++; rounded = true;} m->round_up = !m->round_up; - } else if (half_error < step_error) m->timer_period++; + } 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; } if (!m->timer_period) m->timer_clock = 0; if (!m->timer_clock) m->timer_period = 0; - // Setup the direction, compensating for polarity. - m->negative = travel < 0; - if (m->negative) m->direction = DIRECTION_CCW ^ m->polarity; - else m->direction = DIRECTION_CW ^ m->polarity; - - m->position = round(target); - // Power motor switch (m->power_mode) { case MOTOR_DISABLED: break; @@ -462,6 +478,8 @@ void motor_prep_move(int motor, int32_t seg_clocks, float target) { default: break; // Shouldn't get here } + + return STAT_OK; } @@ -491,11 +509,11 @@ 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;} +uint8_t get_motor_map(int motor) {return motors[motor].axis;} void set_motor_map(int motor, uint16_t value) { - if (value < AXES) motors[motor].motor_map = value; + if (value < AXES) motors[motor].axis = value; } diff --git a/src/motor.h b/src/motor.h index 6c52db1..80bbcf8 100644 --- a/src/motor.h +++ b/src/motor.h @@ -68,8 +68,11 @@ void motor_enable(int motor, bool enable); int motor_get_axis(int motor); float motor_get_steps_per_unit(int motor); +float motor_get_units_per_step(int motor); int32_t motor_get_encoder(int motor); void motor_set_encoder(int motor, float encoder); +int32_t motor_get_error(int motor); +int32_t motor_get_position(int motor); bool motor_error(int motor); bool motor_stalled(int motor); void motor_reset(int motor); @@ -82,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); -void motor_prep_move(int motor, int32_t seg_clocks, float target); +stat_t motor_prep_move(int motor, int32_t seg_clocks, float target); diff --git a/src/plan/exec.c b/src/plan/exec.c index 4126eda..0c8cc53 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -32,6 +32,8 @@ #include "util.h" #include "runtime.h" #include "state.h" +#include "stepper.h" +#include "motor.h" #include "rtc.h" #include "usart.h" #include "config.h" @@ -46,6 +48,7 @@ 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; @@ -83,13 +86,15 @@ static stat_t _exec_aline_segment() { float segment_length = ex.segment_velocity * ex.segment_time; for (int axis = 0; axis < AXES; axis++) - target[axis] = - mp_runtime_get_axis_position(axis) + ex.unit[axis] * segment_length; + target[axis] = ex.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; } @@ -258,7 +263,7 @@ float mp_next_forward_diff(float fdifs[5]) { /// Common code for head and tail sections static stat_t _exec_aline_section(float length, float vin, float vout) { if (ex.section_new) { - if (fp_ZERO(length)) return STAT_NOOP; // end the move + if (fp_ZERO(length)) return STAT_NOOP; // end the section // len / avg. velocity float move_time = 2 * length / (vin + vout); @@ -367,8 +372,7 @@ 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, mp_runtime_get_position()); + float available_length = get_axis_vector_length(ex.final_target, ex.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 @@ -433,13 +437,71 @@ 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] = - mp_runtime_get_axis_position(axis) + ex.unit[axis] * ex.head_length; + ex.position[axis] + ex.unit[axis] * ex.head_length; - ex.waypoint[SECTION_BODY][axis] = mp_runtime_get_axis_position(axis) + + ex.waypoint[SECTION_BODY][axis] = ex.position[axis] + 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 cd13c42..272beb6 100644 --- a/src/plan/runtime.c +++ b/src/plan/runtime.c @@ -156,6 +156,45 @@ stat_t mp_runtime_move_to_target(float target[], float time) { float steps[MOTORS]; mp_kinematics(target, steps); +#if 0 + int32_t error[MOTORS]; + st_get_error(error); + + float travel[MOTORS]; + float new_length_sqr = 0; + float old_length_sqr = 0; + + for (int motor = 0; motor < MOTORS; motor++) { + travel[motor] = steps[motor] - motor_get_position(motor); + + if (fp_ZERO(travel[motor])) { + motor[travel] = 0; + motor[error] = 0; + } + +#if 1 + if (error[motor] < -MAX_STEP_CORRECTION) + error[motor] = -MAX_STEP_CORRECTION; + else if (MAX_STEP_CORRECTION < error[motor]) + error[motor] = MAX_STEP_CORRECTION; +#endif + + old_length_sqr += square(travel[motor]); + new_length_sqr += square(travel[motor] - error[motor]); + } + + 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) { + time = new_time; + + for (int motor = 0; motor < MOTORS; motor++) + steps[motor] -= error[motor]; + } + } +#endif + // Call the stepper prep function RITORNO(st_prep_line(steps, time)); diff --git a/src/stepper.c b/src/stepper.c index 58404f1..4f8cd73 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -193,11 +193,10 @@ ISR(STEP_TIMER_ISR) { */ 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)) - 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 STAT_MINIMUM_TIME_MOVE; + 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); // Setup segment parameters st.move_type = MOVE_TYPE_ALINE; @@ -206,7 +205,7 @@ stat_t st_prep_line(float target[], float seg_time) { // Prepare motor moves for (int motor = 0; motor < MOTORS; motor++) - motor_prep_move(motor, seg_clocks, target[motor]); + RITORNO(motor_prep_move(motor, seg_clocks, target[motor])); st.move_ready = true; // signal prep buffer ready (do this last) @@ -222,3 +221,9 @@ void st_prep_dwell(float seconds) { st.prep_dwell = seconds * 1000; // convert to ms st.move_ready = true; // signal prep buffer ready } + + +void st_get_error(int32_t error[]) { + for (int motor = 0; motor < MOTORS; motor++) + error[motor] = motor_get_error(motor); +} diff --git a/src/stepper.h b/src/stepper.h index 7cf752d..839f440 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -40,3 +40,4 @@ void st_shutdown(); bool st_is_busy(); stat_t st_prep_line(float target[], float segment_time); void st_prep_dwell(float seconds); +void st_get_error(int32_t error[]); -- 2.27.0