* 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 <http://www.gnu.org/licenses/>.
- *
- * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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"
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;
}
-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);
}
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);
}
/*
* 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
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;
}
*/
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;
/// G4, P parameter (seconds)
stat_t cm_dwell(float seconds) {
cm.gm.parameter = seconds;
- mp_dwell(seconds);
- return STAT_OK;
+ return mp_dwell(seconds);
}
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;
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;
}
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;
-}
#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)
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
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 ###
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
};
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
};
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
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
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
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
#include "hardware.h"
#include "report.h"
#include "vars.h"
+#include "plan/jog.h"
#include "config.h"
#include <avr/pgmspace.h>
#include <stdio.h>
+#include <stdlib.h>
#include <ctype.h>
}
-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;
}
+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";
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"));
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;
+}
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")
#define AXIS_W 8 // reserved
// Motor settings
-#define MOTOR_MICROSTEPS 8
+#define MOTOR_MICROSTEPS 16
/// One of:
/// MOTOR_DISABLED
#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
#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
#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
// 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
#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
// 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
// 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
// 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
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
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;
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));
}
// 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
}
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
#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)
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
+++ /dev/null
-/*
- * 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
- * <http://www.gnu.org/licenses/>.
- *
- * 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 <math.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <stdio.h>
-
-
-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;
-}
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
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);
/*
- * 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;
*
* 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 <http://www.gnu.org/licenses/>.
- *
- * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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
#include <stdint.h>
-// 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
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
#include <math.h>
-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
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);
}
/*
* 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
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;
}
/*
* 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);
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);
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;
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:
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);
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;
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);
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
/*
* 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;
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);
//--> 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) {
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
/*
* 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
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);
/// 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
}
/// 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;
}
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
}
/*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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)
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
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();
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
// 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
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;
// 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"));
}
init();
// main loop
- while (1) controller_run(); // single pass through the controller
+ while (1) controller_run(); // single pass through the controller
return 0;
}
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
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;
/*
* 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.
*/
// 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...
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
#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
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
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;
--- /dev/null
+/*
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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 <string.h>
+
+/// 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;
+}
--- /dev/null
+/*
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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_<command>, 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();
+}
--- /dev/null
+/*
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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;
+}
#include <stdbool.h>
#include <math.h>
-// 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();
#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;
}
/* 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;
}
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;
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);
}
}
}
// There are 3 things that can happen here depending on return conditions:
- // status bf->move_state Description
- // ----------- -------------- ----------------------------------------
- // STAT_EAGAIN <don't care> 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 <don't care> 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
/* 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.
*/
#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;
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;
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;
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;
#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;
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;
}
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
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;
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;
#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]);
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
}
--- /dev/null
+/*
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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 <stdbool.h>
+#include <math.h>
+
+/* 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;
+}
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
#include <string.h>
-/*
- * 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
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
#include <math.h>
-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
/// 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;
}
/* 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
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;
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
// '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)
//
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);
}
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;
}
/*
* 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;
}
}
/* 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,
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;
}
}
-/// 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 =
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;
-}
* 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 <http://www.gnu.org/licenses/>.
- *
- * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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"
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();
}
-/*
- * 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;
}
-/************************************************************************************
- * How this works:
- * - The command is called by the Gcode interpreter (cm_<command>, 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;
-}
#define PLANNER_H
#include "canonical_machine.h" // used for GCodeState_t
+#include "util.h"
#include "config.h"
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 {
#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
/// 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[]);
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();
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
/*
- * 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 <http://www.gnu.org/licenses/>.
- *
- * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ * 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 <math.h>
/*
- * 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 Ve<Vt>Vx sufficient length exists for all parts (corner case: HBT')
- * HB Ve<Vt=Vx head accelerates to cruise - exits at full speed (corner case: H')
- * BT Ve=Vt>Vx enter at full speed and decelerate (corner case: T')
- * HT Ve & Vx perfect fit HT (very rare). May be symmetric or asymmetric
- * H Ve<Vx perfect fit H (common, results from planning)
- * T Ve>Vx 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)<Vt symmetric case. Split the length and compute Vt.
- * HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive approximation.
- * HBT' body length < min body length - treated as an HT case
- * H' body length < min body length - subsume body into head length
- * T' body length < min body length - subsume body into tail length
- *
- * Degraded fit cases - line is too short to satisfy both Ve and Vx
- * H" Ve<Vx Ve is degraded (velocity step). Vx is met
- * T" Ve>Vx Ve is degraded (velocity step). Vx is met
- * B" <short> line is very short but drawable; is treated as a body only
- * F <too short> 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 Ve<Vt>Vx sufficient length exists for all parts (corner
+ * case: HBT')
+ * HB Ve<Vt=Vx head accelerates to cruise - exits at full speed
+ * (corner case: H')
+ * BT Ve=Vt>Vx enter at full speed & decelerate (corner case: T')
+ * HT Ve & Vx perfect fit HT (very rare). May be symmetric or
+ * asymmetric
+ * H Ve<Vx perfect fit H (common, results from planning)
+ * T Ve>Vx 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)<Vt symmetric case. Split the length and compute Vt.
+ * HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive
+ * approximation.
+ * HBT' body length < min body length - treated as an HT case
+ * H' body length < min body length - subsume body into head length
+ * T' body length < min body length - subsume body into tail length
+ *
+ * Degraded fit cases - line is too short to satisfy both Ve and Vx
+ * H" Ve<Vx Ve is degraded (velocity step). Vx is met
+ * T" Ve>Vx Ve is degraded (velocity step). Vx is met
+ * B" <short> line is very short but drawable; is treated as a
+ * body only
+ * F <too short> 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;
}
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;
}
// 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;
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;
}
// 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;}
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
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;
}
// 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
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)));
*/
/* 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
// 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
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
// 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
* 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;
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;
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
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;
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
#include <avr/interrupt.h>
-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
}
-/*
- * 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.
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
#include <stdint.h>
-#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
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
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;
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));
}
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
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
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
#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,
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
// 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
}
-
/// 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;
}
-/// 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) {
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
// 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) {
} 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
/// 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
}
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
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
#include <stdbool.h>
+
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
/*
- * 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
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);
}
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...
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
}
}
}
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
#include <stdint.h>
// 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,
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,
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
// 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;
#include <stdio.h>
+void set_dcur(int driver, float value);
+
+
typedef enum {
TMC2660_STATE_CONFIG,
TMC2660_STATE_MONITOR,
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);
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
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;
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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"
* 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))
*
* 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 <http://www.gnu.org/licenses/>.
+ * 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
+ * <http://www.gnu.org/licenses/>.
*
- * 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
#include "config.h"
#include <stdint.h>
+#include <math.h>
// Vector utilities
extern float vector[AXES]; // vector of axes for passing to subroutines
#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
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
}
projects / bbctrl-firmware / commitdiff