hw.st_port[i]->OUTSET = MOTOR_ENABLE_BIT_bm; // disable motor
}
- // Setup step timer (DDA)
+ // Setup step timer
TIMER_STEP.CTRLA = STEP_TIMER_DISABLE; // turn timer off
TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode
TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode
}
-/// Step timer interrupt routine - service ticks from DDA timer
+/// Step timer interrupt routine
ISR(STEP_TIMER_ISR) {
if (st_run.move_type == MOVE_TYPE_DWELL && --st_run.dwell) return;
st_run.busy = false;
}
-/// SW interrupt to request to execute a move
+/// "Software" interrupt to request move execution
void st_request_exec_move() {
if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) {
ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // trigger LO interrupt
}
-/// Interrupt handler for calling exec function
+/// Interrupt handler for calling move exec function.
+/// Uses ADC channel 0 as software interrupt.
ISR(ADCB_CH0_vect) {
- // Use ADC channel 0 as software interrupt
- // Exec move
if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC &&
mp_exec_move() != STAT_NOOP) {
st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // flip it back
}
-/// Fires a software interrupt (timer) to request to load a move
+/// Fires a "software" interrupt to request to load a move
static void _request_load_move() {
if (st_runtime_isbusy()) return; // don't request a load if runtime is busy
ISR(ADCB_CH1_vect) {
// Use ADC channel 1 as software interrupt.
// _load_move() can only be called be called from an ISR at the same or
- // higher level as the DDA ISR. A software interrupt is used to allow a
+ // higher level as the step timer ISR. A software interrupt is used to allow a
// non-ISR to request a load. See st_request_load_move()
_load_move();
}
// Accumulate encoder
en[motor].encoder_steps += pre_mot->steps * pre_mot->step_sign;
+ pre_mot->steps = 0;
}
// Energize motor and start power management
}
+static void _prep_motor_line(int motor, float travel_steps, float error) {
+ stPrepMotor_t *pre_mot = &st_pre.mot[motor];
+ cfgMotor_t *cfg_mot = &st_cfg.mot[motor];
+
+ // Disable this motor's clock if there are no new steps
+ if (fp_ZERO(travel_steps)) {
+ pre_mot->timer_clock = 0; // Off
+ return;
+ }
+
+ // Setup the direction, compensating for polarity.
+ // Set the step_sign which is used by the stepper ISR to accumulate step
+ // position
+ if (0 <= travel_steps) { // positive direction
+ pre_mot->direction = DIRECTION_CW ^ cfg_mot->polarity;
+ pre_mot->step_sign = 1;
+
+ } else {
+ pre_mot->direction = DIRECTION_CCW ^ cfg_mot->polarity;
+ pre_mot->step_sign = -1;
+ }
+
+#ifdef __STEP_CORRECTION
+ float correction;
+
+ // 'Nudge' correction strategy. Inject a single, scaled correction value
+ // then hold off
+ if (--pre_mot->correction_holdoff < 0 &&
+ STEP_CORRECTION_THRESHOLD < fabs(error)) {
+
+ pre_mot->correction_holdoff = STEP_CORRECTION_HOLDOFF;
+ 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);
+
+ pre_mot->corrected_steps += correction;
+ travel_steps -= correction;
+ }
+#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.
+ uint16_t steps = round(fabs(travel_steps));
+ uint32_t seg_clocks = (uint32_t)st_pre.seg_period * STEP_TIMER_DIV;
+ uint32_t ticks_per_step = seg_clocks / (steps + 0.5);
+
+ // Find the right clock rate
+ if (ticks_per_step & 0xffff0000UL) {
+ ticks_per_step /= 2;
+ seg_clocks /= 2;
+
+ if (ticks_per_step & 0xffff0000UL) {
+ ticks_per_step /= 2;
+ seg_clocks /= 2;
+
+ if (ticks_per_step & 0xffff0000UL) {
+ ticks_per_step /= 2;
+ seg_clocks /= 2;
+
+ if (ticks_per_step & 0xffff0000UL) pre_mot->timer_clock = 0; // Off
+ else pre_mot->timer_clock = TC_CLKSEL_DIV8_gc;
+ } else pre_mot->timer_clock = TC_CLKSEL_DIV4_gc;
+ } else pre_mot->timer_clock = TC_CLKSEL_DIV2_gc;
+ } else pre_mot->timer_clock = TC_CLKSEL_DIV1_gc;
+
+ pre_mot->timer_period = ticks_per_step * 2; // TODO why do we need *2 here?
+ pre_mot->steps = seg_clocks / ticks_per_step;
+}
+
+
/* Prepare the next move for the loader
*
* This function does the math on the next pulse segment and gets it ready for
- * the loader. It deals with all the DDA optimizations and timer setups so that
+ * the loader. It deals with all the optimizations and timer setups so that
* loading can be performed as rapidly as possible. It works in joint space
* (motors) and it works in steps, not length units. All args are provided as
* floats and converted to their appropriate integer types for the loader.
* sign indicates direction. Motors that are not in the move should be 0
* steps on input.
*
- * - following_error[] is a vector of measured errors to the step count. Used
- * for correction.
+ * - error[] is a vector of measured errors to the step count. Used for
+ * correction.
*
* - segment_time - how many minutes the segment should run. If timing is not
* 100% accurate this will affect the move velocity, but not the distance
* traveled.
*/
-stat_t st_prep_line(float travel_steps[], float following_error[],
- float segment_time) {
+stat_t st_prep_line(float travel_steps[], float error[], float segment_time) {
// Trap conditions that would prevent queueing the line
if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_EXEC)
return cm_hard_alarm(STAT_INTERNAL_ERROR);
- else if (isinf(segment_time)) // never supposed to happen
+ if (isinf(segment_time))
return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE);
- else if (isnan(segment_time)) // never supposed to happen
+ if (isnan(segment_time))
return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_NAN);
- else if (segment_time < EPSILON) return STAT_MINIMUM_TIME_MOVE;
+ if (segment_time < EPSILON) return STAT_MINIMUM_TIME_MOVE;
- // setup segment parameters
+ // Setup segment parameters (need st_pre.seg_period for motor calcs)
st_pre.seg_period = segment_time * 60 * F_CPU / STEP_TIMER_DIV;
+ st_pre.move_type = MOVE_TYPE_ALINE;
- // setup motor parameters
- for (uint8_t motor = 0; motor < MOTORS; motor++) {
- stPrepMotor_t *pre_mot = &st_pre.mot[motor];
-
- // Disable this motor's clock if there are no new steps
- if (fp_ZERO(travel_steps[motor])) {
- pre_mot->timer_clock = 0; // Off
- continue;
- }
-
- // Setup the direction, compensating for polarity.
- // Set the step_sign which is used by the stepper ISR to accumulate step
- // position
- if (0 <= travel_steps[motor]) { // positive direction
- pre_mot->direction = DIRECTION_CW ^ st_cfg.mot[motor].polarity;
- pre_mot->step_sign = 1;
-
- } else {
- pre_mot->direction = DIRECTION_CCW ^ st_cfg.mot[motor].polarity;
- pre_mot->step_sign = -1;
- }
-
- // Detect segment time changes and setup the accumulator correction factor
- // and flag. Putting this here computes the correct factor even if the motor
- // was dormant for some number of previous moves. Correction is computed
- // based on the last segment time actually used.
- if (0.0000001 < fabs(segment_time - pre_mot->prev_segment_time)) {
- // special case to skip first move
- if (fp_NOT_ZERO(pre_mot->prev_segment_time)) {
- pre_mot->accumulator_correction_flag = true;
- pre_mot->accumulator_correction =
- segment_time / pre_mot->prev_segment_time;
- }
-
- pre_mot->prev_segment_time = segment_time;
- }
-
-#ifdef __STEP_CORRECTION
- float correction;
-
- // 'Nudge' correction strategy. Inject a single, scaled correction value
- // then hold off
- if (--pre_mot->correction_holdoff < 0 &&
- STEP_CORRECTION_THRESHOLD < fabs(following_error[motor])) {
-
- pre_mot->correction_holdoff = STEP_CORRECTION_HOLDOFF;
- correction = following_error[motor] * STEP_CORRECTION_FACTOR;
-
- if (0 < correction)
- correction =
- min3(correction, fabs(travel_steps[motor]), STEP_CORRECTION_MAX);
- else correction =
- max3(correction, -fabs(travel_steps[motor]), -STEP_CORRECTION_MAX);
-
- pre_mot->corrected_steps += correction;
- travel_steps[motor] -= correction;
- }
-#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.
- uint16_t steps = round(fabs(travel_steps[motor]));
- uint32_t seg_clocks = (uint32_t)st_pre.seg_period * STEP_TIMER_DIV;
- uint32_t ticks_per_step = seg_clocks / (steps + 0.5);
-
- // Find the right clock rate
- if (ticks_per_step & 0xffff0000UL) {
- ticks_per_step /= 2;
- seg_clocks /= 2;
-
- if (ticks_per_step & 0xffff0000UL) {
- ticks_per_step /= 2;
- seg_clocks /= 2;
-
- if (ticks_per_step & 0xffff0000UL) {
- ticks_per_step /= 2;
- seg_clocks /= 2;
-
- if (ticks_per_step & 0xffff0000UL) pre_mot->timer_clock = 0; // Off
- else pre_mot->timer_clock = TC_CLKSEL_DIV8_gc;
- } else pre_mot->timer_clock = TC_CLKSEL_DIV4_gc;
- } else pre_mot->timer_clock = TC_CLKSEL_DIV2_gc;
- } else pre_mot->timer_clock = TC_CLKSEL_DIV1_gc;
-
- pre_mot->timer_period = ticks_per_step * 2; // TODO why do we need *2 here?
- pre_mot->steps = seg_clocks / ticks_per_step;
-
- if (false && usart_tx_empty() && motor == 0)
- printf("period=%d steps=%ld time=%0.6f\n",
- pre_mot->timer_period, pre_mot->steps, segment_time * 60);
- }
+ // Setup motor parameters
+ for (uint8_t motor = 0; motor < MOTORS; motor++)
+ _prep_motor_line(motor, travel_steps[motor], error[motor]);
- st_pre.move_type = MOVE_TYPE_ALINE;
- st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal prep buffer ready
+ // Signal prep buffer ready (do this last)
+ st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER;
return STAT_OK;
}
#pragma once
-/* Coordinated motion (line drawing) is performed using a classic
+/* # Coordinated motion
+ *
+ * Coordinated motion (line drawing) is performed using a classic
* Bresenham DDA. A number of additional steps are taken to optimize
* interpolation and pulse train timing accuracy to minimize pulse
* jitter and make for very smooth motion and surface finish.
*
- * - The DDA is not used as a 'ramp' for acceleration
+ * - stepper.c is not used as a 'ramp' for acceleration
* management. Accel is computed upstream in the motion planner as
* 3rd order (controlled jerk) equations. These generate accel/decel
- * segments that are passed to the DDA for step output.
+ * segments that are passed to stepper.c for step output.
*
- * - The DDA accepts and processes fractional motor steps as floating
+ * - stepper.c accepts and processes fractional motor steps as floating
* point numbers from the planner. Steps do not need to be whole
- * numbers, and are not expected to be. The step values are
- * converted to integer by multiplying by a fixed-point precision
- * (DDA_SUBSTEPS, 100000). Rounding is performed to avoid a
- * truncation bias.
- *
- * - Constant Rate DDA clock: The DDA runs at a constant, maximum rate
- * for every segment regardless of actual step rate required. This
- * means that the DDA clock is not tuned to the step rate (or a
- * multiple) of the major axis, as is typical for most DDAs. Running
- * the DDA flat out might appear to be "wasteful", but it ensures
- * that the best aliasing results are achieved, and is part of
- * maintaining step accuracy across motion segments.
- *
- * The observation is this firmware is a hard real-time system in
- * which every clock cycle is knowable and can be accounted for. So
- * if the system is capable of sustaining max pulse rate for the
- * fastest move, it's capable of sustaining this rate for any
- * move. So we just run it flat out and get the best pulse
- * resolution for all moves. If we were running from batteries or
- * otherwise cared about the energy budget we might not be so
- * cavalier about this.
- *
- * At 50 KHz constant clock rate we have 20 uSec between pulse timer
- * (DDA) interrupts. On the Xmega we consume <10 uSec in the
- * interrupt - a whopping 50% of available cycles going into pulse
- * generation.
- *
- * - Pulse timing is also helped by minimizing the time spent loading
- * the next move segment. The time budget for the load is less than
- * the time remaining before the next DDA clock tick. This means
- * that the load must take < 10 uSec or the time between pulses will
- * stretch out when changing segments. This does not affect
- * positional accuracy but it would affect jitter and smoothness. To
- * this end as much as possible about that move is pre-computed
- * during move execution (prep cycles). Also, all moves are loaded
- * from the DDA interrupt level (HI), avoiding the need for mutual
- * exclusion locking or volatiles (which slow things down).
- */
-
-/* Move generation overview and timing illustration
+ * numbers, and are not expected to be. Rounding is performed to
+ * avoid a truncation bias.
+ *
+ * # Move generation overview and timing illustration
*
* This ASCII art illustrates a 4 segment move to show stepper sequencing
* timing.
*
* Once the segment is loaded it will pulse out steps for the
* duration of the segment. Segment timing can vary, but segments
- * take around 5 Msec to pulse out, which is 250 DDA ticks at a 50
- * KHz step clock.
+ * take around 5 Msec to pulse out.
*
* Now the move is pulsing out segment 1 (at HI interrupt
* level). Once the L1 loader is finished it invokes the exec
* being planned in background. As long as a move takes less than
* the segment times (5ms x N) the timing budget is satisfied,
*
- * A few things worth noting:
- * - This scheme uses 2 interrupt levels and background, for 3
- * levels of execution:
- * - STEP pulsing and LOADs occur at HI interrupt level
- * - EXEC and PREP occur at LO interrupt level (leaving MED int
- * level for serial IO)
- * - move PLANning occurs in background and is managed by the controller
- *
- * - Because of the way the timing is laid out there is no contention
- * for resources between the STEP, LOAD, EXEC, and PREP phases. PLANing
- * is similarly isolated. Very few volatiles or mutexes are needed, which
- * makes the code simpler and faster. With the exception of the actual
- * values used in step generation (which runs continuously) you can count
- * on LOAD, EXEC, PREP and PLAN not stepping on each other's variables.
- */
-
-/* Line planning and execution (in more detail)
+ * # Line planning and execution (in more detail)
*
* Move planning, execution and pulse generation takes place at 3
* levels:
* level. Move execution generates the next acceleration, cruise, or
* deceleration segment for planned lines, or just transfers
* parameters needed for dwells and stops. This layer also prepares
- * segments for loading by pre-calculating the values needed by the
- * DDA and converting the segment into parameters that can be directly
+ * segments for loading by pre-calculating the values needed by
+ * stepper.c and converting the segment into parameters that can be directly
* loaded into the steppers ("exec" and "prep" stages).
*
- * Pulse train generation takes place at the HI interrupt level. The
- * stepper DDA fires timer interrupts that generate the stepper
- * pulses. This level also transfers new stepper parameters once each
- * pulse train ("segment") is complete ("load" and "run" stages).
- */
-
-/* What happens when the pulse generator is done with the current pulse train
+ * # Multi stage pull queue
+ *
+ * What happens when the pulse generator is done with the current pulse train
* (segment) is a multi-stage "pull" queue that looks like this:
*
* As long as the steppers are running the sequence of events is:
*
- * - The stepper interrupt (HI) runs the DDA to generate a pulse train for
- * the current move. This runs for the length of the pulse train currently
- * executing
- * - the "segment", usually 5ms worth of pulses
+ * - The stepper interrupt (HI) runs the step clock to setup the motor
+ * timers for the current move. This runs for the length of the pulse
+ * train currently executing. The "segment", usually 5ms worth of pulses
*
* - When the current segment is finished the stepper interrupt LOADs the
* next segment from the prep buffer, reloads the timers, and starts the
* run for the move the exec first gets the next buffer in the planning
* queue and begins execution.
*
- * In some cases the mext "move" is not actually a move, but a dewll, stop,
+ * In some cases the next "move" is not actually a move, but a dewll, stop,
* IO operation (e.g. M5). In this case it executes the requested operation,
* and may attempt to get the next buffer from the planner when its done.
*
* - Once the segment has been computed the exec handler finshes up by
- * running the PREP routine in stepper.c. This computes the DDA values and
+ * running the PREP routine in stepper.c. This computes the timer values and
* gets the segment into the prep buffer - and ready for the next LOAD
* operation.
*
* If the steppers are not running the above is similar, except that the exec is
* invoked from the main loop by the software interrupt, and the stepper load is
* invoked from the exec by another software interrupt.
- */
-
-/* Control flow can be a bit confusing. This is a typical sequence for planning
+ *
+ * # Control flow
+ *
+ * Control flow can be a bit confusing. This is a typical sequence for planning
* executing, and running an acceleration planned line:
*
* 1 planner.mp_aline() is called, which populates a planning buffer (bf)
* PREP routine in stepper.c to derive the stepper parameters that will
* be needed to run the move - in this example st_prep_line().
*
- * 7 st_prep_line() generates the timer and DDA values and stages these into
+ * 7 st_prep_line() generates the timer values and stages these into
* the prep structure (sp) - ready for loading into the stepper runtime
* struct
*
* are modified at what level, and use volatiles where necessary.
*/
-/* Partial steps and phase angle compensation
- *
- * The DDA accepts partial steps as input. Fractional steps are managed by the
- * sub-step value as explained elsewhere. The fraction initially loaded into
- * the DDA and the remainder left at the end of a move (the "residual") can
- * be thought of as a phase angle value for the DDA accumulation. Each 360
- * degrees of phase angle results in a step being generated.
- */
-
#include "config.h"
#include "status.h"
#define STEP_INITIAL_DIRECTION DIRECTION_CW
-/*
- * Stepper control structures
+/* Stepper control structures
*
* There are 5 main structures involved in stepper operations;
*
* better.
*/
-// Motor config structure
-typedef struct cfgMotor { // per-motor configs
+// Per motor config structure
+typedef struct cfgMotor {
uint8_t motor_map; // map motor to axis
uint32_t microsteps; // microsteps to apply for each axis (ex: 8)
uint8_t polarity; // 0=normal polarity, 1=reverse motor direction
} cfgMotor_t;
-typedef struct stConfig { // stepper configs
+/// stepper configs
+typedef struct stConfig {
float motor_power_timeout; // seconds before idle current
cfgMotor_t mot[MOTORS]; // settings for motors 1-N
} stConfig_t;
-// Motor runtime structure. Used by step generation ISR (HI)
+/// Motor runtime structure. Used by step generation ISR (HI)
typedef struct stRunMotor { // one per controlled motor
uint8_t power_state; // state machine for managing motor power
uint32_t power_systick; // for next motor power state transition
} stRunMotor_t;
-typedef struct stRunSingleton { // Stepper static values and axis parameters
+/// Stepper static values and axis parameters
+typedef struct stRunSingleton {
uint8_t move_type;
bool busy;
uint16_t dwell;
} stRunSingleton_t;
-// Motor prep structure. Used by exec/prep ISR (LO) and read-only during load
-// Must be careful about volatiles in this one
+/// Motor prep structure. Used by exec/prep ISR (LO) and read-only during load
+/// Must be careful about volatiles in this one
typedef struct stPrepMotor {
- uint8_t timer_clock;
- uint16_t timer_period;
- uint32_t steps;
+ uint8_t timer_clock; // clock divisor setting or zero for off
+ uint16_t timer_period; // clock period counter
+ uint32_t steps; // expected steps
// direction and direction change
int8_t direction; // travel direction corrected for polarity
uint8_t prev_direction; // travel direction from previous segment run
int8_t step_sign; // set to +1 or -1 for encoders
- // following error correction
+ // step error correction
int32_t correction_holdoff; // count down segments between corrections
float corrected_steps; // accumulated for cycle (diagnostic)
-
- // accumulator phase correction
- float prev_segment_time; // segment time from previous run for motor
- float accumulator_correction; // factor for adjusting between segments
- uint8_t accumulator_correction_flag; // signals accumulator needs correction
} stPrepMotor_t;
typedef struct stPrepSingleton {
volatile uint8_t buffer_state; // prep buffer state - owned by exec or loader
- struct mpBuffer *bf; // static pointer to relevant buffer
uint8_t move_type; // move type
+ struct mpBuffer *bf; // used for command moves
uint16_t seg_period;
uint32_t dwell;
-
stPrepMotor_t mot[MOTORS]; // prep time motor structs
} stPrepSingleton_t;
void st_prep_dwell(float seconds);
stat_t st_prep_line(float travel_steps[], float following_error[],
float segment_time);
-
-
projects / bbctrl-firmware / commitdiff