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;
void motor_init() {
- // Reset position
- mp_runtime_set_steps_from_position();
-
// Enable DMA
DMA.CTRL = DMA_RESET_bm;
DMA.CTRL = DMA_ENABLE_bm;
}
-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) {
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;
}
}
-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;
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:
// 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) {
} 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) {
}
-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) {
}
-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) {
}
-
-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) {
}
-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
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);
mp_kinematics(travel, steps);
// Queue segment
- float error[MOTORS] = {0};
- st_prep_line(steps, error, time);
+ st_prep_line(steps, time);
return STAT_EAGAIN;
}
/// 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;
}
#include <stdio.h>
-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
/// 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]);
}
}
-/*** 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;
* 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))
// 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;
}
#include "status.h"
#include <stdbool.h>
+#include <stdint.h>
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);
projects / bbctrl-firmware / commitdiff