// 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
}
-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);
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.
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;
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);
mp_kinematics(travel, steps);
// Queue segment
- st_prep_line(steps, time);
+ st_prep_line(time, steps, 0);
return STAT_EAGAIN;
}
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;
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;
}
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
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];
}
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};
float steps[MOTORS];
mp_kinematics(target, steps);
-#if 0
+#ifdef STEP_CORRECTION
int32_t error[MOTORS];
st_get_error(error);
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])
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);
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;
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;
* 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;
}
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
}
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[]);
projects / bbctrl-firmware / commitdiff