* _exec_aline_init()
* _exec_aline_head() || _exec_aline_body() || _exec_aline_tail()
* _exec_aline_section()
- * mp_init_forward_dif() || mp_next_forward_dif()
- * _exec_aline_segment()
- * mp_runtime_move_to_target()
- * mp_kinematics() - Converts target mm to steps and maps motors
- * st_prep_line()
- * motor_prep_move()
+ * mp_runtime_move_to_target()
+ * mp_kinematics() - Converts target mm to steps and maps motors
+ * st_prep_line()
+ * motor_prep_move()
# Step Output
* STEP_TIMER_ISR
#define M3_DMA_CH DMA.CH2
#define M4_DMA_CH DMA.CH3
+#define M1_DMA_VECT DMA_CH0_vect
+#define M2_DMA_VECT DMA_CH1_vect
+#define M3_DMA_VECT DMA_CH2_vect
+#define M4_DMA_VECT DMA_CH3_vect
+
#define M1_DMA_TRIGGER DMA_CH_TRIGSRC_TCD0_CCA_gc
#define M2_DMA_TRIGGER DMA_CH_TRIGSRC_TCE0_CCA_gc
#define M3_DMA_TRIGGER DMA_CH_TRIGSRC_TCF0_CCA_gc
#define STEP_TIMER_ISR TCC0_OVF_vect
#define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc
-#define MAX_SEGMENT_TIME ((float)0xffff / 60.0 / STEP_TIMER_FREQ)
-#define NOM_SEGMENT_USEC 5000.0 // nominal segment time
-#define MIN_SEGMENT_USEC 2500.0 // minimum segment time
-
+#define SEGMENT_USEC 5000.0 // segment time
+#define SEGMENT_SEC (SEGMENT_USEC / 1000000.0)
+#define SEGMENT_TIME (SEGMENT_SEC / 60.0)
+#define SEGMENT_CLOCKS (F_CPU * SEGMENT_SEC)
+#define SEGMENT_PERIOD (STEP_TIMER_FREQ * SEGMENT_SEC)
// Huanyang settings
#define HUANYANG_PORT USARTD1
fprintf_P(stdout, PSTR("\n{\"firmware\": \"Buildbotics AVR\", "
"\"version\": \"" VERSION "\"}\n"));
- // Nominal segment time cannot be longer than maximum
- if (MAX_SEGMENT_TIME < NOM_SEGMENT_TIME) ALARM(STAT_INTERNAL_ERROR);
-
// Main loop
while (true) {
hw_reset_handler(); // handle hard reset requests
// Runtime state
motor_power_state_t power_state; // state machine for managing motor power
- uint32_t timeout;
+ uint32_t power_timeout;
motor_flags_t flags;
- int32_t encoder;
- int32_t commanded;
- int32_t steps; // Currently used by the x-axis only
- uint8_t last_clock;
- bool wasEnabled;
- int32_t error;
- bool reading;
- bool last_clockwise;
+ bool active;
// Move prep
- uint8_t timer_clock; // clock divisor setting or zero for off
- uint16_t timer_period; // clock period counter
- bool clockwise; // travel direction corrected for polarity
+ uint16_t steps;
+ bool clockwise;
int32_t position;
- bool round_up; // toggle rounding direction
float power;
} motor_t;
// Setup DMA channel as timer event counter
m->dma->ADDRCTRL = DMA_CH_SRCDIR_FIXED_gc | DMA_CH_DESTDIR_FIXED_gc;
m->dma->TRIGSRC = m->dma_trigger;
- m->dma->REPCNT = 0;
// Note, the DMA transfer must read CCA to clear the trigger
m->dma->SRCADDR0 = (((uintptr_t)&m->timer->CCA) >> 0) & 0xff;
m->dma->DESTADDR1 = (((uintptr_t)&_dummy) >> 8) & 0xff;
m->dma->DESTADDR2 = 0;
- m->dma->CTRLB = 0;
- m->dma->CTRLA =
- DMA_CH_REPEAT_bm | DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc;
- m->dma->CTRLA |= DMA_CH_ENABLE_bm;
+ m->dma->REPCNT = 0;
+ m->dma->CTRLB = DMA_CH_TRNINTLVL_HI_gc;
+ m->dma->CTRLA = DMA_CH_SINGLE_bm | DMA_CH_BURSTLEN_1BYTE_gc;
drv8711_set_microsteps(motor, m->microsteps);
}
}
-int32_t motor_get_encoder(int motor) {return motors[motor].encoder;}
-
-
-void motor_set_encoder(int motor, float encoder) {
+void motor_set_position(int motor, int32_t position) {
//if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO
- motor_t *m = &motors[motor];
- //m->encoder = m->position = m->commanded = round(encoder);
- m->position = m->commanded = round(encoder);
- m->error = 0;
-}
-
-
-int32_t motor_get_error(int motor) {
- motor_t *m = &motors[motor];
-
- m->reading = true;
- int32_t error = motors[motor].error;
- m->reading = false;
-
- return error;
+ motors[motor].position = position;
}
}
-bool motor_stalled(int m) {
- return motors[m].flags & MOTOR_FLAG_STALLED_bm;
-}
-
-
-void motor_reset(int m) {
- motors[m].flags = 0;
-}
+bool motor_stalled(int m) {return motors[m].flags & MOTOR_FLAG_STALLED_bm;}
+void motor_reset(int m) {motors[m].flags = 0;}
static void _set_power_state(int motor, motor_power_state_t state) {
switch (m->power_mode) {
case MOTOR_POWERED_ONLY_WHEN_MOVING:
case MOTOR_POWERED_IN_CYCLE:
- if (rtc_expired(m->timeout)) {
+ if (rtc_expired(m->power_timeout)) {
if (m->power_state == MOTOR_ACTIVE) {
_set_power_state(motor, MOTOR_IDLE);
drv8711_set_state(motor, DRV8711_IDLE);
}
-void motor_load_move(int motor) {
+static void _load_move(int motor) {
motor_t *m = &motors[motor];
- // Get actual step count from DMA channel
- //uint16_t steps = 0xffff - m->dma->TRFCNT;
- m->dma->TRFCNT = 0xffff; // Reset DMA channel counter
- m->dma->CTRLB = DMA_CH_CHBUSY_bm | DMA_CH_CHPEND_bm;
- m->dma->CTRLA |= DMA_CH_ENABLE_bm;
-
- // Adjust clock count
- if (m->last_clock) {
- uint32_t count = m->timer->CNT;
- int8_t freq_change = m->last_clock - m->timer_clock;
-
- count <<= freq_change; // Adjust count
-
- if (m->timer_period < count) count -= m->timer_period;
- if (m->timer_period < count) count -= m->timer_period;
- if (m->timer_period < count) count = m->timer_period / 2;
-
- m->timer->CNT = count;
-
- } else m->timer->CNT = m->timer_period / 2;
+ // Stop clock & DMA
+ m->timer->CTRLA = 0;
+ m->dma->CTRLA &= ~DMA_CH_ENABLE_bm;
+ m->dma->CTRLB |= DMA_CH_TRNIF_bm | DMA_CH_ERRIF_bm; // Clear interrupt flags
- // Set or zero runtime clock and period
- m->timer->CCA = m->timer_period; // Set step pulse period
- m->timer->CTRLA = m->timer_clock; // Clock rate or stop
- m->last_clock = m->timer_clock;
- m->timer_clock = 0; // Clear clock
+ if (!m->steps) {
+ m->active = false;
+ return;
+ }
// Set direction
if (m->clockwise) OUTCLR_PIN(m->dir_pin);
else OUTSET_PIN(m->dir_pin);
- // Compute error
- if (!m->reading) m->error = m->commanded - m->encoder;
- m->commanded = m->position;
-
// Set power
drv8711_set_power(motor, m->power);
+
+ // Get clocks remaining in segment
+ uint32_t clocks =
+ (uint32_t)(SEGMENT_PERIOD - TIMER_STEP.CNT) * STEP_TIMER_DIV;
+
+ // Count steps with phony DMA transfer
+ m->dma->TRFCNT = m->steps;
+ m->dma->CTRLA |= DMA_CH_ENABLE_bm;
+
+ // Find the fastest clock rate that will fit the required number of steps
+ uint32_t ticks_per_step = clocks / m->steps;
+ uint8_t timer_clock;
+ if (ticks_per_step <= 0xffff) timer_clock = TC_CLKSEL_DIV1_gc;
+ else if (ticks_per_step <= 0x1ffff) timer_clock = TC_CLKSEL_DIV2_gc;
+ else if (ticks_per_step <= 0x3ffff) timer_clock = TC_CLKSEL_DIV4_gc;
+ else if (ticks_per_step <= 0x7ffff) timer_clock = TC_CLKSEL_DIV8_gc;
+ else timer_clock = 0; // Clock off, too slow
+
+ // Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc
+ // equal 1, 2, 3 & 4 respectively.
+ uint16_t timer_period = ticks_per_step >> (timer_clock - 1);
+
+ // Set clock and period
+ if ((m->active = timer_period && timer_clock)) {
+ m->timer->CNT = 0;
+ m->timer->CCA = timer_period; // Set step pulse period
+ m->timer->CTRLA = timer_clock; // Set clock rate
+ m->steps = 0;
+ }
}
-void motor_end_move(int motor) {
- motor_t *m = &motors[motor];
- m->dma->CTRLA &= ~DMA_CH_ENABLE_bm;
- m->timer->CTRLA = 0; // Stop clock
+ISR(M1_DMA_VECT) {_load_move(0);}
+ISR(M2_DMA_VECT) {_load_move(1);}
+ISR(M3_DMA_VECT) {_load_move(2);}
+ISR(M4_DMA_VECT) {_load_move(3);}
+
+
+void motor_load_move(int motor) {
+ if (!motors[motor].active) _load_move(motor);
}
-stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error,
- float time) {
+stat_t motor_prep_move(int motor, int32_t target) {
motor_t *m = &motors[motor];
// Validate input
if (motor < 0 || MOTORS < motor) return ALARM(STAT_INTERNAL_ERROR);
- if (clocks < 0) return ALARM(STAT_INTERNAL_ERROR);
if (isinf(target)) return ALARM(STAT_MOVE_TARGET_INFINITE);
if (isnan(target)) return ALARM(STAT_MOVE_TARGET_NAN);
- // 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;
- m->position = round(target);
+ // Compute travel in steps
+ int32_t steps = target - m->position;
+ m->position = target;
- // Setup the direction, compensating for polarity.
- bool negative = travel < 0;
+ // Set direction, compensating for polarity
+ bool negative = steps < 0;
m->clockwise = !(negative ^ m->reverse);
- // Use positive travel from here on
- if (negative) travel = -travel;
-
- // Find the fastest clock rate that will fit the required number of steps
- uint32_t ticks_per_step = travel ? clocks / 2 / travel : 0;
- if (ticks_per_step <= 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc;
- else if (ticks_per_step <= 0x1ffff) m->timer_clock = TC_CLKSEL_DIV2_gc;
- else if (ticks_per_step <= 0x3ffff) m->timer_clock = TC_CLKSEL_DIV4_gc;
- else if (ticks_per_step <= 0x7ffff) m->timer_clock = TC_CLKSEL_DIV8_gc;
- else m->timer_clock = 0; // Clock off, too slow
-
- // Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc
- // equal 1, 2, 3 & 4 respectively.
- m->timer_period = ticks_per_step >> (m->timer_clock - 1);
-
- // Round up if DIV4 or DIV8 and the error is high enough
- 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);
-
- if (step_error == half_error) {
- if (m->round_up) m->timer_period++;
- m->round_up = !m->round_up;
-
- } else if (half_error < step_error) m->timer_period++;
- }
-
- if (!m->timer_period) m->timer_clock = 0;
- if (!m->timer_clock) m->timer_period = 0;
+ // Positive steps from here on
+ if (negative) steps = -steps;
+ m->steps = steps;
// Compute power from axis velocity
- m->power = travel / (_get_max_velocity(motor) * time);
+ m->power = steps / (_get_max_velocity(motor) * SEGMENT_TIME);
// Power motor
switch (m->power_mode) {
case MOTOR_POWERED_ONLY_WHEN_MOVING:
- if (!m->timer_clock) break; // Not moving
+ if (!steps) break; // Not moving
// Fall through
case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE:
// Reset timeout
- m->timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000;
+ m->power_timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000;
break;
default: break;
uint8_t get_status_strings(int m) {return get_status_flags(m);}
-int32_t get_encoder(int m) {return motor_get_encoder(m);}
+int32_t get_encoder(int m) {return 0;}
// Command callback
float motor_get_units_per_step(int motor);
uint16_t motor_get_microsteps(int motor);
void motor_set_microsteps(int motor, uint16_t microsteps);
-int32_t motor_get_encoder(int motor);
-void motor_set_encoder(int motor, float encoder);
-int32_t motor_get_error(int motor);
+void motor_set_position(int motor, int32_t position);
int32_t motor_get_position(int motor);
bool motor_error(int motor);
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 clocks, float target, int32_t error,
- float time);
+stat_t motor_prep_move(int motor, int32_t target);
static stat_t _exec_calibrate(mp_buffer_t *bf) {
- const float time = MIN_SEGMENT_TIME; // In minutes
+ const float time = SEGMENT_TIME; // In minutes
const float max_delta_v = CAL_ACCELERATION * time;
do {
if (cal.stalled) {
if (cal.reverse) {
- int32_t steps = -motor_get_encoder(cal.motor);
+ int32_t steps = -motor_get_position(cal.motor);
float mm = (float)steps / motor_get_steps_per_unit(cal.motor);
STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm);
return STAT_NOOP; // Done, no move queued
} else {
- motor_set_encoder(cal.motor, 0);
+ motor_set_position(cal.motor, 0);
cal.reverse = true;
cal.velocity = 0;
mp_kinematics(travel, steps);
// Queue segment
- st_prep_line(time, steps, 0);
+ st_prep_line(steps);
return STAT_EAGAIN;
}
int16_t delta = sg - cal.stallguard;
if (!sg || CAL_MAX_DELTA_SG < abs(delta)) {
cal.stalled = true;
- motor_end_move(cal.motor);
+ //motor_end_move(cal.motor);
}
}
uint24_t segment_count; // count of running segments
uint24_t segment; // current segment
float segment_velocity; // computed velocity for segment
- float segment_time; // actual time increment per segment
float segment_start[AXES];
float segment_delta;
float segment_dist;
// len / avg. velocity
float move_time = 2 * length / (Vi + Vt); // in mins
- float segments = ceil(move_time / NOM_SEGMENT_TIME);
- ex.segment_time = move_time / segments;
+ float segments = ceil(move_time / SEGMENT_TIME);
ex.segment_count = round(segments);
ex.segment = 0;
ex.segment_dist = 0;
} else ex.segment_delta = 1 / (segments + 1);
- if (ex.segment_time < MIN_SEGMENT_TIME)
- return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position
-
ex.section_new = false;
}
// Compute quintic Bezier curve
ex.segment_velocity =
velocity_curve(Vi, Vt, ex.segment * ex.segment_delta);
- ex.segment_dist += ex.segment_velocity * ex.segment_time;
+ ex.segment_dist += ex.segment_velocity * SEGMENT_TIME;
}
for (int axis = 0; axis < AXES; axis++)
}
mp_runtime_set_velocity(ex.segment_velocity);
- RITORNO(mp_runtime_move_to_target(target, ex.segment_time));
+ RITORNO(mp_runtime_move_to_target(target));
// Return EAGAIN to continue or OK if this segment is done
return ex.segment < ex.segment_count ? STAT_EAGAIN : STAT_OK;
// Compute move time
float move_time = 2 * length / (V + Vt);
- if (move_time < MIN_SEGMENT_TIME) {
+ if (move_time < SEGMENT_TIME) {
V = Vt;
jr.velocity_t[axis] = 1;
} else {
jr.initial_velocity[axis] = V;
jr.velocity_t[axis] = jr.velocity_delta[axis] =
- NOM_SEGMENT_TIME / move_time;
+ SEGMENT_TIME / move_time;
}
}
}
float target[AXES];
for (int axis = 0; axis < AXES; axis++)
target[axis] = mp_runtime_get_axis_position(axis) +
- jr.velocity[axis] * NOM_SEGMENT_TIME;
+ jr.velocity[axis] * SEGMENT_TIME;
// Set velocity and target
mp_runtime_set_velocity(sqrt(velocity_sqr));
- stat_t status = mp_runtime_move_to_target(target, NOM_SEGMENT_TIME);
+ stat_t status = mp_runtime_move_to_target(target);
if (status != STAT_OK) return status;
return STAT_EAGAIN;
if (max_time < time) max_time = time;
}
- return max_time < MIN_SEGMENT_TIME ? MIN_SEGMENT_TIME : max_time;
+ return max_time < SEGMENT_TIME ? SEGMENT_TIME : max_time;
}
// expressions evaluate to the minimum lengths for the current velocity
// settings. Note: The head and tail lengths are 2 minimum segments, the body
// is 1 min segment.
-#define MIN_HEAD_LENGTH \
- (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity))
-#define MIN_TAIL_LENGTH \
- (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->exit_velocity))
-#define MIN_BODY_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * bf->cruise_velocity)
+#define MIN_HEAD_LENGTH \
+ (SEGMENT_TIME * (bf->cruise_velocity + bf->entry_velocity))
+#define MIN_TAIL_LENGTH \
+ (SEGMENT_TIME * (bf->cruise_velocity + bf->exit_velocity))
+#define MIN_BODY_LENGTH (SEGMENT_TIME * bf->cruise_velocity)
/*** Calculate move acceleration / deceleration
float naive_move_time =
2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average
- if (naive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) {
- bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN;
+ if (naive_move_time < SEGMENT_TIME) {
+ bf->cruise_velocity = bf->length / SEGMENT_TIME;
bf->exit_velocity =
max(0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax)));
bf->body_length = bf->length;
}
// B" case: Block is short, but fits into a single body segment
- if (naive_move_time <= NOM_SEGMENT_TIME) {
+ if (naive_move_time <= SEGMENT_TIME) {
bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity;
if (!fp_ZERO(bf->entry_velocity)) {
* bl (function arg) - end of block list (last block in time)
* bl->replannable - start of block list set by last FALSE value
* [Note 1]
- * bl->move_type - typically MOVE_TYPE_ALINE. Other move_types should
- * be set to length=0, entry_vmax=0 and exit_vmax=0
- * and are treated as a momentary stop (plan to zero
- * and from zero).
* bl->length - provides block length
* bl->entry_vmax - used during forward planning to set entry velocity
* bl->cruise_vmax - used during forward planning to set cruise velocity
#define JERK_MULTIPLIER 1000000.0
#define JERK_MATCH_PRECISION 1000.0 // jerk precision to be considered same
-#define NOM_SEGMENT_TIME (NOM_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
-#define MIN_SEGMENT_TIME (MIN_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
-
-#define MIN_SEGMENT_TIME_PLUS_MARGIN \
- ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE)
-
/// Error percentage for iteration convergence. As percent - 0.01 = 1%
#define TRAPEZOID_ITERATION_ERROR_PERCENT 0.1
for (int motor = 0; motor < MOTORS; motor++)
// Write steps to encoder register
- motor_set_encoder(motor, steps[motor]);
+ motor_set_position(motor, steps[motor]);
}
}
-static void _step_correction(const float steps[], float time, int32_t error[]) {
-#ifdef STEP_CORRECTION
- float travel[MOTORS];
- float new_length_sqr = 0;
- float old_length_sqr = 0;
-
- for (int motor = 0; motor < MOTORS; motor++) {
- error[motor] = motor_get_error(motor);
- travel[motor] = steps[motor] - motor_get_position(motor);
-
- if (fp_ZERO(travel[motor])) {
- motor[travel] = 0;
- motor[error] = 0;
- }
-
- error[motor] = 0.5 * (error[motor] - rt.previous_error[motor]);
- rt.previous_error[motor] = error[motor];
-
- if (error[motor] < -MAX_STEP_CORRECTION)
- error[motor] = -MAX_STEP_CORRECTION;
- else if (MAX_STEP_CORRECTION < error[motor])
- error[motor] = MAX_STEP_CORRECTION;
-
- old_length_sqr += square(travel[motor]);
- new_length_sqr += square(travel[motor] - error[motor]);
- }
-
- bool use_error = false;
- if (!fp_ZERO(new_length_sqr)) {
- float new_time = time * invsqrt(old_length_sqr / new_length_sqr);
-
- if (!isnan(new_time) && !isinf(new_time) &&
- EPSILON <= new_time && new_time <= MAX_SEGMENT_TIME) {
- time = new_time;
- use_error = true;
- }
- }
-
- if (!use_error)
- for (int motor = 0; motor < MOTORS; motor++)
- error[motor] = 0;
-
-#endif // STEP_CORRECTION
-}
-
-
/// Segment runner
-stat_t mp_runtime_move_to_target(float target[], float time) {
- ASSERT(isfinite(time));
+stat_t mp_runtime_move_to_target(float target[]) {
+ ASSERT(isfinite(target[0]) && isfinite(target[1]) &&
+ isfinite(target[2]) && isfinite(target[3]));
// Convert target position to steps.
float steps[MOTORS];
mp_kinematics(target, steps);
- int32_t error[MOTORS] = {0};
- _step_correction(steps, time, error);
-
// Call the stepper prep function
- RITORNO(st_prep_line(time, steps, error));
+ RITORNO(st_prep_line(steps));
// Update positions
mp_runtime_set_position(target);
float mp_runtime_get_work_position(uint8_t axis);
void mp_runtime_set_work_offsets(float offset[]);
-stat_t mp_runtime_move_to_target(float target[], float time);
+stat_t mp_runtime_move_to_target(float target[]);
typedef enum {
MOVE_TYPE_NULL, // null move - does a no-op
- MOVE_TYPE_ALINE, // acceleration planned line
+ MOVE_TYPE_LINE, // acceleration planned line
MOVE_TYPE_DWELL, // delay with no movement
} move_type_t;
// Runtime
bool busy;
bool requesting;
- uint16_t dwell;
+ float dwell;
// Move prep
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;
+ float prep_dwell;
} stepper_t;
/// Dequeue move and load into stepper struct
ISR(STEP_TIMER_ISR) {
// Dwell
- if (st.dwell && --st.dwell) return;
+ if (0 < st.dwell) {
+ st.dwell -= SEGMENT_SEC;
+ return;
+ } else st.dwell = 0;
- // End last move
+ // Default clock rate
TIMER_STEP.PER = STEP_TIMER_POLL;
- DMA.INTFLAGS = 0xff; // clear all interrups
- for (int motor = 0; motor < MOTORS; motor++)
- motor_end_move(motor);
-
if (estop_triggered()) {
st.move_type = MOVE_TYPE_NULL;
return;
if (motor_energizing()) return;
// Start move
- if (st.seg_period) {
+ if (st.move_type == MOVE_TYPE_LINE)
for (int motor = 0; motor < MOTORS; motor++)
motor_load_move(motor);
- TIMER_STEP.PER = st.seg_period;
+ if (st.move_type != MOVE_TYPE_NULL) {
+ TIMER_STEP.PER = SEGMENT_PERIOD;
st.busy = true;
// Start dwell
// We are done with this move
st.move_type = MOVE_TYPE_NULL;
- st.seg_period = 0; // clear timer
st.prep_dwell = 0; // clear dwell
st.move_ready = false; // flip the flag back
* @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 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 time, const float target[], const int32_t error[]) {
+stat_t st_prep_line(const float target[]) {
// Trap conditions that would prevent queueing the line
- if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR);
- if (isinf(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_INFINITE);
- if (isnan(time)) return ALARM(STAT_PREP_LINE_MOVE_TIME_NAN);
- if (time < EPSILON) return ALARM(STAT_MINIMUM_TIME_MOVE);
- if (MAX_SEGMENT_TIME < time) return ALARM(STAT_MAXIMUM_TIME_MOVE);
+ if (st.move_ready) return ALARM(STAT_INTERNAL_ERROR);
// Setup segment parameters
- st.move_type = MOVE_TYPE_ALINE;
- st.seg_period = round(time * 60 * STEP_TIMER_FREQ); // Must fit 16-bit
- int32_t seg_clocks = (int32_t)st.seg_period * STEP_TIMER_DIV;
+ st.move_type = MOVE_TYPE_LINE;
// Prepare motor moves
for (int motor = 0; motor < MOTORS; motor++)
- RITORNO
- (motor_prep_move(motor, seg_clocks, target[motor], error[motor], time));
+ RITORNO(motor_prep_move(motor, round(target[motor])));
st.move_queued = true; // signal prep buffer ready (do this last)
void st_prep_dwell(float seconds) {
if (st.move_ready) ALARM(STAT_INTERNAL_ERROR);
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.prep_dwell = seconds;
st.move_queued = true; // signal prep buffer ready
}
void stepper_init();
void st_shutdown();
bool st_is_busy();
-stat_t st_prep_line(float time, const float target[], const int32_t error[]);
+stat_t st_prep_line(const float target[]);
void st_prep_dwell(float seconds);
projects / bbctrl-firmware / commitdiff