From 53a9044583ba78123093baefaff1f3dcf6c48e8a Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 23 Mar 2016 02:16:19 -0700 Subject: [PATCH] Even more efficient stepper driver, cleaned up docs, fixed bug in motor clock rate calculation --- src/config.h | 2 + src/motor.c | 103 +++++++--------------- src/motor.h | 16 ++++ src/plan/buffer.c | 3 - src/plan/feedhold.c | 2 - src/stepper.c | 182 ++++++++++++++------------------------- src/stepper.h | 203 -------------------------------------------- 7 files changed, 113 insertions(+), 398 deletions(-) diff --git a/src/config.h b/src/config.h index 27b4f4a..252fac0 100644 --- a/src/config.h +++ b/src/config.h @@ -313,6 +313,8 @@ typedef enum { #define STEP_TIMER_DISABLE 0 #define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc #define STEP_TIMER_DIV 4 +#define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV) +#define STEP_TIMER_POLL (STEP_TIMER_FREQ * 0.001) #define STEP_TIMER_WGMODE TC_WGMODE_NORMAL_gc // count to TOP & rollover #define STEP_TIMER_ISR TCC0_OVF_vect #define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc diff --git a/src/motor.c b/src/motor.c index 6813d5f..51e92ec 100644 --- a/src/motor.c +++ b/src/motor.c @@ -56,29 +56,13 @@ typedef enum { } motorPowerState_t; -typedef enum { - MOTOR_DISABLED, // motor enable is deactivated - MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON - MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, - // de-powered out of cycle - MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after stopped, even in cycle - MOTOR_POWER_MODE_MAX_VALUE // for input range checking -} cmMotorPowerMode_t; - - -typedef enum { - MOTOR_POLARITY_NORMAL, - MOTOR_POLARITY_REVERSED -} cmMotorPolarity_t; - - typedef struct { // Config uint8_t motor_map; // map motor to axis - uint16_t microsteps; // microsteps to apply for each axis (ex: 8) + uint16_t microsteps; // microsteps per full step cmMotorPolarity_t polarity; cmMotorPowerMode_t power_mode; - float step_angle; // degrees per whole step (ex: 1.8) + float step_angle; // degrees per whole step float travel_rev; // mm or deg of travel per motor revolution TC0_t *timer; @@ -90,14 +74,10 @@ typedef struct { // Move prep 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 + int32_t steps; // expected steps, for encoder cmDirection_t direction; // travel direction corrected for polarity - cmDirection_t prev_direction; // travel direction from previous segment run - int8_t step_sign; // set to +1 or -1 for encoders - // step error correction + // Step error correction int32_t correction_holdoff; // count down segments between corrections float corrected_steps; // accumulated for cycle (diagnostic) } motor_t; @@ -112,7 +92,6 @@ static motor_t motors[MOTORS] = { .polarity = M1_POLARITY, .power_mode = M1_POWER_MODE, .timer = (TC0_t *)&M1_TIMER, - .prev_direction = STEP_INITIAL_DIRECTION }, { .motor_map = M2_MOTOR_MAP, .step_angle = M2_STEP_ANGLE, @@ -121,7 +100,6 @@ static motor_t motors[MOTORS] = { .polarity = M2_POLARITY, .power_mode = M2_POWER_MODE, .timer = &M2_TIMER, - .prev_direction = STEP_INITIAL_DIRECTION }, { .motor_map = M3_MOTOR_MAP, .step_angle = M3_STEP_ANGLE, @@ -130,7 +108,6 @@ static motor_t motors[MOTORS] = { .polarity = M3_POLARITY, .power_mode = M3_POWER_MODE, .timer = &M3_TIMER, - .prev_direction = STEP_INITIAL_DIRECTION }, { .motor_map = M4_MOTOR_MAP, .step_angle = M4_STEP_ANGLE, @@ -139,7 +116,6 @@ static motor_t motors[MOTORS] = { .polarity = M4_POLARITY, .power_mode = M4_POWER_MODE, .timer = &M4_TIMER, - .prev_direction = STEP_INITIAL_DIRECTION } }; @@ -221,7 +197,6 @@ void motor_driver_callback(int motor) { m->flags |= MOTOR_FLAG_ENABLED_bm; } - st_request_load_move(); report_request(); } @@ -264,28 +239,14 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, 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 - m->direction = DIRECTION_CW ^ m->polarity; - m->step_sign = 1; - - } else { - m->direction = DIRECTION_CCW ^ m->polarity; - m->step_sign = -1; - } - #ifdef __STEP_CORRECTION - float 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; - correction = error * STEP_CORRECTION_FACTOR; + float correction = error * STEP_CORRECTION_FACTOR; if (0 < correction) correction = min3(correction, fabs(travel_steps), STEP_CORRECTION_MAX); @@ -303,27 +264,36 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, uint16_t steps = round(fabs(travel_steps)); uint32_t ticks_per_step = seg_clocks / (steps + 0.5); - // Find the right clock rate - if (ticks_per_step & 0xffff0000UL) { + // Find the clock rate that will fit the required number of steps + if (ticks_per_step & 0xffff8000UL) { ticks_per_step /= 2; seg_clocks /= 2; - if (ticks_per_step & 0xffff0000UL) { + if (ticks_per_step & 0xffff8000UL) { ticks_per_step /= 2; seg_clocks /= 2; - if (ticks_per_step & 0xffff0000UL) { + if (ticks_per_step & 0xffff8000UL) { ticks_per_step /= 2; seg_clocks /= 2; - if (ticks_per_step & 0xffff0000UL) m->timer_clock = 0; // Off + if (ticks_per_step & 0xffff8000UL) m->timer_clock = 0; // Off else m->timer_clock = TC_CLKSEL_DIV8_gc; } else m->timer_clock = TC_CLKSEL_DIV4_gc; } else m->timer_clock = TC_CLKSEL_DIV2_gc; } else m->timer_clock = TC_CLKSEL_DIV1_gc; - m->timer_period = ticks_per_step * 2; // TODO why do we need *2 here? - m->steps = seg_clocks / ticks_per_step; + m->timer_period = ticks_per_step * 2; // TODO why do we need x2 here? + m->steps = seg_clocks / ticks_per_step; // Compute actual steps + + // Setup the direction, compensating for polarity. + if (0 <= travel_steps) // positive direction + m->direction = DIRECTION_CW ^ m->polarity; + + else { + m->direction = DIRECTION_CCW ^ m->polarity; + m->steps = -m->steps; + } } @@ -351,29 +321,20 @@ void motor_load_move(int motor) { motor_t *m = &motors[motor]; // Set or zero runtime clock and period - m->timer->CTRLFCLR = TC0_DIR_bm; // Count up - m->timer->CNT = 0; // Start at zero + m->timer->CTRLFCLR = TC0_DIR_bm; // Count up + m->timer->CNT = 0; // Start at zero m->timer->CCA = m->timer_period; // Set frequency m->timer->CTRLA = m->timer_clock; // Start or stop + m->timer_clock = 0; // Clear clock - // If motor has 0 steps the following is all skipped. This ensures that - // state comparisons always operate on the last segment actually run by - // this motor, regardless of how many segments it may have been inactive - // in between. - if (m->timer_clock) { - // Detect direction change and set the direction bit in hardware. - if (m->direction != m->prev_direction) { - m->prev_direction = m->direction; - - if (m->direction == DIRECTION_CW) - hw.st_port[motor]->OUTCLR = DIRECTION_BIT_bm; - else hw.st_port[motor]->OUTSET = DIRECTION_BIT_bm; - } - - // Accumulate encoder - en[motor].encoder_steps += m->steps * m->step_sign; - m->steps = 0; - } + // Set direction + if (m->direction == DIRECTION_CW) + hw.st_port[motor]->OUTCLR = DIRECTION_BIT_bm; + else hw.st_port[motor]->OUTSET = DIRECTION_BIT_bm; + + // Accumulate encoder + en[motor].encoder_steps += m->steps; + m->steps = 0; } diff --git a/src/motor.h b/src/motor.h index 8b408a9..0db5282 100644 --- a/src/motor.h +++ b/src/motor.h @@ -46,6 +46,22 @@ typedef enum { } cmMotorFlags_t; +typedef enum { + MOTOR_DISABLED, // motor enable is deactivated + MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON + MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, + // de-powered out of cycle + MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after stopped, even in cycle + MOTOR_POWER_MODE_MAX_VALUE // for input range checking +} cmMotorPowerMode_t; + + +typedef enum { + MOTOR_POLARITY_NORMAL, + MOTOR_POLARITY_REVERSED +} cmMotorPolarity_t; + + void motor_init(); int motor_get_axis(int motor); diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 4dfb4b5..2354890 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -50,7 +50,6 @@ */ #include "planner.h" -#include "stepper.h" #include @@ -126,8 +125,6 @@ void mp_commit_write_buffer(const uint8_t move_type) { mb.q->move_state = MOVE_NEW; mb.q->buffer_state = MP_BUFFER_QUEUED; mb.q = mb.q->nx; // advance the queued buffer pointer - - st_request_exec_move(); // requests an exec if the runtime is not busy } diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index 1c98492..14cb1b5 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -28,7 +28,6 @@ \******************************************************************************/ #include "planner.h" -#include "stepper.h" #include "util.h" #include @@ -234,7 +233,6 @@ stat_t mp_end_hold() { } cm.motion_state = MOTION_RUN; - st_request_exec_move(); // restart the steppers } return STAT_OK; diff --git a/src/stepper.c b/src/stepper.c index 16e7ca8..c7f4935 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -27,9 +27,6 @@ \******************************************************************************/ -// This module provides the low-level stepper drivers and some related -// functions. See stepper.h for a detailed explanation of this module. - #include "stepper.h" #include "motor.h" @@ -44,29 +41,26 @@ typedef struct { // Runtime bool busy; - moveType_t run_move; - uint16_t run_dwell; + uint16_t dwell; // Move prep - bool move_ready; // preped move ready for loader - moveType_t prep_move; + bool move_ready; // prepped move ready for loader + moveType_t move_type; + uint16_t seg_period; uint32_t prep_dwell; struct mpBuffer *bf; // used for command moves - uint16_t seg_period; } stepper_t; -static stepper_t st; +static stepper_t st = {}; void stepper_init() { - /// clear all values, pointers and status - memset(&st, 0, sizeof(st)); - // 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 + TIMER_STEP.PER = STEP_TIMER_POLL; // timer idle rate + TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // start step timer } @@ -74,132 +68,82 @@ void stepper_init() { uint8_t st_runtime_isbusy() {return st.busy;} -/// "Software" interrupt to request move execution -void st_request_exec_move() { - if (!st.move_ready) { - ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // trigger LO interrupt - ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm; - } -} +/// Step timer interrupt routine +/// Dequeue move and load into stepper struct +ISR(STEP_TIMER_ISR) { + // Dwell + if (st.dwell && --st.dwell) return; + // End last move + if (st.busy) { + for (int motor = 0; motor < MOTORS; motor++) + motor_end_move(motor); -/* Dequeue move and load into stepper struct - * - * This routine can only be called from an ISR at the same or - * higher level as the step timer ISR. A software interrupt has been - * provided to allow a non-ISR to request a load (see st_request_load_move()) - * - * In ALINE code: - * - All axes must set steps and compensate for out-of-range pulse phasing. - * - If axis has 0 steps the direction setting can be omitted - * - If axis has 0 steps the motor must not be enabled to support power - * mode = 1 - */ -static void _load_move() { - if (st_runtime_isbusy()) return; + st.busy = false; + } // If there are no more moves - if (!st.move_ready) return; + if (!st.move_ready) { + TIMER_STEP.PER = STEP_TIMER_POLL; + + sei(); // Enable interupts + mp_exec_move(); + return; + } + // Power up motors if needed for (int motor = 0; motor < MOTORS; motor++) motor_begin_move(motor); // Wait until motors have energized - if (motor_energizing()) return; - - st.run_move = st.prep_move; + if (motor_energizing()) { + TIMER_STEP.PER = STEP_TIMER_POLL; + return; + } - switch (st.run_move) { - case MOVE_TYPE_DWELL: - st.run_dwell = st.prep_dwell; - // Fall through + // Start dwell + st.dwell = st.prep_dwell; - case MOVE_TYPE_ALINE: + // Start move + if (st.seg_period) { for (int motor = 0; motor < MOTORS; motor++) - if (st.run_move == MOVE_TYPE_ALINE) - motor_load_move(motor); + motor_load_move(motor); - st.busy = true; TIMER_STEP.PER = st.seg_period; - TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // enable step timer, if not enabled - break; - - case MOVE_TYPE_COMMAND: // handle synchronous commands - mp_runtime_command(st.bf); - // Fall through - - default: - TIMER_STEP.CTRLA = STEP_TIMER_DISABLE; - break; + st.busy = true; } - // we are done with the prep buffer - flip the flag back - st.prep_move = MOVE_TYPE_NULL; - st.move_ready = false; - st_request_exec_move(); // exec and prep next move -} - - -/// Step timer interrupt routine -ISR(STEP_TIMER_ISR) { - if (st.run_move == MOVE_TYPE_DWELL && --st.run_dwell) return; - - for (int motor = 0; motor < MOTORS; motor++) - motor_end_move(motor); - - st.busy = false; - _load_move(); -} - - -/// Interrupt handler for running the loader. -/// ADC channel 1 triggered by st_request_load_move() as a "software" interrupt. -ISR(ADCB_CH1_vect) { - _load_move(); -} - + // Execute command + if (st.move_type == MOVE_TYPE_COMMAND) mp_runtime_command(st.bf); -/// Fires a "software" interrupt to request a move load. -void st_request_load_move() { - if (st_runtime_isbusy()) return; // don't request a load if runtime is busy + // 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 - if (st.move_ready) { - ADCB_CH1_INTCTRL = ADC_CH_INTLVL_HI_gc; // trigger HI interrupt - ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH1START_bm; - } -} - - -/// Interrupt handler for calling move exec function. -/// ADC channel 0 triggered by st_request_exec_move() as a "software" interrupt. -ISR(ADCB_CH0_vect) { - if (!st.move_ready && mp_exec_move() != STAT_NOOP) { - st.move_ready = true; // flip it back - TODO is this really needed? - st_request_load_move(); - } + // Exec and prep next move + sei(); // Enable interupts + mp_exec_move(); } -/* Prepare the next move for the loader +/* Prepare the next move * - * This function does the math on the next pulse segment and gets it ready for - * 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. + * This function precomputes the next pulse segment (move) so it can + * be executed quickly in the ISR. It works in steps, rather than + * length units. All args are provided as floats which converted here + * to integer values. * * Args: - * - travel_steps[] are signed relative motion in steps for each motor. Steps - * are floats that typically have fractional values (fractional steps). The - * sign indicates direction. Motors that are not in the move should be 0 - * steps on input. + * @param travel_steps signed relative motion in steps for each motor. + * Steps are fractional. Their sign indicates direction. Motors not in the + * move have 0 steps. * - * - error[] is a vector of measured errors to the step count. Used for - * correction. + * @param error is a vector of measured step errors used for correction. * - * - seg_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. + * @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) { // Trap conditions that would prevent queueing the line @@ -210,8 +154,8 @@ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { if (seg_time < EPSILON) return STAT_MINIMUM_TIME_MOVE; // Setup segment parameters - st.prep_move = MOVE_TYPE_ALINE; - st.seg_period = seg_time * 60 * F_CPU / STEP_TIMER_DIV; // Must fit 16-bit + 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; // Prepare motor moves @@ -227,7 +171,7 @@ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { /// Keeps the loader happy. Otherwise performs no action void st_prep_null() { if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); - st.prep_move = MOVE_TYPE_NULL; + st.move_type = MOVE_TYPE_NULL; st.move_ready = false; // signal prep buffer empty } @@ -235,7 +179,7 @@ void st_prep_null() { /// Stage command to execution void st_prep_command(void *bf) { if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); - st.prep_move = MOVE_TYPE_COMMAND; + st.move_type = MOVE_TYPE_COMMAND; st.bf = (mpBuf_t *)bf; st.move_ready = true; // signal prep buffer ready } @@ -244,8 +188,8 @@ void st_prep_command(void *bf) { /// Add a dwell to the move buffer void st_prep_dwell(float seconds) { if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); - st.prep_move = MOVE_TYPE_DWELL; - st.seg_period = F_CPU / STEP_TIMER_DIV / 1000; // 1 ms + 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.move_ready = true; // signal prep buffer ready } diff --git a/src/stepper.h b/src/stepper.h index 4d1b3b8..cca16f7 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -30,190 +30,6 @@ #pragma once -/* # 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. - * - * - 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 stepper.c for step output. - * - * - 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. 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. - * - * LOAD/STEP (~5000uSec) [L1][segment1][L2][segment2][L3] - * PREP (100 uSec) [P1] [P2] [P3] - * EXEC (400 uSec) [EXEC1] [EXEC2] [EXEC3] - * PLAN (<4ms) [planmoveA][plan move B][plan move C][plan move D] etc. - * - * The move begins with the planner PLANning move A - * [planmoveA]. When this is done the computations for the first - * segment of move A's S curve are performed by the planner runtime, - * EXEC1. The runtime computes the number of segments and the - * segment-by-segment accelerations and decelerations for the - * move. Each call to EXEC generates the values for the next segment - * to be run. Once the move is running EXEC is executed as a - * callback from the step loader. - * - * When the runtime calculations are done EXEC calls the segment - * PREParation function [P1]. PREP turns the EXEC results into - * values needed for the loader and does some encoder work. The - * combined exec and prep take about 400 uSec. - * - * PREP takes care of heavy numerics and other cycle-intesive - * operations so the step loader L1 can run as fast as possible. The - * time budget for LOAD is about 10 uSec. In the diagram, when P1 is - * done segment 1 is loaded into the stepper runtime [L1] - * - * 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. - * - * Now the move is pulsing out segment 1 (at HI interrupt - * level). Once the L1 loader is finished it invokes the exec - * function for the next segment (at LO interrupt level). [EXEC2] - * and [P2] compute and prepare the segment 2 for the loader so it - * can be loaded as soon as segment 1 is complete [L2]. When move A - * is done EXEC pulls the next move (moveB) from the planner queue, - * The process repeats until there are no more segments or moves. - * - * While all this is happening subsequent moves (B, C, and D) are - * being planned in background. As long as a move takes less than - * the segment times (5ms x N) the timing budget is satisfied, - * - * # Line planning and execution (in more detail) - * - * Move planning, execution and pulse generation takes place at 3 - * levels: - * - * Move planning occurs in the main-loop. The canonical machine calls - * the planner to generate lines, arcs, dwells, synchronous - * stop/starts, and any other cvommand that needs to be syncronized - * wsith motion. The planner module generates blocks (bf's) that hold - * parameters for lines and the other move types. The blocks are - * backplanned to join lines and to take dwells and stops into - * account. ("plan" stage). - * - * Arc movement is planned above the line planner. The arc planner - * generates short lines that are passed to the line planner. - * - * Once lines are planned the must be broken up into "segments" of - * about 5 milliseconds to be run. These segments are how S curves are - * generated. This is the job of the move runtime (aka. exec or mr). - * - * Move execution and load prep takes place at the LOW interrupt - * 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 - * stepper.c and converting the segment into parameters that can be directly - * loaded into the steppers ("exec" and "prep" stages). - * - * # 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 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 - * next segment. At the of the load the stepper interrupt routine requests - * an "exec" of the next move in order to prepare for the next load - * operation. It does this by calling the exec using a software interrupt - * (actually a timer, since that's all we've got). - * - * - As a result of the above, the EXEC handler fires at the LO interrupt - * level. It computes the next accel/decel or cruise (body) segment for the - * current move (i.e. the move in the planner's runtime buffer) by calling - * back to the exec routine in planner.c. If there are no more segments to - * run for the move the exec first gets the next buffer in the planning - * queue and begins execution. - * - * 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 timer values and - * gets the segment into the prep buffer - and ready for the next LOAD - * operation. - * - * - The main loop runs in background to receive gcode blocks, parse them, - * and send them to the planner in order to keep the planner queue full so - * that when the planner's runtime buffer completes the next move (a gcode - * block or perhaps an arc segment) is ready to run. - * - * 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 - * - * 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) - * and back-plans any pre-existing buffers. - * - * 2 When a new buffer is added _mp_queue_write_buffer() tries to invoke - * execution of the move by calling stepper.st_request_exec_move(). - * - * 3a If the steppers are running this request is ignored. - * 3b If the steppers are not running this will set a timer to cause an - * EXEC "software interrupt" that will ultimately call st_exec_move(). - * - * 4 At this point a call to _exec_move() is made, either by the - * software interrupt from 3b, or once the steppers finish running - * the current segment and have loaded the next segment. In either - * case the call is initated via the EXEC software interrupt which - * causes _exec_move() to run at the MEDium interupt level. - * - * 5 _exec_move() calls back to planner.mp_exec_move() which generates - * the next segment using the mr singleton. - * - * 6 When this operation is complete mp_exec_move() calls the appropriate - * 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 values and stages these into - * the prep structure (sp) - ready for loading into the stepper runtime - * struct - * - * 8 stepper.st_prep_line() returns back to planner.mp_exec_move(), which - * frees the planning buffer (bf) back to the planner buffer pool if the - * move is complete. This is done by calling - * _mp_request_finalize_run_buffer() - * - * 9 At this point the MED interrupt is complete, but the planning buffer has - * not actually been returned to the pool yet. The buffer will be returned - * by the main-loop prior to testing for an available write buffer in order - * to receive the next Gcode block. This handoff prevents possible data - * conflicts between the interrupt and main loop. - * - * 10 The final step in the sequence is _load_move() requesting the next - * segment to be executed and prepared by calling st_request_exec() - * - control goes back to step 4. - * - * Note: For this to work you have to be really careful about what structures - * are modified at what level, and use volatiles where necessary. - */ - #include "config.h" #include "status.h" #include "canonical_machine.h" @@ -222,27 +38,8 @@ #include -/* Stepper control structures - * - * There are 5 main structures involved in stepper operations; - * - * data structure: found in: runs primarily at: - * mpBuffer planning buffers (bf) planner.c main loop - * mrRuntimeSingleton (mr) planner.c MED ISR - * stConfig (st_cfg) stepper.c write=bkgd, read=ISRs - * stPrepSingleton (st_pre) stepper.c MED ISR - * stRunSingleton (st_run) stepper.c HI ISR - * - * Care has been taken to isolate actions on these structures to the execution - * level in which they run and to use the minimum number of volatiles in these - * structures. This allows the compiler to optimize the stepper inner-loops - * better. - */ - void stepper_init(); uint8_t st_runtime_isbusy(); -void st_request_exec_move(); -void st_request_load_move(); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); void st_prep_null(); -- 2.27.0