From: Joseph Coffland Date: Tue, 30 Aug 2016 07:09:41 +0000 (-0700) Subject: Docs and code organization X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=ef98b6c672b3f650fcd01dd95b81eb51ba928405;p=bbctrl-firmware Docs and code organization --- diff --git a/MoveLifecycle.md b/MoveLifecycle.md new file mode 100644 index 0000000..58015ec --- /dev/null +++ b/MoveLifecycle.md @@ -0,0 +1,101 @@ +# Notes on the lifecycle of a movement + +## Parsing +A move first starts off as a line of GCode which is parsed by +``gc_gcode_parser()`` which in turn calls ``_normalize_gcode_block()`` +and ``_parse_gcode_block()``. ``_parse_gcode_block()`` sets flags in +``mach.gf`` indicating which values have changed and the changed values in +``mach.gn``. ``_parse_gcode_block()`` then calls ``_execute_gcode_block()`` +which calls ``mach_*()`` functions which modify the state of the GCode machine. + +## Queuing +Some functions such as ``mach_straight_traverse()``, ``mach_straight_feed()`` +and ``mach_arc_feed()`` result in line moves being entered into the planner +queue. Others enter dwells or commands or into the queue. Planner buffer +entries encode everything needed to execute a move with out referring back to +the machine model. + +Line moves are entered into the queue by calls to ``mp_aline()``. Arcs are +converted to short straight line moves and are feed in as buffer room becomes +available, until complete, via calls to ``mach_arc_callback()`` which results in +multiple calls to ``mp_aline()``. + +``mp_aline()`` plans straight line movements by first calling +``mach_calc_move_time()`` which uses the current GCode state to estimate the +total time required to complete the move at the current feed rate and feed rate +mode. If the move time is long enough, a buffer is allocated. Its jerk, max +cruise velocity, max entry velocity, max delta velocity, max exit velocity and +breaking velocity are all computed. The move velocities are planned by a +call to ``mp_plan_block_list()``. Initially ``bf_func`` is set to +``mp_exec_aline()`` and the buffer is committed to the queue by calling +``mp_commit_write_buffer()``. + +## Planning +### Backward planning pass +``mp_plan_block_list()`` plans movements by first moving backwards through the +planning buffer until either the last entry is reached or a buffer marked not +``replannable`` is encountered. The ``breaking_velocity`` is propagated back +during the backwards pass. Next, begins the forward planning pass. + +### Forward planning pass +During the forward pass the entry velocity, cruise velocity and exit velocity +are computed and ``mp_calculate_trapezoid()`` is called to (re)compute the +velocity trapezoids of each buffer being considered. If a buffer's plan is +deemed optimal then it is marked not ``replannable`` to avoid replanning later. + +### Trapezoid planning +The call to ``mp_calculate_trapezoid()`` computes head, body and tail lengths +for a single planner buffer. Entry, cruse and exit velocities may be modified +to make the trapezoid fit with in the move length. Planning may result in a +degraded trapezoid. I.e. one with out all three sides. + +## Execution +The stepper motor driver interrupt routine calls ``mp_exec_move()`` to prepare +the next move for execution. ``mp_exec_move()`` access the next buffer in the +planner queue and calls the function pointed to by ``bf_func`` which is +initially set to ``mp_exec_aline()`` during planning. Each call to +``mp_exec_move()`` must prepare one and only one move fragment for the stepper +driver. The planner buffer is executed repeatedly as long as ``bf_func`` +returns ``STAT_EAGAIN``. + +### Move initialization +On the first call to ``mp_exec_aline()`` a call is made to +``_exec_aline_init()``. This function may stop processing the move if a +feedhold is in effect. It may also skip a move if it has zero length. +Otherwise, it initializes the move runtime state (``mr``) by copying the move +state from the planner buffer and initializing variables. In addition, it +computes waypoints at the ends of each trapezoid section. Waypoints are used +later for position correction which adjust position for rounding errors. + +### Move execution +After move initialization ``mp_exec_aline()`` calls ``_exec_aline_head()``, +``_exec_aline_body()`` and ``exec_aline_tail()`` on successive callbacks. Each +of these functions are called repeatedly until the section finishes. They +advance through states ``SECTION_NEW`` which initializes the section, +``SECTION_1st_HALF`` which executes the first half of the S-curve and +``SECTION_2nd_HALF``. If any sections have zero length they are skipped and +execution is passed immediately to the next section. During each section +forward differencing is used to map the trapezoid computed during the planning +stage to a fifth-degree Bezier polynomial S-curve. The curve is used to find +the next target position. + +``_exec_aline_segment()`` is called for each non-zero section to convert the +computed target position to target steps by calling ``mp_kinematics()``. The +move fragment is then passed to the stepper driver by a call to +``st_prep_line()``. When a segment is complete ``_exec_aline_segment()`` +returns ``STAT_OK`` indicating the next segment should be loaded. When all +non-zero segments have been executed, the move is complete. + +## Stepper driver +Calls to ``st_prep_line()`` prepare short (~5ms) moves for execution by the +stepper driver. The move time in clock ticks is computed from travel in steps +and the move duration. Then ``motor_prep_move()`` is called for each motor. +``motor_prep_move()`` may perform step correction, enables the motors if needed. +It then computes the optimal step clock divisor, clock ticks and sets the move +direction, taking the motor's configuration in to account. + +The stepper timer ISR, after ending the previous move, calls +``motor_load_move()`` on each motor. This sets up and starts the motor clocks, +sets the motor direction lines and accumulates and resets the step encoders. +After (re)starting the motor clocks the ISR signals a lower level interrupt to +call ``mp_exec_move()`` and load the next move fragment. diff --git a/README.md b/README.md index 5080db1..24ba38a 100644 --- a/README.md +++ b/README.md @@ -3,8 +3,8 @@ high-performance on small to mid-sized machines. It was originally derived from the [TinyG firmware](https://github.com/synthetos/TinyG). # Features -* 4 axis motion -* jerk controlled motion for acceleration planning (3rd order motion planning) + * 4 axis motion + * jerk controlled motion for acceleration planning (3rd order motion planning) # Build Instructions To build in Linux run: @@ -17,6 +17,6 @@ Other make commands are: * **program** - program using AVR dude and an avrispmkII * **erase** - Erase chip * **fuses** - Write AVR fuses bytes - * **read_fuses** - Read and pring AVR fuse bytes + * **read_fuses** - Read and print AVR fuse bytes * **clean** - Remove build files * **tidy** - Remove backup files diff --git a/src/plan/exec.c b/src/plan/exec.c index a1e5598..45900ec 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -68,7 +68,7 @@ static stat_t _exec_aline_segment() { // Set target position for the segment // If the segment ends on a section waypoint, synchronize to the // head, body or tail end. Otherwise, if not at a section waypoint - // compute target from segment time and velocity Don't do waypoint + // compute target from segment time and velocity. Don't do waypoint // correction if you are going into a hold. if (--mr.segment_count == 0 && mr.section_state == SECTION_2nd_HALF && mp_get_state() == STATE_RUNNING) @@ -101,7 +101,7 @@ static stat_t _exec_aline_segment() { // now determine the target steps mp_kinematics(mr.ms.target, mr.target_steps); - // and compute the distances to be traveled + // and compute the distances, in steps, to be traveled for (int i = 0; i < MOTORS; i++) travel_steps[i] = mr.target_steps[i] - mr.position_steps[i]; @@ -131,8 +131,8 @@ static stat_t _exec_aline_segment() { * * The Bezier curve takes the form: * - * V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + - * P_4 * B_4(t) + P_5 * B_5(t) + * V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + + * P_4 * B_4(t) + P_5 * B_5(t) * * Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are * the control points, and B_0(t) through B_5(t) are the Bernstein @@ -364,8 +364,8 @@ static stat_t _exec_aline_tail() { if (mr.section_state == SECTION_1st_HALF) { if (_exec_aline_segment() == STAT_OK) { // For forward differencing we should have one segment in - // SECTION_1st_HALF. However, if it returns from that as STAT_OK, then - // there was only one segment in this section. Show that we did complete + // SECTION_1st_HALF. However, if it returns from that as STAT_OK, then + // there was only one segment in this section. Show that we did complete // section 2 ... effectively. mr.section_state = SECTION_2nd_HALF; return STAT_OK; @@ -424,9 +424,6 @@ static stat_t _exec_aline_tail() { /// Helper for cruise section -/// The body is broken into little segments even though it is a -/// straight line so that feedholds can happen in the middle of a line -/// with a minimum of latency static stat_t _exec_aline_body() { if (mr.section_state == SECTION_NEW) { if (fp_ZERO(mr.body_length)) { @@ -446,7 +443,7 @@ static stat_t _exec_aline_body() { mr.section = SECTION_BODY; - // uses PERIOD_2 so last segment detection works + // use SECTION_2nd_HALF so last segment detection works mr.section_state = SECTION_2nd_HALF; } @@ -533,12 +530,14 @@ static stat_t _exec_aline_head() { return STAT_EAGAIN; } + #else // __JERK_EXEC /// Helper for acceleration section static stat_t _exec_aline_head() { if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) if (fp_ZERO(mr.head_length)) { mr.section = SECTION_BODY; + return _exec_aline_body(); // skip ahead to the body generator } @@ -560,7 +559,7 @@ static stat_t _exec_aline_head() { } // For forward differencing we should have one segment in - // SECTION_1st_HALF However, if it returns from that as STAT_OK, + // SECTION_1st_HALF. However, if it returns from that as STAT_OK, // then there was only one segment in this section. // First half (concave part of accel curve) if (mr.section_state == SECTION_1st_HALF) { @@ -627,6 +626,62 @@ static stat_t _exec_aline_head() { #endif // !__JERK_EXEC +/// Initializes a new planner buffer +static stat_t _exec_aline_init(mpBuf_t *bf) { + // Stop here if holding + if (mp_get_hold_state() == FEEDHOLD_HOLD) return STAT_NOOP; + + // copy in the gcode model state + memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t)); + bf->replannable = false; + report_request(); // Executing line number has changed + + // Remove zero length lines. Short lines have already been removed. + if (fp_ZERO(bf->length)) { + mr.move_state = MOVE_OFF; // reset mr buffer + mr.section_state = SECTION_OFF; + bf->nx->replannable = false; // prevent overplanning (Note 2) + mp_free_run_buffer(); // free buffer + + return STAT_NOOP; + } + + // Initialize the move runtime + bf->move_state = MOVE_RUN; + mr.move_state = MOVE_RUN; + mr.section = SECTION_HEAD; + mr.section_state = SECTION_NEW; + mr.jerk = bf->jerk; +#ifdef __JERK_EXEC + mr.jerk_div2 = bf->jerk / 2; // only needed by __JERK_EXEC +#endif + mr.head_length = bf->head_length; + mr.body_length = bf->body_length; + mr.tail_length = bf->tail_length; + mr.entry_velocity = bf->entry_velocity; + mr.cruise_velocity = bf->cruise_velocity; + mr.exit_velocity = bf->exit_velocity; + + copy_vector(mr.unit, bf->unit); + copy_vector(mr.final_target, bf->ms.target); // save move final target + + // Generate waypoints for position correction at section ends + for (int axis = 0; axis < AXES; axis++) { + mr.waypoint[SECTION_HEAD][axis] = + mr.position[axis] + mr.unit[axis] * mr.head_length; + + mr.waypoint[SECTION_BODY][axis] = + mr.position[axis] + mr.unit[axis] * (mr.head_length + mr.body_length); + + mr.waypoint[SECTION_TAIL][axis] = + mr.position[axis] + mr.unit[axis] * + (mr.head_length + mr.body_length + mr.tail_length); + } + + return STAT_OK; +} + + /* Aline execution routines * * Everything here fires from interrupts and must be interrupt safe @@ -635,14 +690,13 @@ static stat_t _exec_aline_head() { * * STAT_OK move is done * STAT_EAGAIN move is not finished - has more segments to run - * STAT_NOOP cause no operation from the steppers - do not load the - * move - * STAT_xxxxx fatal error. Ends the move and frees the bf buffer + * STAT_NOOP cause no stepper operation - do not load the move + * STAT_xxxxx fatal error. Ends the move and frees the bf buffer * - * This routine is called from the (LO) interrupt level. The interrupt - * sequencing relies on the behaviors of the routines being exactly correct. + * This routine is called from the (LO) interrupt level. The interrupt + * sequencing relies on the correct behavior of these routines. * Each call to _exec_aline() must execute and prep *one and only one* - * segment. If the segment is not the last segment in the bf buffer the + * segment. If the segment is not the last segment in the bf buffer the * _aline() returns STAT_EAGAIN. If it's the last segment it returns * STAT_OK. If it encounters a fatal error that would terminate the move it * returns a valid error code. @@ -655,7 +709,7 @@ static stat_t _exec_aline_head() { * * [2] Solves a potential race condition where the current move ends but * the new move has not started because the previous move is still - * being run by the steppers. Planning can overwrite the new move. + * being run by the steppers. Planning can overwrite the new move. * * Operation: * @@ -681,18 +735,16 @@ static stat_t _exec_aline_head() { * Period 4 V = Vh + As * T + Jm * (T^2) / 2 * * These routines play some games with the acceleration and move timing - * to make sure this actually all works out. move_time is the actual time of - * the move, accel_time is the time valaue needed to compute the velocity - - * which takes the initial velocity into account (move_time does not need - * to). + * to make sure this actually work out. move_time is the actual time of + * the move, accel_time is the time value needed to compute the velocity - + * taking the initial velocity into account (move_time does not need to). * * State transitions - hierarchical state machine: * * bf->move_state transitions: * * from _NEW to _RUN on first call (sub_state set to _OFF) - * from _RUN to _OFF on final call - * or just remains _OFF + * from _RUN to _OFF on final call or just remain _OFF * * mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY, * _TAIL. Within each section state may be: @@ -704,71 +756,22 @@ static stat_t _exec_aline_head() { stat_t mp_exec_aline(mpBuf_t *bf) { if (bf->move_state == MOVE_OFF) return STAT_NOOP; - // start a new move by setting up local context (singleton) - if (mr.move_state == MOVE_OFF) { - // stop here if holding - if (mp_get_hold_state() == FEEDHOLD_HOLD) return STAT_NOOP; - - // initialization to process the new incoming bf buffer (Gcode block) - // copy in the gcode model state - memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t)); - bf->replannable = false; - report_request(); // Executing line number has changed - - // short lines have already been removed, look for an actual zero - if (fp_ZERO(bf->length)) { - mr.move_state = MOVE_OFF; // reset mr buffer - mr.section_state = SECTION_OFF; - // prevent overplanning (Note 2) - bf->nx->replannable = false; - // free buffer & end cycle if planner is empty - mp_free_run_buffer(); - - return STAT_NOOP; - } - - bf->move_state = MOVE_RUN; - mr.move_state = MOVE_RUN; - mr.section = SECTION_HEAD; - mr.section_state = SECTION_NEW; - mr.jerk = bf->jerk; -#ifdef __JERK_EXEC - mr.jerk_div2 = bf->jerk / 2; // only needed by __JERK_EXEC -#endif - mr.head_length = bf->head_length; - mr.body_length = bf->body_length; - mr.tail_length = bf->tail_length; - - mr.entry_velocity = bf->entry_velocity; - mr.cruise_velocity = bf->cruise_velocity; - mr.exit_velocity = bf->exit_velocity; - - copy_vector(mr.unit, bf->unit); - copy_vector(mr.final_target, bf->ms.target); // save move final target - - // generate the waypoints for position correction at section ends - for (int axis = 0; axis < AXES; axis++) { - mr.waypoint[SECTION_HEAD][axis] = - mr.position[axis] + mr.unit[axis] * mr.head_length; - - mr.waypoint[SECTION_BODY][axis] = - mr.position[axis] + mr.unit[axis] * - (mr.head_length + mr.body_length); + stat_t status = STAT_OK; - mr.waypoint[SECTION_TAIL][axis] = - mr.position[axis] + mr.unit[axis] * - (mr.head_length + mr.body_length + mr.tail_length); - } + // Start a new move + if (mr.move_state == MOVE_OFF) { + status = _exec_aline_init(bf); + if (status != STAT_OK) return status; } // Main segment processing dispatch. From this point on the contents of the // bf buffer do not affect execution. - stat_t status = STAT_OK; - - if (mr.section == SECTION_HEAD) status = _exec_aline_head(); - else if (mr.section == SECTION_BODY) status = _exec_aline_body(); - else if (mr.section == SECTION_TAIL) status = _exec_aline_tail(); - else return CM_ALARM(STAT_INTERNAL_ERROR); // never supposed to get here + switch (mr.section) { + case SECTION_HEAD: status = _exec_aline_head(); break; + case SECTION_BODY: status = _exec_aline_body(); break; + case SECTION_TAIL: status = _exec_aline_tail(); break; + default: return CM_ALARM(STAT_INTERNAL_ERROR); // never supposed to get here + } mp_state_hold_callback(status == STAT_OK); diff --git a/src/plan/line.c b/src/plan/line.c index 9bdbaed..1aba97b 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -235,7 +235,8 @@ stat_t mp_aline(MoveState_t *ms) { if (move_time < MIN_BLOCK_TIME) return STAT_MINIMUM_TIME_MOVE; } - // Get a cleared buffer and setup move variables + // Get a *cleared* buffer and setup move variables + // Note, mp_free_run_buffer() initializes all buffer variables to zero mpBuf_t *bf = mp_get_write_buffer(); // current move pointer if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); // never fails @@ -354,7 +355,6 @@ stat_t mp_aline(MoveState_t *ms) { bf->recip_jerk = mm.recip_jerk; bf->cbrt_jerk = mm.cbrt_jerk; - // finish up the current block variables // exact stop cases already zeroed float exact_stop = 0; if (mach_get_path_control() != PATH_EXACT_STOP) { @@ -362,6 +362,7 @@ stat_t mp_aline(MoveState_t *ms) { exact_stop = 8675309; // an arbitrarily large floating point number } + // finish up the current block variables float junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); bf->cruise_vmax = bf->length / bf->ms.move_time; // target velocity requested bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); diff --git a/src/plan/planner.c b/src/plan/planner.c index e0d67d0..b6ffa51 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -213,11 +213,11 @@ void mp_kinematics(const float travel[], float steps[]) { /*** This rather brute-force and long-ish function sets section lengths - * and velocities based on the line length and velocities - * requested. It modifies the incoming bf buffer and returns accurate - * head, body and tail lengths, and accurate or reasonably approximate - * velocities. We care about accuracy on lengths, less so for velocity, - * as long as velocity errs on the side of too slow. + * and velocities based on the line length and velocities requested. It + * modifies the incoming bf buffer and returns accurate head, body and + * tail lengths, and accurate or reasonably approximate velocities. We + * care about accuracy on lengths, less so for velocity, as long as velocity + * errs on the side of too slow. * * Note: We need the velocities to be set even for zero-length * sections (Note: sections, not moves) so we can compute entry and @@ -230,8 +230,7 @@ void mp_kinematics(const float travel[], float steps[]) { * bf->cruise_velocity - requested Vt, is often changed * bf->exit_velocity - requested Vx, may change for degenerate cases * bf->cruise_vmax - used in some comparisons - * bf->delta_vmax - used to degrade velocity of pathologically - * short blocks + * bf->delta_vmax - used to degrade velocity of short blocks * * Variables that may be set/updated are: * @@ -250,13 +249,13 @@ void mp_kinematics(const float travel[], float steps[]) { * Classes of moves: * * Requested-Fit - The move has sufficient length to achieve the - * target velocity (cruise velocity). I.e: it will accommodate + * target velocity (cruise velocity). I.e it will accommodate * the acceleration / deceleration profile in the given length. * * Rate-Limited-Fit - The move does not have sufficient length to - * achieve target velocity. In this case the cruise velocity + * achieve target velocity. In this case the cruise velocity * will be set lower than the requested velocity (incoming - * bf->cruise_velocity). The entry and exit velocities are + * bf->cruise_velocity). The entry and exit velocities are * satisfied. * * Degraded-Fit - The move does not have sufficient length to @@ -264,10 +263,10 @@ void mp_kinematics(const float travel[], float steps[]) { * the available length. These velocities are not negotiable, * so a degraded solution is found. * - * In worst cases the move cannot be executed as the required - * execution time is less than the minimum segment time. The + * In worst cases, the move cannot be executed as the required + * execution time is less than the minimum segment time. The * first degradation is to reduce the move to a body-only - * segment with an average velocity. If that still doesn't fit + * segment with an average velocity. If that still doesn't fit * then the move velocity is reduced so it fits into a minimum * segment. This will reduce the velocities in that region of * the planner buffer as the moves are replanned to that @@ -307,7 +306,7 @@ void mp_kinematics(const float travel[], float steps[]) { * F force fit: This block is slowed down until it can * be executed * - * Note: The order of the cases/tests in the code is important. Start with + * Note: The order of the cases/tests in the code is important. Start with * the shortest cases first and work up. Not only does this simplify the order * of the tests, but it reduces execution time when you need it most - when * tons of pathologically short Gcode blocks are being thrown at you. @@ -360,9 +359,8 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { } // B case: Velocities all match (or close enough) - // This occurs frequently in normal gcode files with lots of short lines - // This case is not really necessary, but saves lots of processing time - + // This occurs frequently in normal gcode files with lots of short lines. + // This case is not really necessary, but saves lots of processing time. if (((bf->cruise_velocity - bf->entry_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) && ((bf->cruise_velocity - bf->exit_velocity) < @@ -378,7 +376,6 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { // H" and T" degraded-fit cases // H' and T' requested-fit cases where the body residual is less than // MIN_BODY_LENGTH - bf->body_length = 0; float minimum_length = mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf);