From: Joseph Coffland Date: Fri, 11 Mar 2016 07:51:08 +0000 (-0800) Subject: Lots of clean up, work on improved stepper algorithm, freeded up several hardware... X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=e3d05b9777efe798daf8f7ea0b5d73b31ddf49b0;p=bbctrl-firmware Lots of clean up, work on improved stepper algorithm, freeded up several hardware timers --- diff --git a/src/canonical_machine.c b/src/canonical_machine.c index 96faa28..c62d371 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -5,88 +5,103 @@ * Copyright (c) 2010 - 2015 Alden S Hart, Jr. * Copyright (c) 2014 - 2015 Robert Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* - * This code is a loose implementation of Kramer, Proctor and Messina's canonical - * machining functions as described in the NIST RS274/NGC v3 * - * The canonical machine is the layer between the Gcode parser and the motion control - * code for a specific robot. It keeps state and executes commands - passing the - * stateless commands to the motion planning layer. - */ - -/* --- System state contexts - Gcode models --- - * - * Useful reference for doing C callbacks http://www.newty.de/fpt/fpt.html - * - * There are 3 temporal contexts for system state: - * - The gcode model in the canonical machine (the MODEL context, held in gm) - * - The gcode model used by the planner (PLANNER context, held in bf's and mm) - * - The gcode model used during motion for reporting (RUNTIME context, held in mr) - * - * It's a bit more complicated than this. The 'gm' struct contains the core Gcode model - * context. This originates in the canonical machine and is copied to each planner buffer - * (bf buffer) during motion planning. Finally, the gm context is passed to the runtime - * (mr) for the RUNTIME context. So at last count the Gcode model exists in as many as - * 30 copies in the system. (1+28+1) - * - * Depending on the need, any one of these contexts may be called for reporting or by - * a function. Most typically, all new commends from the gcode parser work form the MODEL - * context, and status reports pull from the RUNTIME while in motion, and from MODEL when - * at rest. A convenience is provided in the ACTIVE_MODEL pointer to point to the right - * context. - */ - -/* --- Synchronizing command execution --- - * - * Some gcode commands only set the MODEL state for interpretation of the current Gcode - * block. For example, cm_set_feed_rate(). This sets the MODEL so the move time is - * properly calculated for the current (and subsequent) blocks, so it's effected - * immediately. - * - * "Synchronous commands" are commands that affect the runtime need to be synchronized - * with movement. Examples include G4 dwells, program stops and ends, and most M commands. - * These are queued into the planner queue and execute from the queue. Synchronous commands - * work like this: - * - * - Call the cm_xxx_xxx() function which will do any input validation and return an - * error if it detects one. - * - * - The cm_ function calls mp_queue_command(). Arguments are a callback to the _exec_...() - * function, which is the runtime execution routine, and any arguments that are needed - * by the runtime. See typedef for *exec in planner.h for details - * - * - mp_queue_command() stores the callback and the args in a planner buffer. - * - * - When planner execution reaches the buffer it executes the callback w/ the args. - * Take careful note that the callback executes under an interrupt, so beware of - * variables that may need to be volatile. - * - * Note: - * - The synchronous command execution mechanism uses 2 vectors in the bf buffer to store - * and return values for the callback. It's obvious, but impractical to pass the entire - * bf buffer to the callback as some of these commands are actually executed locally - * and have no buffer. + * This code is a loose implementation of Kramer, Proctor and Messina's + * canonical machining functions as described in the NIST RS274/NGC v3 + * + * The canonical machine is the layer between the Gcode parser and + * the motion control code for a specific robot. It keeps state and + * executes commands - passing the stateless commands to the motion + * planning layer. + * + * System state contexts - Gcode models + * + * Useful reference for doing C callbacks + * http://www.newty.de/fpt/fpt.html + * + * There are 3 temporal contexts for system state: - The gcode + * model in the canonical machine (the MODEL context, held in gm) - + * The gcode model used by the planner (PLANNER context, held in + * bf's and mm) - The gcode model used during motion for reporting + * (RUNTIME context, held in mr) + * + * It's a bit more complicated than this. The 'gm' struct contains + * the core Gcode model context. This originates in the canonical + * machine and is copied to each planner buffer (bf buffer) during + * motion planning. Finally, the gm context is passed to the + * runtime (mr) for the RUNTIME context. So at last count the Gcode + * model exists in as many as 30 copies in the system. (1+28+1) + * + * Depending on the need, any one of these contexts may be called + * for reporting or by a function. Most typically, all new commends + * from the gcode parser work form the MODEL context, and status + * reports pull from the RUNTIME while in motion, and from MODEL + * when at rest. A convenience is provided in the ACTIVE_MODEL + * pointer to point to the right context. + * + * Synchronizing command execution + * + * Some gcode commands only set the MODEL state for interpretation + * of the current Gcode block. For example, + * cm_set_feed_rate(). This sets the MODEL so the move time is + * properly calculated for the current (and subsequent) blocks, so + * it's effected immediately. + * + * "Synchronous commands" are commands that affect the runtime need + * to be synchronized with movement. Examples include G4 dwells, + * program stops and ends, and most M commands. These are queued + * into the planner queue and execute from the queue. Synchronous + * commands work like this: + * + * - Call the cm_xxx_xxx() function which will do any input + * validation and return an error if it detects one. + * + * - The cm_ function calls mp_queue_command(). Arguments are a + * callback to the _exec_...() function, which is the runtime + * execution routine, and any arguments that are needed by the + * runtime. See typedef for *exec in planner.h for details + * + * - mp_queue_command() stores the callback and the args in a + planner buffer. + * + * - When planner execution reaches the buffer it executes the + * callback w/ the args. Take careful note that the callback + * executes under an interrupt, so beware of variables that may + * need to be volatile. + * + * Note: - The synchronous command execution mechanism uses 2 + * vectors in the bf buffer to store and return values for the + * callback. It's obvious, but impractical to pass the entire bf + * buffer to the callback as some of these commands are actually + * executed locally and have no buffer. */ #include "canonical_machine.h" @@ -135,7 +150,6 @@ uint8_t cm_get_combined_state() { if (cm.cycle_state == CYCLE_OFF) cm.combined_state = cm.machine_state; else if (cm.cycle_state == CYCLE_PROBE) cm.combined_state = COMBINED_PROBE; else if (cm.cycle_state == CYCLE_HOMING) cm.combined_state = COMBINED_HOMING; - else if (cm.cycle_state == CYCLE_JOG) cm.combined_state = COMBINED_JOG; else { if (cm.motion_state == MOTION_RUN) cm.combined_state = COMBINED_RUN; if (cm.motion_state == MOTION_HOLD) cm.combined_state = COMBINED_HOLD; @@ -201,12 +215,16 @@ void cm_set_spindle_speed_parameter(GCodeState_t *gcode_state, float speed) { } -void cm_set_tool_number(GCodeState_t *gcode_state, uint8_t tool) {gcode_state->tool = tool;} +void cm_set_tool_number(GCodeState_t *gcode_state, uint8_t tool) { + gcode_state->tool = tool; +} -void cm_set_absolute_override(GCodeState_t *gcode_state, uint8_t absolute_override) { +void cm_set_absolute_override(GCodeState_t *gcode_state, + uint8_t absolute_override) { gcode_state->absolute_override = absolute_override; - cm_set_work_offsets(MODEL); // must reset offsets if you change absolute override + // must reset offsets if you change absolute override + cm_set_work_offsets(MODEL); } @@ -349,32 +367,17 @@ float cm_get_work_position(GCodeState_t *gcode_state, uint8_t axis) { void cm_finalize_move() { copy_vector(cm.gmx.position, cm.gm.target); // update model position - // if in ivnerse time mode reset feed rate so next block requires an explicit feed rate setting - if ((cm.gm.feed_rate_mode == INVERSE_TIME_MODE) && (cm.gm.motion_mode == MOTION_MODE_STRAIGHT_FEED)) + // if in ivnerse time mode reset feed rate so next block requires an + // explicit feed rate setting + if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE && + cm.gm.motion_mode == MOTION_MODE_STRAIGHT_FEED) cm.gm.feed_rate = 0; } /// Set endpoint position from final runtime position -void cm_update_model_position_from_runtime() {copy_vector(cm.gmx.position, mr.gm.target);} - - -/* - * Write any changed G10 values back to persistence - * - * Only runs if there is G10 data to write, there is no movement, and the serial queues are quiescent - * This could be made tighter by issuing an XOFF or ~CTS beforehand and releasing it afterwards. - */ -stat_t cm_deferred_write_callback() { - if (cm.cycle_state == CYCLE_OFF && cm.deferred_write_flag && usart_rx_empty()) { - cm.deferred_write_flag = false; - - for (uint8_t i = 1; i <= COORDS; i++) - for (uint8_t j = 0; j < AXES; j++) - ;// TODO store cm.offset[i][j]; - } - - return STAT_OK; +void cm_update_model_position_from_runtime() { + copy_vector(cm.gmx.position, mr.gm.target); } @@ -711,30 +714,28 @@ stat_t cm_set_distance_mode(uint8_t mode) { /* * cm_set_coord_offsets() - G10 L2 Pn (affects MODEL only) * - * This function applies the offset to the GM model but does not persist the offsets - * during the Gcode cycle. The persist flag is used to persist offsets once the cycle - * has ended. You can also use $g54x - $g59c config functions to change offsets. + * This function applies the offset to the GM model. You can also + * use $g54x - $g59c config functions to change offsets. * - * It also does not reset the work_offsets which may be accomplished by calling - * cm_set_work_offsets() immediately afterwards. + * It also does not reset the work_offsets which may be + * accomplished by calling cm_set_work_offsets() immediately + * afterwards. */ -stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]) { - if ((coord_system < G54) || (coord_system > COORD_SYSTEM_MAX)) // you can't set G53 - return STAT_INPUT_VALUE_RANGE_ERROR; +stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], + float flag[]) { + if ((coord_system < G54) || (coord_system > COORD_SYSTEM_MAX)) + return STAT_INPUT_VALUE_RANGE_ERROR; // you can't set G53 for (uint8_t axis = AXIS_X; axis < AXES; axis++) - if (fp_TRUE(flag[axis])) { + if (fp_TRUE(flag[axis])) cm.offset[coord_system][axis] = _to_millimeters(offset[axis]); - cm.deferred_write_flag = true; // persist offsets once machining cycle is over - } return STAT_OK; } -/****************************************************************************************** - * Representation functions that affect gcode model and are queued to planner (synchronous) - */ +// Representation functions that affect gcode model and are queued to planner +// (synchronous) /* * cm_set_coord_system() - G54-G59 * _exec_offset() - callback from planner @@ -742,8 +743,10 @@ stat_t cm_set_coord_offsets(uint8_t coord_system, float offset[], float flag[]) stat_t cm_set_coord_system(uint8_t coord_system) { cm.gm.coord_system = coord_system; - float value[AXES] = { (float)coord_system,0,0,0,0,0 }; // pass coordinate system in value[0] element - mp_queue_command(_exec_offset, value, value); // second vector (flags) is not used, so fake it + // pass coordinate system in value[0] element + float value[AXES] = {(float)coord_system, 0, 0, 0, 0, 0}; + // second vector (flags) is not used, so fake it + mp_queue_command(_exec_offset, value, value); return STAT_OK; } @@ -952,7 +955,7 @@ stat_t cm_goto_g30_position(float target[], float flags[]) { */ stat_t cm_set_feed_rate(float feed_rate) { if (cm.gm.feed_rate_mode == INVERSE_TIME_MODE) - cm.gm.feed_rate = 1 / feed_rate; // normalize to minutes (NB: active for this gcode block only) + cm.gm.feed_rate = 1 / feed_rate; // normalize to minutes (active for this gcode block only) else cm.gm.feed_rate = _to_millimeters(feed_rate); return STAT_OK; @@ -990,8 +993,7 @@ stat_t cm_set_path_control(uint8_t mode) { /// G4, P parameter (seconds) stat_t cm_dwell(float seconds) { cm.gm.parameter = seconds; - mp_dwell(seconds); - return STAT_OK; + return mp_dwell(seconds); } @@ -1009,9 +1011,9 @@ stat_t cm_straight_feed(float target[], float flags[]) { if (status != STAT_OK) return cm_soft_alarm(status); // prep and plan the move - cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to the state - cm_cycle_start(); // required for homing & other cycles - status = mp_aline(&cm.gm); // send the move to the planner + cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to state + cm_cycle_start(); // required for homing & other cycles + status = mp_aline(&cm.gm); // send the move to the planner cm_finalize_move(); return status; @@ -1349,8 +1351,8 @@ static void _exec_program_finalize(float *value, float *flag) { void cm_cycle_start() { cm.machine_state = MACHINE_CYCLE; - if (cm.cycle_state == CYCLE_OFF) // don't (re)start homing, probe or other canned cycles - cm.cycle_state = CYCLE_MACHINING; + // don't (re)start homing, probe or other canned cycles + if (cm.cycle_state == CYCLE_OFF) cm.cycle_state = CYCLE_MACHINING; } @@ -1414,9 +1416,3 @@ void cm_set_axis_jerk(uint8_t axis, float jerk) { cm.a[axis].jerk_max = jerk; cm.a[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER); } - - -// Axis jogging -float cm_get_jogging_dest() { - return cm.jogging_dest; -} diff --git a/src/canonical_machine.h b/src/canonical_machine.h index 0cdba9f..33aa272 100644 --- a/src/canonical_machine.h +++ b/src/canonical_machine.h @@ -43,7 +43,6 @@ #define _to_millimeters(a) ((cm.gm.units_mode == INCHES) ? (a * MM_PER_INCH) : a) -#define JOGGING_START_VELOCITY ((float)10.0) #define DISABLE_SOFT_LIMIT (-1000000) @@ -239,11 +238,9 @@ typedef struct cmSingleton { // struct to manage cm globals and cycles uint8_t g28_flag; // true = complete a G28 move uint8_t g30_flag; // true = complete a G30 move - uint8_t deferred_write_flag; // G10 data has changed (e.g. offsets) - flag to persist them uint8_t feedhold_requested; // feedhold character has been received uint8_t queue_flush_requested; // queue flush character has been received uint8_t cycle_start_requested; // cycle start character has been received (flag to end feedhold) - float jogging_dest; // jogging direction as a relative move from current position struct GCodeState *am; // active Gcode model is maintained by state management // Model states @@ -299,8 +296,7 @@ enum cmCombinedState { // check alignment with messages in config.c COMBINED_PROBE, // [7] probe cycle active COMBINED_CYCLE, // [8] machine is running (cycling) COMBINED_HOMING, // [9] homing is treated as a cycle - COMBINED_JOG, // [10] jogging is treated as a cycle - COMBINED_SHUTDOWN, // [11] machine in hard alarm state (shutdown) + COMBINED_SHUTDOWN, // [10] machine in hard alarm state (shutdown) }; //### END CRITICAL REGION ### @@ -321,7 +317,6 @@ enum cmCycleState { CYCLE_MACHINING, // in normal machining cycle CYCLE_PROBE, // in probe cycle CYCLE_HOMING, // homing is treated as a specialized cycle - CYCLE_JOG // jogging is treated as a specialized cycle }; @@ -394,7 +389,7 @@ enum cmMotionMode { // G Modal Group 1 MOTION_MODE_CANNED_CYCLE_86, // G86 - boring, spindle stop, rapid out MOTION_MODE_CANNED_CYCLE_87, // G87 - back boring MOTION_MODE_CANNED_CYCLE_88, // G88 - boring, spindle stop, manual out - MOTION_MODE_CANNED_CYCLE_89 // G89 - boring, dwell, feed out + MOTION_MODE_CANNED_CYCLE_89, // G89 - boring, dwell, feed out }; @@ -651,9 +646,4 @@ stat_t cm_homing_callback(); // G28.2/.4 mai stat_t cm_straight_probe(float target[], float flags[]); // G38.2 stat_t cm_probe_callback(); // G38.2 main loop callback -// Jogging cycle -stat_t cm_jogging_callback(); // jogging cycle main loop -stat_t cm_jogging_cycle_start(uint8_t axis); // {"jogx":-100.3} -float cm_get_jogging_dest(); - #endif // CANONICAL_MACHINE_H diff --git a/src/clock.c b/src/clock.c index 5d8886d..3ae2852 100644 --- a/src/clock.c +++ b/src/clock.c @@ -3,17 +3,21 @@ * * Copyright (c) 2010 - 2013 Alden S. Hart Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "clock.h" @@ -25,35 +29,36 @@ void CCPWrite(volatile uint8_t * address, uint8_t value); -/* - * This routine is lifted and modified from Boston Android and from - * http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 -*/ +/// This routine is lifted and modified from Boston Android and from +/// http://www.avrfreaks.net/index.php?name=PNphpBB2&file=viewtopic&p=711659 void clock_init() { -#ifdef __CLOCK_EXTERNAL_8MHZ // external 8 Mhx Xtal with 4x PLL = 32 Mhz - OSC.XOSCCTRL = 0x4B; // 2-9 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup - OSC.CTRL = 0x08; // enable external crystal oscillator - while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - OSC.PLLCTRL = 0xC4; // XOSC is PLL Source; 4x Factor (32 MHz sys clock) + // external 8 Mhx Xtal with 4x PLL = 32 Mhz +#ifdef __CLOCK_EXTERNAL_8MHZ + OSC.XOSCCTRL = 0x4B; // 2-9 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup + OSC.CTRL = 0x08; // enable external crystal oscillator + while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready + OSC.PLLCTRL = 0xC4; // XOSC is PLL Source; 4x Factor (32 MHz sys clock) OSC.CTRL = 0x18; // Enable PLL & External Oscillator while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc); // switch to PLL clock OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock #endif -#ifdef __CLOCK_EXTERNAL_16MHZ // external 16 Mhx Xtal with 2x PLL = 32 Mhz - OSC.XOSCCTRL = 0xCB; // 12-16 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup + // external 16 Mhx Xtal with 2x PLL = 32 Mhz +#ifdef __CLOCK_EXTERNAL_16MHZ + OSC.XOSCCTRL = 0xCB; // 12-16 MHz crystal; 0.4-16 MHz XTAL w/16K CLK startup OSC.CTRL = 0x08; // enable external crystal oscillator while (!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for oscillator ready - OSC.PLLCTRL = 0xC2; // XOSC is PLL Source; 2x Factor (32 MHz sys clock) + OSC.PLLCTRL = 0xC2; // XOSC is PLL Source; 2x Factor (32 MHz sys clock) OSC.CTRL = 0x18; // Enable PLL & External Oscillator while(!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL ready CCPWrite(&CLK.CTRL, CLK_SCLKSEL_PLL_gc); // switch to PLL clock OSC.CTRL &= ~OSC_RC2MEN_bm; // disable internal 2 MHz clock #endif -#ifdef __CLOCK_INTERNAL_32MHZ // 32 MHz internal clock (Boston Android code) + // 32 MHz internal clock (Boston Android code) +#ifdef __CLOCK_INTERNAL_32MHZ CCP = CCP_IOREG_gc; // Security Signature to modify clk OSC.CTRL = OSC_RC32MEN_bm; // enable internal 32MHz oscillator while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for oscillator ready diff --git a/src/clock.h b/src/clock.h index 4c377b7..2b21fc8 100644 --- a/src/clock.h +++ b/src/clock.h @@ -3,17 +3,21 @@ * * Copyright (c) 2010 - 2013 Alden S. Hart Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef CLOCK_H diff --git a/src/command.c b/src/command.c index f0903fb..0742043 100644 --- a/src/command.c +++ b/src/command.c @@ -36,11 +36,13 @@ #include "hardware.h" #include "report.h" #include "vars.h" +#include "plan/jog.h" #include "config.h" #include #include +#include #include @@ -117,17 +119,7 @@ int command_exec(int argc, char *argv[]) { } -int command_eval(char *cmd) { - while (*cmd && isspace(*cmd)) cmd++; - - if (!*cmd) { - report_request_full(); - return STAT_OK; - } - - if (*cmd == '{') return vars_parser(cmd); - if (*cmd != '$') return gc_gcode_parser(cmd); - +int command_parser(char *cmd) { // Parse line char *p = cmd + 1; int argc = 0; @@ -151,6 +143,21 @@ int command_eval(char *cmd) { } +int command_eval(char *cmd) { + // Skip leading whitespace + while (*cmd && isspace(*cmd)) cmd++; + + switch (*cmd) { + case 0: report_request_full(); return STAT_OK; + case '{': return vars_parser(cmd); + case '$': return command_parser(cmd); + default: + if (!mp_jog_busy()) return gc_gcode_parser(cmd); + return STAT_OK; + } +} + + // Command functions void static print_command_help(int i) { static const char fmt[] PROGMEM = " %-8S %S\n"; @@ -163,7 +170,7 @@ void static print_command_help(int i) { uint8_t command_help(int argc, char *argv[]) { if (argc == 2) { - int i = command_find(argv[0]); + int i = command_find(argv[1]); if (i == -1) { printf_P(PSTR("Command not found\n")); @@ -192,3 +199,16 @@ uint8_t command_reboot(int argc, char *argv[]) { hw_request_hard_reset(); return 0; } + + +uint8_t command_jog(int argc, char *argv[]) { + float velocity[AXES]; + + for (int axis = 0; axis < AXES; axis++) + if (axis < argc - 1) velocity[axis] = atof(argv[axis + 1]); + else velocity[axis] = 0; + + mp_jog(velocity); + + return 0; +} diff --git a/src/command.def b/src/command.def index f0131f2..cdf9677 100644 --- a/src/command.def +++ b/src/command.def @@ -31,3 +31,4 @@ CMD(help, command_help, 0, 1, "Print this help screen") CMD(reboot, command_reboot, 0, 0, "Reboot the controller") +CMD(jog, command_jog, 1, 4, "Jog") diff --git a/src/config.h b/src/config.h index 362345b..5d793e9 100644 --- a/src/config.h +++ b/src/config.h @@ -24,7 +24,7 @@ #define AXIS_W 8 // reserved // Motor settings -#define MOTOR_MICROSTEPS 8 +#define MOTOR_MICROSTEPS 16 /// One of: /// MOTOR_DISABLED @@ -37,13 +37,13 @@ #define MOTOR_IDLE_TIMEOUT 2.00 -#define M1_MOTOR_MAP AXIS_X // 1ma -#define M1_STEP_ANGLE 1.8 // 1sa -#define M1_TRAVEL_PER_REV 1.25 // 1tr -#define M1_MICROSTEPS MOTOR_MICROSTEPS // 1mi -#define M1_POLARITY 0 // 1po 0=normal, 1=reversed -#define M1_POWER_MODE MOTOR_POWER_MODE // 1pm standard -#define M1_POWER_LEVEL MOTOR_POWER_LEVEL // 1mp +#define M1_MOTOR_MAP AXIS_X +#define M1_STEP_ANGLE 1.8 +#define M1_TRAVEL_PER_REV 1.25 +#define M1_MICROSTEPS MOTOR_MICROSTEPS +#define M1_POLARITY 0 // 0=normal, 1=reversed +#define M1_POWER_MODE MOTOR_POWER_MODE +#define M1_POWER_LEVEL MOTOR_POWER_LEVEL #define M2_MOTOR_MAP AXIS_Y #define M2_STEP_ANGLE 1.8 @@ -71,7 +71,7 @@ #define M5_MOTOR_MAP AXIS_B #define M5_STEP_ANGLE 1.8 -#define M5_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M5_TRAVEL_PER_REV 360 #define M5_MICROSTEPS MOTOR_MICROSTEPS #define M5_POLARITY 0 #define M5_POWER_MODE MOTOR_POWER_MODE @@ -79,7 +79,7 @@ #define M6_MOTOR_MAP AXIS_C #define M6_STEP_ANGLE 1.8 -#define M6_TRAVEL_PER_REV 360 // degrees moved per motor rev +#define M6_TRAVEL_PER_REV 360 #define M6_MICROSTEPS MOTOR_MICROSTEPS #define M6_POLARITY 0 #define M6_POWER_MODE MOTOR_POWER_MODE @@ -87,9 +87,12 @@ // Switch settings -#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN // one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED -#define X_SWITCH_MODE_MIN SW_MODE_HOMING // xsn SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT -#define X_SWITCH_MODE_MAX SW_MODE_DISABLED // xsx SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +/// one of: SW_TYPE_NORMALLY_OPEN, SW_TYPE_NORMALLY_CLOSED +#define SWITCH_TYPE SW_TYPE_NORMALLY_OPEN +/// SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MIN SW_MODE_HOMING +/// SW_MODE_DISABLED, SW_MODE_HOMING, SW_MODE_LIMIT, SW_MODE_HOMING_LIMIT +#define X_SWITCH_MODE_MAX SW_MODE_DISABLED #define Y_SWITCH_MODE_MIN SW_MODE_HOMING #define Y_SWITCH_MODE_MAX SW_MODE_DISABLED #define Z_SWITCH_MODE_MIN SW_MODE_DISABLED @@ -101,31 +104,35 @@ #define C_SWITCH_MODE_MIN SW_MODE_HOMING #define C_SWITCH_MODE_MAX SW_MODE_DISABLED +// Jog settings +#define JOG_ACCELERATION 50000 // mm/min^2 // Machine settings -#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing -#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on -#define JERK_MAX 10 // yes, that's "20,000,000" mm/(min^3) -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal acceleration around corners +#define MOTOR_CURRENT 1.0 // 1.0 is full power +#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing +#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on +#define JERK_MAX 10 // yes, that's "20,000,000" mm/min^3 +#define JUNCTION_DEVIATION 0.05 // default value, in mm +#define JUNCTION_ACCELERATION 100000 // centripetal corner acceleration // Axis settings -#define VELOCITY_MAX 2000 +#define VELOCITY_MAX 1000 #define FEEDRATE_MAX 4000 -#define X_AXIS_MODE AXIS_STANDARD // xam see canonical_machine.h cmAxisMode for valid values -#define X_VELOCITY_MAX VELOCITY_MAX // xvm G0 max velocity in mm/min -#define X_FEEDRATE_MAX FEEDRATE_MAX // xfr G1 max feed rate in mm/min -#define X_TRAVEL_MIN 0 // xtn minimum travel for soft limits -#define X_TRAVEL_MAX 150 // xtm travel between switches or crashes -#define X_JERK_MAX JERK_MAX // xjm -#define X_JERK_HOMING (X_JERK_MAX * 2) // xjh -#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION // xjd -#define X_SEARCH_VELOCITY 500 // xsv move in negative direction -#define X_LATCH_VELOCITY 100 // xlv mm/min -#define X_LATCH_BACKOFF 5 // xlb mm -#define X_ZERO_BACKOFF 1 // xzb mm +// see canonical_machine.h cmAxisMode for valid values +#define X_AXIS_MODE AXIS_STANDARD +#define X_VELOCITY_MAX VELOCITY_MAX // G0 max velocity in mm/min +#define X_FEEDRATE_MAX FEEDRATE_MAX // G1 max feed rate in mm/min +#define X_TRAVEL_MIN 0 // minimum travel for soft limits +#define X_TRAVEL_MAX 150 // between switches or crashes +#define X_JERK_MAX JERK_MAX +#define X_JERK_HOMING (X_JERK_MAX * 2) +#define X_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define X_SEARCH_VELOCITY 500 // move in negative direction +#define X_LATCH_VELOCITY 100 // mm/min +#define X_LATCH_BACKOFF 5 // mm +#define X_ZERO_BACKOFF 1 // mm #define Y_AXIS_MODE AXIS_STANDARD #define Y_VELOCITY_MAX VELOCITY_MAX @@ -171,10 +178,11 @@ // A values are chosen to make the A motor react the same as X for testing #else #define A_AXIS_MODE AXIS_RADIUS -#define A_VELOCITY_MAX ((X_VELOCITY_MAX/M1_TRAVEL_PER_REV)*360) // set to the same speed as X axis +// set to the same speed as X axis +#define A_VELOCITY_MAX ((X_VELOCITY_MAX / M1_TRAVEL_PER_REV) * 360) #define A_FEEDRATE_MAX A_VELOCITY_MAX #define A_TRAVEL_MIN -1 -#define A_TRAVEL_MAX -1 // same number means infinite +#define A_TRAVEL_MAX -1 // same number means infinite #define A_JERK_MAX (X_JERK_MAX * (360 / M1_TRAVEL_PER_REV)) #define A_JERK_HOMING (A_JERK_MAX * 2) #define A_JUNCTION_DEVIATION JUNCTION_DEVIATION @@ -215,10 +223,10 @@ // PWM settings -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) +#define P1_PWM_FREQUENCY 100 // in Hz +#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) #define P1_CW_SPEED_HI 2000 -#define P1_CW_PHASE_LO 0.125 // phase [0..1] +#define P1_CW_PHASE_LO 0.125 // phase [0..1] #define P1_CW_PHASE_HI 0.2 #define P1_CCW_SPEED_LO 1000 #define P1_CCW_SPEED_HI 2000 @@ -229,8 +237,9 @@ // Gcode defaults #define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE CANON_PLANE_XY // CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ -#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58 or G59 +// CANON_PLANE_XY, CANON_PLANE_XZ, or CANON_PLANE_YZ +#define GCODE_DEFAULT_PLANE CANON_PLANE_XY +#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58 or G59 #define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS #define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE diff --git a/src/controller.c b/src/controller.c index b8628ad..332dc3c 100644 --- a/src/controller.c +++ b/src/controller.c @@ -5,25 +5,31 @@ * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * Copyright (c) 2013 - 2015 Robert Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "controller.h" @@ -50,11 +56,12 @@ controller_t cs; // controller state structure -/// Blink rapidly and prevent further activity from occurring -/// Shutdown idler flashes indicator LED rapidly to show everything is not OK. -/// Shutdown idler returns EAGAIN causing the control loop to never advance beyond -/// this point. It's important that the reset handler is still called so a SW reset -/// (ctrl-x) or bootloader request can be processed. +/// Blink rapidly and prevent further activity from occurring Shutdown +/// idler flashes indicator LED rapidly to show everything is not OK. +/// Shutdown idler returns EAGAIN causing the control loop to never +/// advance beyond this point. It's important that the reset handler +/// is still called so a SW reset (ctrl-x) or bootloader request can +/// be processed. static stat_t _shutdown_idler() { if (cm_get_machine_state() != MACHINE_SHUTDOWN) return STAT_OK; @@ -95,7 +102,8 @@ static stat_t _limit_switch_handler() { void controller_init() { - memset(&cs, 0, sizeof(controller_t)); // clear all values, job_id's, pointers and status + // clear all values, job_id's, pointers and status + memset(&cs, 0, sizeof(controller_t)); } @@ -124,23 +132,24 @@ void controller_run() { // Kernel level ISR handlers, flags are set in ISRs, order is important DISPATCH(hw_hard_reset_handler()); // 1. handle hard reset requests - DISPATCH(hw_bootloader_handler()); // 2. handle requests to enter bootloader + DISPATCH(hw_bootloader_handler()); // 2. handle bootloader requests DISPATCH(_shutdown_idler()); // 3. idle in shutdown state - DISPATCH(_limit_switch_handler()); // 4. limit switch has been thrown - DISPATCH(cm_feedhold_sequencing_callback()); // 5a. feedhold state machine runner - DISPATCH(mp_plan_hold_callback()); // 5b. plan a feedhold from line runtime + DISPATCH(_limit_switch_handler()); // 4. limit switch thrown + DISPATCH(cm_feedhold_sequencing_callback()); // 5a. feedhold state machine + DISPATCH(mp_plan_hold_callback()); // 5b. plan a feedhold // Planner hierarchy for gcode and cycles DISPATCH(st_motor_power_callback()); // stepper motor power sequencing - DISPATCH(cm_arc_callback()); // arc generation runs behind lines + DISPATCH(cm_arc_callback()); // arc generation runs DISPATCH(cm_homing_callback()); // G28.2 continuation - DISPATCH(cm_jogging_callback()); // jog function DISPATCH(cm_probe_callback()); // G38.2 continuation - DISPATCH(cm_deferred_write_callback()); // persist G10 changes when not in machining cycle // Command readers and parsers - DISPATCH(_sync_to_planner()); // ensure at least one free buffer in planning queue - DISPATCH(_sync_to_tx_buffer()); // sync with TX buffer (pseudo-blocking) + // ensure at least one free buffer in planning queue + DISPATCH(_sync_to_planner()); + // sync with TX buffer (pseudo-blocking) + DISPATCH(_sync_to_tx_buffer()); + DISPATCH(report_callback()); DISPATCH(command_dispatch()); // read and execute next command } diff --git a/src/controller.h b/src/controller.h index c100bcb..9711390 100644 --- a/src/controller.h +++ b/src/controller.h @@ -5,25 +5,31 @@ * Copyright (c) 2010 - 2014 Alden S. Hart, Jr. * Copyright (c) 2013 - 2014 Robert Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef CONTROLLER_H #define CONTROLLER_H @@ -31,7 +37,6 @@ #include "status.h" -#define LED_NORMAL_TIMER 1000 // blink rate for normal operation (in ms) #define LED_ALARM_TIMER 100 // blink rate for alarm state (in ms) diff --git a/src/cycle_homing.c b/src/cycle_homing.c index 3bca4a9..a96b640 100644 --- a/src/cycle_homing.c +++ b/src/cycle_homing.c @@ -4,25 +4,31 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "canonical_machine.h" diff --git a/src/cycle_jogging.c b/src/cycle_jogging.c deleted file mode 100644 index 18ae8f6..0000000 --- a/src/cycle_jogging.c +++ /dev/null @@ -1,197 +0,0 @@ -/* - * cycle_jogging.c - jogging cycle extension to canonical_machine.c - * - * by Mike Estee - Other Machine Company - * - * This file ("the software") is free software: you can redistribute - * it and/or modify it under the terms of the GNU General Public - * License, version 2 as published by the Free Software - * Foundation. You should have received a copy of the GNU General - * Public License, version 2 along with the software. If not, see - * . - * - * As a special exception, you may use this file as part of a software - * library without restriction. Specifically, if other files - * instantiate templates or use macros or inline functions from this - * file, or you compile this file and link it with other files to - * produce an executable, this file does not by itself cause the - * resulting executable to be covered by the GNU General Public - * License. This exception does not however invalidate any other - * reasons why the executable file might be covered by the GNU General - * Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT - * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT - * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A - * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR - * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR - * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE - * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - */ - -#include "canonical_machine.h" - -#include "plan/planner.h" - -#include -#include -#include -#include - - -struct jmJoggingSingleton { // persistent jogging runtime variables - // controls for jogging cycle - int8_t axis; // axis currently being jogged - float dest_pos; // travel relative to start position - float start_pos; - float velocity_start; // initial jog feed - float velocity_max; - - uint8_t (*func)(int8_t axis); // binding for callback function - - // state saved from gcode model - float saved_feed_rate; // F setting - uint8_t saved_units_mode; // G20, G21 global setting - uint8_t saved_coord_system; // G54 - G59 setting - uint8_t saved_distance_mode; // G90, G91 global setting - uint8_t saved_feed_rate_mode; // G93, G94 global setting - float saved_jerk; // saved and restored for each axis homed -}; - -static struct jmJoggingSingleton jog; - - -// NOTE: global prototypes and other .h info is located in canonical_machine.h -static stat_t _set_jogging_func(uint8_t (*func)(int8_t axis)); -static stat_t _jogging_axis_start(int8_t axis); -static stat_t _jogging_axis_jog(int8_t axis); -static stat_t _jogging_finalize_exit(int8_t axis); - - -/***************************************************************************** - * jogging cycle using soft limits - * - * --- Some further details --- - * - * Note: When coding a cycle (like this one) you get to perform one queued - * move per entry into the continuation, then you must exit. - * - * Another Note: When coding a cycle you must wait until - * the last move has actually been queued (or has finished) before declaring - * the cycle to be done. Otherwise there is a nasty race condition in the - * tg_controller() that will accept the next command before the position of - * the final move has been recorded in the Gcode model. That's what the call - * to cm_isbusy() is about. - */ -stat_t cm_jogging_cycle_start(uint8_t axis) { - // Save relevant non-axis parameters from Gcode model - jog.saved_units_mode = cm_get_units_mode(ACTIVE_MODEL); - jog.saved_coord_system = cm_get_coord_system(ACTIVE_MODEL); - jog.saved_distance_mode = cm_get_distance_mode(ACTIVE_MODEL); - jog.saved_feed_rate_mode = cm_get_feed_rate_mode(ACTIVE_MODEL); - jog.saved_feed_rate = cm_get_feed_rate(ACTIVE_MODEL); - jog.saved_jerk = cm_get_axis_jerk(axis); - - // Set working values - cm_set_units_mode(MILLIMETERS); - cm_set_distance_mode(ABSOLUTE_MODE); - cm_set_coord_system(ABSOLUTE_COORDS); // jog in machine coordinates - cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE); - - jog.velocity_start = JOGGING_START_VELOCITY; // see canonical_machine.h - jog.velocity_max = cm.a[axis].velocity_max; - - jog.start_pos = cm_get_absolute_position(RUNTIME, axis); - jog.dest_pos = cm_get_jogging_dest(); - - jog.axis = axis; - jog.func = _jogging_axis_start; // initial processing function - - cm.cycle_state = CYCLE_JOG; - - return STAT_OK; -} - - -// Jogging axis moves execute in sequence for each axis - -/// main loop callback for running the jogging cycle -stat_t cm_jogging_callback() { - if (cm.cycle_state != CYCLE_JOG) return STAT_NOOP; // not in a jogging cycle - if (cm_get_runtime_busy()) return STAT_EAGAIN; // sync to planner - - return jog.func(jog.axis); // execute current move -} - - -/// a convenience for setting the next dispatch vector and exiting -static stat_t _set_jogging_func(stat_t (*func)(int8_t axis)) { - jog.func = func; - return STAT_EAGAIN; -} - - -/// setup and start -static stat_t _jogging_axis_start(int8_t axis) { - return _set_jogging_func(_jogging_axis_jog); // register jog move callback -} - - -/// ramp the jog -static stat_t _jogging_axis_jog(int8_t axis) { // run jog move - float vect[] = {0, 0, 0, 0, 0, 0}; - float flags[] = {false, false, false, false, false, false}; - flags[axis] = true; - - float velocity = jog.velocity_start; - float direction = jog.start_pos <= jog.dest_pos ? 1 : -1; - float delta = abs(jog.dest_pos - jog.start_pos); - - cm.gm.feed_rate = velocity; - mp_flush_planner(); // don't use cm_request_queue_flush() here - cm_request_cycle_start(); - - float ramp_dist = 2.0; - float steps = 0.0; - float max_steps = 25; - float offset = 0.01; - - while (delta > ramp_dist && offset < delta && steps < max_steps) { - vect[axis] = jog.start_pos + offset * direction; - cm.gm.feed_rate = velocity; - ritorno(cm_straight_feed(vect, flags)); - - steps++; - float scale = pow(10.0, steps / max_steps) / 10.0; - velocity = - jog.velocity_start + (jog.velocity_max - jog.velocity_start) * scale; - offset += ramp_dist * steps / max_steps; - } - - // final move - cm.gm.feed_rate = jog.velocity_max; - vect[axis] = jog.dest_pos; - ritorno(cm_straight_feed(vect, flags)); - - return _set_jogging_func(_jogging_finalize_exit); -} - - -/// back off the cleared limit switch -static stat_t _jogging_finalize_exit(int8_t axis) { - mp_flush_planner(); // FIXME: not sure what to do - - // Restore saved settings - cm_set_axis_jerk(axis, jog.saved_jerk); - cm_set_coord_system(jog.saved_coord_system); - cm_set_units_mode(jog.saved_units_mode); - cm_set_distance_mode(jog.saved_distance_mode); - cm_set_feed_rate_mode(jog.saved_feed_rate_mode); - cm.gm.feed_rate = jog.saved_feed_rate; - cm_set_motion_mode(MODEL, MOTION_MODE_CANCEL_MOTION_MODE); - cm_cycle_end(); - cm.cycle_state = CYCLE_OFF; - - return STAT_OK; -} diff --git a/src/encoder.c b/src/encoder.c index fb13d72..4d2f09a 100644 --- a/src/encoder.c +++ b/src/encoder.c @@ -4,25 +4,31 @@ * * Copyright (c) 2013 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "encoder.h" @@ -35,16 +41,16 @@ enEncoders_t en; void encoder_init() { - memset(&en, 0, sizeof(en)); // clear all values, pointers and status + memset(&en, 0, sizeof(en)); // clear all values, pointers and status } /* * Set encoder values to a current step count * - * Sets the encoder_position steps. Takes floating point steps as input, - * writes integer steps. So it's not an exact representation of machine - * position except if the machine is at zero. + * Sets the encoder_position steps. Takes floating point steps as input, + * writes integer steps. So it's not an exact representation of machine + * position except if the machine is at zero. */ void en_set_encoder_steps(uint8_t motor, float steps) { en.en[motor].encoder_steps = (int32_t)round(steps); @@ -52,11 +58,12 @@ void en_set_encoder_steps(uint8_t motor, float steps) { /* - * The stepper ISR count steps into steps_run(). These values are accumulated to - * encoder_position during LOAD (HI interrupt level). The encoder position is - * therefore always stable. But be advised: the position lags target and position - * valaues elsewherein the system becuase the sample is taken when the steps for - * that segment are complete. + * The stepper ISR count steps into steps_run(). These values are + * accumulated to encoder_position during LOAD (HI interrupt + * level). The encoder position is therefore always stable. But be + * advised: the position lags target and position valaues + * elsewherein the system becuase the sample is taken when the + * steps for that segment are complete. */ float en_read_encoder(uint8_t motor) { return (float)en.en[motor].encoder_steps; diff --git a/src/encoder.h b/src/encoder.h index 1d4bcd4..a40dff6 100644 --- a/src/encoder.h +++ b/src/encoder.h @@ -4,87 +4,105 @@ * * Copyright (c) 2013 - 2014 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ + /* - * ENCODERS - * - * Calling this file "encoders" is kind of a lie, at least for now. There are no encoders. - * Instead the steppers count steps to provide a "truth" reference for position. In the - * future when we have real encoders we'll stop counting steps and actually measure the - * position. Which should be a lot easier than how this module currently works. - * - * *** Measuring position *** - * - * The challenge is that you can't just measure the position at any arbitrary point - * because the system is so heavily queued (pipelined) by the planner queue and the stepper - * sequencing. - * - * You only know where the machine should be at known "targets", which are at the end of - * each move section (end of head, body, and tail). You need to take encoder readings at - * these points. This synchronization is taken care of by the Target, Position, Position_delayed - * sequence in exec. Referring to ASCII art in stepper.h and reproduced here: - * - * LOAD/STEP (~5000uSec) [L1][Segment1][L2][Segment2][L3][Segment3][L4][Segment4][Lb1][Segmentb1] - * PREP (100 uSec) [P1] [P2] [P3] [P4] [Pb1] [Pb2] - * EXEC (400 uSec) [EXEC1] [EXEC2] [EXEC3] [EXEC4] [EXECb1] [EXECb2] - * PLAN (<4ms) [PLANmoveA][PLANmoveB][PLANmoveC][PLANmoveD][PLANmoveE] etc. - * - * You can collect the target for moveA as early as the end of [PLANmoveA]. The system will - * not reach that target position until the end of [Segment4]. Data from Segment4 can only be - * processed during the EXECb2 or Pb2 interval as it's the first time that is not time-critical - * and you actually have enough cycles to calculate the position and error terms. We use Pb2. - * - * Additionally, by this time the target in Gcode model knows about has advanced quite a bit, - * so the moveA target needs to be saved somewhere. Targets are propagated downward to the planner - * runtime (the EXEC), but the exec will have moved on to moveB by the time we need it. So moveA's - * target needs to be saved somewhere. + * Encoders + * + * Calling this file "encoders" is kind of a lie, at least for + * now. There are no encoders. Instead the steppers count steps to + * provide a "truth" reference for position. In the future when we + * have real encoders we'll stop counting steps and actually measure + * the position. Which should be a lot easier than how this module + * currently works. + * + * Measuring position + * + * The challenge is that you can't just measure the position at any + * arbitrary point because the system is so heavily queued (pipelined) + * by the planner queue and the stepper sequencing. + * + * You only know where the machine should be at known "targets", which + * are at the end of each move section (end of head, body, and + * tail). You need to take encoder readings at these points. This + * synchronization is taken care of by the Target, Position, + * Position_delayed sequence in exec. Referring to ASCII art in + * stepper.h and reproduced here: + * + * LOAD/STEP ~5000uS [L1][Seg1][L2][Seg2][L3][Seg3][L4][Seg4][Lb1][Segb1] + * PREP 100 uS [P1] [P2] [P3] [P4] [Pb1] [Pb2] + * EXEC 400 uS [EXEC1] [EXEC2] [EXEC3] [EXEC4] [EXECb1] [EXECb2] + * PLAN <4ms [PLANmoveA][PLANmoveB][PLANmoveC][PLANmoveD][PLANmoveE] etc. + * + * You can collect the target for moveA as early as the end of + * [PLANmoveA]. The system will not reach that target position until + * the end of [Seg4]. Data from Seg4 can only be processed + * during the EXECb2 or Pb2 interval as it's the first time that is + * not time-critical and you actually have enough cycles to calculate + * the position and error terms. We use Pb2. + * + * Additionally, by this time the target in Gcode model knows about + * has advanced quite a bit, so the moveA target needs to be saved + * somewhere. Targets are propagated downward to the planner runtime + * (the EXEC), but the exec will have moved on to moveB by the time we + * need it. So moveA's target needs to be saved somewhere. */ -/* - * ERROR CORRECTION - * - * The purpose of this module is to calculate an error term between the programmed - * position (target) and the actual measured position (position). The error term is - * used during move execution (exec) to adjust the move to compensate for accumulated - * positional errors. It's also the basis of closed-loop (servoed) systems. - * - * Positional error occurs due to floating point numerical inaccuracies. TinyG uses - * 32 bit floating point (GCC 32 bit, which is NOT IEEE 32 bit). Errors creep in - * during planning, move execution, and stepper output phases. Care has been taken - * to minimize introducing errors throughout the process, but they still occur. - * In most cases errors are not noticeable as they fall below the step resolution - * for most jobs. For jobs that run > 1 hour the errors can accumulate and send - * results off by as much as a millimeter if not corrected. - * - * Note: Going to doubles (from floats) would reduce the errors but not eliminate - * them altogether. But this moot on AVRGCC which only does single precision floats. - * - * *** Applying the error term for error correction *** - * - * So if you want to use the error from moveA to correct moveB it has to be done in a region that - * is not already running (i.e. the head, body, or tail) as moveB is already 2 segments into run. - * Since most moves in very short line Gcode files are body only, for practical purposes the - * correction will be applied to moveC. (It's possible to recompute the body of moveB, but it may - * not be worth the trouble). +/* Error correction + * + * The purpose of this module is to calculate an error term between + * the programmed position (target) and the actual measured position + * (position). The error term is used during move execution (exec) to + * adjust the move to compensate for accumulated positional + * errors. It's also the basis of closed-loop (servoed) systems. + * + * Positional error occurs due to floating point numerical + * inaccuracies. TinyG uses 32 bit floating point (GCC 32 bit, which + * is NOT IEEE 32 bit). Errors creep in during planning, move + * execution, and stepper output phases. Care has been taken to + * minimize introducing errors throughout the process, but they still + * occur. In most cases errors are not noticeable as they fall below + * the step resolution for most jobs. For jobs that run > 1 hour the + * errors can accumulate and send results off by as much as a + * millimeter if not corrected. + * + * Note: Going to doubles (from floats) would reduce the errors but + * not eliminate them altogether. But this moot on AVRGCC which only + * does single precision floats. + * + * Applying the error term for error correction + * + * So if you want to use the error from moveA to correct moveB it has + * to be done in a region that is not already running (i.e. the head, + * body, or tail) as moveB is already 2 segments into run. Since most + * moves in very short line Gcode files are body only, for practical + * purposes the correction will be applied to moveC. (It's possible to + * recompute the body of moveB, but it may not be worth the trouble). */ #ifndef ENCODER_H @@ -94,10 +112,12 @@ #include -// used to abstract the encoder code out of the stepper so it can be managed in one place +// used to abstract the encoder code out of the stepper so it can be managed in +// one place #define SET_ENCODER_STEP_SIGN(m, s) en.en[m].step_sign = s; -#define INCREMENT_ENCODER(m) en.en[m].steps_run += en.en[m].step_sign; -#define ACCUMULATE_ENCODER(m) en.en[m].encoder_steps += en.en[m].steps_run; en.en[m].steps_run = 0; +#define INCREMENT_ENCODER(m) en.en[m].steps_run += en.en[m].step_sign; +#define ACCUMULATE_ENCODER(m) \ + en.en[m].encoder_steps += en.en[m].steps_run; en.en[m].steps_run = 0; typedef struct enEncoder { // one real or virtual encoder per controlled motor diff --git a/src/gcode_parser.c b/src/gcode_parser.c index a93536f..52f9dae 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -4,17 +4,21 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "gcode_parser.h" @@ -31,21 +35,24 @@ #include -struct gcodeParserSingleton { // struct to manage globals +struct gcodeParserSingleton { // struct to manage globals uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block }; struct gcodeParserSingleton gp; // local helper functions and macros -static void _normalize_gcode_block(char *str, char **com, char **msg, uint8_t *block_delete_flag); +static void _normalize_gcode_block(char *str, char **com, char **msg, + uint8_t *block_delete_flag); static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value); static stat_t _point(float value); static stat_t _validate_gcode_block(); -static stat_t _parse_gcode_block(char *line); // Parse the block into the GN/GF structs +/// Parse the block into the GN/GF structs +static stat_t _parse_gcode_block(char *line); static stat_t _execute_gcode_block(); // Execute the gcode block -#define SET_MODAL(m,parm,val) ({cm.gn.parm=val; cm.gf.parm=1; gp.modals[m]+=1; break;}) -#define SET_NON_MODAL(parm,val) ({cm.gn.parm=val; cm.gf.parm=1; break;}) -#define EXEC_FUNC(f,v) if((uint8_t)cm.gf.v != false) { status = f(cm.gn.v);} +#define SET_MODAL(m, parm, val) \ + {cm.gn.parm = val; cm.gf.parm = 1; gp.modals[m] += 1; break;} +#define SET_NON_MODAL(parm, val) {cm.gn.parm = val; cm.gf.parm = 1; break;} +#define EXEC_FUNC(f, v) if ((uint8_t)cm.gf.v) {status = f(cm.gn.v);} /// Parse a block (line) of gcode @@ -67,7 +74,7 @@ stat_t gc_gcode_parser(char *block) { if (block_delete_flag) return STAT_NOOP; // queue a "(MSG" response - if (*msg) cm_message(msg); // queue the message + if (*msg) cm_message(msg); // queue the message return _parse_gcode_block(block); } @@ -76,41 +83,45 @@ stat_t gc_gcode_parser(char *block) { /* * Normalize a block (line) of gcode in place * - * Normalization functions: + * Normalization functions: * - convert all letters to upper case - * - remove white space, control and other invalid characters - * - remove (erroneous) leading zeros that might be taken to mean Octal - * - identify and return start of comments and messages - * - signal if a block-delete character (/) was encountered in the first space + * - remove white space, control and other invalid characters + * - remove (erroneous) leading zeros that might be taken to mean Octal + * - identify and return start of comments and messages + * - signal if a block-delete character (/) was encountered in the first space * - * So this: " g1 x100 Y100 f400" becomes this: "G1X100Y100F400" + * So this: " g1 x100 Y100 f400" becomes this: "G1X100Y100F400" * - * Comment and message handling: - * - Comments field start with a '(' char or alternately a semicolon ';' - * - Comments and messages are not normalized - they are left alone - * - The 'MSG' specifier in comment can have mixed case but cannot cannot have embedded white spaces - * - Normalization returns true if there was a message to display, false otherwise - * - Comments always terminate the block - i.e. leading or embedded comments are not supported - * - Valid cases (examples) Notes: - * G0X10 - command only - no comment - * (comment text) - There is no command on this line - * G0X10 (comment text) - * G0X10 (comment text - It's OK to drop the trailing paren - * G0X10 ;comment text - It's OK to drop the trailing paren + * Comment and message handling: + * - Comments field start with a '(' char or alternately a semicolon ';' + * - Comments and messages are not normalized - they are left alone + * - The 'MSG' specifier in comment can have mixed case but cannot cannot + * have embedded white spaces + * - Normalization returns true if there was a message to display, false + * otherwise + * - Comments always terminate the block - i.e. leading or embedded comments + * are not supported + * - Valid cases (examples) Notes: + * G0X10 - command only - no comment + * (comment text) - There is no command on this line + * G0X10 (comment text) + * G0X10 (comment text - It's OK to drop the trailing paren + * G0X10 ;comment text - It's OK to drop the trailing paren * - * - Invalid cases (examples) Notes: - * G0X10 comment text - Comment with no separator - * N10 (comment) G0X10 - embedded comment. G0X10 will be ignored - * (comment) G0X10 - leading comment. G0X10 will be ignored - * G0X10 # comment - invalid separator + * - Invalid cases (examples) Notes: + * G0X10 comment text - Comment with no separator + * N10 (comment) G0X10 - embedded comment. G0X10 will be ignored + * (comment) G0X10 - leading comment. G0X10 will be ignored + * G0X10 # comment - invalid separator * - * Returns: - * - com points to comment string or to 0 if no comment - * - msg points to message string or to 0 if no comment - * - block_delete_flag is set true if block delete encountered, false otherwise + * Returns: + * - com points to comment string or to 0 if no comment + * - msg points to message string or to 0 if no comment + * - block_delete_flag is set true if block delete encountered, false otherwise */ -static void _normalize_gcode_block(char *str, char **com, char **msg, uint8_t *block_delete_flag) { +static void _normalize_gcode_block(char *str, char **com, char **msg, + uint8_t *block_delete_flag) { char *rd = str; // read pointer char *wr = str; // write pointer @@ -119,96 +130,98 @@ static void _normalize_gcode_block(char *str, char **com, char **msg, uint8_t *b else { *block_delete_flag = false; } // normalize the command block & find the comment (if any) - for (; *wr != 0; rd++) { - if (*rd == 0) { *wr = 0; } - else if ((*rd == '(') || (*rd == ';')) { *wr = 0; *com = rd+1; } - else if ((isalnum((char)*rd)) || (strchr("-.", *rd))) { // all valid characters - *(wr++) = (char)toupper((char)*(rd)); - } - } + for (; *wr != 0; rd++) + if (*rd == 0) *wr = 0; + else if (*rd == '(' || *rd == ';') {*wr = 0; *com = rd + 1;} + else if (isalnum((char)*rd) || strchr("-.", *rd)) // all valid characters + *wr++ = (char)toupper((char)*rd); // Perform Octal stripping - remove invalid leading zeros in number strings rd = str; while (*rd != 0) { - if (*rd == '.') break; // don't strip past a decimal point - if ((!isdigit(*rd)) && (*(rd + 1) == '0') && (isdigit(*(rd + 2)))) { + if (*rd == '.') break; // don't strip past a decimal point + if (!isdigit(*rd) && *(rd + 1) == '0' && isdigit(*(rd + 2))) { wr = rd + 1; while (*wr != 0) {*wr = *(wr + 1); wr++;} // copy forward w/overwrite continue; } + rd++; } // process comments and messages if (**com != 0) { rd = *com; - while (isspace(*rd)) { rd++; } // skip any leading spaces before "msg" - if ((tolower(*rd) == 'm') && (tolower(*(rd+1)) == 's') && (tolower(*(rd+2)) == 'g')) + while (isspace(*rd)) rd++; // skip any leading spaces before "msg" + + if (tolower(*rd) == 'm' && tolower(*(rd + 1)) == 's' && + tolower(*(rd + 2)) == 'g') *msg = rd + 3; for (; *rd != 0; rd++) - if (*rd == ')') *rd = 0; // 0 terminate on trailing parenthesis, if any + // 0 terminate on trailing parenthesis, if any + if (*rd == ')') *rd = 0; } } -/* - * Get gcode word consisting of a letter and a value +/* Get gcode word consisting of a letter and a value * - * This function requires the Gcode string to be normalized. - * Normalization must remove any leading zeros or they will be converted to Octal - * G0X... is not interpreted as hexadecimal. This is trapped. + * This function requires the Gcode string to be normalized. + * Normalization must remove any leading zeros or they will be converted to + * octal. G0X... is not interpreted as hexadecimal. This is trapped. */ static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) { - if (**pstr == 0) return STAT_COMPLETE; // no more words to process + if (**pstr == 0) return STAT_COMPLETE; // no more words to process // get letter part - if(isupper(**pstr) == false) - return STAT_INVALID_OR_MALFORMED_COMMAND; + if (!isupper(**pstr)) return STAT_INVALID_OR_MALFORMED_COMMAND; *letter = **pstr; (*pstr)++; // X-axis-becomes-a-hexadecimal-number get-value case, e.g. G0X100 --> G255 - if ((**pstr == '0') && (*(*pstr+1) == 'X')) { + if (**pstr == '0' && *(*pstr + 1) == 'X') { *value = 0; (*pstr)++; - return STAT_OK; // pointer points to X + return STAT_OK; // pointer points to X } // get-value general case char *end; *value = strtod(*pstr, &end); - if (end == *pstr) return STAT_BAD_NUMBER_FORMAT; // more robust test then checking for value=0; + // more robust test then checking for value == 0 + if (end == *pstr) return STAT_BAD_NUMBER_FORMAT; *pstr = end; - return STAT_OK; // pointer points to next character after the word + return STAT_OK; // pointer points to next character after the word } /// Isolate the decimal point value as an integer static uint8_t _point(float value) { - return (uint8_t)(value * 10 - trunc(value) * 10); // isolate the decimal point as an int + // isolate the decimal point as an int + return (uint8_t)(value * 10 - trunc(value) * 10); } /// Check for some gross Gcode block semantic violations static stat_t _validate_gcode_block() { - // Check for modal group violations. From NIST, section 3.4 "It is an error to put - // a G-code from group 1 and a G-code from group 0 on the same line if both of them - // use axis words. If an axis word-using G-code from group 1 is implicitly in effect - // on a line (by having been activated on an earlier line), and a group 0 G-code that - // uses axis words appears on the line, the activity of the group 1 G-code is suspended - // for that line. The axis word-using G-codes from group 0 are G10, G28, G30, and G92" - - // if ((gp.modals[MODAL_GROUP_G0] == true) && (gp.modals[MODAL_GROUP_G1] == true)) { - // return STAT_MODAL_GROUP_VIOLATION; - // } + // Check for modal group violations. From NIST, section 3.4 "It + // is an error to put a G-code from group 1 and a G-code from + // group 0 on the same line if both of them use axis words. If an + // axis word-using G-code from group 1 is implicitly in effect on + // a line (by having been activated on an earlier line), and a + // group 0 G-code that uses axis words appears on the line, the + // activity of the group 1 G-code is suspended for that line. The + // axis word-using G-codes from group 0 are G10, G28, G30, and G92" + + // if (gp.modals[MODAL_GROUP_G0] && gp.modals[MODAL_GROUP_G1]) + // return STAT_MODAL_GROUP_VIOLATION; // look for commands that require an axis word to be present - // if ((gp.modals[MODAL_GROUP_G0] == true) || (gp.modals[MODAL_GROUP_G1] == true)) { - // if (_axis_changed() == false) - // return STAT_GCODE_AXIS_IS_MISSING; - // } + // if (gp.modals[MODAL_GROUP_G0] || gp.modals[MODAL_GROUP_G1]) + // if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING; + return STAT_OK; } @@ -216,36 +229,41 @@ static stat_t _validate_gcode_block() { /* * Parses one line of 0 terminated G-Code. * - * All the parser does is load the state values in gn (next model state) and set flags - * in gf (model state flags). The execute routine applies them. The buffer is assumed to - * contain only uppercase characters and signed floats (no whitespace). + * All the parser does is load the state values in gn (next model + * state) and set flags in gf (model state flags). The execute + * routine applies them. The buffer is assumed to contain only + * uppercase characters and signed floats (no whitespace). * * A number of implicit things happen when the gn struct is zeroed: * - inverse feed rate mode is canceled - set back to units_per_minute mode */ static stat_t _parse_gcode_block(char *buf) { - char *pstr = (char *)buf; // persistent pointer into gcode block for parsing words - char letter; // parsed letter, eg.g. G or X or Y - float value = 0; // value parsed from letter (e.g. 2 for G2) + char *pstr = (char *)buf; // persistent pointer for parsing words + char letter; // parsed letter, eg.g. G or X or Y + float value = 0; // value parsed from letter (e.g. 2 for G2) stat_t status = STAT_OK; // set initial state for new move memset(&gp, 0, sizeof(gp)); // clear all parser values memset(&cm.gf, 0, sizeof(GCodeInput_t)); // clear all next-state flags memset(&cm.gn, 0, sizeof(GCodeInput_t)); // clear all next-state values - cm.gn.motion_mode = cm_get_motion_mode(MODEL); // get motion mode from previous block + // get motion mode from previous block + cm.gn.motion_mode = cm_get_motion_mode(MODEL); // extract commands and parameters - while((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { + while ((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { switch(letter) { case 'G': switch((uint8_t)value) { - case 0: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_TRAVERSE); - case 1: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_FEED); - case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC); - case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC); - case 4: SET_NON_MODAL(next_action, NEXT_ACTION_DWELL); - case 10: SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA); + case 0: + SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_TRAVERSE); + case 1: + SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_FEED); + case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CW_ARC); + case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CCW_ARC); + case 4: SET_NON_MODAL(next_action, NEXT_ACTION_DWELL); + case 10: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_COORD_DATA); case 17: SET_MODAL(MODAL_GROUP_G2, select_plane, CANON_PLANE_XY); case 18: SET_MODAL(MODAL_GROUP_G2, select_plane, CANON_PLANE_XZ); case 19: SET_MODAL(MODAL_GROUP_G2, select_plane, CANON_PLANE_YZ); @@ -253,8 +271,10 @@ static stat_t _parse_gcode_block(char *buf) { case 21: SET_MODAL(MODAL_GROUP_G6, units_mode, MILLIMETERS); case 28: switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION); - case 1: SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION); + case 0: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G28_POSITION); + case 1: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G28_POSITION); case 2: SET_NON_MODAL(next_action, NEXT_ACTION_SEARCH_HOME); case 3: SET_NON_MODAL(next_action, NEXT_ACTION_SET_ABSOLUTE_ORIGIN); case 4: SET_NON_MODAL(next_action, NEXT_ACTION_HOMING_NO_SET); @@ -264,8 +284,10 @@ static stat_t _parse_gcode_block(char *buf) { case 30: switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION); - case 1: SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION); + case 0: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_GOTO_G30_POSITION); + case 1: + SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_G30_POSITION); default: status = STAT_GCODE_COMMAND_UNSUPPORTED; } break; @@ -295,7 +317,8 @@ static stat_t _parse_gcode_block(char *buf) { break; case 64: SET_MODAL(MODAL_GROUP_G13,path_control, PATH_CONTINUOUS); - case 80: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_CANCEL_MOTION_MODE); + case 80: SET_MODAL(MODAL_GROUP_G1, motion_mode, + MOTION_MODE_CANCEL_MOTION_MODE); // case 90: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); // case 91: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); case 90: @@ -316,7 +339,8 @@ static stat_t _parse_gcode_block(char *buf) { case 92: switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G0, next_action, NEXT_ACTION_SET_ORIGIN_OFFSETS); + case 0: SET_MODAL(MODAL_GROUP_G0, next_action, + NEXT_ACTION_SET_ORIGIN_OFFSETS); case 1: SET_NON_MODAL(next_action, NEXT_ACTION_RESET_ORIGIN_OFFSETS); case 2: SET_NON_MODAL(next_action, NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS); case 3: SET_NON_MODAL(next_action, NEXT_ACTION_RESUME_ORIGIN_OFFSETS); @@ -326,7 +350,8 @@ static stat_t _parse_gcode_block(char *buf) { case 93: SET_MODAL(MODAL_GROUP_G5, feed_rate_mode, INVERSE_TIME_MODE); case 94: SET_MODAL(MODAL_GROUP_G5, feed_rate_mode, UNITS_PER_MINUTE_MODE); - // case 95: SET_MODAL(MODAL_GROUP_G5, feed_rate_mode, UNITS_PER_REVOLUTION_MODE); + // case 95: + // SET_MODAL(MODAL_GROUP_G5, feed_rate_mode, UNITS_PER_REVOLUTION_MODE); default: status = STAT_GCODE_COMMAND_UNSUPPORTED; } break; @@ -346,15 +371,16 @@ static stat_t _parse_gcode_block(char *buf) { case 9: SET_MODAL(MODAL_GROUP_M8, flood_coolant, false); case 48: SET_MODAL(MODAL_GROUP_M9, override_enables, true); case 49: SET_MODAL(MODAL_GROUP_M9, override_enables, false); - case 50: SET_MODAL(MODAL_GROUP_M9, feed_rate_override_enable, true); // conditionally true - case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true); // conditionally true + case 50: SET_MODAL(MODAL_GROUP_M9, feed_rate_override_enable, true); + case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true); default: status = STAT_MCODE_COMMAND_UNSUPPORTED; } break; case 'T': SET_NON_MODAL(tool_select, (uint8_t)trunc(value)); case 'F': SET_NON_MODAL(feed_rate, value); - case 'P': SET_NON_MODAL(parameter, value); // used for dwell time, G10 coord select, rotations + // used for dwell time, G10 coord select, rotations + case 'P': SET_NON_MODAL(parameter, value); case 'S': SET_NON_MODAL(spindle_speed, value); case 'X': SET_NON_MODAL(target[AXIS_X], value); case 'Y': SET_NON_MODAL(target[AXIS_Y], value); @@ -369,14 +395,16 @@ static stat_t _parse_gcode_block(char *buf) { case 'J': SET_NON_MODAL(arc_offset[1], value); case 'K': SET_NON_MODAL(arc_offset[2], value); case 'R': SET_NON_MODAL(arc_radius, value); - case 'N': SET_NON_MODAL(linenum,(uint32_t)value); // line number - case 'L': break; // not used for anything + case 'N': SET_NON_MODAL(linenum,(uint32_t)value); // line number + case 'L': break; // not used for anything + case 0: break; default: status = STAT_GCODE_COMMAND_UNSUPPORTED; } + if (status != STAT_OK) break; } - if ((status != STAT_OK) && (status != STAT_COMPLETE)) return status; + if (status != STAT_OK && status != STAT_COMPLETE) return status; ritorno(_validate_gcode_block()); return _execute_gcode_block(); // if successful execute the block @@ -386,40 +414,40 @@ static stat_t _parse_gcode_block(char *buf) { /* * Execute parsed block * - * Conditionally (based on whether a flag is set in gf) call the canonical - * machining functions in order of execution as per RS274NGC_3 table 8 - * (below, with modifications): + * Conditionally (based on whether a flag is set in gf) call the canonical + * machining functions in order of execution as per RS274NGC_3 table 8 + * (below, with modifications): * - * 0. record the line number - * 1. comment (includes message) [handled during block normalization] - * 2. set feed rate mode (G93, G94 - inverse time or per minute) - * 3. set feed rate (F) - * 3a. set feed override rate (M50.1) - * 3a. set traverse override rate (M50.2) - * 4. set spindle speed (S) - * 4a. set spindle override rate (M51.1) - * 5. select tool (T) - * 6. change tool (M6) - * 7. spindle on or off (M3, M4, M5) - * 8. coolant on or off (M7, M8, M9) - * 9. enable or disable overrides (M48, M49, M50, M51) - * 10. dwell (G4) - * 11. set active plane (G17, G18, G19) - * 12. set length units (G20, G21) - * 13. cutter radius compensation on or off (G40, G41, G42) - * 14. cutter length compensation on or off (G43, G49) - * 15. coordinate system selection (G54, G55, G56, G57, G58, G59) - * 16. set path control mode (G61, G61.1, G64) - * 17. set distance mode (G90, G91) - * 18. set retract mode (G98, G99) - * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) - * 19b. update system data (G10) - * 19c. set axis offsets (G92, G92.1, G92.2, G92.3) - * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 - * 21. stop and end (M0, M1, M2, M30, M60) + * 0. record the line number + * 1. comment (includes message) [handled during block normalization] + * 2. set feed rate mode (G93, G94 - inverse time or per minute) + * 3. set feed rate (F) + * 3a. set feed override rate (M50.1) + * 3a. set traverse override rate (M50.2) + * 4. set spindle speed (S) + * 4a. set spindle override rate (M51.1) + * 5. select tool (T) + * 6. change tool (M6) + * 7. spindle on or off (M3, M4, M5) + * 8. coolant on or off (M7, M8, M9) + * 9. enable or disable overrides (M48, M49, M50, M51) + * 10. dwell (G4) + * 11. set active plane (G17, G18, G19) + * 12. set length units (G20, G21) + * 13. cutter radius compensation on or off (G40, G41, G42) + * 14. cutter length compensation on or off (G43, G49) + * 15. coordinate system selection (G54, G55, G56, G57, G58, G59) + * 16. set path control mode (G61, G61.1, G64) + * 17. set distance mode (G90, G91) + * 18. set retract mode (G98, G99) + * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) + * 19b. update system data (G10) + * 19c. set axis offsets (G92, G92.1, G92.2, G92.3) + * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 + * 21. stop and end (M0, M1, M2, M30, M60) * - * Values in gn are in original units and should not be unit converted prior - * to calling the canonical functions (which do the unit conversions) + * Values in gn are in original units and should not be unit converted prior + * to calling the canonical functions (which do the unit conversions) */ static stat_t _execute_gcode_block() { stat_t status = STAT_OK; @@ -431,18 +459,21 @@ static stat_t _execute_gcode_block() { EXEC_FUNC(cm_traverse_override_factor, traverse_override_factor); EXEC_FUNC(cm_set_spindle_speed, spindle_speed); EXEC_FUNC(cm_spindle_override_factor, spindle_override_factor); - EXEC_FUNC(cm_select_tool, tool_select); // tool_select is where it's written + // tool_select is where it's written + EXEC_FUNC(cm_select_tool, tool_select); EXEC_FUNC(cm_change_tool, tool_change); - EXEC_FUNC(cm_spindle_control, spindle_mode); // spindle on or off + EXEC_FUNC(cm_spindle_control, spindle_mode); // spindle on or off EXEC_FUNC(cm_mist_coolant_control, mist_coolant); - EXEC_FUNC(cm_flood_coolant_control, flood_coolant); // also disables mist coolant if OFF + // also disables mist coolant if OFF + EXEC_FUNC(cm_flood_coolant_control, flood_coolant); EXEC_FUNC(cm_feed_rate_override_enable, feed_rate_override_enable); EXEC_FUNC(cm_traverse_override_enable, traverse_override_enable); EXEC_FUNC(cm_spindle_override_enable, spindle_override_enable); EXEC_FUNC(cm_override_enables, override_enables); - if (cm.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell - ritorno(cm_dwell(cm.gn.parameter)); // return if error, otherwise complete the block + if (cm.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell + // return if error, otherwise complete the block + ritorno(cm_dwell(cm.gn.parameter)); EXEC_FUNC(cm_select_plane, select_plane); EXEC_FUNC(cm_set_units_mode, units_mode); @@ -454,37 +485,69 @@ static stat_t _execute_gcode_block() { //--> set retract mode goes here switch (cm.gn.next_action) { - case NEXT_ACTION_SET_G28_POSITION: status = cm_set_g28_position(); break; // G28.1 - case NEXT_ACTION_GOTO_G28_POSITION: status = cm_goto_g28_position(cm.gn.target, cm.gf.target); break; // G28 - case NEXT_ACTION_SET_G30_POSITION: status = cm_set_g30_position(); break; // G30.1 - case NEXT_ACTION_GOTO_G30_POSITION: status = cm_goto_g30_position(cm.gn.target, cm.gf.target); break; // G30 - - case NEXT_ACTION_SEARCH_HOME: status = cm_homing_cycle_start(); break; // G28.2 - case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: status = cm_set_absolute_origin(cm.gn.target, cm.gf.target); break; // G28.3 - case NEXT_ACTION_HOMING_NO_SET: status = cm_homing_cycle_start_no_set(); break; // G28.4 - - case NEXT_ACTION_STRAIGHT_PROBE: status = cm_straight_probe(cm.gn.target, cm.gf.target); break; // G38.2 - - case NEXT_ACTION_SET_COORD_DATA: status = cm_set_coord_offsets(cm.gn.parameter, cm.gn.target, cm.gf.target); break; - case NEXT_ACTION_SET_ORIGIN_OFFSETS: status = cm_set_origin_offsets(cm.gn.target, cm.gf.target); break; - case NEXT_ACTION_RESET_ORIGIN_OFFSETS: status = cm_reset_origin_offsets(); break; - case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: status = cm_suspend_origin_offsets(); break; - case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: status = cm_resume_origin_offsets(); break; + case NEXT_ACTION_SET_G28_POSITION: // G28.1 + status = cm_set_g28_position(); + break; + case NEXT_ACTION_GOTO_G28_POSITION: // G28 + status = cm_goto_g28_position(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_SET_G30_POSITION: // G30.1 + status = cm_set_g30_position(); + break; + case NEXT_ACTION_GOTO_G30_POSITION: // G30 + status = cm_goto_g30_position(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_SEARCH_HOME: // G28.2 + status = cm_homing_cycle_start(); + break; + case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3 + status = cm_set_absolute_origin(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_HOMING_NO_SET: // G28.4 + status = cm_homing_cycle_start_no_set(); + break; + case NEXT_ACTION_STRAIGHT_PROBE: // G38.2 + status = cm_straight_probe(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_SET_COORD_DATA: + status = cm_set_coord_offsets(cm.gn.parameter, cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_SET_ORIGIN_OFFSETS: + status = cm_set_origin_offsets(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_RESET_ORIGIN_OFFSETS: + status = cm_reset_origin_offsets(); + break; + case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: + status = cm_suspend_origin_offsets(); + break; + case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: + status = cm_resume_origin_offsets(); + break; case NEXT_ACTION_DEFAULT: - cm_set_absolute_override(MODEL, cm.gn.absolute_override); // apply override setting to gm struct + // apply override setting to gm struct + cm_set_absolute_override(MODEL, cm.gn.absolute_override); switch (cm.gn.motion_mode) { - case MOTION_MODE_CANCEL_MOTION_MODE: cm.gm.motion_mode = cm.gn.motion_mode; break; - case MOTION_MODE_STRAIGHT_TRAVERSE: status = cm_straight_traverse(cm.gn.target, cm.gf.target); break; - case MOTION_MODE_STRAIGHT_FEED: status = cm_straight_feed(cm.gn.target, cm.gf.target); break; + case MOTION_MODE_CANCEL_MOTION_MODE: + cm.gm.motion_mode = cm.gn.motion_mode; + break; + case MOTION_MODE_STRAIGHT_TRAVERSE: + status = cm_straight_traverse(cm.gn.target, cm.gf.target); + break; + case MOTION_MODE_STRAIGHT_FEED: + status = cm_straight_feed(cm.gn.target, cm.gf.target); + break; case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: // gf.radius sets radius mode if radius was collected in gn - status = cm_arc_feed(cm.gn.target, cm.gf.target, cm.gn.arc_offset[0], cm.gn.arc_offset[1], - cm.gn.arc_offset[2], cm.gn.arc_radius, cm.gn.motion_mode); + status = cm_arc_feed(cm.gn.target, cm.gf.target, cm.gn.arc_offset[0], + cm.gn.arc_offset[1], cm.gn.arc_offset[2], + cm.gn.arc_radius, cm.gn.motion_mode); break; } } - cm_set_absolute_override(MODEL, false); // un-set absolute override once the move is planned + // un-set absolute override once the move is planned + cm_set_absolute_override(MODEL, false); // do the program stops and ends : M0, M1, M2, M30, M60 if (cm.gf.program_flow == true) { diff --git a/src/gcode_parser.h b/src/gcode_parser.h index de632d1..fc97eab 100644 --- a/src/gcode_parser.h +++ b/src/gcode_parser.h @@ -4,17 +4,21 @@ * * Copyright (c) 2010 - 2014 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef GCODE_PARSER_H diff --git a/src/gpio.c b/src/gpio.c index 431e6ae..0178785 100644 --- a/src/gpio.c +++ b/src/gpio.c @@ -4,47 +4,53 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* - * This GPIO file is where all parallel port bits are managed that are - * not already taken up by steppers, serial ports, SPI or PDI programming + * This GPIO file is where all parallel port bits are managed that are + * not already taken up by steppers, serial ports, SPI or PDI programming * - * There are 2 GPIO ports: + * There are 2 GPIO ports: * - * gpio1 Located on 5x2 header next to the PDI programming plugs (on v7's) - * Four (4) output bits capable of driving 3.3v or 5v logic + * gpio1 Located on 5x2 header next to the PDI programming plugs (on v7's) + * Four (4) output bits capable of driving 3.3v or 5v logic * - * Note: On v6 and earlier boards there are also 4 inputs: - * Four (4) level converted input bits capable of being driven - * by 3.3v or 5v logic - connected to B0 - B3 (now used for SPI) + * Note: On v6 and earlier boards there are also 4 inputs: + * Four (4) level converted input bits capable of being driven + * by 3.3v or 5v logic - connected to B0 - B3 (now used for SPI) * - * gpio2 Located on 9x2 header on "bottom" edge of the board - * Eight (8) non-level converted input bits - * Eight (8) ground pins - one each "under" each input pin - * Two (2) 3.3v power pins (on left edge of connector) - * Inputs can be used as switch contact inputs or - * 3.3v input bits depending on port configuration - * **** These bits CANNOT be used as 5v inputs **** + * gpio2 Located on 9x2 header on "bottom" edge of the board + * Eight (8) non-level converted input bits + * Eight (8) ground pins - one each "under" each input pin + * Two (2) 3.3v power pins (on left edge of connector) + * Inputs can be used as switch contact inputs or + * 3.3v input bits depending on port configuration + * **** These bits CANNOT be used as 5v inputs **** */ #include "controller.h" diff --git a/src/gpio.h b/src/gpio.h index 0311c26..1b51b2c 100644 --- a/src/gpio.h +++ b/src/gpio.h @@ -4,25 +4,31 @@ * * Copyright (c) 2010 - 2014 Alden S. Hart Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef GPIO_H diff --git a/src/hardware.c b/src/hardware.c index 50055a4..416fde9 100644 --- a/src/hardware.c +++ b/src/hardware.c @@ -4,25 +4,31 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "hardware.h" @@ -67,12 +73,12 @@ void hardware_init() { /* * Get a human readable signature * - * Produce a unique deviceID based on the factory calibration data. - * Format is: 123456-ABC + * Produce a unique deviceID based on the factory calibration data. + * Format is: 123456-ABC * - * The number part is a direct readout of the 6 digit lot number - * The alpha is the low 5 bits of wafer number and XY coords in printable ASCII - * Refer to NVM_PROD_SIGNATURES_t in iox192a3.h for details. + * The number part is a direct readout of the 6 digit lot number + * The alpha is the low 5 bits of wafer number and XY coords in printable ASCII + * Refer to NVM_PROD_SIGNATURES_t in iox192a3.h for details. */ enum { LOTNUM0 = 8, // Lot Number Byte 0, ASCII @@ -93,7 +99,8 @@ void hw_get_id(char *id) { char printable[33] = "ABCDEFGHJKLMNPQRSTUVWXYZ23456789"; uint8_t i; - NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc; // Load NVM Command register to read the calibration row + // Load NVM Command register to read the calibration row + NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc; for (i = 0; i < 6; i++) id[i] = pgm_read_byte(LOTNUM0 + i); @@ -112,7 +119,8 @@ void hw_request_hard_reset() {cs.hard_reset_requested = true;} /// Hard reset using watchdog timer -void hw_hard_reset() { // software hard reset using the watchdog timer +/// software hard reset using the watchdog timer +void hw_hard_reset() { wdt_enable(WDTO_15MS); while (true); // loops for about 15ms then resets } @@ -120,8 +128,8 @@ void hw_hard_reset() { // software hard reset using the watchdog time /// Controller's rest handler stat_t hw_hard_reset_handler() { - if (cs.hard_reset_requested == false) return STAT_NOOP; - hw_hard_reset(); // hard reset - identical to hitting RESET button + if (!cs.hard_reset_requested) return STAT_NOOP; + hw_hard_reset(); // identical to hitting RESET button return STAT_EAGAIN; } @@ -131,11 +139,10 @@ void hw_request_bootloader() {cs.bootloader_requested = true;} stat_t hw_bootloader_handler() { - if (cs.bootloader_requested == false) - return STAT_NOOP; + if (!cs.bootloader_requested) return STAT_NOOP; cli(); CCPWrite(&RST.CTRL, RST_SWRST_bm); // fire a software reset - return STAT_EAGAIN; // never gets here but keeps the compiler happy + return STAT_EAGAIN; // never gets here but keeps the compiler happy } diff --git a/src/hardware.h b/src/hardware.h index c05e2fd..e330288 100644 --- a/src/hardware.h +++ b/src/hardware.h @@ -1,35 +1,41 @@ /* * hardware.h - system hardware configuration - * THIS FILE IS HARDWARE PLATFORM SPECIFIC - AVR Xmega version + * This file is hardware platform specific - AVR Xmega version * * This file is part of the TinyG project * * Copyright (c) 2013 - 2015 Alden S. Hart, Jr. * Copyright (c) 2013 - 2015 Robert Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* - * INTERRUPT USAGE - TinyG uses a lot of them all over the place + * Interrupt usage - TinyG uses a lot of them all over the place * * HI Stepper DDA pulse generation (set in stepper.h) * HI Stepper load routine SW interrupt (set in stepper.h) @@ -117,72 +123,47 @@ enum cfgPortBits { // motor control port bit positions SW_MAX_BIT_bp // bit 7 (4 input bits for homing/limit switches) }; -#define STEP_BIT_bm (1 << STEP_BIT_bp) -#define DIRECTION_BIT_bm (1 << DIRECTION_BIT_bp) -#define MOTOR_ENABLE_BIT_bm (1 << MOTOR_ENABLE_BIT_bp) -#define CHIP_SELECT_BIT_bm (1 << CHIP_SELECT_BIT_bp) -#define FAULT_BIT_bm (1 << FAULT_BIT_bp) -#define GPIO1_OUT_BIT_bm (1 << GPIO1_OUT_BIT_bp) // spindle and coolant -#define SW_MIN_BIT_bm (1 << SW_MIN_BIT_bp) // minimum switch inputs -#define SW_MAX_BIT_bm (1 << SW_MAX_BIT_bp) // maximum switch inputs +#define STEP_BIT_bm (1 << STEP_BIT_bp) +#define DIRECTION_BIT_bm (1 << DIRECTION_BIT_bp) +#define MOTOR_ENABLE_BIT_bm (1 << MOTOR_ENABLE_BIT_bp) +#define CHIP_SELECT_BIT_bm (1 << CHIP_SELECT_BIT_bp) +#define FAULT_BIT_bm (1 << FAULT_BIT_bp) +#define GPIO1_OUT_BIT_bm (1 << GPIO1_OUT_BIT_bp) // spindle and coolant +#define SW_MIN_BIT_bm (1 << SW_MIN_BIT_bp) // minimum switch inputs +#define SW_MAX_BIT_bm (1 << SW_MAX_BIT_bp) // maximum switch inputs // Bit assignments for GPIO1_OUTs for spindle, PWM and coolant -#define SPINDLE_BIT 0x08 // spindle on/off -#define SPINDLE_DIR 0x04 // spindle direction, 1=CW, 0=CCW -#define SPINDLE_PWM 0x02 // spindle PWMs output bit -#define MIST_COOLANT_BIT 0x01 // coolant on/off (same as flood) -#define FLOOD_COOLANT_BIT 0x01 // coolant on/off (same as mist) +#define SPINDLE_BIT 8 // spindle on/off +#define SPINDLE_DIR 4 // spindle direction, 1=CW, 0=CCW +#define SPINDLE_PWM 2 // spindle PWMs output bit +#define MIST_COOLANT_BIT 1 // coolant on/off (same as flood) +#define FLOOD_COOLANT_BIT 1 // coolant on/off (same as mist) -#define SPINDLE_LED 0 -#define SPINDLE_DIR_LED 1 -#define SPINDLE_PWM_LED 2 -#define COOLANT_LED 3 +#define SPINDLE_LED 0 +#define SPINDLE_DIR_LED 1 +#define SPINDLE_PWM_LED 2 +#define COOLANT_LED 3 // Can use the spindle direction as an indicator LED -#define INDICATOR_LED SPINDLE_DIR_LED - -// Timer assignments - see specific modules for details) -#define TIMER_DDA TCC0 // DDA timer (see stepper.h) -#define TIMER_DWELL TCD0 // Dwell timer (see stepper.h) -#define TIMER_LOAD TCE0 // Loader time (see stepper.h) -#define TIMER_EXEC TCF0 // Exec timer (see stepper.h) -#define TIMER_TMC2660 TCC1 // TMC2660 timer (see tmc2660.h) -#define TIMER_PWM1 TCD1 // PWM timer #1 (see pwm.c) -#define TIMER_PWM2 TCE1 // PWM timer #2 (see pwm.c) +#define INDICATOR_LED SPINDLE_DIR_LED + +// Timer assignments - see specific modules for details +#define TIMER_DDA TCC0 // DDA timer (see stepper.h) +#define TIMER_TMC2660 TCC1 // TMC2660 timer (see tmc2660.h) +#define TIMER_PWM1 TCD1 // PWM timer #1 (see pwm.c) +#define TIMER_PWM2 TCE1 // PWM timer #2 (see pwm.c) +#define TIMER_MOTOR1 TCD0 +#define TIMER_MOTOR2 TCE0 +#define TIMER_MOTOR3 TCF0 +#define TIMER_MOTOR4 TCF1 // Timer setup for stepper and dwells -#define FREQUENCY_DDA (float)50000 // DDA frequency in hz. -#define FREQUENCY_DWELL (float)10000 // Dwell count frequency in hz. -#define LOAD_TIMER_PERIOD 100 // cycles you have to shut off SW interrupt -#define EXEC_TIMER_PERIOD 100 // cycles you have to shut off SW interrupt - -#define STEP_TIMER_TYPE TC0_struct // stepper subsybstem uses all the TC0's -#define STEP_TIMER_DISABLE 0 // turn timer off (clock = 0 Hz) -#define STEP_TIMER_ENABLE 1 // turn timer clock on (F_CPU = 32 Mhz) -#define STEP_TIMER_WGMODE 0 // normal mode (count to TOP and rollover) - -#define LOAD_TIMER_DISABLE 0 // turn load timer off (clock = 0 Hz) -#define LOAD_TIMER_ENABLE 1 // turn load timer clock on (F_CPU = 32 Mhz) -#define LOAD_TIMER_WGMODE 0 // normal mode (count to TOP and rollover) - -#define EXEC_TIMER_DISABLE 0 // turn exec timer off (clock = 0 Hz) -#define EXEC_TIMER_ENABLE 1 // turn exec timer clock on (F_CPU = 32 Mhz) -#define EXEC_TIMER_WGMODE 0 // normal mode (count to TOP and rollover) - -#define TIMER_DDA_ISR_vect TCC0_OVF_vect -#define TIMER_DWELL_ISR_vect TCD0_OVF_vect -#define TIMER_LOAD_ISR_vect TCE0_OVF_vect -#define TIMER_EXEC_ISR_vect TCF0_OVF_vect - -#define TIMER_OVFINTLVL_HI 3 // timer interrupt level (3=hi) -#define TIMER_OVFINTLVL_MED 2 // timer interrupt level (2=med) -#define TIMER_OVFINTLVL_LO 1 // timer interrupt level (1=lo) - -#define TIMER_DDA_INTLVL TIMER_OVFINTLVL_HI -#define TIMER_DWELL_INTLVL TIMER_OVFINTLVL_HI -#define TIMER_LOAD_INTLVL TIMER_OVFINTLVL_HI -#define TIMER_EXEC_INTLVL TIMER_OVFINTLVL_LO - +#define FREQUENCY_DDA 50000 // DDA frequency in hz. +#define STEP_TIMER_DISABLE 0 // turn timer off +#define STEP_TIMER_ENABLE 1 // turn timer clock on +#define STEP_TIMER_WGMODE 0 // normal mode (count to TOP and rollover) +#define TIMER_DDA_ISR_vect TCC0_OVF_vect +#define TIMER_DDA_INTLVL 3 // Timer overflow HI /* Device singleton - global structure to allow iteration through similar devices @@ -190,23 +171,18 @@ enum cfgPortBits { // motor control port bit positions Ports are shared between steppers and GPIO so we need a global struct. Each xmega port has 3 bindings; motors, switches and the output bit - The initialization sequence is important. the order is: - - sys_init() binds all ports to the device struct - - st_init() sets IO directions and sets stepper VPORTS and stepper - specific functions - Care needs to be taken in routines that use ports not to write to bits that are not assigned to the designated function - ur unpredicatable results will occur. */ typedef struct { - PORT_t *st_port[MOTORS]; // bindings for stepper motor ports (stepper.c) - PORT_t *sw_port[MOTORS]; // bindings for switch ports (GPIO2) - PORT_t *out_port[MOTORS]; // bindings for output ports (GPIO1) + PORT_t *st_port[MOTORS]; // bindings for stepper motor ports (stepper.c) + PORT_t *sw_port[MOTORS]; // bindings for switch ports (GPIO2) + PORT_t *out_port[MOTORS]; // bindings for output ports (GPIO1) } hwSingleton_t; hwSingleton_t hw; -void hardware_init(); // master hardware init +void hardware_init(); void hw_get_id(char *id); void hw_request_hard_reset(); void hw_hard_reset(); diff --git a/src/main.c b/src/main.c index 9793e76..782da0d 100644 --- a/src/main.c +++ b/src/main.c @@ -5,17 +5,21 @@ * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * Copyright (c) 2013 - 2015 Robert Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "hardware.h" @@ -39,10 +43,10 @@ static void init() { // There are a lot of dependencies in the order of these inits. // Don't change the ordering unless you understand this. - cli(); + cli(); // disable global interrupts // do these first - hardware_init(); // system hardware setup - must be first + hardware_init(); // system hardware setup - must be first usart_init(); // serial port // do these next @@ -54,7 +58,7 @@ static void init() { controller_init(); // must be first app init planner_init(); // motion planning subsystem - canonical_machine_init(); // canonical machine - must follow config_init() + canonical_machine_init(); // must follow config_init() // set vector location to application, as opposed to boot ROM uint8_t temp = PMIC.CTRL & ~PMIC_IVSEL_bm; @@ -64,7 +68,7 @@ static void init() { // all levels are used, so don't bother to abstract them PMIC.CTRL |= PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm; - sei(); // enable global interrupts + sei(); // enable global interrupts fprintf_P(stderr, PSTR("TinyG firmware\n")); } @@ -74,7 +78,7 @@ int main() { init(); // main loop - while (1) controller_run(); // single pass through the controller + while (1) controller_run(); // single pass through the controller return 0; } diff --git a/src/plan/arc.c b/src/plan/arc.c index 5e893b2..e581951 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -4,22 +4,27 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/* This module actually contains some parts that belong ion the canonical machine, - * and other parts that belong at the motion planner level, but the whole thing is - * treated as if it were part of the motion planner. +/* This module actually contains some parts that belong ion the + * canonical machine, * and other parts that belong at the motion planner + * level, but the whole thing is * treated as if it were part of the + * motion planner. */ #include "arc.h" @@ -81,21 +86,25 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints if (radius_f) { // must have at least one endpoint specified - if (!(target_x || target_y)) return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + if (!(target_x || target_y)) + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; - } else if (offset_k) // center format arc tests, it's OK to be missing either or both i and j, but error if k is present + } else if (offset_k) + // center format arc tests, it's OK to be missing either or both i and j, + // but error if k is present return STAT_ARC_SPECIFICATION_ERROR; - } else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18 + } else if (cm.gm.select_plane == CANON_PLANE_XZ) { // G18 arc.plane_axis_0 = AXIS_X; arc.plane_axis_1 = AXIS_Z; arc.linear_axis = AXIS_Y; if (radius_f) { - if (!(target_x || target_z)) return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; + if (!(target_x || target_z)) + return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; } else if (offset_j) return STAT_ARC_SPECIFICATION_ERROR; - } else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19 + } else if (cm.gm.select_plane == CANON_PLANE_YZ) { // G19 arc.plane_axis_0 = AXIS_Y; arc.plane_axis_1 = AXIS_Z; arc.linear_axis = AXIS_X; @@ -149,8 +158,8 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints /* * Generate an arc * - * cm_arc_callback() is called from the controller main loop. Each time it's called it - * queues as many arc segments (lines) as it can before it blocks, then returns. + * Called from the controller main loop. Each time it's called it queues + * as many arc segments (lines) as it can before it blocks, then returns. * * Parts of this routine were originally sourced from the grbl project. */ @@ -256,7 +265,7 @@ static stat_t _compute_arc() { // arc.length is the total mm of travel of the helix (or just a planar arc) arc.linear_travel = arc.gm.target[arc.linear_axis] - arc.position[arc.linear_axis]; arc.planar_travel = arc.angular_travel * arc.radius; - arc.length = hypotf(arc.planar_travel, arc.linear_travel); // NB: hypot is insensitive to +/- signs + arc.length = hypotf(arc.planar_travel, arc.linear_travel); // hypot is insensitive to +/- signs _estimate_arc_time(); // get an estimate of execution time to inform arc_segment calculation // Find the minimum number of arc_segments that meets these constraints... diff --git a/src/plan/arc.h b/src/plan/arc.h index 2815dcd..78f252c 100644 --- a/src/plan/arc.h +++ b/src/plan/arc.h @@ -4,17 +4,21 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef PLAN_ARC_H @@ -22,10 +26,13 @@ #include "canonical_machine.h" -// Arc radius tests. See http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc -#define ARC_RADIUS_ERROR_MAX ((float)1.0) // max allowable mm between start and end radius -#define ARC_RADIUS_ERROR_MIN ((float)0.005) // min mm where 1% rule applies -#define ARC_RADIUS_TOLERANCE ((float)0.001) // 0.1% radius variance test +// Arc radius tests. +// See http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc + +/// max allowable mm between start and end radius +#define ARC_RADIUS_ERROR_MAX ((float)1.0) +#define ARC_RADIUS_ERROR_MIN ((float)0.005) // min mm where 1% rule applies +#define ARC_RADIUS_TOLERANCE ((float)0.001) // 0.1% radius variance test // See planner.h for MM_PER_ARC_SEGMENT and other arc setting #defines @@ -43,7 +50,7 @@ typedef struct arArcSingleton { // persistent planner and runtime variables float linear_travel; // travel along linear axis of arc float planar_travel; uint8_t full_circle; // set true if full circle arcs specified - uint32_t rotations; // Number of full rotations for full circles (P value) + uint32_t rotations; // Full rotations for full circles (P value) uint8_t plane_axis_0; // arc plane axis 0 - e.g. X for G17 uint8_t plane_axis_1; // arc plane axis 1 - e.g. Y for G17 @@ -54,10 +61,10 @@ typedef struct arArcSingleton { // persistent planner and runtime variables int32_t arc_segment_count; // count of running segments float arc_segment_theta; // angular motion per segment float arc_segment_linear_travel; // linear motion per segment - float center_0; // center of circle at plane axis 0 (e.g. X for G17) - float center_1; // center of circle at plane axis 1 (e.g. Y for G17) + float center_0; // center of circle at plane axis 0 (e.g. X for G17) + float center_1; // center of circle at plane axis 1 (e.g. Y for G17) - GCodeState_t gm; // Gcode state struct is passed for each arc segment. Usage: + GCodeState_t gm; // state struct is passed for each arc segment. } arc_t; extern arc_t arc; diff --git a/src/plan/buffer.c b/src/plan/buffer.c new file mode 100644 index 0000000..c4dc99c --- /dev/null +++ b/src/plan/buffer.c @@ -0,0 +1,210 @@ +/* + * buffer.c - Planner buffers + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2015 Rob Giseburt + * + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* Planner buffers are used to queue and operate on Gcode blocks. Each + * buffer contains one Gcode block which may be a move, and M code, or + * other command that must be executed synchronously with movement. + * + * Buffers are in a circularly linked list managed by a WRITE pointer + * and a RUN pointer. New blocks are populated by (1) getting a write + * buffer, (2) populating the buffer, then (3) placing it in the queue + * (queue write buffer). If an exception occurs during population you + * can unget the write buffer before queuing it, which returns it to + * the pool of available buffers. + * + * The RUN buffer is the buffer currently executing. It may be + * retrieved once for simple commands, or multiple times for + * long-running commands like moves. When the command is complete the + * run buffer is returned to the pool by freeing it. + * + * Notes: + * The write buffer pointer only moves forward on _queue_write_buffer, and + * the read buffer pointer only moves forward on free_read calls. + * (test, get and unget have no effect) + */ + +#include "planner.h" +#include "stepper.h" + +#include + +/// buffer incr & wrap +#define _bump(a) ((a < PLANNER_BUFFER_POOL_SIZE - 1) ? a + 1 : 0) + + +/// Returns # of available planner buffers +uint8_t mp_get_planner_buffers_available() {return mb.buffers_available;} + + +/// Initializes or resets buffers +void mp_init_buffers() { + mpBuf_t *pv; + + memset(&mb, 0, sizeof(mb)); // clear all values, pointers and status + + mb.w = &mb.bf[0]; // init write and read buffer pointers + mb.q = &mb.bf[0]; + mb.r = &mb.bf[0]; + pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE - 1]; + + // setup ring pointers + for (uint8_t i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { + mb.bf[i].nx = &mb.bf[_bump(i)]; + mb.bf[i].pv = pv; + pv = &mb.bf[i]; + } + + mb.buffers_available = PLANNER_BUFFER_POOL_SIZE; +} + + +/// Get pointer to next available write buffer +/// Returns pointer or 0 if no buffer available. +mpBuf_t *mp_get_write_buffer() { + // get & clear a buffer + if (mb.w->buffer_state == MP_BUFFER_EMPTY) { + mpBuf_t *w = mb.w; + mpBuf_t *nx = mb.w->nx; // save linked list pointers + mpBuf_t *pv = mb.w->pv; + memset(mb.w, 0, sizeof(mpBuf_t)); // clear all values + w->nx = nx; // restore pointers + w->pv = pv; + w->buffer_state = MP_BUFFER_LOADING; + mb.buffers_available--; + mb.w = w->nx; + + return w; + } + + return 0; +} + + +/// Free write buffer if you decide not to commit it. +void mp_unget_write_buffer() { + mb.w = mb.w->pv; // queued --> write + mb.w->buffer_state = MP_BUFFER_EMPTY; // not loading anymore + mb.buffers_available++; +} + + +/* Commit the next write buffer to the queue + * Advances write pointer & changes buffer state + * + * WARNING: The routine calling mp_commit_write_buffer() must not use the write + * buffer once it has been queued. Action may start on the buffer immediately, + * invalidating its contents + */ +void mp_commit_write_buffer(const uint8_t move_type) { + mb.q->move_type = 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 +} + + +/* Get pointer to the next or current run buffer + * Returns a new run buffer if prev buf was ENDed + * Returns same buf if called again before ENDing + * Returns 0 if no buffer available + * The behavior supports continuations (iteration) + */ +mpBuf_t *mp_get_run_buffer() { + // CASE: fresh buffer; becomes running if queued or pending + if ((mb.r->buffer_state == MP_BUFFER_QUEUED) || + (mb.r->buffer_state == MP_BUFFER_PENDING)) + mb.r->buffer_state = MP_BUFFER_RUNNING; + + // CASE: asking for the same run buffer for the Nth time + if (mb.r->buffer_state == MP_BUFFER_RUNNING) + return mb.r; // return same buffer + + return 0; // CASE: no queued buffers. fail it. +} + + +/* Release the run buffer & return to buffer pool. + * Returns true if queue is empty, false otherwise. + * This is useful for doing queue empty / end move functions. + */ +uint8_t mp_free_run_buffer() { // EMPTY current run buf & adv to next + mp_clear_buffer(mb.r); // clear it out (& reset replannable) + mb.r = mb.r->nx; // advance to next run buffer + + if (mb.r->buffer_state == MP_BUFFER_QUEUED) // only if queued... + mb.r->buffer_state = MP_BUFFER_PENDING; // pend next buffer + + mb.buffers_available++; + + return mb.w == mb.r; // return true if the queue emptied +} + + +/// Returns pointer to first buffer, i.e. the running block +mpBuf_t *mp_get_first_buffer() { + return mp_get_run_buffer(); // returns buffer or 0 if nothing's running +} + + +/// Returns pointer to last buffer, i.e. last block (zero) +mpBuf_t *mp_get_last_buffer() { + mpBuf_t *bf = mp_get_run_buffer(); + mpBuf_t *bp; + + for (bp = bf; bp && bp->nx != bf; bp = mp_get_next_buffer(bp)) + if (bp->nx->move_state == MOVE_OFF) break; + + return bp; +} + + +/// Zeroes the contents of the buffer +void mp_clear_buffer(mpBuf_t *bf) { + mpBuf_t *nx = bf->nx; // save pointers + mpBuf_t *pv = bf->pv; + memset(bf, 0, sizeof(mpBuf_t)); + bf->nx = nx; // restore pointers + bf->pv = pv; +} + + +/// Copies the contents of bp into bf - preserves links +void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp) { + mpBuf_t *nx = bf->nx; // save pointers + mpBuf_t *pv = bf->pv; + memcpy(bf, bp, sizeof(mpBuf_t)); + bf->nx = nx; // restore pointers + bf->pv = pv; +} diff --git a/src/plan/command.c b/src/plan/command.c new file mode 100644 index 0000000..dd8fe15 --- /dev/null +++ b/src/plan/command.c @@ -0,0 +1,103 @@ +/* + * command.c + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2015 Rob Giseburt + * + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* How this works: + * - The command is called by the Gcode interpreter (cm_, e.g. an M + * code) + * - cm_ function calls mp_queue_command which puts it in the planning queue + * (bf buffer). + * This involves setting some parameters and registering a callback to the + * execution function in the canonical machine + * - the planning queue gets to the function and calls _exec_command() + * - ...which puts a pointer to the bf buffer in the prep struct (st_pre) + * - When the runtime gets to the end of the current activity (sending steps, + * counting a dwell) + * if executes mp_runtime_command... + * - ...which uses the callback function in the bf and the saved parameters in + * the vectors + * - To finish up mp_runtime_command() needs to free the bf buffer + * + * Doing it this way instead of synchronizing on queue empty simplifies the + * handling of feedholds, feed overrides, buffer flushes, and thread blocking, + * and makes keeping the queue full much easier - therefore avoiding starvation + */ + +#include "planner.h" +#include "canonical_machine.h" +#include "stepper.h" + + +#define spindle_speed move_time // local alias for spindle_speed to time var +#define value_vector gm.target // alias for vector of values +#define flag_vector unit // alias for vector of flags + + +/// callback to execute command +static stat_t _exec_command(mpBuf_t *bf) { + st_prep_command(bf); + return STAT_OK; +} + + +/// Queue a synchronous Mcode, program control, or other command +void mp_queue_command(void(*cm_exec)(float[], float[]), float *value, + float *flag) { + mpBuf_t *bf; + + // Never supposed to fail as buffer availability was checked upstream in the + // controller + if (!(bf = mp_get_write_buffer())) { + cm_hard_alarm(STAT_BUFFER_FULL_FATAL); + return; + } + + bf->move_type = MOVE_TYPE_COMMAND; + bf->bf_func = _exec_command; // callback to planner queue exec function + bf->cm_func = cm_exec; // callback to canonical machine exec function + + for (uint8_t axis = AXIS_X; axis < AXES; axis++) { + bf->value_vector[axis] = value[axis]; + bf->flag_vector[axis] = flag[axis]; + } + + // must be final operation before exit + mp_commit_write_buffer(MOVE_TYPE_COMMAND); +} + + +void mp_runtime_command(mpBuf_t *bf) { + bf->cm_func(bf->value_vector, bf->flag_vector); // 2 vectors used by callbacks + + // free buffer & perform cycle_end if planner is empty + if (mp_free_run_buffer()) cm_cycle_end(); +} diff --git a/src/plan/dwell.c b/src/plan/dwell.c new file mode 100644 index 0000000..97d2267 --- /dev/null +++ b/src/plan/dwell.c @@ -0,0 +1,69 @@ +/* + * dwell.c + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2015 Rob Giseburt + * + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "planner.h" +#include "canonical_machine.h" +#include "stepper.h" + + +// Dwells are performed by passing a dwell move to the stepper drivers. +// When the stepper driver sees a dwell it times the dwell on a separate +// timer than the stepper pulse timer. + + +/// Dwell execution +static stat_t _exec_dwell(mpBuf_t *bf) { + // convert seconds to uSec + st_prep_dwell((uint32_t)(bf->gm.move_time * 1000000)); + // free buffer & perform cycle_end if planner is empty + if (mp_free_run_buffer()) cm_cycle_end(); + + return STAT_OK; +} + + +/// Queue a dwell +stat_t mp_dwell(float seconds) { + mpBuf_t *bf; + + if (!(bf = mp_get_write_buffer())) // get write buffer + return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // never supposed to fail + + bf->bf_func = _exec_dwell; // register callback to dwell start + bf->gm.move_time = seconds; // in seconds, not minutes + bf->move_state = MOVE_NEW; + // must be final operation before exit + mp_commit_write_buffer(MOVE_TYPE_DWELL); + + return STAT_OK; +} diff --git a/src/plan/exec.c b/src/plan/exec.c index 4a116cb..a3fd8f9 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -43,7 +43,7 @@ #include #include -// Execute routines (NB: These are all called from the LO interrupt) +// Execute routines. Called from the LO interrupt static stat_t _exec_aline_head(); static stat_t _exec_aline_body(); static stat_t _exec_aline_tail(); @@ -54,12 +54,12 @@ static void _init_forward_diffs(float Vi, float Vt); #endif -/// Dequeues the buffer queue and executes the move continuations. -/// Manages run buffers and other details +/// Dequeues buffer and executes move continuations. Manages run buffers and +/// other details. stat_t mp_exec_move() { mpBuf_t *bf; - if ((bf = mp_get_run_buffer()) == 0) { // 0 means nothing's running + if (!(bf = mp_get_run_buffer())) { // 0 means nothing's running st_prep_null(); return STAT_NOOP; } @@ -78,97 +78,106 @@ stat_t mp_exec_move() { /* Aline execution routines * - * ---> Everything here fires from interrupts and must be interrupt safe - * - * _exec_aline() - acceleration line main routine - * _exec_aline_head() - helper for acceleration section - * _exec_aline_body() - helper for cruise section - * _exec_aline_tail() - helper for deceleration section - * _exec_aline_segment() - helper for running a segment - * - * Returns: - * 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 - * - * This routine is called from the (LO) interrupt level. The interrupt - * sequencing relies on the behaviors of the routines being exactly correct. - * Each call to _exec_aline() must execute and prep *one and only one* - * segment. If the segment is the not the last segment in the bf buffer the - * _aline() must return STAT_EAGAIN. If it's the last segment it must return - * STAT_OK. If it encounters a fatal error that would terminate the move it - * should return a valid error code. Failure to obey this will introduce - * subtle and very difficult to diagnose bugs (trust me on this). - * - * Note 1 Returning STAT_OK ends the move and frees the bf buffer. - * Returning STAT_OK at this point does NOT advance position meaning any - * position error will be compensated by the next move. - * - * Note 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. - */ -/* Operation: - * Aline generates jerk-controlled S-curves as per Ed Red's course notes: - * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf - * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations - * - * A full trapezoid is divided into 5 periods Periods 1 and 2 are the - * first and second halves of the acceleration ramp (the concave and convex - * parts of the S curve in the "head"). Periods 3 and 4 are the first - * and second parts of the deceleration ramp (the tail). There is also - * a period for the constant-velocity plateau of the trapezoid (the body). - * There are various degraded trapezoids possible, including 2 section - * combinations (head and tail; head and body; body and tail), and single - * sections - any one of the three. - * - * The equations that govern the acceleration and deceleration ramps are: - * - * Period 1 V = Vi + Jm*(T^2)/2 - * Period 2 V = Vh + As*T - Jm*(T^2)/2 - * Period 3 V = Vi - Jm*(T^2)/2 - * 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). + * Everything here fires from interrupts and must be interrupt safe + * + * _exec_aline() - acceleration line main routine + * _exec_aline_head() - helper for acceleration section + * _exec_aline_body() - helper for cruise section + * _exec_aline_tail() - helper for deceleration section + * _exec_aline_segment() - helper for running a segment + * + * Returns: + * 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 + * + * This routine is called from the (LO) interrupt level. The interrupt + * sequencing relies on the behaviors of the routines being exactly correct. + * Each call to _exec_aline() must execute and prep *one and only one* + * segment. If the segment is the not the last segment in the bf buffer the + * _aline() must return STAT_EAGAIN. If it's the last segment it must return + * STAT_OK. If it encounters a fatal error that would terminate the move it + * should return a valid error code. Failure to obey this will introduce + * subtle and very difficult to diagnose bugs (trust me on this). + * + * Note 1 Returning STAT_OK ends the move and frees the bf buffer. + * Returning STAT_OK at this point does NOT advance position meaning + * any position error will be compensated by the next move. + * + * Note 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. + * + * Operation: + * Aline generates jerk-controlled S-curves as per Ed Red's course notes: + * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf + * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations + * + * A full trapezoid is divided into 5 periods Periods 1 and 2 are the + * first and second halves of the acceleration ramp (the concave and convex + * parts of the S curve in the "head"). Periods 3 and 4 are the first + * and second parts of the deceleration ramp (the tail). There is also + * a period for the constant-velocity plateau of the trapezoid (the body). + * There are various degraded trapezoids possible, including 2 section + * combinations (head and tail; head and body; body and tail), and single + * sections - any one of the three. + * + * The equations that govern the acceleration and deceleration ramps are: + * + * Period 1 V = Vi + Jm*(T^2)/2 + * Period 2 V = Vh + As*T - Jm*(T^2)/2 + * Period 3 V = Vi - Jm*(T^2)/2 + * 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). * * --- 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 + * 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 * - * mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY, _TAIL - * Within each section state may be - * _NEW - trigger initialization - * _RUN1 - run the first part - * _RUN2 - run the second part + * mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY, + * _TAIL + * Within each section state may be + * _NEW - trigger initialization + * _RUN1 - run the first part + * _RUN2 - run the second part * - * Note: For a direct math implementation see build 357.xx or earlier - * Builds 358 onward have only forward difference code + * Note: For a direct math implementation see build 357.xx or earlier. + * Builds 358 onward have only forward difference code. */ 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) { - if (cm.hold_state == FEEDHOLD_HOLD) return STAT_NOOP; // stops here if holding + if (cm.hold_state == FEEDHOLD_HOLD) + return STAT_NOOP; // stops here if holding // initialization to process the new incoming bf buffer (Gcode block) - memcpy(&mr.gm, &(bf->gm), sizeof(GCodeState_t)); // copy in the gcode model state + // copy in the gcode model state + memcpy(&mr.gm, &(bf->gm), sizeof(GCodeState_t)); bf->replannable = false; // too short lines have already been removed - if (fp_ZERO(bf->length)) { // ...looks for an actual zero here - mr.move_state = MOVE_OFF; // reset mr buffer + // looks for an actual zero here + 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) - st_prep_null(); // call this to keep the loader happy - if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty + // prevent overplanning (Note 2) + bf->nx->replannable = false; + // call this to keep the loader happy + st_prep_null(); + // free buffer & end cycle if planner is empty + if (mp_free_run_buffer()) cm_cycle_end(); return STAT_NOOP; } @@ -179,7 +188,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { mr.section_state = SECTION_NEW; mr.jerk = bf->jerk; #ifdef __JERK_EXEC - mr.jerk_div2 = bf->jerk/2; // only needed by __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; @@ -190,15 +199,18 @@ stat_t mp_exec_aline(mpBuf_t *bf) { mr.exit_velocity = bf->exit_velocity; copy_vector(mr.unit, bf->unit); - copy_vector(mr.target, bf->gm.target); // save the final target of the move + copy_vector(mr.target, bf->gm.target); // save the final target of the move // generate the waypoints for position correction at section ends for (uint8_t axis = 0; axis < AXES; axis++) { - mr.waypoint[SECTION_HEAD][axis] = mr.position[axis] + mr.unit[axis] * mr.head_length; + 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.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); + mr.position[axis] + mr.unit[axis] * + (mr.head_length + mr.body_length + mr.tail_length); } } @@ -224,16 +236,17 @@ stat_t mp_exec_aline(mpBuf_t *bf) { } // There are 3 things that can happen here depending on return conditions: - // status bf->move_state Description - // ----------- -------------- ---------------------------------------- - // STAT_EAGAIN mr buffer has more segments to run - // STAT_OK MOVE_RUN mr and bf buffers are done - // STAT_OK MOVE_NEW mr done; bf must be run again (it's been reused) + // status bf->move_state Description + // ----------- -------------- ------------------------------------ + // STAT_EAGAIN mr buffer has more segments to run + // STAT_OK MOVE_RUN mr and bf buffers are done + // STAT_OK MOVE_NEW mr done; bf must be run again + // (it's been reused) if (status == STAT_EAGAIN) report_request(); else { - mr.move_state = MOVE_OFF; // reset mr buffer + mr.move_state = MOVE_OFF; // reset mr buffer mr.section_state = SECTION_OFF; - bf->nx->replannable = false; // prevent overplanning (Note 2) + bf->nx->replannable = false; // prevent overplanning (Note 2) if (bf->move_state == MOVE_RUN && mp_free_run_buffer()) cm_cycle_end(); // free buffer & end cycle if planner is empty @@ -245,102 +258,115 @@ stat_t mp_exec_aline(mpBuf_t *bf) { /* Forward difference math explained: * - * We are using a quintic (fifth-degree) Bezier polynomial for the velocity curve. - * This gives us a "linear pop" velocity curve; with pop being the sixth derivative of position: - * velocity - 1st, acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - 6th + * We are using a quintic (fifth-degree) Bezier polynomial for the + * velocity curve. This gives us a "linear pop" velocity curve; + * with pop being the sixth derivative of position: velocity - 1st, + * acceleration - 2nd, jerk - 3rd, snap - 4th, crackle - 5th, pop - + * 6th * * 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) - * - * 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 basis as follows: + * 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) * - * B_0(t) = (1-t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1 - * B_1(t) = 5(1-t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t - * B_2(t) = 10(1-t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2 - * B_3(t) = 10(1-t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3 - * B_4(t) = 5(1-t) * t^4 = -5t^5 + 5t^4 - * B_5(t) = t^5 = t^5 - * ^ ^ ^ ^ ^ ^ - * | | | | | | - * A B C D E F + * 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 + * basis as follows: * + * B_0(t) = (1-t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1 + * B_1(t) = 5(1-t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t + * B_2(t) = 10(1-t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2 + * B_3(t) = 10(1-t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3 + * B_4(t) = 5(1-t) * t^4 = -5t^5 + 5t^4 + * B_5(t) = t^5 = t^5 + * ^ ^ ^ ^ ^ ^ + * | | | | | | + * A B C D E F * * We use forward-differencing to calculate each position through the curve. - * This requires a formula of the form: - * - * V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F - * - * Looking at the above B_0(t) through B_5(t) expanded forms, if we take the coefficients of t^5 - * through t of the Bezier form of V(t), we can determine that: - * - * A = -P_0 + 5*P_1 - 10*P_2 + 10*P_3 - 5*P_4 + P_5 - * B = 5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 + 5*P_4 - * C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3 - * D = 10*P_0 - 20*P_1 + 10*P_2 - * E = - 5*P_0 + 5*P_1 - * F = P_0 - * - * Now, since we will (currently) *always* want the initial acceleration and jerk values to be 0, - * We set P_i = P_0 = P_1 = P_2 (initial velocity), and P_t = P_3 = P_4 = P_5 (target velocity), - * which, after simplification, resolves to: - * - * A = - 6*P_i + 6*P_t - * B = 15*P_i - 15*P_t - * C = -10*P_i + 10*P_t - * D = 0 - * E = 0 - * F = P_i - * - * Given an interval count of I to get from P_i to P_t, we get the parametric "step" size of h = 1/I. - * We need to calculate the initial value of forward differences (F_0 - F_5) such that the inital - * velocity V = P_i, then we iterate over the following I times: - * - * V += F_5 - * F_5 += F_4 - * F_4 += F_3 - * F_3 += F_2 - * F_2 += F_1 - * - * See http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 for an example of - * how to calculate F_0 - F_5 for a cubic bezier curve. Since this is a quintic bezier curve, we - * need to extend the formulas somewhat. I'll not go into the long-winded step-by-step here, - * but it gives the resulting formulas: - * - * a = A, b = B, c = C, d = D, e = E, f = F - * F_5(t+h)-F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 + (10ah^3 + 6bh^2 + 3ch)t^2 + - * (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 + bh^4 + ch^3 + dh^2 + eh - * - * a = 5ah - * b = 10ah^2 + 4bh - * c = 10ah^3 + 6bh^2 + 3ch - * d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh + * This requires a formula of the form: + * + * V_f(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F + * + * Looking at the above B_0(t) through B_5(t) expanded forms, if we + * take the coefficients of t^5 through t of the Bezier form of V(t), + * we can determine that: + * + * A = -P_0 + 5*P_1 - 10*P_2 + 10*P_3 - 5*P_4 + P_5 + * B = 5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 + 5*P_4 + * C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3 + * D = 10*P_0 - 20*P_1 + 10*P_2 + * E = - 5*P_0 + 5*P_1 + * F = P_0 + * + * Now, since we will (currently) *always* want the initial + * acceleration and jerk values to be 0, We set P_i = P_0 = P_1 = + * P_2 (initial velocity), and P_t = P_3 = P_4 = P_5 (target + * velocity), which, after simplification, resolves to: + * + * A = - 6*P_i + 6*P_t + * B = 15*P_i - 15*P_t + * C = -10*P_i + 10*P_t + * D = 0 + * E = 0 + * F = P_i + * + * Given an interval count of I to get from P_i to P_t, we get the + * parametric "step" size of h = 1/I. We need to calculate the + * initial value of forward differences (F_0 - F_5) such that the + * inital velocity V = P_i, then we iterate over the following I + * times: + * + * V += F_5 + * F_5 += F_4 + * F_4 += F_3 + * F_3 += F_2 + * F_2 += F_1 + * + * See + * http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417 + * for an example of how to calculate F_0 - F_5 for a cubic bezier + * curve. Since this is a quintic bezier curve, we need to extend + * the formulas somewhat. I'll not go into the long-winded + * step-by-step here, but it gives the resulting formulas: + * + * a = A, b = B, c = C, d = D, e = E, f = F + * F_5(t+h)-F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 + + * (10ah^3 + 6bh^2 + 3ch)t^2 + (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 + + * bh^4 + ch^3 + dh^2 + eh + * + * a = 5ah + * b = 10ah^2 + 4bh + * c = 10ah^3 + 6bh^2 + 3ch + * d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh * * (After substitution, simplification, and rearranging): - * F_4(t+h)-F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 + (70ah^4 + 24bh^3 + 6ch^2)t + - * 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2 + * F_4(t+h)-F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 + + * (70ah^4 + 24bh^3 + 6ch^2)t + 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2 * - * a = (20ah^2) - * b = (60ah^3 + 12bh^2) - * c = (70ah^4 + 24bh^3 + 6ch^2) + * a = (20ah^2) + * b = (60ah^3 + 12bh^2) + * c = (70ah^4 + 24bh^3 + 6ch^2) * * (After substitution, simplification, and rearranging): - * F_3(t+h)-F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 + 36bh^4 + 6ch^3 + * F_3(t+h)-F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 + + * 36bh^4 + 6ch^3 * * (You get the picture...) - * F_2(t+h)-F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4 - * F_1(t+h)-F_1(t) = 120ah^5 + * F_2(t+h)-F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4 + * F_1(t+h)-F_1(t) = 120ah^5 * - * Normally, we could then assign t = 0, use the A-F values from above, and get out initial F_* values. - * However, for the sake of "averaging" the velocity of each segment, we actually want to have the initial - * V be be at t = h/2 and iterate I-1 times. So, the resulting F_* values are (steps not shown): + * Normally, we could then assign t = 0, use the A-F values from + * above, and get out initial F_* values. However, for the sake of + * "averaging" the velocity of each segment, we actually want to have + * the initial V be be at t = h/2 and iterate I-1 times. So, the + * resulting F_* values are (steps not shown): * - * F_5 = (121Ah^5)/16 + 5Bh^4 + (13Ch^3)/4 + 2Dh^2 + Eh - * F_4 = (165Ah^5)/2 + 29Bh^4 + 9Ch^3 + 2Dh^2 - * F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3 - * F_2 = 300Ah^5 + 24Bh^4 - * F_1 = 120Ah^5 + * F_5 = (121Ah^5)/16 + 5Bh^4 + (13Ch^3)/4 + 2Dh^2 + Eh + * F_4 = (165Ah^5)/2 + 29Bh^4 + 9Ch^3 + 2Dh^2 + * F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3 + * F_2 = 300Ah^5 + 24Bh^4 + * F_1 = 120Ah^5 * * Note that with our current control points, D and E are actually 0. */ @@ -386,49 +412,59 @@ static void _init_forward_diffs(float Vi, float Vt) { #ifdef __JERK_EXEC static stat_t _exec_aline_head() { - if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) + 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 + return _exec_aline_body(); // skip ahead to the body generator } mr.midpoint_velocity = (mr.entry_velocity + mr.cruise_velocity) / 2; - mr.gm.move_time = mr.head_length / mr.midpoint_velocity; // time for entire accel region - mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC)); // # of segments in *each half* + // time for entire accel region + mr.gm.move_time = mr.head_length / mr.midpoint_velocity; + // # of segments in *each half* + mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC)); mr.segment_time = mr.gm.move_time / (2 * mr.segments); - mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk); - mr.midpoint_acceleration = 2 * (mr.cruise_velocity - mr.entry_velocity) / mr.accel_time; - mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; // elapsed time starting point (offset) + mr.accel_time = + 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk); + mr.midpoint_acceleration = + 2 * (mr.cruise_velocity - mr.entry_velocity) / mr.accel_time; + // time to advance for each segment + mr.segment_accel_time = mr.accel_time / (2 * mr.segments); + // elapsed time starting point (offset) + mr.elapsed_accel_time = mr.segment_accel_time / 2; mr.segment_count = (uint32_t)mr.segments; if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + return STAT_MINIMUM_TIME_MOVE; // exit without advancing position mr.section = SECTION_HEAD; mr.section_state = SECTION_1st_HALF; } - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) - mr.segment_velocity = mr.entry_velocity + (square(mr.elapsed_accel_time) * mr.jerk_div2); + // FIRST HALF (concave part of accel curve) + if (mr.section_state == SECTION_1st_HALF) { + mr.segment_velocity = + mr.entry_velocity + (square(mr.elapsed_accel_time) * mr.jerk_div2); - if (_exec_aline_segment() == STAT_OK) { // set up for second half + if (_exec_aline_segment() == STAT_OK) { // set up for second half mr.segment_count = (uint32_t)mr.segments; mr.section_state = SECTION_2nd_HALF; - mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment + // start time from midpoint of segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; } return STAT_EAGAIN; } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HAF (convex part of accel curve) + // SECOND HAF (convex part of accel curve) + if (mr.section_state == SECTION_2nd_HALF) { mr.segment_velocity = mr.midpoint_velocity + (mr.elapsed_accel_time * mr.midpoint_acceleration) - (square(mr.elapsed_accel_time) * mr.jerk_div2); - if (_exec_aline_segment() == STAT_OK) { // OK means this section is done + if (_exec_aline_segment() == STAT_OK) { // OK means this section is done if ((fp_ZERO(mr.body_length)) && (fp_ZERO(mr.tail_length))) - return STAT_OK; // ends the move + return STAT_OK; // ends the move mr.section = SECTION_BODY; mr.section_state = SECTION_NEW; @@ -441,15 +477,17 @@ static stat_t _exec_aline_head() { static stat_t _exec_aline_head() { - if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) + 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 + return _exec_aline_body(); // skip ahead to the body generator } // Time for entire accel region - mr.gm.move_time = 2 * mr.head_length / (mr.entry_velocity + mr.cruise_velocity); - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); // # of segments for the section + mr.gm.move_time = + 2 * mr.head_length / (mr.entry_velocity + mr.cruise_velocity); + // # of segments for the section + mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); mr.segment_time = mr.gm.move_time / mr.segments; _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); mr.segment_count = (uint32_t)mr.segments; @@ -458,13 +496,16 @@ static stat_t _exec_aline_head() { return STAT_MINIMUM_TIME_MOVE; // exit without advancing position mr.section = SECTION_HEAD; - mr.section_state = SECTION_1st_HALF; // Note: Set to SECTION_1st_HALF for one segment + // Note: Set to SECTION_1st_HALF for one segment + mr.section_state = SECTION_1st_HALF; } - // 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. - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF (concave part of accel curve) - if (_exec_aline_segment() == STAT_OK) { // set up for second half + // 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. + // FIRST HALF (concave part of accel curve) + if (mr.section_state == SECTION_1st_HALF) { + if (_exec_aline_segment() == STAT_OK) { // set up for second half mr.section = SECTION_BODY; mr.section_state = SECTION_NEW; @@ -473,11 +514,12 @@ static stat_t _exec_aline_head() { return STAT_EAGAIN; } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF (convex part of accel curve) + // SECOND HALF (convex part of accel curve) + if (mr.section_state == SECTION_2nd_HALF) { #ifndef __KAHAN mr.segment_velocity += mr.forward_diff_5; - -#else // Use Kahan summation algorithm to mitigate floating-point errors for the above +#else + // Use Kahan summation algorithm to mitigate floating-point errors for above float y = mr.forward_diff_5 - mr.forward_diff_5_c; float v = mr.segment_velocity + y; mr.forward_diff_5_c = (v - mr.segment_velocity) - y; @@ -526,13 +568,14 @@ static stat_t _exec_aline_head() { #endif // __ JERK_EXEC -/// 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 +/// 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)) { mr.section = SECTION_TAIL; - return _exec_aline_tail(); // skip ahead to tail periods + return _exec_aline_tail(); // skip ahead to tail periods } mr.gm.move_time = mr.body_length / mr.cruise_velocity; @@ -542,15 +585,16 @@ static stat_t _exec_aline_body() { mr.segment_count = (uint32_t)mr.segments; if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + return STAT_MINIMUM_TIME_MOVE; // exit without advancing position mr.section = SECTION_BODY; - mr.section_state = SECTION_2nd_HALF; // uses PERIOD_2 so last segment detection works + // uses PERIOD_2 so last segment detection works + mr.section_state = SECTION_2nd_HALF; } - if (mr.section_state == SECTION_2nd_HALF) // straight part (period 3) - if (_exec_aline_segment() == STAT_OK) { // OK means this section is done - if (fp_ZERO(mr.tail_length)) return STAT_OK; // ends the move + if (mr.section_state == SECTION_2nd_HALF) // straight part (period 3) + if (_exec_aline_segment() == STAT_OK) { // OK means this section is done + if (fp_ZERO(mr.tail_length)) return STAT_OK; // ends the move mr.section = SECTION_TAIL; mr.section_state = SECTION_NEW; } @@ -566,35 +610,45 @@ static stat_t _exec_aline_tail() { mr.midpoint_velocity = (mr.cruise_velocity + mr.exit_velocity) / 2; mr.gm.move_time = mr.tail_length / mr.midpoint_velocity; - mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC));// # of segments in *each half* - mr.segment_time = mr.gm.move_time / (2 * mr.segments); // time to advance for each segment + // # of segments in *each half* + mr.segments = ceil(uSec(mr.gm.move_time) / (2 * NOM_SEGMENT_USEC)); + // time to advance for each segment + mr.segment_time = mr.gm.move_time / (2 * mr.segments); mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.exit_velocity) / mr.jerk); - mr.midpoint_acceleration = 2 * (mr.cruise_velocity - mr.exit_velocity) / mr.accel_time; - mr.segment_accel_time = mr.accel_time / (2 * mr.segments); // time to advance for each segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; // compute time from midpoint of segment + mr.midpoint_acceleration = + 2 * (mr.cruise_velocity - mr.exit_velocity) / mr.accel_time; + // time to advance for each segment + mr.segment_accel_time = mr.accel_time / (2 * mr.segments); + // compute time from midpoint of segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position + if (mr.segment_time < MIN_SEGMENT_TIME) + return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position mr.section = SECTION_TAIL; mr.section_state = SECTION_1st_HALF; } - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) - mr.segment_velocity = mr.cruise_velocity - (square(mr.elapsed_accel_time) * mr.jerk_div2); + // FIRST HALF - convex part (period 4) + if (mr.section_state == SECTION_1st_HALF) { + mr.segment_velocity = + mr.cruise_velocity - (square(mr.elapsed_accel_time) * mr.jerk_div2); - if (_exec_aline_segment() == STAT_OK) { // set up for second half + if (_exec_aline_segment() == STAT_OK) { // set up for second half mr.segment_count = (uint32_t)mr.segments; mr.section_state = SECTION_2nd_HALF; - mr.elapsed_accel_time = mr.segment_accel_time / 2; // start time from midpoint of segment + // start time from midpoint of segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; } return STAT_EAGAIN; } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) + // SECOND HALF - concave part (period 5) + if (mr.section_state == SECTION_2nd_HALF) { mr.segment_velocity = mr.midpoint_velocity - (mr.elapsed_accel_time * mr.midpoint_acceleration) + (square(mr.elapsed_accel_time) * mr.jerk_div2); - return _exec_aline_segment(); // ends the move or continues EAGAIN + return _exec_aline_segment(); // ends the move or continues EAGAIN } return STAT_EAGAIN; // should never get here @@ -606,21 +660,28 @@ static stat_t _exec_aline_tail() { if (mr.section_state == SECTION_NEW) { // INITIALIZATION if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move - mr.gm.move_time = 2 * mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); // len/avg. velocity - mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); // # of segments for the section - mr.segment_time = mr.gm.move_time / mr.segments; // time to advance for each segment + // len/avg. velocity + mr.gm.move_time = + 2 * mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); + // # of segments for the section + mr.segments = ceil(uSec(mr.gm.move_time) / NOM_SEGMENT_USEC); + // time to advance for each segment + mr.segment_time = mr.gm.move_time / mr.segments; _init_forward_diffs(mr.cruise_velocity, mr.exit_velocity); mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position + if (mr.segment_time < MIN_SEGMENT_TIME) + return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position mr.section = SECTION_TAIL; mr.section_state = SECTION_1st_HALF; } - if (mr.section_state == SECTION_1st_HALF) { // FIRST HALF - convex part (period 4) + // FIRST HALF - convex part (period 4) + 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 2 ... effectively. + // 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 2 ... effectively. mr.section_state = SECTION_2nd_HALF; return STAT_OK; @@ -629,10 +690,12 @@ static stat_t _exec_aline_tail() { return STAT_EAGAIN; } - if (mr.section_state == SECTION_2nd_HALF) { // SECOND HALF - concave part (period 5) + // SECOND HALF - concave part (period 5) + if (mr.section_state == SECTION_2nd_HALF) { #ifndef __KAHAN mr.segment_velocity += mr.forward_diff_5; -#else // Use Kahan summation algorithm to mitigate floating-point errors for the above +#else + // Use Kahan summation algorithm to mitigate floating-point errors for above float y = mr.forward_diff_5 - mr.forward_diff_5_c; float v = mr.segment_velocity + y; mr.forward_diff_5_c = (v - mr.segment_velocity) - y; @@ -675,33 +738,36 @@ static stat_t _exec_aline_tail() { #endif // __JERK_EXEC -/********************************************************************************************* - * Segment runner helper +/* Segment runner helper * - * NOTES ON STEP ERROR CORRECTION: + * Notes on step error correction: * - * The commanded_steps are the target_steps delayed by one more segment. - * This lines them up in time with the encoder readings so a following error can be generated + * The commanded_steps are the target_steps delayed by one more + * segment. This lines them up in time with the encoder readings so a + * following error can be generated * - * The following_error term is positive if the encoder reading is greater than (ahead of) - * the commanded steps, and negative (behind) if the encoder reading is less than the - * commanded steps. The following error is not affected by the direction of movement - - * it's purely a statement of relative position. Examples: + * The following_error term is positive if the encoder reading is + * greater than (ahead of) the commanded steps, and negative (behind) + * if the encoder reading is less than the commanded steps. The + * following error is not affected by the direction of movement - it's + * purely a statement of relative position. Examples: * * Encoder Commanded Following Err - * 100 90 +10 encoder is 10 steps ahead of commanded steps - * -90 -100 +10 encoder is 10 steps ahead of commanded steps - * 90 100 -10 encoder is 10 steps behind commanded steps - * -100 -90 -10 encoder is 10 steps behind commanded steps + * 100 90 +10 encoder is 10 steps ahead of commanded steps + * -90 -100 +10 encoder is 10 steps ahead of commanded steps + * 90 100 -10 encoder is 10 steps behind commanded steps + * -100 -90 -10 encoder is 10 steps behind commanded steps */ static stat_t _exec_aline_segment() { uint8_t i; float travel_steps[MOTORS]; // 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 correction if you are going into a hold. + // 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 + // correction if you are going into a hold. + if (--mr.segment_count == 0 && mr.section_state == SECTION_2nd_HALF && cm.motion_state == MOTION_RUN && cm.cycle_state == CYCLE_MACHINING) copy_vector(mr.gm.target, mr.waypoint[mr.section]); @@ -713,31 +779,40 @@ static stat_t _exec_aline_segment() { mr.gm.target[i] = mr.position[i] + (mr.unit[i] * segment_length); } - // Convert target position to steps - // Bucket-brigade the old target down the chain before getting the new target from kinematics + // Convert target position to steps. Bucket-brigade the old target + // down the chain before getting the new target from kinematics // - // The direct manipulation of steps to compute travel_steps only works for Cartesian kinematics. - // Other kinematics may require transforming travel distance as opposed to simply subtracting steps. + // The direct manipulation of steps to compute travel_steps only + // works for Cartesian kinematics. Other kinematics may require + // transforming travel distance as opposed to simply subtracting + // steps. for (i = 0; i < MOTORS; i++) { - mr.commanded_steps[i] = mr.position_steps[i]; // previous segment's position, delayed by 1 segment - mr.position_steps[i] = mr.target_steps[i]; // previous segment's target becomes position - mr.encoder_steps[i] = en_read_encoder(i); // current encoder position (aligns to commanded_steps) + // previous segment's position, delayed by 1 segment + mr.commanded_steps[i] = mr.position_steps[i]; + // previous segment's target becomes position + mr.position_steps[i] = mr.target_steps[i]; + // current encoder position (aligns to commanded_steps) + mr.encoder_steps[i] = en_read_encoder(i); mr.following_error[i] = mr.encoder_steps[i] - mr.commanded_steps[i]; } - ik_kinematics(mr.gm.target, mr.target_steps); // now determine the target steps... + // now determine the target steps + ik_kinematics(mr.gm.target, mr.target_steps); - for (i = 0; i < MOTORS; i++) // and compute the distances to be traveled + // and compute the distances to be traveled + for (i = 0; i < MOTORS; i++) travel_steps[i] = mr.target_steps[i] - mr.position_steps[i]; // Call the stepper prep function ritorno(st_prep_line(travel_steps, mr.following_error, mr.segment_time)); - copy_vector(mr.position, mr.gm.target); // update position from target + // update position from target + copy_vector(mr.position, mr.gm.target); #ifdef __JERK_EXEC - mr.elapsed_accel_time += mr.segment_accel_time; // needed by jerk-based exec (ignored if running body) + // needed by jerk-based exec (ignored if running body) + mr.elapsed_accel_time += mr.segment_accel_time; #endif - if (!mr.segment_count) return STAT_OK; // this section has run all its segments - return STAT_EAGAIN; // this section still has more segments to run + if (!mr.segment_count) return STAT_OK; // this section has run all segments + return STAT_EAGAIN; // this section still has more segments to run } diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c new file mode 100644 index 0000000..a8fdedb --- /dev/null +++ b/src/plan/feedhold.c @@ -0,0 +1,247 @@ +/* + * feedhold.c - functions for performing holds + * + * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. + * Copyright (c) 2012 - 2015 Rob Giseburt + * + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +#include "planner.h" +#include "stepper.h" +#include "util.h" + +#include +#include + +/* Feedhold is executed as cm.hold_state transitions executed inside + * _exec_aline() and main loop callbacks to these functions: + * mp_plan_hold_callback() and mp_end_hold(). + * + * Holds work like this: + * + * - Hold is asserted by calling cm_feedhold() (usually invoked via a + * ! char) If hold_state is OFF and motion_state is RUNning it sets + * hold_state to SYNC and motion_state to HOLD. + * + * - Hold state == SYNC tells the aline exec routine to execute the + * next aline segment then set hold_state to PLAN. This gives the + * planner sufficient time to replan the block list for the hold + * before the next aline segment needs to be processed. + * + * - Hold state == PLAN tells the planner to replan the mr buffer, the + * current run buffer (bf), and any subsequent bf buffers as + * necessary to execute a hold. Hold planning replans the planner + * buffer queue down to zero and then back up from zero. Hold state + * is set to DECEL when planning is complete. + * + * - Hold state == DECEL persists until the aline execution runs to + * zero velocity, at which point hold state transitions to HOLD. + * + * - Hold state == HOLD persists until the cycle is restarted. A cycle + * start is an asynchronous event that sets the cycle_start_flag + * TRUE. It can occur any time after the hold is requested - either + * before or after motion stops. + * + * - mp_end_hold() is executed from cm_feedhold_sequencing_callback() + * once the hold state == HOLD and a cycle_start has been + * requested.This sets the hold state to OFF which enables + * _exec_aline() to continue processing. Move execution begins with + * the first buffer after the hold. + * + * Terms used: + * - mr is the runtime buffer. It was initially loaded from the bf + * buffer + * - bp+0 is the "companion" bf buffer to the mr buffer. + * - bp+1 is the bf buffer following bp+0. This runs through bp+N + * - bp (by itself) just refers to the current buffer being + * adjusted / replanned + * + * Details: Planning re-uses bp+0 as an "extra" buffer. Normally bp+0 + * is returned to the buffer pool as it is redundant once mr is + * loaded. Use the extra buffer to split the move in two where the + * hold decelerates to zero. Use one buffer to go to zero, the other + * to replan up from zero. All buffers past that point are + * unaffected other than that they need to be replanned for + * velocity. + * + * Note: There are multiple opportunities for more efficient + * organization of code in this module, but the code is so + * complicated I just left it organized for clarity and hoped for + * the best from compiler optimization. + */ + + +/// Resets all blocks in the planning list to be replannable +static void _reset_replannable_list() { + mpBuf_t *bf = mp_get_first_buffer(); + if (!bf) return; + + mpBuf_t *bp = bf; + + do { + bp->replannable = true; + } while ((bp = mp_get_next_buffer(bp)) != bf && bp->move_state != MOVE_OFF); +} + + +static float _compute_next_segment_velocity() { + if (mr.section == SECTION_BODY) return mr.segment_velocity; +#ifdef __JERK_EXEC + return mr.segment_velocity; // an approximation +#else + return mr.segment_velocity + mr.forward_diff_5; +#endif +} + + +/// replan block list to execute hold +stat_t mp_plan_hold_callback() { + if (cm.hold_state != FEEDHOLD_PLAN) + return STAT_NOOP; // not planning a feedhold + + mpBuf_t *bp; // working buffer pointer + if ((bp = mp_get_run_buffer()) == 0) + return STAT_NOOP; // Oops! nothing's running + + uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx + float mr_available_length; // length left in mr buffer for deceleration + float braking_velocity; // velocity left to shed to brake to zero + float braking_length; // distance to brake to zero from braking_velocity + + // examine and process mr buffer + mr_available_length = get_axis_vector_length(mr.target, mr.position); + + // compute next_segment velocity + braking_velocity = _compute_next_segment_velocity(); + // bp is OK to use here + braking_length = mp_get_target_length(braking_velocity, 0, bp); + + // Hack to prevent Case 2 moves for perfect-fit decels. Happens in + // homing situations The real fix: The braking velocity cannot + // simply be the mr.segment_velocity as this is the velocity of the + // last segment, not the one that's going to be executed next. The + // braking_velocity needs to be the velocity of the next segment + // that has not yet been computed. In the mean time, this hack will + // work. + if (braking_length > mr_available_length && fp_ZERO(bp->exit_velocity)) + braking_length = mr_available_length; + + // Case 1: deceleration fits entirely into the length remaining in mr buffer + if (braking_length <= mr_available_length) { + // set mr to a tail to perform the deceleration + mr.exit_velocity = 0; + mr.tail_length = braking_length; + mr.cruise_velocity = braking_velocity; + mr.section = SECTION_TAIL; + mr.section_state = SECTION_NEW; + + // re-use bp+0 to be the hold point and to run the remaining block length + bp->length = mr_available_length - braking_length; + bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); + bp->entry_vmax = 0; // set bp+0 as hold point + bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer + + _reset_replannable_list(); // make it replan all the blocks + mp_plan_block_list(mp_get_last_buffer(), &mr_flag); + cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit + + return STAT_OK; + } + + // Case 2: deceleration exceeds length remaining in mr buffer + // First, replan mr to minimum (but non-zero) exit velocity + + mr.section = SECTION_TAIL; + mr.section_state = SECTION_NEW; + mr.tail_length = mr_available_length; + mr.cruise_velocity = braking_velocity; + mr.exit_velocity = + braking_velocity - mp_get_target_velocity(0, mr_available_length, bp); + + // Find where deceleration reaches zero. This could span multiple buffers. + braking_velocity = mr.exit_velocity; // adjust braking velocity downward + bp->move_state = MOVE_NEW; // tell _exec to re-use buffer + + // a safety to avoid wraparound + for (uint8_t i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { + mp_copy_buffer(bp, bp->nx); // copy bp+1 into bp+0, and onward + + if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers + bp = mp_get_next_buffer(bp); // point to next buffer + continue; + } + + bp->entry_vmax = braking_velocity; // velocity we need to shed + braking_length = mp_get_target_length(braking_velocity, 0, bp); + + if (braking_length > bp->length) { // decel does not fit in bp buffer + bp->exit_vmax = + braking_velocity - mp_get_target_velocity(0, bp->length, bp); + braking_velocity = bp->exit_vmax; // braking velocity for next buffer + bp = mp_get_next_buffer(bp); // point to next buffer + continue; + } + + break; + } + + // Deceleration now fits in the current bp buffer + // Plan the first buffer of the pair as the decel, the second as the accel + bp->length = braking_length; + bp->exit_vmax = 0; + + bp = mp_get_next_buffer(bp); // point to the acceleration buffer + bp->entry_vmax = 0; + bp->length -= braking_length; // identical buffers and hence their lengths + bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); + bp->exit_vmax = bp->delta_vmax; + + _reset_replannable_list(); // replan all the blocks + mp_plan_block_list(mp_get_last_buffer(), &mr_flag); + cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit + + return STAT_OK; +} + + +/// End a feedhold, release the hold and restart block list +stat_t mp_end_hold() { + if (cm.hold_state == FEEDHOLD_END_HOLD) { + cm.hold_state = FEEDHOLD_OFF; + + if (!mp_get_run_buffer()) { // 0 means nothing's running + cm_set_motion_state(MOTION_STOP); + return STAT_NOOP; + } + + cm.motion_state = MOTION_RUN; + st_request_exec_move(); // restart the steppers + } + + return STAT_OK; +} diff --git a/src/plan/kinematics.c b/src/plan/kinematics.c index 87d0f22..a89040d 100644 --- a/src/plan/kinematics.c +++ b/src/plan/kinematics.c @@ -4,25 +4,31 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "kinematics.h" @@ -32,22 +38,26 @@ #include -/* - * Wrapper routine for inverse kinematics +/* Wrapper routine for inverse kinematics * - * Calls kinematics function(s). - * Performs axis mapping & conversion of length units to steps (and deals with inhibited axes) + * Calls kinematics function(s). Performs axis mapping & conversion + * of length units to steps (and deals with inhibited axes) * - * The reason steps are returned as floats (as opposed to, say, uint32_t) is to accommodate - * fractional DDA steps. The DDA deals with fractional step values as fixed-point binary in - * order to get the smoothest possible operation. Steps are passed to the move prep routine - * as floats and converted to fixed-point binary during queue loading. See stepper.c for details. + * The reason steps are returned as floats (as opposed to, say, + * uint32_t) is to accommodate fractional DDA steps. The DDA deals + * with fractional step values as fixed-point binary in order to get + * the smoothest possible operation. Steps are passed to the move prep + * routine as floats and converted to fixed-point binary during queue + * loading. See stepper.c for details. */ void ik_kinematics(const float travel[], float steps[]) { float joint[AXES]; - // _inverse_kinematics(travel, joint); // you can insert inverse kinematics transformations here - memcpy(joint, travel, sizeof(float) * AXES); //...or just do a memcpy for Cartesian machines + // you can insert inverse kinematics transformations here + // _inverse_kinematics(travel, joint); + + //...or just do a memcpy for Cartesian machines + memcpy(joint, travel, sizeof(float) * AXES); // Map motors to axes and convert length units to steps // Most of the conversion math has already been done during config in diff --git a/src/plan/kinematics.h b/src/plan/kinematics.h index 4e62647..6cf2e2b 100644 --- a/src/plan/kinematics.h +++ b/src/plan/kinematics.h @@ -4,25 +4,31 @@ * * Copyright (c) 2013 - 2014 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef KINEMATICS_H diff --git a/src/plan/line.c b/src/plan/line.c index c083b69..895aa63 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -45,10 +45,9 @@ #include -static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const float axis_square[]); -static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag); +static void _calc_move_times(GCodeState_t *gms, const float axis_length[], + const float axis_square[]); static float _get_junction_vmax(const float a_unit[], const float b_unit[]); -static void _reset_replannable_list(); /// Correct velocity in last segment for reporting purposes @@ -62,20 +61,23 @@ float mp_get_runtime_velocity() {return mr.segment_velocity;} /// Returns current axis position in machine coordinates float mp_get_runtime_absolute_position(uint8_t axis) {return mr.position[axis];} + /// Set offsets in the MR struct -void mp_set_runtime_work_offset(float offset[]) {copy_vector(mr.gm.work_offset, offset);} +void mp_set_runtime_work_offset(float offset[]) { + copy_vector(mr.gm.work_offset, offset); +} + /// Returns current axis position in work coordinates /// that were in effect at move planning time -float mp_get_runtime_work_position(uint8_t axis) {return mr.position[axis] - mr.gm.work_offset[axis];} +float mp_get_runtime_work_position(uint8_t axis) { + return mr.position[axis] - mr.gm.work_offset[axis]; +} -/* - * Return TRUE if motion control busy (i.e. robot is moving) - * - * Use this function to sync to the queue. If you wait until it returns - * FALSE you know the queue is empty and the motors have stopped. - */ +/// Return TRUE if motion control busy (i.e. robot is moving) +/// Use this function to sync to the queue. If you wait until it returns +/// FALSE you know the queue is empty and the motors have stopped. uint8_t mp_get_runtime_busy() { return st_runtime_isbusy() || mr.move_state == MOVE_RUN; } @@ -83,21 +85,21 @@ uint8_t mp_get_runtime_busy() { /* Plan a line with acceleration / deceleration * - * This function uses constant jerk motion equations to plan - * acceleration and deceleration The jerk is the rate of change of - * acceleration; it's the 1st derivative of acceleration, and the - * 3rd derivative of position. Jerk is a measure of impact to the - * machine. Controlling jerk smooths transitions between moves and - * allows for faster feeds while controlling machine oscillations - * and other undesirable side-effects. - * - * Note All math is done in absolute coordinates using single - * precision floating point (float). - * - * Note: Returning a status that is not STAT_OK means the endpoint - * is NOT advanced. So lines that are too short to move will - * accumulate and get executed once the accumulated error exceeds - * the minimums. + * This function uses constant jerk motion equations to plan + * acceleration and deceleration The jerk is the rate of change of + * acceleration; it's the 1st derivative of acceleration, and the + * 3rd derivative of position. Jerk is a measure of impact to the + * machine. Controlling jerk smooths transitions between moves and + * allows for faster feeds while controlling machine oscillations + * and other undesirable side-effects. + * + * Note All math is done in absolute coordinates using single + * precision floating point (float). + * + * Note: Returning a status that is not STAT_OK means the endpoint + * is NOT advanced. So lines that are too short to move will + * accumulate and get executed once the accumulated error exceeds + * the minimums. */ stat_t mp_aline(GCodeState_t *gm_in) { mpBuf_t *bf; // current move pointer @@ -105,7 +107,7 @@ stat_t mp_aline(GCodeState_t *gm_in) { float junction_velocity; uint8_t mr_flag = false; - // compute some reusable terms + // Compute some reusable terms float axis_length[AXES]; float axis_square[AXES]; float length_square = 0; @@ -122,41 +124,48 @@ stat_t mp_aline(GCodeState_t *gm_in) { return STAT_OK; } - // If _calc_move_times() says the move will take less than the minimum move time - // get a more accurate time estimate based on starting velocity and acceleration. - // The time of the move is determined by its initial velocity (Vi) and how much - // acceleration will be occur. For this we need to look at the exit velocity of + // If _calc_move_times() says the move will take less than the + // minimum move time get a more accurate time estimate based on + // starting velocity and acceleration. The time of the move is + // determined by its initial velocity (Vi) and how much acceleration + // will be occur. For this we need to look at the exit velocity of // the previous block. There are 3 possible cases: // - // (1) There is no previous block. Vi = 0 - // (2) Previous block is optimally planned. Vi = previous block's exit_velocity - // (3) Previous block is not optimally planned. Vi <= previous block's - // entry_velocity + delta_velocity + // (1) There is no previous block. + // Vi = 0 + // (2) Previous block is optimally planned. + // Vi = previous block's exit_velocity + // (3) Previous block is not optimally planned. + // Vi <= previous block's entry_velocity + delta_velocity - _calc_move_times(gm_in, axis_length, axis_square); // set move & minimum time in state + // Set move & minimum time in state + _calc_move_times(gm_in, axis_length, axis_square); if (gm_in->move_time < MIN_BLOCK_TIME) { - float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; // max velocity change for this move - float entry_velocity = 0; // pre-set as if no previous block + // Max velocity change for this move + float delta_velocity = pow(length, 0.66666666) * mm.cbrt_jerk; + float entry_velocity = 0; // pre-set as if no previous block if ((bf = mp_get_run_buffer())) { - if (bf->replannable) // not optimally planned + if (bf->replannable) // not optimally planned entry_velocity = bf->entry_velocity + bf->delta_vmax; - else entry_velocity = bf->exit_velocity; // optimally planned + else entry_velocity = bf->exit_velocity; // optimally planned } - // compute execution time for this move + // Compute execution time for this move float move_time = 2 * length / (2 * entry_velocity + delta_velocity); 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 if (!(bf = mp_get_write_buffer())) - return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // never supposed to fail + return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // never supposed to fail - bf->bf_func = mp_exec_aline; // register callback to exec function + // Register callback to exec function + bf->bf_func = mp_exec_aline; bf->length = length; - memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // copy model state into planner buffer + // Copy model state into planner buffer + memcpy(&bf->gm, gm_in, sizeof(GCodeState_t)); // Compute the unit vector and find the right jerk to use (combined // operations) To determine the jerk value to use for the block we @@ -164,42 +173,40 @@ stat_t mp_aline(GCodeState_t *gm_in) { // 'jerk-limit' axis. This is the axis for which the time to // decelerate from the target velocity to zero would be the longest. // - // We can determine the "longest" deceleration in terms of time or distance. + // We can determine the "longest" deceleration in terms of time or distance. // - // The math for time-to-decelerate T from speed S to speed E with constant - // jerk J is: + // The math for time-to-decelerate T from speed S to speed E with constant + // jerk J is: + // T = 2*sqrt((S-E)/J[n]) // - // T = 2*sqrt((S-E)/J[n]) + // Since E is always zero in this calculation, we can simplify: + // T = 2*sqrt(S/J[n]) // - // Since E is always zero in this calculation, we can simplify: - // T = 2*sqrt(S/J[n]) + // The math for distance-to-decelerate l from speed S to speed E with + // constant jerk J is: + // l = (S+E)*sqrt((S-E)/J) // - // The math for distance-to-decelerate l from speed S to speed E with constant - // jerk J is: + // Since E is always zero in this calculation, we can simplify: + // l = S*sqrt(S/J) // - // l = (S+E)*sqrt((S-E)/J) + // The final value we want is to know which one is *longest*, + // compared to the others, so we don't need the actual value. This + // means that the scale of the value doesn't matter, so for T we + // can remove the "2 *" and For L we can remove the "S*". This + // leaves both to be simply "sqrt(S/J)". Since we don't need the + // scale, it doesn't matter what speed we're coming from, so S can + // be replaced with 1. // - // Since E is always zero in this calculation, we can simplify: - // l = S*sqrt(S/J) + // However, we *do* need to compensate for the fact that each axis + // contributes differently to the move, so we will scale each + // contribution C[n] by the proportion of the axis movement length + // D[n] to the total length of the move L. 0Using that, we + // construct the following, with these definitions: // - // The final value we want is to know which one is *longest*, - // compared to the others, so we don't need the actual value. This - // means that the scale of the value doesn't matter, so for T we - // can remove the "2 *" and For L we can remove the "S*". This - // leaves both to be simply "sqrt(S/J)". Since we don't need the - // scale, it doesn't matter what speed we're coming from, so S can - // be replaced with 1. - // - // However, we *do* need to compensate for the fact that each axis - // contributes differently to the move, so we will scale each - // contribution C[n] by the proportion of the axis movement length - // D[n] to the total length of the move L. 0Using that, we - // construct the following, with these definitions: - // - // J[n] = max_jerk for axis n - // D[n] = distance traveled for this move for axis n - // L = total length of the move in all axes - // C[n] = "axis contribution" of axis n + // J[n] = max_jerk for axis n + // D[n] = distance traveled for this move for axis n + // L = total length of the move in all axes + // C[n] = "axis contribution" of axis n // // For each axis n: C[n] = sqrt(1/J[n]) * (D[n]/L) // @@ -230,22 +237,27 @@ stat_t mp_aline(GCodeState_t *gm_in) { float recip_L2 = 1 / length_square; for (uint8_t axis = 0; axis < AXES; axis++) - if (fabs(axis_length[axis]) > 0) { // You cannot use the fp_XXX comparisons here! - bf->unit[axis] = axis_length[axis] / bf->length; // compute unit vector term (zeros are already zero) - C = axis_square[axis] * recip_L2 * cm.a[axis].recip_jerk; // squaring axis_length ensures it's + + // You cannot use the fp_XXX comparisons here! + if (fabs(axis_length[axis]) > 0) { + // compute unit vector term (zeros are already zero) + bf->unit[axis] = axis_length[axis] / bf->length; + // squaring axis_length ensures it's positive + C = axis_square[axis] * recip_L2 * cm.a[axis].recip_jerk; if (C > maxC) { maxC = C; - bf->jerk_axis = axis; // also needed for junction vmax calculation + bf->jerk_axis = axis; // also needed for junction vmax calculation } } // set up and pre-compute the jerk terms needed for this round of planning - bf->jerk = cm.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / fabs(bf->unit[bf->jerk_axis]); // scale jerk + bf->jerk = cm.a[bf->jerk_axis].jerk_max * JERK_MULTIPLIER / + fabs(bf->unit[bf->jerk_axis]); // scale jerk - if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { // specialized comparison for tolerance of delta - mm.jerk = bf->jerk; // used before this point next time around - mm.recip_jerk = 1/bf->jerk; // compute cached jerk terms used by planning + // specialized comparison for tolerance of delta + if (fabs(bf->jerk - mm.jerk) > JERK_MATCH_PRECISION) { + mm.jerk = bf->jerk; // used before this point next time around + mm.recip_jerk = 1 / bf->jerk; // compute cached jerk terms used by planning mm.cbrt_jerk = cbrt(bf->jerk); } @@ -253,22 +265,26 @@ stat_t mp_aline(GCodeState_t *gm_in) { bf->cbrt_jerk = mm.cbrt_jerk; // finish up the current block variables - if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { // exact stop cases already zeroed + // exact stop cases already zeroed + if (cm_get_path_control(MODEL) != PATH_EXACT_STOP) { bf->replannable = true; - exact_stop = 8675309; // an arbitrarily large floating point number + exact_stop = 8675309; // an arbitrarily large floating point number } - bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested + bf->cruise_vmax = bf->length / bf->gm.move_time; // target velocity requested junction_velocity = _get_junction_vmax(bf->pv->unit, bf->unit); bf->entry_vmax = min3(bf->cruise_vmax, junction_velocity, exact_stop); bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); - bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), exact_stop); + bf->exit_vmax = min3(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax), + exact_stop); bf->braking_velocity = bf->delta_vmax; - // Note: these next lines must remain in exact order. Position must update before committing the buffer. - _plan_block_list(bf, &mr_flag); // replan block list + // Note: these next lines must remain in exact order. Position must update + // before committing the buffer. + mp_plan_block_list(bf, &mr_flag); // replan block list copy_vector(mm.position, bf->gm.target); // set the planner position - mp_commit_write_buffer(MOVE_TYPE_ALINE); // commit current block (must follow the position update) + // commit current block (must follow the position update) + mp_commit_write_buffer(MOVE_TYPE_ALINE); return STAT_OK; } @@ -277,94 +293,99 @@ stat_t mp_aline(GCodeState_t *gm_in) { /* * Compute optimal and minimum move times into the gcode_state * - * "Minimum time" is the fastest the move can be performed given - * the velocity constraints on each participating axis - regardless - * of the feed rate requested. The minimum time is the time limited - * by the rate-limiting axis. The minimum time is needed to compute - * the optimal time and is recorded for possible feed override - * computation.. + * "Minimum time" is the fastest the move can be performed given + * the velocity constraints on each participating axis - regardless + * of the feed rate requested. The minimum time is the time limited + * by the rate-limiting axis. The minimum time is needed to compute + * the optimal time and is recorded for possible feed override + * computation. * - * "Optimal time" is either the time resulting from the requested - * feed rate or the minimum time if the requested feed rate is not - * achievable. Optimal times for traverses are always the minimum - * time. + * "Optimal time" is either the time resulting from the requested + * feed rate or the minimum time if the requested feed rate is not + * achievable. Optimal times for traverses are always the minimum + * time. * - * The gcode state must have targets set prior by having - * cm_set_target(). Axis modes are taken into account by this. + * The gcode state must have targets set prior by having + * cm_set_target(). Axis modes are taken into account by this. * - * The following times are compared and the longest is returned: - * - G93 inverse time (if G93 is active) - * - time for coordinated move at requested feed rate - * - time that the slowest axis would require for the move + * The following times are compared and the longest is returned: + * - G93 inverse time (if G93 is active) + * - time for coordinated move at requested feed rate + * - time that the slowest axis would require for the move * - * Sets the following variables in the gcode_state struct - * - move_time is set to optimal time - * - minimum_time is set to minimum time + * Sets the following variables in the gcode_state struct + * - move_time is set to optimal time + * - minimum_time is set to minimum time * * NIST RS274NGC_v3 Guidance * - * The following is verbatim text from NIST RS274NGC_v3. As I - * interpret A for moves that combine both linear and rotational - * movement, the feed rate should apply to the XYZ movement, with - * the rotational axis (or axes) timed to start and end at the same - * time the linear move is performed. It is possible under this - * case for the rotational move to rate-limit the linear move. - * - * 2.1.2.5 Feed Rate - * - * The rate at which the controlled point or the axes move is - * nominally a steady rate which may be set by the user. In the - * Interpreter, the interpretation of the feed rate is as follows - * unless inverse time feed rate mode is being used in the - * RS274/NGC view (see Section 3.5.19). The canonical machining - * functions view of feed rate, as described in Section 4.3.5.1, - * has conditions under which the set feed rate is applied - * differently, but none of these is used in the Interpreter. - * - * A. For motion involving one or more of the X, Y, and Z axes - * (with or without simultaneous rotational axis motion), the - * feed rate means length units per minute along the programmed - * XYZ path, as if the rotational axes were not moving. - * - * B. For motion of one rotational axis with X, Y, and Z axes not - * moving, the feed rate means degrees per minute rotation of - * the rotational axis. - * - * C. For motion of two or three rotational axes with X, Y, and Z - * axes not moving, the rate is applied as follows. Let dA, dB, - * and dC be the angles in degrees through which the A, B, and - * C axes, respectively, must move. Let D = sqrt(dA^2 + dB^2 + - * dC^2). Conceptually, D is a measure of total angular motion, - * using the usual Euclidean metric. Let T be the amount of - * time required to move through D degrees at the current feed - * rate in degrees per minute. The rotational axes should be - * moved in coordinated linear motion so that the elapsed time - * from the start to the end of the motion is T plus any time - * required for acceleration or deceleration. + * The following is verbatim text from NIST RS274NGC_v3. As I + * interpret A for moves that combine both linear and rotational + * movement, the feed rate should apply to the XYZ movement, with + * the rotational axis (or axes) timed to start and end at the same + * time the linear move is performed. It is possible under this + * case for the rotational move to rate-limit the linear move. + * + * 2.1.2.5 Feed Rate + * + * The rate at which the controlled point or the axes move is + * nominally a steady rate which may be set by the user. In the + * Interpreter, the interpretation of the feed rate is as follows + * unless inverse time feed rate mode is being used in the + * RS274/NGC view (see Section 3.5.19). The canonical machining + * functions view of feed rate, as described in Section 4.3.5.1, + * has conditions under which the set feed rate is applied + * differently, but none of these is used in the Interpreter. + * + * A. For motion involving one or more of the X, Y, and Z axes + * (with or without simultaneous rotational axis motion), the + * feed rate means length units per minute along the programmed + * XYZ path, as if the rotational axes were not moving. + * + * B. For motion of one rotational axis with X, Y, and Z axes not + * moving, the feed rate means degrees per minute rotation of + * the rotational axis. + * + * C. For motion of two or three rotational axes with X, Y, and Z + * axes not moving, the rate is applied as follows. Let dA, dB, + * and dC be the angles in degrees through which the A, B, and + * C axes, respectively, must move. Let D = sqrt(dA^2 + dB^2 + + * dC^2). Conceptually, D is a measure of total angular motion, + * using the usual Euclidean metric. Let T be the amount of + * time required to move through D degrees at the current feed + * rate in degrees per minute. The rotational axes should be + * moved in coordinated linear motion so that the elapsed time + * from the start to the end of the motion is T plus any time + * required for acceleration or deceleration. */ -static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const float axis_square[]) { +static void _calc_move_times(GCodeState_t *gms, const float axis_length[], + const float axis_square[]) { // gms = Gcode model state - float inv_time = 0; // inverse time if doing a feed in G93 mode - float xyz_time = 0; // coordinated move linear part at requested feed rate - float abc_time = 0; // coordinated move rotary part at requested feed rate - float max_time = 0; // time required for the rate-limiting axis - float tmp_time = 0; // used in computation - gms->minimum_time = 8675309; // arbitrarily large number + float inv_time = 0; // inverse time if doing a feed in G93 mode + float xyz_time = 0; // linear coordinated move at requested feed + float abc_time = 0; // rotary coordinated move at requested feed + float max_time = 0; // time required for the rate-limiting axis + float tmp_time = 0; // used in computation + gms->minimum_time = 8675309; // arbitrarily large number // compute times for feed motion if (gms->motion_mode != MOTION_MODE_STRAIGHT_TRAVERSE) { if (gms->feed_rate_mode == INVERSE_TIME_MODE) { - inv_time = gms->feed_rate; // NB: feed rate was un-inverted to minutes by cm_set_feed_rate() + // feed rate was un-inverted to minutes by cm_set_feed_rate() + inv_time = gms->feed_rate; gms->feed_rate_mode = UNITS_PER_MINUTE_MODE; } else { - // compute length of linear move in millimeters. Feed rate is provided as mm/min - xyz_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + axis_square[AXIS_Z]) / gms->feed_rate; + // compute length of linear move in millimeters. Feed rate is provided as + // mm/min + xyz_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + + axis_square[AXIS_Z]) / gms->feed_rate; // if no linear axes, compute length of multi-axis rotary move in degrees. // Feed rate is provided as degrees/min if (fp_ZERO(xyz_time)) - abc_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + axis_square[AXIS_C]) / gms->feed_rate; + abc_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + + axis_square[AXIS_C]) / gms->feed_rate; } } @@ -387,97 +408,101 @@ static void _calc_move_times(GCodeState_t *gms, const float axis_length[], const /* Plans the entire block list * - * The block list is the circular buffer of planner buffers - * (bf's). The block currently being planned is the "bf" block. The - * "first block" is the next block to execute; queued immediately - * behind the currently executing block, aka the "running" block. - * In some cases there is no first block because the list is empty - * or there is only one block and it is already running. + * The block list is the circular buffer of planner buffers + * (bf's). The block currently being planned is the "bf" block. The + * "first block" is the next block to execute; queued immediately + * behind the currently executing block, aka the "running" block. + * In some cases there is no first block because the list is empty + * or there is only one block and it is already running. * - * If blocks following the first block are already optimally - * planned (non replannable) the first block that is not optimally - * planned becomes the effective first block. + * If blocks following the first block are already optimally + * planned (non replannable) the first block that is not optimally + * planned becomes the effective first block. * - * _plan_block_list() plans all blocks between and including the - * (effective) first block and the bf. It sets entry, exit and - * cruise v's from vmax's then calls trapezoid generation. + * mp_plan_block_list() plans all blocks between and including the + * (effective) first block and the bf. It sets entry, exit and + * cruise v's from vmax's then calls trapezoid generation. * - * Variables that must be provided in the mpBuffers that will be - * processed: + * Variables that must be provided in the mpBuffers that will be + * processed: * - * bf (function arg) - end of block list (last block in time) - * bf->replannable - start of block list set by last FALSE value [Note 1] - * bf->move_type - typically MOVE_TYPE_ALINE. Other move_types should be set to - * length=0, entry_vmax=0 and exit_vmax=0 and are treated - * as a momentary stop (plan to zero and from zero). + * bf (function arg) - end of block list (last block in time) + * bf->replannable - start of block list set by last FALSE value + * [Note 1] + * bf->move_type - typically MOVE_TYPE_ALINE. Other move_types should + * be set to length=0, entry_vmax=0 and exit_vmax=0 + * and are treated as a momentary stop (plan to zero + * and from zero). * - * bf->length - provides block length - * bf->entry_vmax - used during forward planning to set entry velocity - * bf->cruise_vmax - used during forward planning to set cruise velocity - * bf->exit_vmax - used during forward planning to set exit velocity - * bf->delta_vmax - used during forward planning to set exit velocity + * bf->length - provides block length + * bf->entry_vmax - used during forward planning to set entry velocity + * bf->cruise_vmax - used during forward planning to set cruise velocity + * bf->exit_vmax - used during forward planning to set exit velocity + * bf->delta_vmax - used during forward planning to set exit velocity * - * bf->recip_jerk - used during trapezoid generation - * bf->cbrt_jerk - used during trapezoid generation + * bf->recip_jerk - used during trapezoid generation + * bf->cbrt_jerk - used during trapezoid generation * - * Variables that will be set during processing: + * Variables that will be set during processing: * - * bf->replannable - set if the block becomes optimally planned + * bf->replannable - set if the block becomes optimally planned * - * bf->braking_velocity - set during backward planning - * bf->entry_velocity - set during forward planning - * bf->cruise_velocity - set during forward planning - * bf->exit_velocity - set during forward planning + * bf->braking_velocity - set during backward planning + * bf->entry_velocity - set during forward planning + * bf->cruise_velocity - set during forward planning + * bf->exit_velocity - set during forward planning * - * bf->head_length - set during trapezoid generation - * bf->body_length - set during trapezoid generation - * bf->tail_length - set during trapezoid generation + * bf->head_length - set during trapezoid generation + * bf->body_length - set during trapezoid generation + * bf->tail_length - set during trapezoid generation * - * Variables that are ignored but here's what you would expect them to be: + * Variables that are ignored but here's what you would expect them to be: * - * bf->move_state - NEW for all blocks but the earliest - * bf->target[] - block target position - * bf->unit[] - block unit vector - * bf->time - gets set later - * bf->jerk - source of the other jerk variables. Used in mr. + * bf->move_state - NEW for all blocks but the earliest + * bf->target[] - block target position + * bf->unit[] - block unit vector + * bf->time - gets set later + * bf->jerk - source of the other jerk variables. Used in mr. * * Notes: * - * [1] Whether or not a block is planned is controlled by the - * bf->replannable setting (set TRUE if it should be). Replan flags - * are checked during the backwards pass and prune the replan list - * to include only the the latest blocks that require planning - * - * In normal operation the first block (currently running - * block) is not replanned, but may be for feedholds and feed - * overrides. In these cases the prep routines modify the - * contents of the mr buffer and re-shuffle the block list, - * re-enlisting the current bf buffer with new parameters. - * These routines also set all blocks in the list to be - * replannable so the list can be recomputed regardless of - * exact stops and previous replanning optimizations. - * - * [2] The mr_flag is used to tell replan to account for mr - * buffer's exit velocity (Vx) mr's Vx is always found in the - * provided bf buffer. Used to replan feedholds + * [1] Whether or not a block is planned is controlled by the + * bf->replannable setting (set TRUE if it should be). Replan flags + * are checked during the backwards pass and prune the replan list + * to include only the the latest blocks that require planning + * + * In normal operation the first block (currently running + * block) is not replanned, but may be for feedholds and feed + * overrides. In these cases the prep routines modify the + * contents of the mr buffer and re-shuffle the block list, + * re-enlisting the current bf buffer with new parameters. + * These routines also set all blocks in the list to be + * replannable so the list can be recomputed regardless of + * exact stops and previous replanning optimizations. + * + * [2] The mr_flag is used to tell replan to account for mr + * buffer's exit velocity (Vx) mr's Vx is always found in the + * provided bf buffer. Used to replan feedholds */ -static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { +void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { mpBuf_t *bp = bf; // Backward planning pass. Find first block and update the braking velocities. // At the end *bp points to the buffer before the first block. while ((bp = mp_get_prev_buffer(bp)) != bf) { if (!bp->replannable) break; - bp->braking_velocity = min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax; + bp->braking_velocity = + min(bp->nx->entry_vmax, bp->nx->braking_velocity) + bp->delta_vmax; } - // forward planning pass - recomputes trapezoids in the list from the first block to the bf block. + // forward planning pass - recomputes trapezoids in the list from the first + // block to the bf block. while ((bp = mp_get_next_buffer(bp)) != bf) { if (bp->pv == bf || *mr_flag == true) { bp->entry_velocity = bp->entry_vmax; // first block in the list *mr_flag = false; - } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks in the list + } else bp->entry_velocity = bp->pv->exit_velocity; // other blocks in list bp->cruise_velocity = bp->cruise_vmax; bp->exit_velocity = min4(bp->exit_vmax, @@ -487,9 +512,12 @@ static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { mp_calculate_trapezoid(bp); - // test for optimally planned trapezoids - only need to check various exit conditions - if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) || fp_EQ(bp->exit_velocity, bp->nx->entry_vmax)) || - (!bp->pv->replannable && fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax)))) + // test for optimally planned trapezoids - only need to check various exit + // conditions + if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) || + fp_EQ(bp->exit_velocity, bp->nx->entry_vmax)) || + (!bp->pv->replannable && + fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax)))) bp->replannable = false; } @@ -501,85 +529,72 @@ static void _plan_block_list(mpBuf_t *bf, uint8_t *mr_flag) { } -/// Resets all blocks in the planning list to be replannable -static void _reset_replannable_list() { - mpBuf_t *bf = mp_get_first_buffer(); - if (!bf) return; - - mpBuf_t *bp = bf; - - do { - bp->replannable = true; - } while (((bp = mp_get_next_buffer(bp)) != bf) && (bp->move_state != MOVE_OFF)); -} - - /* Sonny's algorithm - simple * - * Computes the maximum allowable junction speed by finding the - * velocity that will yield the centripetal acceleration in the - * corner_acceleration value. The value of delta sets the effective - * radius of curvature. Here's Sonny's (Sungeun K. Jeon's) - * explanation of what's going on: - * - * "First let's assume that at a junction we only look a centripetal - * acceleration to simply things. At a junction of two lines, let's - * place a circle such that both lines are tangent to the circle. The - * circular segment joining the lines represents the path for - * constant centripetal acceleration. This creates a deviation from - * the path (let's call this delta), which is the distance from the - * junction to the edge of the circular segment. Delta needs to be - * defined, so let's replace the term max_jerk (see note 1) with - * max_junction_deviation, or "delta". This indirectly sets the - * radius of the circle, and hence limits the velocity by the - * centripetal acceleration. Think of the this as widening the race - * track. If a race car is driving on a track only as wide as a car, - * it'll have to slow down a lot to turn corners. If we widen the - * track a bit, the car can start to use the track to go into the - * turn. The wider it is, the faster through the corner it can go. + * Computes the maximum allowable junction speed by finding the + * velocity that will yield the centripetal acceleration in the + * corner_acceleration value. The value of delta sets the effective + * radius of curvature. Here's Sonny's (Sungeun K. Jeon's) + * explanation of what's going on: + * + * "First let's assume that at a junction we only look a centripetal + * acceleration to simply things. At a junction of two lines, let's + * place a circle such that both lines are tangent to the circle. The + * circular segment joining the lines represents the path for + * constant centripetal acceleration. This creates a deviation from + * the path (let's call this delta), which is the distance from the + * junction to the edge of the circular segment. Delta needs to be + * defined, so let's replace the term max_jerk (see note 1) with + * max_junction_deviation, or "delta". This indirectly sets the + * radius of the circle, and hence limits the velocity by the + * centripetal acceleration. Think of the this as widening the race + * track. If a race car is driving on a track only as wide as a car, + * it'll have to slow down a lot to turn corners. If we widen the + * track a bit, the car can start to use the track to go into the + * turn. The wider it is, the faster through the corner it can go. * * (Note 1: "max_jerk" refers to the old grbl/marlin max_jerk" - * approximation term, not the tinyG jerk terms) - * - * If you do the geometry in terms of the known variables, you get: - * sin(theta/2) = R/(R+delta) - * Re-arranging in terms of circle radius (R) - * R = delta*sin(theta/2)/(1-sin(theta/2). - * - * Theta is the angle between line segments given by: - * cos(theta) = dot(a,b)/(norm(a)*norm(b)). - * - * Most of these calculations are already done in the planner. - * To remove the acos() and sin() computations, use the trig half - * angle identity: - * sin(theta/2) = +/- sqrt((1-cos(theta))/2). - * - * For our applications, this should always be positive. Now just - * plug the equations into the centripetal acceleration equation: - * v_c = sqrt(a_max*R). You'll see that there are only two sqrt - * computations and no sine/cosines." - * - * How to compute the radius using brute-force trig: - * float theta = acos(costheta); - * float radius = delta * sin(theta/2)/(1-sin(theta/2)); - * - * This version extends Chamnit's algorithm by computing a value for - * delta that takes the contributions of the individual axes in the - * move into account. This allows the control radius to vary by - * axis. This is necessary to support axes that have different - * dynamics; such as a Z axis that doesn't move as fast as X and Y - * (such as a screw driven Z axis on machine with a belt driven XY - - * like a Shapeoko), or rotary axes ABC that have completely - * different dynamics than their linear counterparts. - * - * The function takes the absolute values of the sum of the unit - * vector components as a measure of contribution to the move, then - * scales the delta values from the non-zero axes into a composite - * delta to be used for the move. Shown for an XY vector: - * - * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) - * Usum Length of sums Ux + Uy - * d Delta of sums (Dx*Ux+DY*UY)/Usum + * approximation term, not the tinyG jerk terms) + * + * If you do the geometry in terms of the known variables, you get: + * sin(theta/2) = R/(R+delta) + * Re-arranging in terms of circle radius (R) + * R = delta*sin(theta/2)/(1-sin(theta/2). + * + * Theta is the angle between line segments given by: + * cos(theta) = dot(a,b)/(norm(a)*norm(b)). + * + * Most of these calculations are already done in the planner. + * To remove the acos() and sin() computations, use the trig half + * angle identity: + * sin(theta/2) = +/- sqrt((1-cos(theta))/2). + * + * For our applications, this should always be positive. Now just + * plug the equations into the centripetal acceleration equation: + * v_c = sqrt(a_max*R). You'll see that there are only two sqrt + * computations and no sine/cosines." + * + * How to compute the radius using brute-force trig: + * float theta = acos(costheta); + * float radius = delta * sin(theta/2)/(1-sin(theta/2)); + * + * This version extends Chamnit's algorithm by computing a value for + * delta that takes the contributions of the individual axes in the + * move into account. This allows the control radius to vary by + * axis. This is necessary to support axes that have different + * dynamics; such as a Z axis that doesn't move as fast as X and Y + * (such as a screw driven Z axis on machine with a belt driven XY - + * like a Shapeoko), or rotary axes ABC that have completely + * different dynamics than their linear counterparts. + * + * The function takes the absolute values of the sum of the unit + * vector components as a measure of contribution to the move, then + * scales the delta values from the non-zero axes into a composite + * delta to be used for the move. Shown for an XY vector: + * + * U[i] Unit sum of i'th axis fabs(unit_a[i]) + fabs(unit_b[i]) + * Usum Length of sums Ux + Uy + * d Delta of sums (Dx*Ux+DY*UY)/Usum */ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { float costheta = @@ -615,199 +630,3 @@ static float _get_junction_vmax(const float a_unit[], const float b_unit[]) { return velocity; } - - -/* feedholds - functions for performing holds - * - * Feedhold is executed as cm.hold_state transitions executed inside - * _exec_aline() and main loop callbacks to these functions: - * mp_plan_hold_callback() and mp_end_hold(). - * - * Holds work like this: - * - * - Hold is asserted by calling cm_feedhold() (usually invoked - * via a ! char) If hold_state is OFF and motion_state is - * RUNning it sets hold_state to SYNC and motion_state to HOLD. - * - * - Hold state == SYNC tells the aline exec routine to execute - * the next aline segment then set hold_state to PLAN. This - * gives the planner sufficient time to replan the block list - * for the hold before the next aline segment needs to be - * processed. - * - * - Hold state == PLAN tells the planner to replan the mr - * buffer, the current run buffer (bf), and any subsequent bf - * buffers as necessary to execute a hold. Hold planning - * replans the planner buffer queue down to zero and then back - * up from zero. Hold state is set to DECEL when planning is - * complete. - * - * - Hold state == DECEL persists until the aline execution runs - * to zero velocity, at which point hold state transitions to - * HOLD. - * - * - Hold state == HOLD persists until the cycle is restarted. A - * cycle start is an asynchronous event that sets the - * cycle_start_flag TRUE. It can occur any time after the hold - * is requested - either before or after motion stops. - * - * - mp_end_hold() is executed from - * cm_feedhold_sequencing_callback() once the hold state == - * HOLD and a cycle_start has been requested.This sets the hold - * state to OFF which enables _exec_aline() to continue - * processing. Move execution begins with the first buffer - * after the hold. - * - * Terms used: - * - mr is the runtime buffer. It was initially loaded from the bf - * buffer - * - bp+0 is the "companion" bf buffer to the mr buffer. - * - bp+1 is the bf buffer following bp+0. This runs through bp+N - * - bp (by itself) just refers to the current buffer being - * adjusted / replanned - * - * Details: Planning re-uses bp+0 as an "extra" buffer. Normally - * bp+0 is returned to the buffer pool as it is redundant once - * mr is loaded. Use the extra buffer to split the move in two - * where the hold decelerates to zero. Use one buffer to go to - * zero, the other to replan up from zero. All buffers past - * that point are unaffected other than that they need to be - * replanned for velocity. - * - * Note: There are multiple opportunities for more efficient - * organization of code in this module, but the code is so - * complicated I just left it organized for clarity and hoped - * for the best from compiler optimization. - */ -static float _compute_next_segment_velocity() { - if (mr.section == SECTION_BODY) return mr.segment_velocity; -#ifdef __JERK_EXEC - return mr.segment_velocity; // an approximation -#else - return mr.segment_velocity + mr.forward_diff_5; -#endif -} - - -/// replan block list to execute hold -stat_t mp_plan_hold_callback() { - if (cm.hold_state != FEEDHOLD_PLAN) - return STAT_NOOP; // not planning a feedhold - - mpBuf_t *bp; // working buffer pointer - if ((bp = mp_get_run_buffer()) == 0) - return STAT_NOOP; // Oops! nothing's running - - uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx - float mr_available_length; // available length left in mr buffer for deceleration - float braking_velocity; // velocity left to shed to brake to zero - float braking_length; // distance required to brake to zero from braking_velocity - - // examine and process mr buffer - mr_available_length = get_axis_vector_length(mr.target, mr.position); - - // compute next_segment velocity - braking_velocity = _compute_next_segment_velocity(); - braking_length = mp_get_target_length(braking_velocity, 0, bp); // bp is OK to use here - - // Hack to prevent Case 2 moves for perfect-fit decels. Happens in - // homing situations The real fix: The braking velocity cannot - // simply be the mr.segment_velocity as this is the velocity of the - // last segment, not the one that's going to be executed next. The - // braking_velocity needs to be the velocity of the next segment - // that has not yet been computed. In the mean time, this hack will - // work. - if ((braking_length > mr_available_length) && (fp_ZERO(bp->exit_velocity))) - braking_length = mr_available_length; - - // Case 1: deceleration fits entirely into the length remaining in mr buffer - if (braking_length <= mr_available_length) { - // set mr to a tail to perform the deceleration - mr.exit_velocity = 0; - mr.tail_length = braking_length; - mr.cruise_velocity = braking_velocity; - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - - // re-use bp+0 to be the hold point and to run the remaining block length - bp->length = mr_available_length - braking_length; - bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); - bp->entry_vmax = 0; // set bp+0 as hold point - bp->move_state = MOVE_NEW; // tell _exec to re-use the bf buffer - - _reset_replannable_list(); // make it replan all the blocks - _plan_block_list(mp_get_last_buffer(), &mr_flag); - cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit - - return STAT_OK; - } - - // Case 2: deceleration exceeds length remaining in mr buffer - // First, replan mr to minimum (but non-zero) exit velocity - - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - mr.tail_length = mr_available_length; - mr.cruise_velocity = braking_velocity; - mr.exit_velocity = braking_velocity - mp_get_target_velocity(0, mr_available_length, bp); - - // Find the point where deceleration reaches zero. This could span multiple buffers. - braking_velocity = mr.exit_velocity; // adjust braking velocity downward - bp->move_state = MOVE_NEW; // tell _exec to re-use buffer - - for (uint8_t i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { // a safety to avoid wraparound - mp_copy_buffer(bp, bp->nx); // copy bp+1 into bp+0 (and onward...) - - if (bp->move_type != MOVE_TYPE_ALINE) { // skip any non-move buffers - bp = mp_get_next_buffer(bp); // point to next buffer - continue; - } - - bp->entry_vmax = braking_velocity; // velocity we need to shed - braking_length = mp_get_target_length(braking_velocity, 0, bp); - - if (braking_length > bp->length) { // decel does not fit in bp buffer - bp->exit_vmax = braking_velocity - mp_get_target_velocity(0, bp->length, bp); - braking_velocity = bp->exit_vmax; // braking velocity for next buffer - bp = mp_get_next_buffer(bp); // point to next buffer - continue; - } - - break; - } - - // Deceleration now fits in the current bp buffer - // Plan the first buffer of the pair as the decel, the second as the accel - bp->length = braking_length; - bp->exit_vmax = 0; - - bp = mp_get_next_buffer(bp); // point to the acceleration buffer - bp->entry_vmax = 0; - bp->length -= braking_length; // the buffers were identical (and hence their lengths) - bp->delta_vmax = mp_get_target_velocity(0, bp->length, bp); - bp->exit_vmax = bp->delta_vmax; - - _reset_replannable_list(); // make it replan all the blocks - _plan_block_list(mp_get_last_buffer(), &mr_flag); - cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit - - return STAT_OK; -} - - -/// End a feedhold, release the hold and restart block list -stat_t mp_end_hold() { - if (cm.hold_state == FEEDHOLD_END_HOLD) { - cm.hold_state = FEEDHOLD_OFF; - - if (!mp_get_run_buffer()) { // 0 means nothing's running - cm_set_motion_state(MOTION_STOP); - return STAT_NOOP; - } - - cm.motion_state = MOTION_RUN; - st_request_exec_move(); // restart the steppers - } - - return STAT_OK; -} diff --git a/src/plan/planner.c b/src/plan/planner.c index b392574..507c9ea 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -5,52 +5,61 @@ * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * Copyright (c) 2012 - 2015 Rob Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ -/* --- Planner Notes ---- - * - * The planner works below the canonical machine and above the motor mapping - * and stepper execution layers. A rudimentary multitasking capability is - * implemented for long-running commands such as lines, arcs, and dwells. - * These functions are coded as non-blocking continuations - which are simple - * state machines that are re-entered multiple times until a particular - * operation is complete. These functions have 2 parts - the initial call, - * which sets up the local context, and callbacks (continuations) that are - * called from the main loop (in controller.c). - * - * One important concept is isolation of the three layers of the data model - - * the Gcode model (gm), planner model (bf queue & mm), and runtime model (mr). - * These are designated as "model", "planner" and "runtime" in function names. - * - * The Gcode model is owned by the canonical machine and should only be accessed - * by cm_xxxx() functions. Data from the Gcode model is transferred to the planner - * by the mp_xxx() functions called by the canonical machine. - * - * The planner should only use data in the planner model. When a move (block) - * is ready for execution the planner data is transferred to the runtime model, - * which should also be isolated. - * - * Lower-level models should never use data from upper-level models as the data - * may have changed and lead to unpredictable results. +/* Planner Notes + * + * The planner works below the canonical machine and above the + * motor mapping and stepper execution layers. A rudimentary + * multitasking capability is implemented for long-running commands + * such as lines, arcs, and dwells. These functions are coded as + * non-blocking continuations - which are simple state machines + * that are re-entered multiple times until a particular operation + * is complete. These functions have 2 parts - the initial call, + * which sets up the local context, and callbacks (continuations) + * that are called from the main loop (in controller.c). + * + * One important concept is isolation of the three layers of the + * data model - the Gcode model (gm), planner model (bf queue & + * mm), and runtime model (mr). These are designated as "model", + * "planner" and "runtime" in function names. + * + * The Gcode model is owned by the canonical machine and should + * only be accessed by cm_xxxx() functions. Data from the Gcode + * model is transferred to the planner by the mp_xxx() functions + * called by the canonical machine. + * + * The planner should only use data in the planner model. When a + * move (block) is ready for execution the planner data is + * transferred to the runtime model, which should also be isolated. + * + * Lower-level models should never use data from upper-level models + * as the data may have changed and lead to unpredictable results. */ #include "planner.h" @@ -69,32 +78,22 @@ mpBufferPool_t mb; // move buffer queue mpMoveMasterSingleton_t mm; // context for line planning mpMoveRuntimeSingleton_t mr; // context for line runtime -#define _bump(a) ((a < PLANNER_BUFFER_POOL_SIZE - 1) ? a + 1 : 0) // buffer incr & wrap -#define spindle_speed move_time // local alias for spindle_speed to the time variable -#define value_vector gm.target // alias for vector of values -#define flag_vector unit // alias for vector of flags - - -// Execution routines (NB: These are all called from the LO interrupt) -static stat_t _exec_dwell(mpBuf_t *bf); -static stat_t _exec_command(mpBuf_t *bf); - void planner_init() { - // If you know all memory has been zeroed by a hard reset you don't need these next 2 lines + // If you know all memory has been zeroed by a hard reset you don't need + // these next 2 lines memset(&mr, 0, sizeof(mr)); // clear all values, pointers and status memset(&mm, 0, sizeof(mm)); // clear all values, pointers and status mp_init_buffers(); } -/* - * Flush all moves in the planner and all arcs +/* Flush all moves in the planner and all arcs * - * Does not affect the move currently running in mr. - * Does not affect mm or gm model positions - * This function is designed to be called during a hold to reset the planner - * This function should not generally be called; call cm_queue_flush() instead + * Does not affect the move currently running in mr. Does not affect + * mm or gm model positions. This function is designed to be called + * during a hold to reset the planner. This function should not + * generally be called; call cm_queue_flush() instead. */ void mp_flush_planner() { cm_abort_arc(); @@ -103,44 +102,54 @@ void mp_flush_planner() { } -/* - * Since steps are in motor space you have to run the position vector through inverse - * kinematics to get the right numbers. This means that in a non-Cartesian robot changing - * any position can result in changes to multiple step values. So this operation is provided - * as a single function and always uses the new position vector as an input. +/* Since steps are in motor space you have to run the position vector + * through inverse kinematics to get the right numbers. This means + * that in a non-Cartesian robot changing any position can result in + * changes to multiple step values. So this operation is provided as a + * single function and always uses the new position vector as an + * input. * - * Keeping track of position is complicated by the fact that moves exist in several reference - * frames. The scheme to keep this straight is: + * Keeping track of position is complicated by the fact that moves + * exist in several reference frames. The scheme to keep this + * straight is: * - * - mm.position - start and end position for planning - * - mr.position - current position of runtime segment - * - mr.target - target position of runtime segment - * - mr.endpoint - final target position of runtime segment + * - mm.position - start and end position for planning + * - mr.position - current position of runtime segment + * - mr.target - target position of runtime segment + * - mr.endpoint - final target position of runtime segment * - * Note that position is set immediately when called and may not be not an accurate representation - * of the tool position. The motors are still processing the action and the real tool position is - * still close to the starting point. + * Note that position is set immediately when called and may not be + * not an accurate representation of the tool position. The motors + * are still processing the action and the real tool position is + * still close to the starting point. */ /// Set planner position for a single axis -void mp_set_planner_position(uint8_t axis, const float position) {mm.position[axis] = position;} +void mp_set_planner_position(uint8_t axis, const float position) { + mm.position[axis] = position; +} /// Set runtime position for a single axis -void mp_set_runtime_position(uint8_t axis, const float position) {mr.position[axis] = position;} +void mp_set_runtime_position(uint8_t axis, const float position) { + mr.position[axis] = position; +} /// Set encoder counts to the runtime position void mp_set_steps_to_runtime_position() { float step_position[MOTORS]; - ik_kinematics(mr.position, step_position); // convert lengths to steps in floating point + // convert lengths to steps in floating point + ik_kinematics(mr.position, step_position); for (uint8_t motor = 0; motor < MOTORS; motor++) { mr.target_steps[motor] = step_position[motor]; mr.position_steps[motor] = step_position[motor]; mr.commanded_steps[motor] = step_position[motor]; - en_set_encoder_steps(motor, step_position[motor]); // write steps to encoder register + + // write steps to encoder register + en_set_encoder_steps(motor, step_position[motor]); // These must be zero: mr.following_error[motor] = 0; @@ -149,263 +158,3 @@ void mp_set_steps_to_runtime_position() { } -/************************************************************************************ - * How this works: - * - The command is called by the Gcode interpreter (cm_, e.g. an M code) - * - cm_ function calls mp_queue_command which puts it in the planning queue (bf buffer). - * This involves setting some parameters and registering a callback to the - * execution function in the canonical machine - * - the planning queue gets to the function and calls _exec_command() - * - ...which puts a pointer to the bf buffer in the prep struct (st_pre) - * - When the runtime gets to the end of the current activity (sending steps, counting a dwell) - * if executes mp_runtime_command... - * - ...which uses the callback function in the bf and the saved parameters in the vectors - * - To finish up mp_runtime_command() needs to free the bf buffer - * - * Doing it this way instead of synchronizing on queue empty simplifies the - * handling of feedholds, feed overrides, buffer flushes, and thread blocking, - * and makes keeping the queue full much easier - therefore avoiding starvation - */ - -/// Queue a synchronous Mcode, program control, or other command -void mp_queue_command(void(*cm_exec)(float[], float[]), float *value, float *flag) { - mpBuf_t *bf; - - // Never supposed to fail as buffer availability was checked upstream in the controller - if ((bf = mp_get_write_buffer()) == 0) { - cm_hard_alarm(STAT_BUFFER_FULL_FATAL); - return; - } - - bf->move_type = MOVE_TYPE_COMMAND; - bf->bf_func = _exec_command; // callback to planner queue exec function - bf->cm_func = cm_exec; // callback to canonical machine exec function - - for (uint8_t axis = AXIS_X; axis < AXES; axis++) { - bf->value_vector[axis] = value[axis]; - bf->flag_vector[axis] = flag[axis]; - } - - mp_commit_write_buffer(MOVE_TYPE_COMMAND); // must be final operation before exit -} - - -/// callback to execute command -static stat_t _exec_command(mpBuf_t *bf) { - st_prep_command(bf); - return STAT_OK; -} - - -stat_t mp_runtime_command(mpBuf_t *bf) { - bf->cm_func(bf->value_vector, bf->flag_vector); // 2 vectors used by callbacks - - // free buffer & perform cycle_end if planner is empty - if (mp_free_run_buffer()) cm_cycle_end(); - - return STAT_OK; -} - - -/* Dwells are performed by passing a dwell move to the stepper drivers. - * When the stepper driver sees a dwell it times the dwell on a separate - * timer than the stepper pulse timer. - */ - -/// Queue a dwell -stat_t mp_dwell(float seconds) { - mpBuf_t *bf; - - if ((bf = mp_get_write_buffer()) == 0) // get write buffer or fail - return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // not ever supposed to fail - - bf->bf_func = _exec_dwell; // register callback to dwell start - bf->gm.move_time = seconds; // in seconds, not minutes - bf->move_state = MOVE_NEW; - mp_commit_write_buffer(MOVE_TYPE_DWELL); // must be final operation before exit - - return STAT_OK; -} - - -/// Dwell execution -static stat_t _exec_dwell(mpBuf_t *bf) { - st_prep_dwell((uint32_t)(bf->gm.move_time * 1000000)); // convert seconds to uSec - if (mp_free_run_buffer()) cm_cycle_end(); // free buffer & perform cycle_end if planner is empty - - return STAT_OK; -} - - -/**** PLANNER BUFFERS ***************************************************** - * - * Planner buffers are used to queue and operate on Gcode blocks. Each buffer - * contains one Gcode block which may be a move, and M code, or other command - * that must be executed synchronously with movement. - * - * Buffers are in a circularly linked list managed by a WRITE pointer and a RUN pointer. - * New blocks are populated by (1) getting a write buffer, (2) populating the buffer, - * then (3) placing it in the queue (queue write buffer). If an exception occurs - * during population you can unget the write buffer before queuing it, which returns - * it to the pool of available buffers. - * - * The RUN buffer is the buffer currently executing. It may be retrieved once for - * simple commands, or multiple times for long-running commands like moves. When - * the command is complete the run buffer is returned to the pool by freeing it. - * - * Notes: - * The write buffer pointer only moves forward on _queue_write_buffer, and - * the read buffer pointer only moves forward on free_read calls. - * (test, get and unget have no effect) - */ - - -/// Returns # of available planner buffers -uint8_t mp_get_planner_buffers_available() {return mb.buffers_available;} - - -/// Initializes or resets buffers -void mp_init_buffers() { - mpBuf_t *pv; - - memset(&mb, 0, sizeof(mb)); // clear all values, pointers and status - - mb.w = &mb.bf[0]; // init write and read buffer pointers - mb.q = &mb.bf[0]; - mb.r = &mb.bf[0]; - pv = &mb.bf[PLANNER_BUFFER_POOL_SIZE - 1]; - - for (uint8_t i = 0; i < PLANNER_BUFFER_POOL_SIZE; i++) { // setup ring pointers - mb.bf[i].nx = &mb.bf[_bump(i)]; - mb.bf[i].pv = pv; - pv = &mb.bf[i]; - } - - mb.buffers_available = PLANNER_BUFFER_POOL_SIZE; -} - - -/// Get pointer to next available write buffer -/// Returns pointer or 0 if no buffer available. -mpBuf_t *mp_get_write_buffer() { - // get & clear a buffer - if (mb.w->buffer_state == MP_BUFFER_EMPTY) { - mpBuf_t *w = mb.w; - mpBuf_t *nx = mb.w->nx; // save linked list pointers - mpBuf_t *pv = mb.w->pv; - memset(mb.w, 0, sizeof(mpBuf_t)); // clear all values - w->nx = nx; // restore pointers - w->pv = pv; - w->buffer_state = MP_BUFFER_LOADING; - mb.buffers_available--; - mb.w = w->nx; - - return w; - } - - return 0; -} - - -/// Free write buffer if you decide not to commit it. -void mp_unget_write_buffer() { - mb.w = mb.w->pv; // queued --> write - mb.w->buffer_state = MP_BUFFER_EMPTY; // not loading anymore - mb.buffers_available++; -} - - -/* Commit the next write buffer to the queue - * Advances write pointer & changes buffer state - * - * WARNING: The calling routine must not use the write buffer - * once it has been queued as it may be processed and freed (wiped) - * before mp_queue_write_buffer() returns. - * - * WARNING: The routine calling mp_commit_write_buffer() must not use the write buffer - * once it has been queued. Action may start on the buffer immediately, - * invalidating its contents - */ -void mp_commit_write_buffer(const uint8_t move_type) { - mb.q->move_type = 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 - // NB: BEWARE! the exec may result in the planner buffer being - // processed immediately and then freed - invalidating the contents -} - - -/* Get pointer to the next or current run buffer - * Returns a new run buffer if prev buf was ENDed - * Returns same buf if called again before ENDing - * Returns 0 if no buffer available - * The behavior supports continuations (iteration) - */ -mpBuf_t *mp_get_run_buffer() { - // CASE: fresh buffer; becomes running if queued or pending - if ((mb.r->buffer_state == MP_BUFFER_QUEUED) || - (mb.r->buffer_state == MP_BUFFER_PENDING)) - mb.r->buffer_state = MP_BUFFER_RUNNING; - - // CASE: asking for the same run buffer for the Nth time - if (mb.r->buffer_state == MP_BUFFER_RUNNING) return mb.r; // return same buffer - - return 0; // CASE: no queued buffers. fail it. -} - - -/* Release the run buffer & return to buffer pool. - * Returns true if queue is empty, false otherwise. - * This is useful for doing queue empty / end move functions. - */ -uint8_t mp_free_run_buffer() { // EMPTY current run buf & adv to next - mp_clear_buffer(mb.r); // clear it out (& reset replannable) - mb.r = mb.r->nx; // advance to next run buffer - - if (mb.r->buffer_state == MP_BUFFER_QUEUED) // only if queued... - mb.r->buffer_state = MP_BUFFER_PENDING; // pend next buffer - - mb.buffers_available++; - - return mb.w == mb.r; // return true if the queue emptied -} - - -/// Returns pointer to first buffer, i.e. the running block -mpBuf_t *mp_get_first_buffer() { - return mp_get_run_buffer(); // returns buffer or 0 if nothing's running -} - - -/// Returns pointer to last buffer, i.e. last block (zero) -mpBuf_t *mp_get_last_buffer() { - mpBuf_t *bf = mp_get_run_buffer(); - mpBuf_t *bp; - - for (bp = bf; bp && bp->nx != bf; bp = mp_get_next_buffer(bp)) - if (bp->nx->move_state == MOVE_OFF) break; - - return bp; -} - - -/// Zeroes the contents of the buffer -void mp_clear_buffer(mpBuf_t *bf) { - mpBuf_t *nx = bf->nx; // save pointers - mpBuf_t *pv = bf->pv; - memset(bf, 0, sizeof(mpBuf_t)); - bf->nx = nx; // restore pointers - bf->pv = pv; -} - - -/// Copies the contents of bp into bf - preserves links -void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp) { - mpBuf_t *nx = bf->nx; // save pointers - mpBuf_t *pv = bf->pv; - memcpy(bf, bp, sizeof(mpBuf_t)); - bf->nx = nx; // restore pointers - bf->pv = pv; -} diff --git a/src/plan/planner.h b/src/plan/planner.h index ba347d4..4e58e95 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -36,6 +36,7 @@ #define PLANNER_H #include "canonical_machine.h" // used for GCodeState_t +#include "util.h" #include "config.h" enum moveType { // bf->move_type values @@ -43,10 +44,7 @@ enum moveType { // bf->move_type values MOVE_TYPE_ALINE, // acceleration planned line MOVE_TYPE_DWELL, // delay with no movement MOVE_TYPE_COMMAND, // general command - MOVE_TYPE_TOOL, // T command - MOVE_TYPE_SPINDLE_SPEED, // S command - MOVE_TYPE_STOP, // program stop - MOVE_TYPE_END // program end + MOVE_TYPE_JOG, // interactive jogging }; enum moveState { @@ -93,12 +91,12 @@ enum sectionState { #define MIN_BLOCK_TIME MIN_SEGMENT_TIME #define MIN_SEGMENT_TIME_PLUS_MARGIN \ - ((MIN_SEGMENT_USEC+1) / MICROSECONDS_PER_MINUTE) + ((MIN_SEGMENT_USEC + 1) / MICROSECONDS_PER_MINUTE) /* PLANNER_BUFFER_POOL_SIZE - * Should be at least the number of buffers requires to support optimal - * planning in the case of very short lines or arc segments. - * Suggest 12 min. Limit is 255 + * Should be at least the number of buffers requires to support optimal + * planning in the case of very short lines or arc segments. + * Suggest 12 min. Limit is 255 */ #define PLANNER_BUFFER_POOL_SIZE 32 /// Buffers to reserve in planner before processing new input line @@ -109,18 +107,18 @@ enum sectionState { /// Max iterations for convergence in the HT asymmetric case. #define TRAPEZOID_ITERATION_MAX 10 -/// Error percentage for iteration convergence. As percent - 0.01 = 1% +/// Error percentage for iteration convergence. As percent - 0.01 = 1% #define TRAPEZOID_ITERATION_ERROR_PERCENT ((float)0.10) /// Tolerance for "exact fit" for H and T cases /// allowable mm of error in planning phase #define TRAPEZOID_LENGTH_FIT_TOLERANCE ((float)0.0001) -/// Adaptive velocity tolerance term +/// Adaptive velocity tolerance term #define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100)) -/// callback to canonical_machine execution function +/// Callback to canonical_machine execution function typedef void (*cm_exec_t)(float[], float[]); @@ -265,29 +263,38 @@ extern mpBufferPool_t mb; // move buffer queue extern mpMoveMasterSingleton_t mm; // context for line planning extern mpMoveRuntimeSingleton_t mr; // context for line runtime +// planner.c functions void planner_init(); -void planner_init_assertions(); -stat_t planner_test_assertions(); - void mp_flush_planner(); void mp_set_planner_position(uint8_t axis, const float position); void mp_set_runtime_position(uint8_t axis, const float position); void mp_set_steps_to_runtime_position(); -void mp_queue_command(void(*cm_exec_t)(float[], float[]), float *value, - float *flag); -stat_t mp_runtime_command(mpBuf_t *bf); +// line.c functions +float mp_get_runtime_velocity(); +float mp_get_runtime_work_position(uint8_t axis); +float mp_get_runtime_absolute_position(uint8_t axis); +void mp_set_runtime_work_offset(float offset[]); +void mp_zero_segment_velocity(); +uint8_t mp_get_runtime_busy(); +float* mp_get_planner_position_vector(); +void mp_plan_block_list(mpBuf_t *bf, uint8_t *mr_flag); +stat_t mp_aline(GCodeState_t *gm_in); -stat_t mp_dwell(const float seconds); -void mp_end_dwell(); +// zoid.c functions +void mp_calculate_trapezoid(mpBuf_t *bf); +float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf); +float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf); -stat_t mp_aline(GCodeState_t *gm_in); +// exec.c functions +stat_t mp_exec_move(); +stat_t mp_exec_aline(mpBuf_t *bf); +// feedhold.c functions stat_t mp_plan_hold_callback(); stat_t mp_end_hold(); -stat_t mp_feed_rate_override(uint8_t flag, float parameter); -// planner buffer handlers +// buffer.c functions uint8_t mp_get_planner_buffers_available(); void mp_init_buffers(); mpBuf_t *mp_get_write_buffer(); @@ -304,22 +311,12 @@ mpBuf_t *mp_get_last_buffer(); void mp_clear_buffer(mpBuf_t *bf); void mp_copy_buffer(mpBuf_t *bf, const mpBuf_t *bp); -// line.c functions -float mp_get_runtime_velocity(); -float mp_get_runtime_work_position(uint8_t axis); -float mp_get_runtime_absolute_position(uint8_t axis); -void mp_set_runtime_work_offset(float offset[]); -void mp_zero_segment_velocity(); -uint8_t mp_get_runtime_busy(); -float* mp_get_planner_position_vector(); - -// zoid.c functions -void mp_calculate_trapezoid(mpBuf_t *bf); -float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf); -float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf); +// dwell.c functions +stat_t mp_dwell(const float seconds); -// exec.c functions -stat_t mp_exec_move(); -stat_t mp_exec_aline(mpBuf_t *bf); +// command.c functions +typedef void (*cm_exec_t)(float[], float[]); +void mp_queue_command(cm_exec_t, float *value, float *flag); +void mp_runtime_command(mpBuf_t *bf); #endif // PLANNER_H diff --git a/src/plan/zoid.c b/src/plan/zoid.c index f6952eb..a50099f 100644 --- a/src/plan/zoid.c +++ b/src/plan/zoid.c @@ -1,29 +1,36 @@ /* - * zoid.c - acceleration managed line planning and motion execution - trapezoid planner + * zoid.c - acceleration managed line planning and motion execution - + * trapezoid planner. * This file is part of the TinyG project * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * Copyright (c) 2012 - 2015 Rob Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . - * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. - * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . + * + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. + * + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "planner.h" @@ -32,117 +39,136 @@ #include /* - * mp_calculate_trapezoid() - calculate trapezoid parameters - * - * 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 err's 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 exits for adjacent sections. - * - * Inputs used are: - * bf->length - actual block length (length is never changed) - * bf->entry_velocity - requested Ve (entry velocity is never changed) - * bf->cruise_velocity - requested Vt (is often changed) - * bf->exit_velocity - requested Vx (may be changed for degenerate cases) - * bf->cruise_vmax - used in some comparisons - * bf->delta_vmax - used to degrade velocity of pathologically short blocks - * - * Variables that may be set/updated are: - * bf->entry_velocity - requested Ve - * bf->cruise_velocity - requested Vt - * bf->exit_velocity - requested Vx - * bf->head_length - bf->length allocated to head - * bf->body_length - bf->length allocated to body - * bf->tail_length - bf->length allocated to tail - * - * Note: The following conditions must be met on entry: - * bf->length must be non-zero (filter these out upstream) - * bf->entry_velocity <= bf->cruise_velocity >= bf->exit_velocity - * - * Classes of moves: - * - * Requested-Fit - The move has sufficient length to achieve the 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 will be set lower than the requested - * velocity (incoming bf->cruise_velocity). The entry and exit velocities are satisfied. - * - * Degraded-Fit - The move does not have sufficient length to transition from - * the entry velocity to the exit velocity in 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 first degradation is to reduce the - * move to a body-only 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 worst-case move. - * - * Various cases handled (H=head, B=body, T=tail) - * - * Requested-Fit cases - * HBT VeVx sufficient length exists for all parts (corner case: HBT') - * HB VeVx enter at full speed and decelerate (corner case: T') - * HT Ve & Vx perfect fit HT (very rare). May be symmetric or asymmetric - * H VeVx perfect fit T (common, results from planning) - * B Ve=Vt=Vx Velocities are close to each other and within matching tolerance - * - * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot - * HT (Ve=Vx)Vx Ve is degraded (velocity step). Vx is met - * B" line is very short but drawable; is treated as a body only - * F force fit: This block is slowed down until it can be executed - * - * NOTE: The order of the cases/tests in the code is pretty 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. + * 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 err's 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 + * exits for adjacent sections. + * + * Inputs used are: + * bf->length - actual block length, length is never changed + * bf->entry_velocity - requested Ve, entry velocity is never changed + * 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 + * + * Variables that may be set/updated are: + * bf->entry_velocity - requested Ve + * bf->cruise_velocity - requested Vt + * bf->exit_velocity - requested Vx + * bf->head_length - bf->length allocated to head + * bf->body_length - bf->length allocated to body + * bf->tail_length - bf->length allocated to tail + * + * Note: The following conditions must be met on entry: + * bf->length must be non-zero (filter these out upstream) + * bf->entry_velocity <= bf->cruise_velocity >= bf->exit_velocity + * + * Classes of moves: + * + * Requested-Fit - The move has sufficient length to achieve the + * 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 + * will be set lower than the requested velocity (incoming + * bf->cruise_velocity). The entry and exit velocities are + * satisfied. + * + * Degraded-Fit - The move does not have sufficient length to + * transition from the entry velocity to the exit velocity in + * 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 + * first degradation is to reduce the move to a body-only + * 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 + * worst-case move. + * + * Various cases handled (H=head, B=body, T=tail) + * + * Requested-Fit cases + * HBT VeVx sufficient length exists for all parts (corner + * case: HBT') + * HB VeVx enter at full speed & decelerate (corner case: T') + * HT Ve & Vx perfect fit HT (very rare). May be symmetric or + * asymmetric + * H VeVx perfect fit T (common, results from planning) + * B Ve=Vt=Vx Velocities are close to each other and within + * matching tolerance + * + * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot + * HT (Ve=Vx)Vx Ve is degraded (velocity step). Vx is met + * B" line is very short but drawable; is treated as a + * body only + * F force fit: This block is slowed down until it can + * be executed + * + * NOTE: The order of the cases/tests in the code is pretty + * 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. */ -// The minimum lengths are dynamic and depend on the velocity -// These expressions evaluate to the minimum lengths for the current velocity settings -// Note: The head and tail lengths are 2 minimum segments, the body is 1 min segment -#define MIN_HEAD_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity)) -#define MIN_TAIL_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->exit_velocity)) +// The minimum lengths are dynamic and depend on the velocity. +// These expressions evaluate to the minimum lengths for the current velocity +// settings. +// Note: The head and tail lengths are 2 minimum segments, the body is 1 min +// segment. +#define MIN_HEAD_LENGTH \ + (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->entry_velocity)) +#define MIN_TAIL_LENGTH \ + (MIN_SEGMENT_TIME_PLUS_MARGIN * (bf->cruise_velocity + bf->exit_velocity)) #define MIN_BODY_LENGTH (MIN_SEGMENT_TIME_PLUS_MARGIN * bf->cruise_velocity) +/// calculate trapezoid parameters void mp_calculate_trapezoid(mpBuf_t *bf) { - //******************************************** - //******************************************** - //** RULE #1 of mp_calculate_trapezoid() ** - //** DON'T CHANGE bf->length ** - //******************************************** - //******************************************** - + // RULE #1 of mp_calculate_trapezoid(): Don't change bf->length + // // F case: Block is too short - run time < minimum segment time // Force block into a single segment body with limited velocities - // Accept the entry velocity, limit the cruise, and go for the best exit velocity - // you can get given the delta_vmax (maximum velocity slew) supportable. + // Accept the entry velocity, limit the cruise, and go for the best exit + // velocity you can get given the delta_vmax (maximum velocity slew) + // supportable. - bf->naiive_move_time = 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average + bf->naiive_move_time = + 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average if (bf->naiive_move_time < MIN_SEGMENT_TIME_PLUS_MARGIN) { bf->cruise_velocity = bf->length / MIN_SEGMENT_TIME_PLUS_MARGIN; - bf->exit_velocity = max(0.0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); + bf->exit_velocity = max(0.0, min(bf->cruise_velocity, + (bf->entry_velocity - bf->delta_vmax))); bf->body_length = bf->length; bf->head_length = 0; bf->tail_length = 0; - // We are violating the jerk value but since it's a single segment move we don't use it. + + // We are violating the jerk value but since it's a single segment move we + // don't use it. return; } @@ -163,16 +189,19 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { bf->head_length = 0; bf->tail_length = 0; - // We are violating the jerk value but since it's a single segment move we don't use it. + // We are violating the jerk value but since it's a single segment move we + // don't use it. return; } // 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) < TRAPEZOID_VELOCITY_TOLERANCE)) { + if (((bf->cruise_velocity - bf->entry_velocity) < + TRAPEZOID_VELOCITY_TOLERANCE) && + ((bf->cruise_velocity - bf->exit_velocity) < + TRAPEZOID_VELOCITY_TOLERANCE)) { bf->body_length = bf->length; bf->head_length = 0; bf->tail_length = 0; @@ -181,16 +210,22 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { } // Head-only and tail-only short-line cases - // H" and T" degraded-fit cases - // H' and T' requested-fit cases where the body residual is less than MIN_BODY_LENGTH + // 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); - - if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) { // head-only & tail-only cases - if (bf->entry_velocity > bf->exit_velocity) { // tail-only cases (short decelerations) - if (bf->length < minimum_length) // T" (degraded case) - bf->entry_velocity = mp_get_target_velocity(bf->exit_velocity, bf->length, bf); + float minimum_length = + mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf); + + // head-only & tail-only cases + if (bf->length <= (minimum_length + MIN_BODY_LENGTH)) { + // tail-only cases (short decelerations) + if (bf->entry_velocity > bf->exit_velocity) { + // T" (degraded case) + if (bf->length < minimum_length) + bf->entry_velocity = + mp_get_target_velocity(bf->exit_velocity, bf->length, bf); bf->cruise_velocity = bf->entry_velocity; bf->tail_length = bf->length; @@ -199,9 +234,12 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { return; } - if (bf->entry_velocity < bf->exit_velocity) { // head-only cases (short accelerations) - if (bf->length < minimum_length) // H" (degraded case) - bf->exit_velocity = mp_get_target_velocity(bf->entry_velocity, bf->length, bf); + // head-only cases (short accelerations) + if (bf->entry_velocity < bf->exit_velocity) { + // H" (degraded case) + if (bf->length < minimum_length) + bf->exit_velocity = + mp_get_target_velocity(bf->entry_velocity, bf->length, bf); bf->cruise_velocity = bf->exit_velocity; bf->head_length = bf->length; @@ -212,8 +250,10 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { } // Set head and tail lengths for evaluating the next cases - bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); - bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf); + bf->head_length = + mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); + bf->tail_length = + mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf); if (bf->head_length < MIN_HEAD_LENGTH) { bf->head_length = 0;} if (bf->tail_length < MIN_TAIL_LENGTH) { bf->tail_length = 0;} @@ -221,11 +261,13 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { if (bf->length < (bf->head_length + bf->tail_length)) { // it's rate limited // Symmetric rate-limited case (HT) - if (fabs(bf->entry_velocity - bf->exit_velocity) < TRAPEZOID_VELOCITY_TOLERANCE) { + if (fabs(bf->entry_velocity - bf->exit_velocity) < + TRAPEZOID_VELOCITY_TOLERANCE) { bf->head_length = bf->length/2; bf->tail_length = bf->head_length; bf->cruise_velocity = - min(bf->cruise_vmax, mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf)); + min(bf->cruise_vmax, + mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf)); if (bf->head_length < MIN_HEAD_LENGTH) { // Convert this to a body-only move @@ -242,31 +284,42 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { return; } - // Asymmetric HT' rate-limited case. This is relatively expensive but it's not called very often + // Asymmetric HT' rate-limited case. This is relatively expensive but it's + // not called very often float computed_velocity = bf->cruise_vmax; do { - bf->cruise_velocity = computed_velocity; // initialize from previous iteration - bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); - bf->tail_length = mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf); + // initialize from previous iteration + bf->cruise_velocity = computed_velocity; + bf->head_length = + mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); + bf->tail_length = + mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf); if (bf->head_length > bf->tail_length) { - bf->head_length = (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length; - computed_velocity = mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf); + bf->head_length = + (bf->head_length / (bf->head_length + bf->tail_length)) * bf->length; + computed_velocity = + mp_get_target_velocity(bf->entry_velocity, bf->head_length, bf); } else { - bf->tail_length = (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length; - computed_velocity = mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf); + bf->tail_length = + (bf->tail_length / (bf->head_length + bf->tail_length)) * bf->length; + computed_velocity = + mp_get_target_velocity(bf->exit_velocity, bf->tail_length, bf); } - } while ((fabs(bf->cruise_velocity - computed_velocity) / computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT); + } while ((fabs(bf->cruise_velocity - computed_velocity) / + computed_velocity) > TRAPEZOID_ITERATION_ERROR_PERCENT); // set velocity and clean up any parts that are too short bf->cruise_velocity = computed_velocity; - bf->head_length = mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); + bf->head_length = + mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf); bf->tail_length = bf->length - bf->head_length; if (bf->head_length < MIN_HEAD_LENGTH) { - bf->tail_length = bf->length; // adjust the move to be all tail... + // adjust the move to be all tail... + bf->tail_length = bf->length; bf->head_length = 0; } @@ -281,9 +334,9 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { // Requested-fit cases: remaining of: HBT, HB, BT, BT, H, T, B, cases bf->body_length = bf->length - bf->head_length - bf->tail_length; - // If a non-zero body is < minimum length distribute it to the head and/or tail - // This will generate small (acceptable) velocity errors in runtime execution - // but preserve correct distance, which is more important. + // If a non-zero body is < minimum length distribute it to the head and/or + // tail. This will generate small (acceptable) velocity errors in runtime + // execution but preserve correct distance, which is more important. if ((bf->body_length < MIN_BODY_LENGTH) && (fp_NOT_ZERO(bf->body_length))) { if (fp_NOT_ZERO(bf->head_length)) { if (fp_NOT_ZERO(bf->tail_length)) { // HBT reduces to HT @@ -295,51 +348,58 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { bf->body_length = 0; - // If the body is a standalone make the cruise velocity match the entry velocity - // This removes a potential velocity discontinuity at the expense of top speed + // If the body is a standalone make the cruise velocity match the entry + // velocity. This removes a potential velocity discontinuity at the expense + // of top speed } else if ((fp_ZERO(bf->head_length)) && (fp_ZERO(bf->tail_length))) bf->cruise_velocity = bf->entry_velocity; } -/* - * This set of functions returns the fourth thing knowing the other three. - * - * Jm = the given maximum jerk - * T = time of the entire move - * Vi = initial velocity - * Vf = final velocity - * T = 2*sqrt((Vt-Vi)/Jm) - * As = The acceleration at inflection point between convex and concave portions of the S-curve. - * As = (Jm*T)/2 - * Ar = ramp acceleration - * Ar = As/2 = (Jm*T)/4 - * - * mp_get_target_length() is a convenient function for determining the optimal_length (L) - * of a line given the initial velocity (Vi), final velocity (Vf) and maximum jerk (Jm). - * - * The length (distance) equation is derived from: - * - * a) L = (Vf-Vi) * T - (Ar*T^2)/2 ... which becomes b) with substitutions for Ar and T - * b) L = (Vf-Vi) * 2*sqrt((Vf-Vi)/Jm) - (2*sqrt((Vf-Vi)/Jm) * (Vf-Vi))/2 - * c) L = (Vf-Vi)^(3/2) / sqrt(Jm) ...is an alternate form of b) (see Wolfram Alpha) - * c') L = (Vf-Vi) * sqrt((Vf-Vi)/Jm) ... second alternate form; requires Vf >= Vi - * - * Notes: Ar = (Jm*T)/4 Ar is ramp acceleration - * T = 2*sqrt((Vf-Vi)/Jm) T is time - * Assumes Vi, Vf and L are positive or zero - * Cannot assume Vf>=Vi due to rounding errors and use of PLANNER_VELOCITY_TOLERANCE - * necessitating the introduction of fabs() - * - * mp_get_target_velocity() is a convenient function for determining Vf target velocity for - * a given the initial velocity (Vi), length (L), and maximum jerk (Jm). - * Equation d) is b) solved for Vf. Equation e) is c) solved for Vf. Use e) (obviously) - * - * d) Vf = (sqrt(L)*(L/sqrt(1/Jm))^(1/6)+(1/Jm)^(1/4)*Vi)/(1/Jm)^(1/4) - * e) Vf = L^(2/3) * Jm^(1/3) + Vi +/* This set of functions returns the fourth thing knowing the other three. + * + * Jm = the given maximum jerk + * T = time of the entire move + * Vi = initial velocity + * Vf = final velocity + * T = 2*sqrt((Vt-Vi)/Jm) + * As = The acceleration at inflection point between convex and concave + * portions of the S-curve. + * As = (Jm*T)/2 + * Ar = ramp acceleration + * Ar = As/2 = (Jm*T)/4 + * + * mp_get_target_length() is a convenient function for determining the + * optimal_length (L) of a line given the initial velocity (Vi), final + * velocity (Vf) and maximum jerk (Jm). + * + * The length (distance) equation is derived from: + * + * a) L = (Vf-Vi) * T - (Ar*T^2)/2 ... which becomes b) with + * substitutions for Ar and T + * b) L = (Vf-Vi) * 2*sqrt((Vf-Vi)/Jm) - (2*sqrt((Vf-Vi)/Jm) * (Vf-Vi))/2 + * c) L = (Vf-Vi)^(3/2) / sqrt(Jm) ...is an alternate form of b) + * (see Wolfram Alpha) + * c') L = (Vf-Vi) * sqrt((Vf-Vi)/Jm) ... second alternate form; requires + * Vf >= Vi + * + * Notes: Ar = (Jm*T)/4 Ar is ramp acceleration + * T = 2*sqrt((Vf-Vi)/Jm) T is time + * Assumes Vi, Vf and L are positive or zero + * Cannot assume Vf>=Vi due to rounding errors and use of + * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of + * fabs() + * + * mp_get_target_velocity() is a convenient function for determining Vf + * target velocity for a given the initial velocity (Vi), length (L), and + * maximum jerk (Jm). Equation d) is b) solved for Vf. Equation e) is + * c) solved for Vf. Use e) (obviously) + * + * d) Vf = (sqrt(L)*(L/sqrt(1/Jm))^(1/6)+(1/Jm)^(1/4)*Vi)/(1/Jm)^(1/4) + * e) Vf = L^(2/3) * Jm^(1/3) + Vi * * FYI: Here's an expression that returns the jerk for a given deltaV and L: - * return cube(deltaV / (pow(L, 0.66666666))); + * return cube(deltaV / (pow(L, 0.66666666))); */ @@ -352,46 +412,46 @@ float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf) { /* Regarding mp_get_target_velocity: * * We do some Newton-Raphson iterations to narrow it down. - * We need a formula that includes known variables except the one we want to find, - * and has a root [Z(x) = 0] at the value (x) we are looking for. + * We need a formula that includes known variables except the one we want to + * find, and has a root [Z(x) = 0] at the value (x) we are looking for. * - * Z(x) = zero at x -- we calculate the value from the knowns and the estimate - * (see below) and then subtract the known value to get zero (root) if - * x is the correct value. - * Vi = initial velocity (known) - * Vf = estimated final velocity - * J = jerk (known) - * L = length (know) + * Z(x) = zero at x -- we calculate the value from the knowns and the + * estimate (see below) and then subtract the known + * value to get zero (root) if x is the correct value. + * Vi = initial velocity (known) + * Vf = estimated final velocity + * J = jerk (known) + * L = length (know) * * There are (at least) two such functions we can use: - * L from J, Vi, and Vf - * L = sqrt((Vf - Vi) / J) (Vi + Vf) + * L from J, Vi, and Vf + * L = sqrt((Vf - Vi) / J) (Vi + Vf) * - * Replacing Vf with x, and subtracting the known L: - * 0 = sqrt((x - Vi) / J) (Vi + x) - L - * Z(x) = sqrt((x - Vi) / J) (Vi + x) - L + * Replacing Vf with x, and subtracting the known L: + * 0 = sqrt((x - Vi) / J) (Vi + x) - L + * Z(x) = sqrt((x - Vi) / J) (Vi + x) - L * - * OR - * J from L, Vi, and Vf - * J = ((Vf - Vi) (Vi + Vf)²) / L² + * Or + * J from L, Vi, and Vf + * J = ((Vf - Vi) (Vi + Vf)²) / L² * - * Replacing Vf with x, and subtracting the known J: - * 0 = ((x - Vi) (Vi + x)²) / L² - J - * Z(x) = ((x - Vi) (Vi + x)²) / L² - J + * Replacing Vf with x, and subtracting the known J: + * 0 = ((x - Vi) (Vi + x)²) / L² - J + * Z(x) = ((x - Vi) (Vi + x)²) / L² - J * - * L doesn't resolve to the value very quickly (it graphs near-vertical). - * So, we'll use J, which resolves in < 10 iterations, often in only two or three - * with a good estimate. + * L doesn't resolve to the value very quickly (it graphs near-vertical). + * So, we'll use J, which resolves in < 10 iterations, often in only two or + * three with a good estimate. * - * In order to do a Newton-Raphson iteration, we need the derivative. Here they are - * for both the (unused) L and the (used) J formulas above: + * In order to do a Newton-Raphson iteration, we need the derivative. Here + * they are for both the (unused) L and the (used) J formulas above: * - * J > 0, Vi > 0, Vf > 0 - * SqrtDeltaJ = sqrt((x-Vi) * J) - * SqrtDeltaOverJ = sqrt((x-Vi) / J) - * L'(x) = SqrtDeltaOverJ + (Vi + x) / (2*J) + (Vi + x) / (2*SqrtDeltaJ) + * J > 0, Vi > 0, Vf > 0 + * SqrtDeltaJ = sqrt((x - Vi) * J) + * SqrtDeltaOverJ = sqrt((x - Vi) / J) + * L'(x) = SqrtDeltaOverJ + (Vi + x) / (2*J) + (Vi + x) / (2 * SqrtDeltaJ) * - * J'(x) = (2*Vi*x - Vi² + 3*x²) / L² + * J'(x) = (2 * Vi * x - Vi² + 3 * x²) / L² */ #define GET_VELOCITY_ITERATIONS 0 // must be 0, 1, or 2 @@ -405,14 +465,17 @@ float mp_get_target_velocity(const float Vi, const float L, const mpBuf_t *bf) { // 1st iteration float L_squared = L * L; float Vi_squared = Vi * Vi; - float J_z = (estimate - Vi) * (Vi + estimate) * (Vi + estimate) / L_squared - bf->jerk; - float J_d = (2 * Vi * estimate - Vi_squared + 3 * estimate * estimate) / L_squared; + float J_z = + (estimate - Vi) * (Vi + estimate) * (Vi + estimate) / L_squared - bf->jerk; + float J_d = + (2 * Vi * estimate - Vi_squared + 3 * estimate * estimate) / L_squared; estimate = estimate - J_z / J_d; #endif #if (GET_VELOCITY_ITERATIONS >= 2) // 2nd iteration - J_z = (estimate - Vi) * (Vi + estimate) * (Vi + estimate) / L_squared - bf->jerk; + J_z = + (estimate - Vi) * (Vi + estimate) * (Vi + estimate) / L_squared - bf->jerk; J_d = (2 * Vi * estimate - Vi_squared + 3 * estimate * estimate) / L_squared; estimate = estimate - J_z / J_d; #endif diff --git a/src/pwm.c b/src/pwm.c index 2eddd39..5331d3c 100644 --- a/src/pwm.c +++ b/src/pwm.c @@ -4,25 +4,31 @@ * * Copyright (c) 2012 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "pwm.h" @@ -39,8 +45,8 @@ pwmSingleton_t pwm; // defines common to all PWM channels #define PWM_TIMER_t TC1_t // PWM uses TC1's #define PWM_TIMER_DISABLE 0 // turn timer off (clock = 0 Hz) -#define PWM_MAX_FREQ (F_CPU / 256) // max frequency with 8-bits duty cycle precision -#define PWM_MIN_FREQ (F_CPU / 64 / 65536) // min frequency with supported prescaling +#define PWM_MAX_FREQ (F_CPU / 256) // with 8-bits duty cycle precision +#define PWM_MIN_FREQ (F_CPU / 64 / 65536) // min frequency for prescaling /* CLKSEL is used to configure default PWM clock operating ranges @@ -54,35 +60,46 @@ pwmSingleton_t pwm; * TC_CLKSEL_DIV64_gc - good for about 8 Hz to 2 Khz */ #define PWM1_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc // starting clock select value -#define PWM1_CTRLB (3 | TC0_CCBEN_bm) // single slope PWM enabled on channel B +/// single slope PWM enabled on channel B +#define PWM1_CTRLB (3 | TC0_CCBEN_bm) #define PWM1_ISR_vect TCD1_CCB_vect -#define PWM1_INTCTRLB 0 // timer interrupt level (0=off, 1=lo, 2=med, 3=hi) +/// timer interrupt level (0=off, 1=lo, 2=med, 3=hi) +#define PWM1_INTCTRLB 0 #define PWM2_CTRLA_CLKSEL TC_CLKSEL_DIV1_gc -#define PWM2_CTRLB 3 // single slope PWM enabled, no output channel +/// single slope PWM enabled, no output channel +#define PWM2_CTRLB 3 #define PWM2_ISR_vect TCE1_CCB_vect -#define PWM2_INTCTRLB 0 // timer interrupt level (0=off, 1=lo, 2=med, 3=hi) +/// timer interrupt level (0=off, 1=lo, 2=med, 3=hi) +#define PWM2_INTCTRLB 0 /* * Initialize pwm channels * - * Notes: - * - Whatever level interrupts you use must be enabled in main() - * - init assumes PWM1 output bit (D5) has been set to output previously (stepper.c) + * Notes: + * - Whatever level interrupts you use must be enabled in main() + * - init assumes PWM1 output bit (D5) has been set to output previously + * (stepper.c) */ void pwm_init() { gpio_set_bit_off(SPINDLE_PWM); // setup PWM channel 1 - memset(&pwm.p[PWM_1], 0, sizeof(pwmChannel_t)); // clear parent structure - pwm.p[PWM_1].timer = &TIMER_PWM1; // bind timer struct to PWM struct array - pwm.p[PWM_1].ctrla = PWM1_CTRLA_CLKSEL; // initialize starting clock operating range + // clear parent structure + memset(&pwm.p[PWM_1], 0, sizeof(pwmChannel_t)); + + // bind timer struct to PWM struct array + pwm.p[PWM_1].timer = &TIMER_PWM1; + // initialize starting clock operating range + pwm.p[PWM_1].ctrla = PWM1_CTRLA_CLKSEL; pwm.p[PWM_1].timer->CTRLB = PWM1_CTRLB; - pwm.p[PWM_1].timer->INTCTRLB = PWM1_INTCTRLB; // set interrupt level + pwm.p[PWM_1].timer->INTCTRLB = PWM1_INTCTRLB; // set interrupt level // setup PWM channel 2 - memset(&pwm.p[PWM_2], 0, sizeof(pwmChannel_t)); // clear all values, pointers and status + // clear all values, pointers and status + memset(&pwm.p[PWM_2], 0, sizeof(pwmChannel_t)); + pwm.p[PWM_2].timer = &TIMER_PWM2; pwm.p[PWM_2].ctrla = PWM2_CTRLA_CLKSEL; pwm.p[PWM_2].timer->CTRLB = PWM2_CTRLB; @@ -122,7 +139,8 @@ stat_t pwm_set_freq(uint8_t chan, float freq) { if (freq < PWM_MIN_FREQ) return STAT_INPUT_LESS_THAN_MIN_VALUE; // Set the period and the prescaler - float prescale = F_CPU / 65536 / freq; // optimal non-integer prescaler value + // optimal non-integer prescaler value + float prescale = F_CPU / 65536 / freq; if (prescale <= 1) { pwm.p[chan].timer->PER = F_CPU / freq; pwm.p[chan].timer->CTRLA = TC_CLKSEL_DIV1_gc; diff --git a/src/pwm.h b/src/pwm.h index 2f5324a..e3796ba 100644 --- a/src/pwm.h +++ b/src/pwm.h @@ -4,25 +4,31 @@ * * Copyright (c) 2012 - 2014 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef PWM_H @@ -39,22 +45,22 @@ typedef struct pwmConfigChannel { - float frequency; // base frequency for PWM driver, in Hz - float cw_speed_lo; // minimum clockwise spindle speed [0..N] - float cw_speed_hi; // maximum clockwise spindle speed - float cw_phase_lo; // pwm phase at minimum CW spindle speed, clamped [0..1] - float cw_phase_hi; // pwm phase at maximum CW spindle speed, clamped [0..1] - float ccw_speed_lo; // minimum counter-clockwise spindle speed [0..N] - float ccw_speed_hi; // maximum counter-clockwise spindle speed - float ccw_phase_lo; // pwm phase at minimum CCW spindle speed, clamped [0..1] - float ccw_phase_hi; // pwm phase at maximum CCW spindle speed, clamped - float phase_off; // pwm phase when spindle is disabled + float frequency; // base frequency for PWM driver, in Hz + float cw_speed_lo; // minimum clockwise spindle speed [0..N] + float cw_speed_hi; // maximum clockwise spindle speed + float cw_phase_lo; // pwm phase at minimum CW spindle speed, clamped [0..1] + float cw_phase_hi; // pwm phase at maximum CW spindle speed, clamped [0..1] + float ccw_speed_lo; // minimum counter-clockwise spindle speed [0..N] + float ccw_speed_hi; // maximum counter-clockwise spindle speed + float ccw_phase_lo; // pwm phase at minimum CCW spindle speed, clamped [0..1] + float ccw_phase_hi; // pwm phase at maximum CCW spindle speed, clamped + float phase_off; // pwm phase when spindle is disabled } pwmConfigChannel_t; typedef struct pwmChannel { - uint8_t ctrla; // byte needed to active CTRLA (it's dynamic - rest are static) - TC1_t *timer; // assumes TC1 flavor timers used for PWM channels + uint8_t ctrla; // byte needed to active CTRLA (it's dynamic - rest are static) + TC1_t *timer; // assumes TC1 flavor timers used for PWM channels } pwmChannel_t; diff --git a/src/rtc.c b/src/rtc.c index e061812..63db2a9 100644 --- a/src/rtc.c +++ b/src/rtc.c @@ -4,17 +4,21 @@ * * Copyright (c) 2010 - 2013 Alden S. Hart Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "rtc.h" @@ -25,13 +29,10 @@ #include -rtClock_t rtc; // allocate clock control struct +rtClock_t rtc; // allocate clock control struct -/* - * Initialize and start the clock - * - * This routine follows the code in app note AVR1314. - */ +/// Initialize and start the clock +/// This routine follows the code in app note AVR1314. void rtc_init() { OSC.CTRL |= OSC_RC32KEN_bm; // enable internal 32kHz. while (!(OSC.STATUS & OSC_RC32KRDY_bm)); // 32kHz osc stabilize @@ -51,8 +52,7 @@ void rtc_init() { } -/* - * rtc ISR +/* rtc ISR * * It is the responsibility of callback code to ensure atomicity and volatiles * are observed correctly as the callback will be run at the interrupt level. diff --git a/src/rtc.h b/src/rtc.h index 7957caf..32ca346 100644 --- a/src/rtc.h +++ b/src/rtc.h @@ -4,17 +4,21 @@ * * Copyright (c) 2010 - 2013 Alden S. Hart Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef RTC_H @@ -22,22 +26,21 @@ #include -#define RTC_MILLISECONDS 10 // interrupt on every 10 RTC ticks (~10 ms) +#define RTC_MILLISECONDS 10 // interrupt on every 10 RTC ticks (~10 ms) -// Interrupt level: pick one -#define RTC_COMPINTLVL RTC_COMPINTLVL_LO_gc; // lo interrupt on compare -//#define RTC_COMPINTLVL RTC_COMPINTLVL_MED_gc; // med interrupt on compare -//#define RTC_COMPINTLVL RTC_COMPINTLVL_HI_gc; // hi interrupt on compare +// Interrupt level +#define RTC_COMPINTLVL RTC_COMPINTLVL_LO_gc; -// Note: sys_ticks is in ms but is only accurate to 10 ms as it's derived from rtc_ticks +// Note: sys_ticks is in ms but is only accurate to 10 ms as it's derived from +// rtc_ticks typedef struct rtClock { - uint32_t rtc_ticks; // RTC tick counter, 10 uSec each - uint32_t sys_ticks; // system tick counter, 1 ms each + uint32_t rtc_ticks; // RTC tick counter, 10 uSec each + uint32_t sys_ticks; // system tick counter, 1 ms each } rtClock_t; extern rtClock_t rtc; -void rtc_init(); // initialize and start general timer +void rtc_init(); // initialize and start general timer uint32_t rtc_get_time(); #endif // RTC_H diff --git a/src/spindle.c b/src/spindle.c index 8f1f203..cb76b8f 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -4,25 +4,31 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "spindle.h" @@ -38,11 +44,8 @@ static void _exec_spindle_control(float *value, float *flag); static void _exec_spindle_speed(float *value, float *flag); -/* - * cm_spindle_init() - */ -void cm_spindle_init() -{ + +void cm_spindle_init() { if( pwm.c[PWM_1].frequency < 0 ) pwm.c[PWM_1].frequency = 0; @@ -50,88 +53,82 @@ void cm_spindle_init() pwm_set_duty(PWM_1, pwm.c[PWM_1].phase_off); } -/* - * cm_get_spindle_pwm() - return PWM phase (duty cycle) for dir and speed - */ -float cm_get_spindle_pwm( uint8_t spindle_mode ) -{ + +/// return PWM phase (duty cycle) for dir and speed +float cm_get_spindle_pwm( uint8_t spindle_mode ) { float speed_lo=0, speed_hi=0, phase_lo=0, phase_hi=0; - if (spindle_mode == SPINDLE_CW ) { + if (spindle_mode == SPINDLE_CW) { speed_lo = pwm.c[PWM_1].cw_speed_lo; speed_hi = pwm.c[PWM_1].cw_speed_hi; phase_lo = pwm.c[PWM_1].cw_phase_lo; phase_hi = pwm.c[PWM_1].cw_phase_hi; - } else if (spindle_mode == SPINDLE_CCW ) { + + } else if (spindle_mode == SPINDLE_CCW) { speed_lo = pwm.c[PWM_1].ccw_speed_lo; speed_hi = pwm.c[PWM_1].ccw_speed_hi; phase_lo = pwm.c[PWM_1].ccw_phase_lo; phase_hi = pwm.c[PWM_1].ccw_phase_hi; } - if (spindle_mode==SPINDLE_CW || spindle_mode==SPINDLE_CCW ) { + if (spindle_mode == SPINDLE_CW || spindle_mode == SPINDLE_CCW) { // clamp spindle speed to lo/hi range - if( cm.gm.spindle_speed < speed_lo ) cm.gm.spindle_speed = speed_lo; - if( cm.gm.spindle_speed > speed_hi ) cm.gm.spindle_speed = speed_hi; + if (cm.gm.spindle_speed < speed_lo) cm.gm.spindle_speed = speed_lo; + if (cm.gm.spindle_speed > speed_hi) cm.gm.spindle_speed = speed_hi; // normalize speed to [0..1] float speed = (cm.gm.spindle_speed - speed_lo) / (speed_hi - speed_lo); return speed * (phase_hi - phase_lo) + phase_lo; - } else { - return pwm.c[PWM_1].phase_off; - } + + } else return pwm.c[PWM_1].phase_off; } -/* - * cm_spindle_control() - queue the spindle command to the planner buffer - * cm_exec_spindle_control() - execute the spindle command (called from planner) - */ -stat_t cm_spindle_control(uint8_t spindle_mode) -{ - float value[AXES] = { (float)spindle_mode, 0,0,0,0,0 }; +/// queue the spindle command to the planner buffer +stat_t cm_spindle_control(uint8_t spindle_mode) { + float value[AXES] = {spindle_mode, 0, 0, 0, 0, 0}; + mp_queue_command(_exec_spindle_control, value, value); + return STAT_OK; } -//static void _exec_spindle_control(uint8_t spindle_mode, float f, float *vector, float *flag) -static void _exec_spindle_control(float *value, float *flag) -{ +/// execute the spindle command (called from planner) +static void _exec_spindle_control(float *value, float *flag) { uint8_t spindle_mode = (uint8_t)value[0]; + cm_set_spindle_mode(MODEL, spindle_mode); if (spindle_mode == SPINDLE_CW) { gpio_set_bit_on(SPINDLE_BIT); gpio_set_bit_off(SPINDLE_DIR); + } else if (spindle_mode == SPINDLE_CCW) { gpio_set_bit_on(SPINDLE_BIT); gpio_set_bit_on(SPINDLE_DIR); - } else { - gpio_set_bit_off(SPINDLE_BIT); // failsafe: any error causes stop - } + + } else gpio_set_bit_off(SPINDLE_BIT); // failsafe: any error causes stop // PWM spindle control pwm_set_duty(PWM_1, cm_get_spindle_pwm(spindle_mode) ); } -/* - * cm_set_spindle_speed() - queue the S parameter to the planner buffer - * cm_exec_spindle_speed() - execute the S command (called from the planner buffer) - * _exec_spindle_speed() - spindle speed callback from planner queue - */ -stat_t cm_set_spindle_speed(float speed) -{ +/// Queue the S parameter to the planner buffer +stat_t cm_set_spindle_speed(float speed) { float value[AXES] = { speed, 0,0,0,0,0 }; mp_queue_command(_exec_spindle_speed, value, value); return STAT_OK; } -void cm_exec_spindle_speed(float speed) -{ + +/// Execute the S command (called from the planner buffer) +void cm_exec_spindle_speed(float speed) { cm_set_spindle_speed(speed); } -static void _exec_spindle_speed(float *value, float *flag) -{ + +/// Spindle speed callback from planner queue +static void _exec_spindle_speed(float *value, float *flag) { cm_set_spindle_speed_parameter(MODEL, value[0]); - pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode) ); // update spindle speed if we're running + // update spindle speed if we're running + pwm_set_duty(PWM_1, cm_get_spindle_pwm(cm.gm.spindle_mode)); } diff --git a/src/spindle.h b/src/spindle.h index 19b6283..3f25479 100644 --- a/src/spindle.h +++ b/src/spindle.h @@ -4,25 +4,31 @@ * * Copyright (c) 2010 - 2014 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #ifndef SPINDLE_H @@ -35,7 +41,7 @@ void cm_spindle_init(); stat_t cm_set_spindle_speed(float speed); // S parameter void cm_exec_spindle_speed(float speed); // callback for above -stat_t cm_spindle_control(uint8_t spindle_mode); // M3, M4, M5 integrated spindle control +stat_t cm_spindle_control(uint8_t spindle_mode); // M3, M4, M5 void cm_exec_spindle_control(uint8_t spindle_mode); // callback for above #endif // SPINDLE_H diff --git a/src/status.c b/src/status.c index ad71a1a..2f3efcb 100644 --- a/src/status.c +++ b/src/status.c @@ -5,17 +5,21 @@ * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * Copyright (c) 2013 - 2015 Robert Giseburt * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "status.h" diff --git a/src/stepper.c b/src/stepper.c index 6d2e168..9ecdbaa 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -63,13 +63,14 @@ static void _request_load_move(); #define _f_to_period(f) (uint16_t)((float)F_CPU / (float)f) -#define MOTOR_1 0 -#define MOTOR_2 1 -#define MOTOR_3 2 -#define MOTOR_4 3 -#define MOTOR_5 4 -#define MOTOR_6 5 - +enum { + MOTOR_1, + MOTOR_2, + MOTOR_3, + MOTOR_4, + MOTOR_5, + MOTOR_6, +}; static VPORT_t *vports[] = { &PORT_MOTOR_1_VPORT, @@ -109,23 +110,6 @@ void stepper_init() { TIMER_DDA.CTRLB = STEP_TIMER_WGMODE; // waveform mode TIMER_DDA.INTCTRLA = TIMER_DDA_INTLVL; // interrupt mode - // setup DWELL timer - TIMER_DWELL.CTRLA = STEP_TIMER_DISABLE; // turn timer off - TIMER_DWELL.CTRLB = STEP_TIMER_WGMODE; // waveform mode - TIMER_DWELL.INTCTRLA = TIMER_DWELL_INTLVL; // interrupt mode - - // setup software interrupt load timer - TIMER_LOAD.CTRLA = LOAD_TIMER_DISABLE; // turn timer off - TIMER_LOAD.CTRLB = LOAD_TIMER_WGMODE; // waveform mode - TIMER_LOAD.INTCTRLA = TIMER_LOAD_INTLVL; // interrupt mode - TIMER_LOAD.PER = LOAD_TIMER_PERIOD; // set period - - // setup software interrupt exec timer - TIMER_EXEC.CTRLA = EXEC_TIMER_DISABLE; // turn timer off - TIMER_EXEC.CTRLB = EXEC_TIMER_WGMODE; // waveform mode - TIMER_EXEC.INTCTRLA = TIMER_EXEC_INTLVL; // interrupt mode - TIMER_EXEC.PER = EXEC_TIMER_PERIOD; // set period - st_pre.buffer_state = PREP_BUFFER_OWNED_BY_EXEC; // defaults @@ -266,7 +250,7 @@ stat_t st_motor_power_callback() { // called by controller // Run the countdown if you are in a countdown if (st_run.mot[m].power_state == MOTOR_POWER_TIMEOUT_COUNTDOWN) { - if (rtc_get_time() > st_run.mot[m].power_systick ) { + if (rtc_get_time() > st_run.mot[m].power_systick) { st_run.mot[m].power_state = MOTOR_IDLE; _deenergize_motor(m); report_request(); // request a status report when motors shut down @@ -289,14 +273,15 @@ static inline void _step_motor(int motor) { } - /// Stepper Interrupt Service Routine /// DDA timer interrupt routine - service ticks from DDA timer ISR(TIMER_DDA_ISR_vect) { - _step_motor(MOTOR_1); - _step_motor(MOTOR_2); - _step_motor(MOTOR_3); - _step_motor(MOTOR_4); + if (st_run.move_type == MOVE_TYPE_ALINE) { + _step_motor(MOTOR_1); + _step_motor(MOTOR_2); + _step_motor(MOTOR_3); + _step_motor(MOTOR_4); + } if (--st_run.dda_ticks_downcount) return; @@ -305,28 +290,18 @@ ISR(TIMER_DDA_ISR_vect) { } -/// DWELL timer interrupt -ISR(TIMER_DWELL_ISR_vect) { - if (--st_run.dda_ticks_downcount == 0) { - TIMER_DWELL.CTRLA = STEP_TIMER_DISABLE; // disable DWELL timer - _load_move(); - } -} - - /// SW interrupt to request to execute a move void st_request_exec_move() { if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC) { - TIMER_EXEC.PER = EXEC_TIMER_PERIOD; - TIMER_EXEC.CTRLA = EXEC_TIMER_ENABLE; // trigger a LO interrupt + ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // trigger LO interrupt + ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm; } } /// Interrupt handler for calling exec function -ISR(TIMER_EXEC_ISR_vect) { - TIMER_EXEC.CTRLA = EXEC_TIMER_DISABLE; // disable SW interrupt timer - +/// Use ADC channel 0 as software interrupt +ISR(ADCB_CH0_vect) { // Exec move if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_EXEC && mp_exec_move() != STAT_NOOP) { @@ -341,16 +316,15 @@ static void _request_load_move() { if (st_runtime_isbusy()) return; // don't request a load if runtime is busy if (st_pre.buffer_state == PREP_BUFFER_OWNED_BY_LOADER) { - TIMER_LOAD.PER = LOAD_TIMER_PERIOD; - TIMER_LOAD.CTRLA = LOAD_TIMER_ENABLE; // trigger a HI interrupt + ADCB_CH1_INTCTRL = ADC_CH_INTLVL_HI_gc; // trigger HI interrupt + ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH1START_bm; } } /// Interrupt handler for running the loader -ISR(TIMER_LOAD_ISR_vect) { - TIMER_LOAD.CTRLA = LOAD_TIMER_DISABLE; // disable SW interrupt timer - +/// Use ADC channel 1 as software interrupt +ISR(ADCB_CH1_vect) { // _load_move() can only be called be called from an ISR at the same or // higher level as the DDA or dwell ISR. A software interrupt has been // provided to allow a non-ISR to request a load @@ -433,7 +407,9 @@ static void _load_move() { // initialization if (st_runtime_isbusy()) return; // exit if the runtime is busy if (st_pre.buffer_state != PREP_BUFFER_OWNED_BY_LOADER) - return; // if there are no moves to load... + return; // no more moves to load + + st_run.move_type = st_pre.move_type; // Handle aline loads first (most common case) if (st_pre.move_type == MOVE_TYPE_ALINE) { @@ -459,8 +435,8 @@ static void _load_move() { } else if (st_pre.move_type == MOVE_TYPE_DWELL) { // handle dwells st_run.dda_ticks_downcount = st_pre.dda_ticks; - TIMER_DWELL.PER = st_pre.dda_period; // load dwell timer period - TIMER_DWELL.CTRLA = STEP_TIMER_ENABLE; // enable the dwell timer + TIMER_DDA.PER = st_pre.dda_period; // load dwell timer period + TIMER_DDA.CTRLA = STEP_TIMER_ENABLE; // enable the dwell timer } else if (st_pre.move_type == MOVE_TYPE_COMMAND) // handle synchronous commands @@ -613,8 +589,8 @@ void st_prep_command(void *bf) { /// Add a dwell to the move buffer void st_prep_dwell(float microseconds) { st_pre.move_type = MOVE_TYPE_DWELL; - st_pre.dda_period = _f_to_period(FREQUENCY_DWELL); - st_pre.dda_ticks = (uint32_t)(microseconds / 1000000 * FREQUENCY_DWELL); + st_pre.dda_period = _f_to_period(FREQUENCY_DDA); + st_pre.dda_ticks = (uint32_t)(microseconds / 1000000 * FREQUENCY_DDA); st_pre.buffer_state = PREP_BUFFER_OWNED_BY_LOADER; // signal prep buffer ready } diff --git a/src/stepper.h b/src/stepper.h index 1a81d7f..d1cfb32 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -427,6 +427,7 @@ typedef struct stRunMotor { // one per controlled motor typedef struct stRunSingleton { // Stepper static values and axis parameters + uint8_t move_type; uint32_t dda_ticks_downcount; // tick down-counter (unscaled) uint32_t dda_ticks_X_substeps; // ticks multiplied by scaling factor stRunMotor_t mot[MOTORS]; // runtime motor structures diff --git a/src/switch.c b/src/switch.c index 957993e..abbf828 100644 --- a/src/switch.c +++ b/src/switch.c @@ -4,40 +4,51 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Switch Modes * - * The switches are considered to be homing switches when machine_state is - * MACHINE_HOMING. At all other times they are treated as limit switches: - * - Hitting a homing switch puts the current move into feedhold - * - Hitting a limit switch causes the machine to shut down and go into lockdown until reset + * The switches are considered to be homing switches when machine_state is + * MACHINE_HOMING. At all other times they are treated as limit switches: * - * The normally open switch modes (NO) trigger an interrupt on the falling edge - * and lockout subsequent interrupts for the defined lockout period. This approach - * beats doing debouncing as an integration as switches fire immediately. + * - Hitting a homing switch puts the current move into feedhold * - * The normally closed switch modes (NC) trigger an interrupt on the rising edge - * and lockout subsequent interrupts for the defined lockout period. Ditto on the method. + * - Hitting a limit switch causes the machine to shut down and go into + * lockdown until reset + * + * The normally open switch modes (NO) trigger an interrupt on the + * falling edge and lockout subsequent interrupts for the defined + * lockout period. This approach beats doing debouncing as an + * integration as switches fire immediately. + * + * The normally closed switch modes (NC) trigger an interrupt on the + * rising edge and lockout subsequent interrupts for the defined + * lockout period. Ditto on the method. */ #include "switch.h" @@ -50,35 +61,39 @@ #include + static void _switch_isr_helper(uint8_t sw_num); -/* - * Initialize homing/limit switches +/* Initialize homing/limit switches * - * This function assumes sys_init() and st_init() have been run previously to - * bind the ports and set bit IO directions, repsectively. + * This function assumes sys_init() and st_init() have been run previously to + * bind the ports and set bit IO directions, repsectively. */ -#define PIN_MODE PORT_OPC_PULLUP_gc // pin mode. see iox192a3.h for details +#define PIN_MODE PORT_OPC_PULLUP_gc // pin mode. see iox192a3.h for details + void switch_init() { for (uint8_t i = 0; i < NUM_SWITCH_PAIRS; i++) { // setup input bits and interrupts (previously set to inputs by st_init()) if (sw.mode[MIN_SWITCH(i)] != SW_MODE_DISABLED) { - hw.sw_port[i]->DIRCLR = SW_MIN_BIT_bm; // set min input - see 13.14.14 + // set min input - see 13.14.14 + hw.sw_port[i]->DIRCLR = SW_MIN_BIT_bm; hw.sw_port[i]->PIN6CTRL = (PIN_MODE | PORT_ISC_BOTHEDGES_gc); - hw.sw_port[i]->INT0MASK = SW_MIN_BIT_bm; // interrupt on min switch + // interrupt on min switch + hw.sw_port[i]->INT0MASK = SW_MIN_BIT_bm; - } else hw.sw_port[i]->INT0MASK = 0; // disable interrupt + } else hw.sw_port[i]->INT0MASK = 0; // disable interrupt if (sw.mode[MAX_SWITCH(i)] != SW_MODE_DISABLED) { - hw.sw_port[i]->DIRCLR = SW_MAX_BIT_bm; // set max input - see 13.14.14 + // set max input - see 13.14.14 + hw.sw_port[i]->DIRCLR = SW_MAX_BIT_bm; hw.sw_port[i]->PIN7CTRL = (PIN_MODE | PORT_ISC_BOTHEDGES_gc); - hw.sw_port[i]->INT1MASK = SW_MAX_BIT_bm; // max on INT1 + hw.sw_port[i]->INT1MASK = SW_MAX_BIT_bm; // max on INT1 } else hw.sw_port[i]->INT1MASK = 0; // set interrupt levels. Interrupts must be enabled in main() - hw.sw_port[i]->INTCTRL = GPIO1_INTLVL; // see gpio.h for setting + hw.sw_port[i]->INTCTRL = GPIO1_INTLVL; // see gpio.h for setting } // Defaults @@ -97,11 +112,12 @@ void switch_init() { /* - * These functions interact with each other to process switch closures and firing. - * Each switch has a counter which is initially set to negative SW_DEGLITCH_TICKS. - * When a switch closure is DETECTED the count increments for each RTC tick. - * When the count reaches zero the switch is tripped and action occurs. - * The counter continues to increment positive until the lockout is exceeded. + * These functions interact with each other to process switch closures + * and firing. Each switch has a counter which is initially set to + * negative SW_DEGLITCH_TICKS. When a switch closure is DETECTED the + * count increments for each RTC tick. When the count reaches zero + * the switch is tripped and action occurs. The counter continues to + * increment positive until the lockout is exceeded. */ // Switch interrupt handler vectors @@ -116,13 +132,16 @@ ISR(A_MAX_ISR_vect) {_switch_isr_helper(SW_MAX_A);} static void _switch_isr_helper(uint8_t sw_num) { - if (sw.mode[sw_num] == SW_MODE_DISABLED) return; // this is never supposed to happen - if (sw.debounce[sw_num] == SW_LOCKOUT) return; // exit if switch is in lockout + if (sw.mode[sw_num] == SW_MODE_DISABLED) return; // never supposed to happen + if (sw.debounce[sw_num] == SW_LOCKOUT) return; // switch is in lockout - sw.debounce[sw_num] = SW_DEGLITCHING; // either transitions state from IDLE or overwrites it - sw.count[sw_num] = -SW_DEGLITCH_TICKS; // reset deglitch count regardless of entry state + // either transitions state from IDLE or overwrites it + sw.debounce[sw_num] = SW_DEGLITCHING; + // reset deglitch count regardless of entry state + sw.count[sw_num] = -SW_DEGLITCH_TICKS; - read_switch(sw_num); // sets the state value in the struct + // sets the state value in the struct + read_switch(sw_num); } @@ -132,7 +151,8 @@ void switch_rtc_callback() { if (sw.mode[i] == SW_MODE_DISABLED || sw.debounce[i] == SW_IDLE) continue; - if (++sw.count[i] == SW_LOCKOUT_TICKS) { // state is either lockout or deglitching + // state is either lockout or deglitching + if (++sw.count[i] == SW_LOCKOUT_TICKS) { sw.debounce[i] = SW_IDLE; // check if the state has changed while we were in lockout... @@ -145,15 +165,17 @@ void switch_rtc_callback() { continue; } - if (sw.count[i] == 0) { // trigger point - sw.sw_num_thrown = i; // record number of thrown switch + if (sw.count[i] == 0) { // trigger point + sw.sw_num_thrown = i; // record number of thrown switch sw.debounce[i] = SW_LOCKOUT; - if ((cm.cycle_state == CYCLE_HOMING) || (cm.cycle_state == CYCLE_PROBE)) // regardless of switch type + // regardless of switch type + if (cm.cycle_state == CYCLE_HOMING || cm.cycle_state == CYCLE_PROBE) cm_request_feedhold(); - else if (sw.mode[i] & SW_LIMIT_BIT) // should be a limit switch, so fire it. - sw.limit_flag = true; // triggers an emergency shutdown + // should be a limit switch, so fire it. + else if (sw.mode[i] & SW_LIMIT_BIT) + sw.limit_flag = true; // triggers an emergency shutdown } } } diff --git a/src/switch.h b/src/switch.h index 7e567ef..ffa25d4 100644 --- a/src/switch.h +++ b/src/switch.h @@ -4,38 +4,46 @@ * * Copyright (c) 2013 - 2014 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* Switch processing functions under Motate * - * Switch processing turns pin transitions into reliable switch states. - * There are 2 main operations: + * Switch processing turns pin transitions into reliable switch states. + * There are 2 main operations: * - * - read pin get raw data from a pin - * - read switch return processed switch closures + * - read pin get raw data from a pin + * - read switch return processed switch closures * - * Read pin may be a polled operation or an interrupt on pin change. If interrupts - * are used they must be provided for both leading and trailing edge transitions. + * Read pin may be a polled operation or an interrupt on pin + * change. If interrupts are used they must be provided for both + * leading and trailing edge transitions. * - * Read switch contains the results of read pin and manages edges and debouncing. + * Read switch contains the results of read pin and manages edges and + * debouncing. */ #ifndef SWITCH_H #define SWITCH_H @@ -43,17 +51,17 @@ #include // timer for debouncing switches -#define SW_LOCKOUT_TICKS 25 // 25=250ms. RTC ticks are ~10ms each -#define SW_DEGLITCH_TICKS 3 // 3=30ms +#define SW_LOCKOUT_TICKS 25 // 25=250ms. RTC ticks are ~10ms each +#define SW_DEGLITCH_TICKS 3 // 3=30ms // switch modes #define SW_HOMING_BIT 0x01 #define SW_LIMIT_BIT 0x02 -#define SW_MODE_DISABLED 0 // disabled for all operations -#define SW_MODE_HOMING SW_HOMING_BIT // enable switch for homing only -#define SW_MODE_LIMIT SW_LIMIT_BIT // enable switch for limits only -#define SW_MODE_HOMING_LIMIT (SW_HOMING_BIT | SW_LIMIT_BIT) // homing and limits -#define SW_MODE_MAX_VALUE SW_MODE_HOMING_LIMIT +#define SW_MODE_DISABLED 0 // disabled for all operations +#define SW_MODE_HOMING SW_HOMING_BIT // enable switch for homing only +#define SW_MODE_LIMIT SW_LIMIT_BIT // enable switch for limits only +#define SW_MODE_HOMING_LIMIT (SW_HOMING_BIT | SW_LIMIT_BIT) // homing & limits +#define SW_MODE_MAX_VALUE SW_MODE_HOMING_LIMIT enum swType { SW_TYPE_NORMALLY_OPEN = 0, @@ -62,21 +70,21 @@ enum swType { enum swState { SW_DISABLED = -1, - SW_OPEN = 0, // also read as 'false' - SW_CLOSED // also read as 'true' + SW_OPEN = 0, // also read as 'false' + SW_CLOSED // also read as 'true' }; // macros for finding the index into the switch table give the axis number -#define MIN_SWITCH(axis) (axis*2) -#define MAX_SWITCH(axis) (axis*2+1) +#define MIN_SWITCH(axis) (axis * 2) +#define MAX_SWITCH(axis) (axis * 2 + 1) -enum swDebounce { // state machine for managing debouncing and lockout +enum swDebounce { // state machine for managing debouncing and lockout SW_IDLE = 0, SW_DEGLITCHING, SW_LOCKOUT }; -enum swNums { // indexes into switch arrays +enum swNums { // indexes into switch arrays SW_MIN_X = 0, SW_MAX_X, SW_MIN_Y, @@ -85,19 +93,14 @@ enum swNums { // indexes into switch arrays SW_MAX_Z, SW_MIN_A, SW_MAX_A, - NUM_SWITCHES // must be last one. Used for array sizing and for loops + NUM_SWITCHES // must be last one. Used for array sizing and for loops }; -#define SW_OFFSET SW_MAX_X // offset between MIN and MAX switches +#define SW_OFFSET SW_MAX_X // offset between MIN and MAX switches #define NUM_SWITCH_PAIRS (NUM_SWITCHES/2) -/* - * Interrupt levels and vectors - The vectors are hard-wired to xmega ports - * If you change axis port assignments you need to change these, too. - */ -// Interrupt level: pick one: -//#define GPIO1_INTLVL (PORT_INT0LVL_HI_gc|PORT_INT1LVL_HI_gc) // can't be hi +/// Interrupt levels and vectors - The vectors are hard-wired to xmega ports +/// If you change axis port assignments you need to change these, too. #define GPIO1_INTLVL (PORT_INT0LVL_MED_gc|PORT_INT1LVL_MED_gc) -//#define GPIO1_INTLVL (PORT_INT0LVL_LO_gc|PORT_INT1LVL_LO_gc) // shouldn;t be low // port assignments for vectors #define X_MIN_ISR_vect PORTA_INT0_vect @@ -114,14 +117,17 @@ enum swNums { // indexes into switch arrays // Note 1: The term "thrown" is used because switches could be normally-open // or normally-closed. "Thrown" means activated or hit. */ -struct swStruct { // switch state - uint8_t switch_type; // 0=NO, 1=NC - applies to all switches - uint8_t limit_flag; // 1=limit switch thrown - do a lockout - uint8_t sw_num_thrown; // number of switch that was just thrown - uint8_t state[NUM_SWITCHES]; // 0=OPEN, 1=CLOSED (depends on switch type) - volatile uint8_t mode[NUM_SWITCHES]; // 0=disabled, 1=homing, 2=homing+limit, 3=limit - volatile uint8_t debounce[NUM_SWITCHES]; // switch debouncer state machine - see swDebounce - volatile int8_t count[NUM_SWITCHES]; // deglitching and lockout counter +struct swStruct { // switch state + uint8_t switch_type; // 0=NO, 1=NC - applies to all switches + uint8_t limit_flag; // 1=limit switch thrown - do a lockout + uint8_t sw_num_thrown; // number of switch that was just thrown + /// 0=OPEN, 1=CLOSED (depends on switch type) + uint8_t state[NUM_SWITCHES]; + /// 0=disabled, 1=homing, 2=homing+limit, 3=limit + volatile uint8_t mode[NUM_SWITCHES]; + /// debouncer state machine - see swDebounce + volatile uint8_t debounce[NUM_SWITCHES]; + volatile int8_t count[NUM_SWITCHES]; // deglitching and lockout counter }; struct swStruct sw; diff --git a/src/tmc2660.c b/src/tmc2660.c index 3d46ffc..d58c61c 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -41,6 +41,9 @@ #include +void set_dcur(int driver, float value); + + typedef enum { TMC2660_STATE_CONFIG, TMC2660_STATE_MONITOR, @@ -222,8 +225,22 @@ void tmc2660_init() { drivers[i].state = TMC2660_STATE_CONFIG; drivers[i].reg = 0; - drivers[i].regs[TMC2660_DRVCTRL] = TMC2660_DRVCTRL_DEDGE | - TMC2660_DRVCTRL_MRES_8; + uint32_t mstep = 0; + switch (MOTOR_MICROSTEPS) { + case 1: mstep = TMC2660_DRVCTRL_MRES_1; break; + case 2: mstep = TMC2660_DRVCTRL_MRES_2; break; + case 4: mstep = TMC2660_DRVCTRL_MRES_4; break; + case 8: mstep = TMC2660_DRVCTRL_MRES_8; break; + case 16: mstep = TMC2660_DRVCTRL_MRES_16; break; + case 32: mstep = TMC2660_DRVCTRL_MRES_32; break; + case 64: mstep = TMC2660_DRVCTRL_MRES_64; break; + case 128: mstep = TMC2660_DRVCTRL_MRES_128; break; + case 256: mstep = TMC2660_DRVCTRL_MRES_256; break; + default: break; // Invalid + } + + drivers[i].regs[TMC2660_DRVCTRL] = TMC2660_DRVCTRL_DEDGE | mstep | + TMC2660_DRVCTRL_INTPOL; drivers[i].regs[TMC2660_CHOPCONF] = TMC2660_CHOPCONF_TBL_36 | TMC2660_CHOPCONF_HEND(3) | TMC2660_CHOPCONF_HSTART(7) | TMC2660_CHOPCONF_TOFF(4); @@ -233,8 +250,11 @@ void tmc2660_init() { drivers[i].regs[TMC2660_SMARTEN] = TMC2660_SMARTEN_SEIMIN | TMC2660_SMARTEN_MAX(2) | TMC2660_SMARTEN_MIN(2); drivers[i].regs[TMC2660_SGCSCONF] = TMC2660_SGCSCONF_SFILT | - TMC2660_SGCSCONF_THRESH(63) | TMC2660_SGCSCONF_CS(6); + TMC2660_SGCSCONF_THRESH(63); drivers[i].regs[TMC2660_DRVCONF] = TMC2660_DRVCONF_RDSEL_SG; + + set_dcur(i, MOTOR_CURRENT); + drivers[driver].reset = 0; // No need to reset } // Setup pins @@ -327,8 +347,8 @@ float get_dcur(int driver) { void set_dcur(int driver, float value) { if (value < 0 || 1 < value) return; - uint8_t x = value * 32.0 - 1; - if (x < 0) x = 1; + uint8_t x = value ? value * 32.0 - 1 : 0; + if (x < 0) x = 0; tmc2660_driver_t *d = &drivers[driver]; d->regs[TMC2660_SGCSCONF] = (d->regs[TMC2660_SGCSCONF] & ~31) | x; diff --git a/src/util.c b/src/util.c index c152bf9..f98ce10 100644 --- a/src/util.c +++ b/src/util.c @@ -4,30 +4,36 @@ * * Copyright (c) 2010 - 2015 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* util contains: - * - math and min/max utilities and extensions - * - vector manipulation utilities + * - math and min/max utilities and extensions + * - vector manipulation utilities */ #include "util.h" @@ -104,9 +110,11 @@ float *set_vector_by_axis(float value, uint8_t axis) { * max3() - return maximum of 3 numbers * max4() - return maximum of 4 numbers * - * Implementation tip: Order the min and max values from most to least likely in the calling args + * Implementation tip: Order the min and max values from most to least likely + * in the calling args * - * (*) Macro min4 is about 20uSec, inline function version is closer to 10 uSec (Xmega 32 MHz) + * (*) Macro min4 is about 20uSec, inline function version is closer to 10 + * uSec (Xmega 32 MHz) * #define min3(a,b,c) (min(min(a,b),c)) * #define min4(a,b,c,d) (min(min(a,b),min(c,d))) * #define max3(a,b,c) (max(max(a,b),c)) diff --git a/src/util.h b/src/util.h index e7ee9dd..d2c6ee3 100644 --- a/src/util.h +++ b/src/util.h @@ -4,25 +4,31 @@ * * Copyright (c) 2010 - 2014 Alden S. Hart, Jr. * - * This file ("the software") is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License, version 2 as published by the - * Free Software Foundation. You should have received a copy of the GNU General Public - * License, version 2 along with the software. If not, see . + * This file ("the software") is free software: you can redistribute + * it and/or modify it under the terms of the GNU General Public + * License, version 2 as published by the Free Software + * Foundation. You should have received a copy of the GNU General + * Public License, version 2 along with the software. If not, see + * . * - * As a special exception, you may use this file as part of a software library without - * restriction. Specifically, if other files instantiate templates or use macros or - * inline functions from this file, or you compile this file and link it with other - * files to produce an executable, this file does not by itself cause the resulting - * executable to be covered by the GNU General Public License. This exception does not - * however invalidate any other reasons why the executable file might be covered by the - * GNU General Public License. + * As a special exception, you may use this file as part of a software + * library without restriction. Specifically, if other files + * instantiate templates or use macros or inline functions from this + * file, or you compile this file and link it with other files to + * produce an executable, this file does not by itself cause the + * resulting executable to be covered by the GNU General Public + * License. This exception does not however invalidate any other + * reasons why the executable file might be covered by the GNU General + * Public License. * - * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT WITHOUT ANY - * WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES - * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT - * SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF - * OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * THE SOFTWARE IS DISTRIBUTED IN THE HOPE THAT IT WILL BE USEFUL, BUT + * WITHOUT ANY WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT + * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A + * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR + * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR + * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE + * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* util.c/.h contains a dog's breakfast of supporting functions that are @@ -39,6 +45,7 @@ #include "config.h" #include +#include // Vector utilities extern float vector[AXES]; // vector of axes for passing to subroutines @@ -84,27 +91,30 @@ uint16_t compute_checksum(char const *string, const uint16_t length); #define avg(a,b) ((a+b)/2) #endif +// allowable rounding error for floats #ifndef EPSILON -#define EPSILON ((float)0.00001) // allowable rounding error for floats +#define EPSILON ((float)0.00001) #endif #ifndef fp_EQ -#define fp_EQ(a,b) (fabs(a - b) < EPSILON) // requires math.h to be included in each file used +#define fp_EQ(a,b) (fabs(a - b) < EPSILON) #endif #ifndef fp_NE -#define fp_NE(a,b) (fabs(a - b) > EPSILON) // requires math.h to be included in each file used +#define fp_NE(a,b) (fabs(a - b) > EPSILON) #endif #ifndef fp_ZERO -#define fp_ZERO(a) (fabs(a) < EPSILON) // requires math.h to be included in each file used +#define fp_ZERO(a) (fabs(a) < EPSILON) #endif #ifndef fp_NOT_ZERO -#define fp_NOT_ZERO(a) (fabs(a) > EPSILON) // requires math.h to be included in each file used +#define fp_NOT_ZERO(a) (fabs(a) > EPSILON) #endif +/// float is interpreted as FALSE (equals zero) #ifndef fp_FALSE -#define fp_FALSE(a) (a < EPSILON) // float is interpreted as FALSE (equals zero) +#define fp_FALSE(a) (a < EPSILON) #endif +/// float is interpreted as TRUE (not equal to zero) #ifndef fp_TRUE -#define fp_TRUE(a) (a > EPSILON) // float is interpreted as TRUE (not equal to zero) +#define fp_TRUE(a) (a > EPSILON) #endif // Constants diff --git a/src/vars.c b/src/vars.c index c611452..65c0067 100644 --- a/src/vars.c +++ b/src/vars.c @@ -243,7 +243,8 @@ int vars_parser(char *vars) { void vars_print_help() { static const char fmt[] PROGMEM = " %-8S %-10S %S\n"; -#define VAR(NAME, TYPE, ...) printf_P(fmt, NAME##_name, TYPE##_name, NAME##_help); +#define VAR(NAME, TYPE, ...) \ + printf_P(fmt, NAME##_name, TYPE##_name, NAME##_help); #include "vars.def" #undef VAR }