// 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
#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
/// 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
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;
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,
.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,
.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,
.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,
}
-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) {
}
-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();
}
+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;
}
-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;
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;
// 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;
default: break; // Shouldn't get here
}
+
+ return STAT_OK;
}
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;
}
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);
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);
#include "util.h"
#include "runtime.h"
#include "state.h"
+#include "stepper.h"
+#include "motor.h"
#include "rtc.h"
#include "usart.h"
#include "config.h"
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] =
- 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;
}
/// 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);
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
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];
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));
*/
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;
// 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)
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);
+}
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[]);
projects / bbctrl-firmware / commitdiff