From d30d8c972164fb8fd6f893dd40c0e0b0ec76e1ae Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 13 Mar 2016 20:40:29 -0700 Subject: [PATCH] Some cleanup after stepper driver overhaul --- src/stepper.c | 207 +++++++++++++++++++++++--------------------------- src/stepper.h | 155 ++++++++++--------------------------- 2 files changed, 138 insertions(+), 224 deletions(-) diff --git a/src/stepper.c b/src/stepper.c index ff14654..8b03884 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -83,7 +83,7 @@ void stepper_init() { 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 @@ -243,7 +243,7 @@ ISR(TCE1_CCA_vect) { } -/// 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; @@ -251,7 +251,7 @@ ISR(STEP_TIMER_ISR) { } -/// 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 @@ -260,10 +260,9 @@ void st_request_exec_move() { } -/// 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 @@ -272,7 +271,7 @@ ISR(ADCB_CH0_vect) { } -/// 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 @@ -287,7 +286,7 @@ static void _request_load_move() { 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(); } @@ -320,6 +319,7 @@ static inline void _load_motor_move(int motor) { // Accumulate encoder en[motor].encoder_steps += pre_mot->steps * pre_mot->step_sign; + pre_mot->steps = 0; } // Energize motor and start power management @@ -386,10 +386,84 @@ static void _load_move() { } +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. @@ -400,122 +474,33 @@ static void _load_move() { * 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; } diff --git a/src/stepper.h b/src/stepper.h index ddc7718..43b3ee3 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -30,58 +30,24 @@ #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. @@ -112,8 +78,7 @@ * * 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 @@ -127,23 +92,7 @@ * 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: @@ -167,25 +116,20 @@ * 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 @@ -201,12 +145,12 @@ * 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. * @@ -218,9 +162,10 @@ * 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) @@ -246,7 +191,7 @@ * 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 * @@ -269,15 +214,6 @@ * 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" @@ -351,8 +287,7 @@ enum { #define STEP_INITIAL_DIRECTION DIRECTION_CW -/* - * Stepper control structures +/* Stepper control structures * * There are 5 main structures involved in stepper operations; * @@ -369,8 +304,8 @@ enum { * 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 @@ -382,20 +317,22 @@ typedef struct cfgMotor { // per-motor configs } 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; @@ -403,36 +340,30 @@ typedef struct stRunSingleton { // Stepper static values and axis parameters } 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; @@ -455,5 +386,3 @@ void st_prep_command(void *bf); // void * since mpBuf_t is not visible here void st_prep_dwell(float seconds); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); - - -- 2.27.0