From ee9d4872c9f11bb5fbe38f4d5cd70d268d637b76 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Fri, 1 Dec 2017 10:58:00 -0800 Subject: [PATCH] New main AVR code --- src/avr/src/action.c | 136 ++++ src/avr/src/{plan/dwell.h => action.h} | 28 +- src/avr/src/axis.c | 9 +- src/avr/src/axis.h | 1 - src/avr/src/command.c | 33 +- src/avr/src/command.def | 1 - src/avr/src/config.h | 57 +- src/avr/src/coolant.h | 2 - src/avr/src/estop.c | 9 +- src/avr/src/exec.c | 343 ++++++++ src/avr/src/{plan/line.h => exec.h} | 20 +- src/avr/src/gcode_expr.c | 296 ------- src/avr/src/gcode_expr.h | 33 - src/avr/src/gcode_parser.c | 518 ------------ src/avr/src/gcode_parser.h | 87 -- src/avr/src/gcode_state.c | 179 ----- src/avr/src/gcode_state.def | 69 -- src/avr/src/gcode_state.h | 206 ----- src/avr/src/huanyang.c | 24 +- src/avr/src/huanyang.h | 4 +- src/avr/src/{plan => }/jog.c | 43 +- src/avr/src/{plan => }/jog.h | 6 + src/avr/src/machine.c | 934 ---------------------- src/avr/src/machine.h | 142 ---- src/avr/src/main.c | 15 +- src/avr/src/messages.def | 91 +-- src/avr/src/motor.c | 37 +- src/avr/src/motor.h | 6 +- src/avr/src/plan/arc.c | 510 ------------ src/avr/src/plan/arc.h | 47 -- src/avr/src/plan/buffer.c | 240 ------ src/avr/src/plan/buffer.h | 119 --- src/avr/src/plan/calibrate.c | 176 ---- src/avr/src/plan/dwell.c | 54 -- src/avr/src/plan/exec.c | 482 ----------- src/avr/src/plan/exec.h | 35 - src/avr/src/plan/line.c | 437 ---------- src/avr/src/plan/planner.c | 768 ------------------ src/avr/src/plan/planner.h | 86 -- src/avr/src/plan/runtime.c | 171 ---- src/avr/src/plan/runtime.h | 58 -- src/avr/src/plan/velocity_curve.c | 75 -- src/avr/src/plan/velocity_curve.h | 32 - src/avr/src/pwm_spindle.c | 16 +- src/avr/src/pwm_spindle.h | 4 +- src/avr/src/queue.c | 103 +++ src/avr/src/{plan/calibrate.h => queue.h} | 27 +- src/avr/src/ringbuf.def | 4 +- src/avr/src/spindle.c | 46 +- src/avr/src/spindle.h | 5 +- src/avr/src/{plan => }/state.c | 188 +++-- src/avr/src/{plan => }/state.h | 72 +- src/avr/src/stepper.c | 41 +- src/avr/src/stepper.h | 5 +- src/avr/src/usart.c | 41 +- src/avr/src/varcb.c | 141 ---- src/avr/src/vars.c | 2 +- src/avr/src/vars.def | 17 +- 58 files changed, 931 insertions(+), 6400 deletions(-) create mode 100644 src/avr/src/action.c rename src/avr/src/{plan/dwell.h => action.h} (71%) create mode 100644 src/avr/src/exec.c rename src/avr/src/{plan/line.h => exec.h} (78%) delete mode 100644 src/avr/src/gcode_expr.c delete mode 100644 src/avr/src/gcode_expr.h delete mode 100644 src/avr/src/gcode_parser.c delete mode 100644 src/avr/src/gcode_parser.h delete mode 100644 src/avr/src/gcode_state.c delete mode 100644 src/avr/src/gcode_state.def delete mode 100644 src/avr/src/gcode_state.h rename src/avr/src/{plan => }/jog.c (88%) rename src/avr/src/{plan => }/jog.h (96%) delete mode 100644 src/avr/src/machine.c delete mode 100644 src/avr/src/machine.h delete mode 100644 src/avr/src/plan/arc.c delete mode 100644 src/avr/src/plan/arc.h delete mode 100644 src/avr/src/plan/buffer.c delete mode 100644 src/avr/src/plan/buffer.h delete mode 100644 src/avr/src/plan/calibrate.c delete mode 100644 src/avr/src/plan/dwell.c delete mode 100644 src/avr/src/plan/exec.c delete mode 100644 src/avr/src/plan/exec.h delete mode 100644 src/avr/src/plan/line.c delete mode 100644 src/avr/src/plan/planner.c delete mode 100644 src/avr/src/plan/planner.h delete mode 100644 src/avr/src/plan/runtime.c delete mode 100644 src/avr/src/plan/runtime.h delete mode 100644 src/avr/src/plan/velocity_curve.c delete mode 100644 src/avr/src/plan/velocity_curve.h create mode 100644 src/avr/src/queue.c rename src/avr/src/{plan/calibrate.h => queue.h} (69%) rename src/avr/src/{plan => }/state.c (52%) rename src/avr/src/{plan => }/state.h (68%) delete mode 100644 src/avr/src/varcb.c diff --git a/src/avr/src/action.c b/src/avr/src/action.c new file mode 100644 index 0000000..4cc1222 --- /dev/null +++ b/src/avr/src/action.c @@ -0,0 +1,136 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "action.h" + +#include "queue.h" + +#include +#include +#include + + +static stat_t _queue(const char **block) { + queue_push(**block++); + return STAT_OK; +} + + +static stat_t _queue_int(const char **block) { + char *end = 0; + + int32_t value = strtol(*block + 1, &end, 0); + if (end == *block + 1) return STAT_INVALID_ARGUMENTS; + + queue_push_int(**block, value); + *block = end; + + return STAT_OK; +} + + +static stat_t _queue_bool(const char **block) { + if (4 < strlen(*block) && tolower((*block)[1]) == 't' && + tolower((*block)[2]) == 'r' && tolower((*block)[3]) == 'u' && + tolower((*block)[4]) == 'e' && !isalpha((*block)[5])) { + queue_push_bool(**block, true); + *block += 5; + return STAT_OK; + } + + char *end = 0; + int32_t value = strtol(*block + 1, &end, 0); + + queue_push_bool(**block, value); + *block = end; + + return STAT_OK; +} + + +static stat_t _queue_float(const char **block) { + char *end = 0; + + float value = strtod(*block + 1, &end); + if (end == *block + 1) return STAT_INVALID_ARGUMENTS; + + queue_push_float(**block, value); + *block = end; + + return STAT_OK; +} + + +static stat_t _queue_pair(const char **block) { + char *end = 0; + + int32_t left = strtol(*block + 1, &end, 0); + if (end == *block + 1) return STAT_INVALID_ARGUMENTS; + *block = end; + + int32_t right = strtol(*block, &end, 0); + if (*block == end) return STAT_INVALID_ARGUMENTS; + + queue_push_pair(**block, left, right); + *block = end; + + return STAT_OK; +} + + +stat_t action_parse(const char *block) { + stat_t status = STAT_OK; + + while (*block && status == STAT_OK) { + if (isspace(*block)) {block++; continue;} + + status = STAT_INVALID_COMMAND; + + switch (*block) { + case ACTION_SCURVE: status = _queue_int(&block); break; + case ACTION_DATA: status = _queue_float(&block); break; + case ACTION_VELOCITY: status = _queue_float(&block); break; + case ACTION_X: status = _queue_float(&block); break; + case ACTION_Y: status = _queue_float(&block); break; + case ACTION_Z: status = _queue_float(&block); break; + case ACTION_A: status = _queue_float(&block); break; + case ACTION_B: status = _queue_float(&block); break; + case ACTION_C: status = _queue_float(&block); break; + case ACTION_SEEK: status = _queue_pair(&block); break; + case ACTION_OUTPUT: status = _queue_pair(&block); break; + case ACTION_DWELL: status = _queue_float(&block); break; + case ACTION_PAUSE: status = _queue_bool(&block); break; + case ACTION_TOOL: status = _queue_int(&block); break; + case ACTION_SPEED: status = _queue_float(&block); break; + case ACTION_JOG: status = _queue(&block); break; + case ACTION_LINE_NUM: status = _queue_int(&block); break; + case ACTION_SET_HOME: status = _queue_pair(&block); break; + } + } + + return status; +} diff --git a/src/avr/src/plan/dwell.h b/src/avr/src/action.h similarity index 71% rename from src/avr/src/plan/dwell.h rename to src/avr/src/action.h index 0345084..d8678fa 100644 --- a/src/avr/src/plan/dwell.h +++ b/src/avr/src/action.h @@ -29,7 +29,31 @@ #include "status.h" -#include +typedef enum { + ACTION_SCURVE = 's', + ACTION_DATA = 'd', + ACTION_VELOCITY = 'v', -stat_t mp_dwell(float seconds, int32_t line); + ACTION_X = 'X', + ACTION_Y = 'Y', + ACTION_Z = 'Z', + ACTION_A = 'A', + ACTION_B = 'B', + ACTION_C = 'C', + + ACTION_SEEK = 'K', + ACTION_OUTPUT = 'O', + + ACTION_DWELL = 'D', + ACTION_PAUSE = 'P', + ACTION_TOOL = 'T', + ACTION_SPEED = 'S', + ACTION_JOG = 'J', + ACTION_LINE_NUM = 'N', + + ACTION_SET_HOME = 'H', +} action_t; + + +stat_t action_parse(const char *block); diff --git a/src/avr/src/axis.c b/src/avr/src/axis.c index a000439..3af8ef1 100644 --- a/src/avr/src/axis.c +++ b/src/avr/src/axis.c @@ -28,7 +28,8 @@ #include "axis.h" #include "motor.h" -#include "plan/planner.h" +#include "switch.h" +#include "util.h" #include #include @@ -140,7 +141,11 @@ AXIS_GET(latch_backoff, float, 0) * * The axis_jerk() functions expect the jerk in divided by 1,000,000 form. */ -AXIS_GET(jerk_max, float, 0) +float axis_get_jerk_max(int axis) { + int motor = axis_get_motor(axis); + return motor == -1 ? 0 : axes[motor].jerk_max * JERK_MULTIPLIER; +} +AXIS_VAR_GET(jerk_max, float) AXIS_VAR_SET(velocity_max, float) diff --git a/src/avr/src/axis.h b/src/avr/src/axis.h index 0d8251a..79305b0 100644 --- a/src/avr/src/axis.h +++ b/src/avr/src/axis.h @@ -36,7 +36,6 @@ enum { AXIS_X, AXIS_Y, AXIS_Z, AXIS_A, AXIS_B, AXIS_C, - AXIS_U, AXIS_V, AXIS_W // reserved }; diff --git a/src/avr/src/command.c b/src/avr/src/command.c index 8910de6..565e98e 100644 --- a/src/avr/src/command.c +++ b/src/avr/src/command.c @@ -27,17 +27,17 @@ #include "command.h" -#include "gcode_parser.h" #include "usart.h" #include "hardware.h" #include "report.h" #include "vars.h" #include "estop.h" #include "i2c.h" -#include "plan/buffer.h" -#include "plan/state.h" #include "config.h" #include "pgmspace.h" +#include "state.h" +#include "exec.h" +#include "action.h" #ifdef __AVR__ #include @@ -58,11 +58,11 @@ static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) { case I2C_NULL: break; case I2C_ESTOP: estop_trigger(STAT_ESTOP_USER); break; case I2C_CLEAR: estop_clear(); break; - case I2C_PAUSE: mp_request_hold(); break; - case I2C_OPTIONAL_PAUSE: mp_request_optional_pause(); break; - case I2C_RUN: mp_request_start(); break; - case I2C_STEP: mp_request_step(); break; - case I2C_FLUSH: mp_request_flush(); break; + case I2C_PAUSE: state_request_hold(); break; + case I2C_OPTIONAL_PAUSE: state_request_optional_pause(); break; + case I2C_RUN: state_request_start(); break; + case I2C_STEP: state_request_step(); break; + case I2C_FLUSH: state_request_flush(); break; case I2C_REPORT: report_request_full(); break; case I2C_REBOOT: hw_request_hard_reset(); break; } @@ -128,8 +128,7 @@ int command_exec(int argc, char *argv[]) { return cb(argc, argv); } - } else if (argc != 1) - return STAT_INVALID_OR_MALFORMED_COMMAND; + } else if (argc != 1) return STAT_INVALID_COMMAND; // Get or set variable char *value = strchr(argv[0], '='); @@ -241,17 +240,17 @@ void command_callback() { switch (*_cmd) { case 0: break; // Empty line - case '{': status = vars_parser(_cmd); break; + case '{': status = vars_parser(_cmd); break; // TODO is this necessary? case '$': status = command_parser(_cmd); break; - case '%': break; // GCode program separator, ignore it default: if (estop_triggered()) {status = STAT_MACHINE_ALARMED; break;} - else if (mp_is_flushing()) break; // Flush GCode command - else if (!mp_is_ready()) return; // Wait for motion planner + else if (state_is_flushing()) break; // Flush command + else if (!state_is_ready()) return; // Wait for exec queue - // Parse and execute GCode command - status = gc_gcode_parser(_cmd); + // Parse and execute action + status = action_parse(_cmd); + if (status == STAT_QUEUE_FULL) return; // Try again later } _cmd = 0; // Command consumed @@ -331,6 +330,6 @@ uint8_t command_messages(int argc, char *argv[]) { uint8_t command_resume(int argc, char *argv[]) { - mp_request_resume(); + state_request_resume(); return 0; } diff --git a/src/avr/src/command.def b/src/avr/src/command.def index b11766f..130fa10 100644 --- a/src/avr/src/command.def +++ b/src/avr/src/command.def @@ -31,6 +31,5 @@ CMD(report, 1, 2, "[var] . Enable or disable var reporting") CMD(reboot, 0, 0, "Reboot the controller") CMD(jog, 1, 4, "Jog") CMD(mreset, 0, 1, "Reset motor") -CMD(calibrate, 0, 0, "Calibrate motors") CMD(messages, 0, 0, "Dump all possible status messages") CMD(resume, 0, 0, "Resume processing after a flush") diff --git a/src/avr/src/config.h b/src/avr/src/config.h index 247054c..02069ca 100644 --- a/src/avr/src/config.h +++ b/src/avr/src/config.h @@ -96,13 +96,12 @@ enum { #define AXES 6 // number of axes #define MOTORS 4 // number of motors on the board -#define COORDS 6 // number of supported coordinate systems #define SWITCHES 10 // number of supported switches #define OUTS 5 // number of supported pin outputs // Switch settings. See switch.c -#define SWITCH_INTLVL PORT_INT0LVL_MED_gc +#define SWITCH_INTLVL PORT_INT0LVL_MED_gc // Motor ISRs @@ -114,10 +113,9 @@ enum { * * HI Stepper timers (set in stepper.h) * LO Segment execution SW interrupt (set in stepper.h) - * MED GPIO1 switch port (set in gpio.h) * MED Serial RX (set in usart.c) * MED Serial TX (set in usart.c) (* see note) - * LO Real time clock interrupt (set in xmega_rtc.h) + * LO Real time clock interrupt (set in rtc.h) * * (*) The TX cannot run at LO level or exception reports and other prints * called from a LO interrupt (as in prep_line()) will kill the system @@ -146,7 +144,6 @@ enum { // Timer setup for stepper and dwells -#define STEP_TIMER_DISABLE 0 #define STEP_TIMER_ENABLE TC_CLKSEL_DIV4_gc #define STEP_TIMER_DIV 4 #define STEP_TIMER_FREQ (F_CPU / STEP_TIMER_DIV) @@ -156,11 +153,7 @@ enum { #define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc #define STEP_LOW_LEVEL_ISR ADCB_CH0_vect -#define SEGMENT_USEC 5000.0 // segment time -#define SEGMENT_SEC (SEGMENT_USEC / 1000000.0) -#define SEGMENT_TIME (SEGMENT_SEC / 60.0) -#define SEGMENT_CLOCKS ((uint24_t)(F_CPU * SEGMENT_SEC)) -#define SEGMENT_PERIOD ((uint16_t)(STEP_TIMER_FREQ * SEGMENT_SEC)) +#define SEGMENT_TIME (0.005 / 60.0) // mins // DRV8711 settings @@ -199,23 +192,6 @@ enum { #define INPUT_BUFFER_LEN 255 // text buffer size (255 max) -// Planner -/// Should be at least the number of buffers required to support optimal -/// planning in the case of very short lines or arc segments. Suggest no less -/// than 12. Must leave headroom for stack. -#define PLANNER_BUFFER_POOL_SIZE 32 - -/// Buffers to reserve in planner before processing new input line -#define PLANNER_BUFFER_HEADROOM 4 - -/// Minimum number of filled buffers before timeout until execution starts -#define PLANNER_EXEC_MIN_FILL 4 - -/// Delay before executing new buffers unless PLANNER_EXEC_MIN_FILL is met -/// This gives the planner a chance to make a good plan before execution starts -#define PLANNER_EXEC_DELAY 250 // In ms - - // I2C #define I2C_DEV TWIC #define I2C_ISR TWIC_TWIS_vect @@ -227,30 +203,13 @@ enum { // Motor settings. See motor.c #define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time - - #define MIN_HALF_STEP_CORRECTION 4 -#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal corner accel + #define JOG_MIN_VELOCITY 10 // mm/min -#define CAL_ACCELERATION 500000 // mm/min^2 #define CURRENT_SENSE_RESISTOR 0.05 // ohms #define CURRENT_SENSE_REF 2.75 // volts #define MAX_CURRENT 10 // amps - -// Arc -#define ARC_RADIUS_ERROR_MAX 1.0 // max mm diff between start and end radius -#define ARC_RADIUS_ERROR_MIN 0.005 // min mm where 1% rule applies -#define ARC_RADIUS_TOLERANCE 0.001 // 0.1% radius variance test - - -// Gcode defaults -#define GCODE_DEFAULT_UNITS MILLIMETERS // MILLIMETERS or INCHES -#define GCODE_DEFAULT_PLANE PLANE_XY // See machine.h -#define GCODE_DEFAULT_COORD_SYSTEM G54 // G54, G55, G56, G57, G58, G59 -#define GCODE_DEFAULT_PATH_CONTROL PATH_CONTINUOUS -#define GCODE_DEFAULT_DISTANCE_MODE ABSOLUTE_MODE -#define GCODE_DEFAULT_ARC_DISTANCE_MODE INCREMENTAL_MODE -#define GCODE_MAX_OPERATOR_DEPTH 16 -#define GCODE_MAX_VALUE_DEPTH 32 +#define JERK_MULTIPLIER 1000000.0 +#define QUEUE_SIZE 256 +#define EXEC_FILL_TARGET 64 +#define EXEC_DELAY 250 // ms diff --git a/src/avr/src/coolant.h b/src/avr/src/coolant.h index 0d9b074..f3cfed0 100644 --- a/src/avr/src/coolant.h +++ b/src/avr/src/coolant.h @@ -33,5 +33,3 @@ void coolant_init(); void coolant_set_mist(bool x); void coolant_set_flood(bool x); -bool coolant_get_mist(); -bool coolant_get_flood(); diff --git a/src/avr/src/estop.c b/src/avr/src/estop.c index 99ed0f5..50cb37a 100644 --- a/src/avr/src/estop.c +++ b/src/avr/src/estop.c @@ -33,9 +33,8 @@ #include "report.h" #include "hardware.h" #include "config.h" - -#include "plan/planner.h" -#include "plan/state.h" +#include "state.h" +#include "exec.h" #include @@ -73,7 +72,7 @@ void estop_init() { switch_set_callback(SW_ESTOP, _switch_callback); - if (estop.triggered) mp_state_estop(); + if (estop.triggered) state_estop(); // Fault signal SET_PIN(FAULT_PIN, estop.triggered); @@ -93,7 +92,7 @@ void estop_trigger(stat_t reason) { spindle_stop(); // Set machine state - mp_state_estop(); + state_estop(); // Save reason _set_reason(reason); diff --git a/src/avr/src/exec.c b/src/avr/src/exec.c new file mode 100644 index 0000000..99c1d6c --- /dev/null +++ b/src/avr/src/exec.c @@ -0,0 +1,343 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "exec.h" +#include "queue.h" +#include "jog.h" +#include "stepper.h" +#include "axis.h" +#include "coolant.h" +#include "spindle.h" +#include "rtc.h" +#include "util.h" +#include "config.h" + +#include +#include + + +typedef struct { + bool new; + + float position[AXES]; + float target[AXES]; + float unit[AXES]; + + uint16_t steps; + uint16_t step; + float delta; + + float dist; + float vel; + float jerk; +} segment_t; + + +static struct { + bool busy; + bool new; + + float position[AXES]; + float velocity; + float accel; + float jerk; + + int tool; + int line; + + float feed_override; + float spindle_override; + + int scurve; + float time; + float leftover_time; + + bool seek_error; + bool seek_open; + int seek_switch; + + uint32_t last_empty; + + segment_t seg; +} ex; + + +void exec_init() { + memset(&ex, 0, sizeof(ex)); + ex.feed_override = 1; + ex.spindle_override = 1; + ex.seek_switch = -1; + // TODO implement seek + // TODO implement feedhold + // TODO implement move stepping + // TODO implement overrides +} + + +// Var callbacks +int32_t get_line() {return ex.line;} +uint8_t get_tool() {return ex.tool;} +float get_velocity() {return ex.velocity;} +float get_acceleration() {return ex.accel;} +float get_jerk() {return ex.jerk;} +float get_feed_override() {return ex.feed_override;} +float get_speed_override() {return ex.spindle_override;} +float get_axis_mach_coord(int axis) {return exec_get_axis_position(axis);} + +void set_tool(uint8_t tool) {ex.tool = tool;} +void set_feed_override(float value) {ex.feed_override = value;} +void set_speed_override(float value) {ex.spindle_override = value;} +void set_axis_mach_coord(int axis, float position) { + exec_set_axis_position(axis, position); +} + + +bool exec_is_busy() {return ex.busy;} +float exec_get_axis_position(int axis) {return ex.position[axis];} +void exec_set_axis_position(int axis, float p) {ex.position[axis] = p;} +void exec_set_velocity(float v) {ex.velocity = v;} +void exec_set_line(int line) {ex.line = line;} + + +stat_t exec_move_to_target(float time, const float target[]) { + ASSERT(isfinite(target[AXIS_X]) && isfinite(target[AXIS_Y]) && + isfinite(target[AXIS_Z]) && isfinite(target[AXIS_A]) && + isfinite(target[AXIS_B]) && isfinite(target[AXIS_C])); + + // Update position + copy_vector(ex.position, target); + + // No move if time is too short + if (time < 0.5 * SEGMENT_TIME) { + ex.leftover_time += time; + return STAT_NOOP; + } + + time += ex.leftover_time; + ex.leftover_time = 0; + + // Call the stepper prep function + st_prep_line(time, target); + + return STAT_OK; +} + + +void exec_reset_encoder_counts() {st_set_position(ex.position);} + + +static float _compute_distance(double t, double v, double a, double j) { + // v * t + 1/2 * a * t^2 + 1/6 * j * t^3 + return t * (v + t * (0.5 * a + 1.0 / 6.0 * j * t)); +} + + +static float _compute_velocity(double t, double a, double j) { + // a * t + 1/2 * j * t^2 + return t * (a + 0.5 * j * t); +} + + +static void _segment_init(float time) { + ASSERT(isfinite(time) && 0 < time && time < 0x10000 * SEGMENT_TIME); + + // Init segment + ex.seg.new = false; + ex.time = time; + ex.seg.step = 0; + ex.seg.steps = ceil(ex.time / SEGMENT_TIME); + ex.seg.delta = time / ex.seg.steps; + if (ex.scurve == 0) ex.seg.dist = 0; + + // Record starting position and compute unit vector + float length = 0; + for (int i = 0; i < AXES; i++) { + ex.seg.position[i] = ex.position[i]; + ex.seg.unit[i] = ex.seg.target[i] - ex.position[i]; + length = ex.seg.unit[i] * ex.seg.unit[i]; + } + length = sqrt(length); + for (int i = 0; i < AXES; i++) ex.seg.unit[i] /= length; + + // Compute axis limited jerk + if (ex.scurve == 0) { + ex.seg.jerk = FLT_MAX; + + for (int i = 0; i < AXES; i++) + if (ex.seg.unit[i]) { + double j = fabs(axis_get_jerk_max(i) / ex.seg.unit[i]); + if (j < ex.seg.jerk) ex.seg.jerk = j; + } + } + + // Jerk + switch (ex.scurve) { + case 0: case 6: ex.jerk = ex.seg.jerk; break; + case 2: case 4: ex.jerk = -ex.seg.jerk; break; + default: ex.jerk = 0; + } + + // Acceleration + switch (ex.scurve) { + case 1: ex.accel = ex.seg.jerk * ex.time; break; + case 3: ex.accel = 0; break; + case 5: ex.accel = -ex.seg.jerk * ex.time; break; + } +} + + +static stat_t _move_distance(float dist, bool end) { + // Compute target position from distance + float target[AXES]; + for (int i = 0; i < AXES; i++) + target[i] = ex.seg.position[i] + ex.seg.unit[i] * dist; + + // Check if we have reached the end of the segment + for (int i = 0; end && i < AXES; i++) + if (0.000001 < fabs(ex.seg.target[i] - target[i])) end = false; + + // Do move + return exec_move_to_target(ex.seg.delta, end ? ex.seg.target : target); +} + + +static stat_t _segment_body() { + // Compute time, distance and velocity offsets + float t = ex.seg.delta * (ex.seg.step + 1); + float d = _compute_distance(t, ex.seg.vel, ex.accel, ex.jerk); + float v = _compute_velocity(t, ex.accel, ex.jerk); + + // Update velocity + exec_set_velocity(ex.seg.vel + v); + + return _move_distance(ex.seg.dist + d, false); +} + + +static void _set_scurve(int scurve) { + ex.scurve = scurve; + ex.seg.new = true; +} + + +static stat_t _segment_end() { + // Update distance and velocity + ex.seg.dist += _compute_distance(ex.time, ex.seg.vel, ex.accel, ex.jerk); + ex.seg.vel += _compute_velocity(ex.time, ex.accel, ex.jerk); + + // Set final segment velocity + exec_set_velocity(ex.seg.vel); + + // Automatically advance S-curve segment + if (++ex.scurve == 7) ex.scurve = 0; + _set_scurve(ex.scurve); + + return _move_distance(ex.seg.dist, true); +} + + +static stat_t _scurve_action(float time) { + if (time <= 0) return STAT_NOOP; // Skip invalid curves + if (ex.seg.new) _segment_init(time); + return ++ex.seg.step == ex.seg.steps ? _segment_end() : _segment_body(); +} + + +static void _set_output() { + int output = queue_head_left(); + bool enable = queue_head_right(); + + switch (output) { + case 0: coolant_set_mist(enable); break; + case 1: coolant_set_flood(enable); break; + } +} + + +static void _seek() { + ex.seek_switch = queue_head_left(); + ex.seek_open = queue_head_right() & (1 << 0); + ex.seek_error = queue_head_right() & (1 << 1); +} + + +static void _set_home() { + axis_set_homed(queue_head_left(), queue_head_right()); +} + + +static void _pause(bool optional) { + // TODO Initial immediate feedhold +} + + +stat_t exec_next_action() { + if (queue_is_empty()) { + ex.busy = false; + ex.last_empty = rtc_get_time(); + return STAT_NOOP; + } + + // On restart wait a bit to give queue a chance to fill + if (!ex.busy && queue_get_fill() < EXEC_FILL_TARGET && + !rtc_expired(ex.last_empty + EXEC_DELAY)) return STAT_NOOP; + + ex.busy = true; + stat_t status = STAT_NOOP; + + switch (queue_head()) { + case ACTION_SCURVE: _set_scurve(queue_head_int()); break; + case ACTION_DATA: status = _scurve_action(queue_head_float()); break; // TODO + case ACTION_VELOCITY: ex.seg.vel = queue_head_float(); break; + case ACTION_X: ex.seg.target[AXIS_X] = queue_head_float(); break; + case ACTION_Y: ex.seg.target[AXIS_Y] = queue_head_float(); break; + case ACTION_Z: ex.seg.target[AXIS_Z] = queue_head_float(); break; + case ACTION_A: ex.seg.target[AXIS_A] = queue_head_float(); break; + case ACTION_B: ex.seg.target[AXIS_B] = queue_head_float(); break; + case ACTION_C: ex.seg.target[AXIS_C] = queue_head_float(); break; + case ACTION_SEEK: _seek(); break; + case ACTION_OUTPUT: _set_output(); break; + case ACTION_DWELL: st_prep_dwell(queue_head_float()); status = STAT_OK; break; + case ACTION_PAUSE: _pause(queue_head_bool()); status = STAT_PAUSE; break; + case ACTION_TOOL: set_tool(queue_head_int()); break; + case ACTION_SPEED: spindle_set_speed(queue_head_float()); break; + case ACTION_JOG: status = jog_exec(); break; + case ACTION_LINE_NUM: exec_set_line(queue_head_int()); break; + case ACTION_SET_HOME: _set_home(); break; + default: status = STAT_INTERNAL_ERROR; break; + } + + if (status != STAT_EAGAIN) queue_pop(); + + switch (status) { + case STAT_NOOP: return STAT_EAGAIN; + case STAT_PAUSE: return STAT_NOOP; + case STAT_EAGAIN: return STAT_OK; + default: break; + } + return status; +} diff --git a/src/avr/src/plan/line.h b/src/avr/src/exec.h similarity index 78% rename from src/avr/src/plan/line.h rename to src/avr/src/exec.h index e1f3b35..6ae311f 100644 --- a/src/avr/src/plan/line.h +++ b/src/avr/src/exec.h @@ -27,13 +27,23 @@ #pragma once + +#include "pgmspace.h" #include "status.h" -#include "buffer.h" #include -#include -stat_t mp_aline(const float target[], buffer_flags_t flags, switch_id_t sw, - float feed_rate, float feed_override, int32_t line); -int mp_find_jerk_axis(const float axis_square[]); +void exec_init(); + +bool exec_is_busy(); + +float exec_get_axis_position(int axis); // jog.c +void exec_set_axis_position(int axis, float position); + +void exec_set_velocity(float v); // jog.c +void exec_set_line(int line); + +stat_t exec_move_to_target(float time, const float target[]); +void exec_reset_encoder_counts(); +stat_t exec_next_action(); diff --git a/src/avr/src/gcode_expr.c b/src/avr/src/gcode_expr.c deleted file mode 100644 index 8ac1a50..0000000 --- a/src/avr/src/gcode_expr.c +++ /dev/null @@ -1,296 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "gcode_expr.h" - -#include "gcode_parser.h" -#include "vars.h" - -#include -#include -#include - - -float parse_gcode_number(char **p) { - // Avoid parsing G0X10 as a hexadecimal number - if (**p == '0' && toupper(*(*p + 1)) == 'X') { - (*p)++; // pointer points to X - return 0; - } - - // Skip leading zeros so we don't parse as octal - while (**p == '0' && isdigit(*(*p + 1))) p++; - - // Parse number - char *end; - float value = strtod(*p, &end); - if (end == *p) { - parser.error = STAT_BAD_NUMBER_FORMAT; - return 0; - } - *p = end; // Point to next character after the word - - return value; -} - - -static float _parse_gcode_var(char **p) { - (*p)++; // Skip # - - if (isdigit(**p)) { - // TODO numbered parameters - parser.error = STAT_GCODE_NUM_PARAM_UNSUPPORTED; - - } else if (**p == '<') { - (*p)++; - - // Assigning vars is not supported so the '_' global prefix is optional - if (**p == '_') (*p)++; - - char *name = *p; - while (**p && **p != '>') (*p)++; - - if (**p != '>') parser.error = STAT_GCODE_UNTERMINATED_VAR; - else { - *(*p)++ = 0; // Null terminate - return vars_get_number(name); - } - } - - return 0; -} - - -static float _parse_gcode_func(char **p) { - // TODO LinuxCNC supports GCode functions: ATAN, ABS, ACOS, ASIN, COS, EXP, - // FIX, FUP, ROUND, LN, SIN, TAN & EXISTS. - // See http://linuxcnc.org/docs/html/gcode/overview.html#gcode:functions - parser.error = STAT_GCODE_FUNC_UNSUPPORTED; - return 0; -} - - -static int _op_precedence(op_t op) { - switch (op) { - case OP_INVALID: break; - case OP_MINUS: return 6; - case OP_EXP: return 5; - case OP_MUL: case OP_DIV: case OP_MOD: return 4; - case OP_ADD: case OP_SUB: return 3; - case OP_EQ: case OP_NE: case OP_GT: case OP_GE: case OP_LT: case OP_LE: - return 2; - case OP_AND: case OP_OR: case OP_XOR: return 1; - } - return 0; -} - - -static op_t _parse_gcode_op(char **_p) { - char *p = *_p; - op_t op = OP_INVALID; - - switch (toupper(p[0])) { - case '*': op = p[1] == '*' ? OP_EXP : OP_MUL; break; - case '/': op = OP_DIV; break; - - case 'M': - if (toupper(p[1]) == 'O' && toupper(p[1]) == 'O') op = OP_EXP; - break; - - case '+': op = OP_ADD; break; - case '-': op = OP_SUB; break; - - case 'E': if (toupper(p[1]) == 'Q') op = OP_EQ; break; - case 'N': if (toupper(p[1]) == 'E') op = OP_NE; break; - - case 'G': - if (toupper(p[1]) == 'T') op = OP_GT; - if (toupper(p[1]) == 'E') op = OP_GE; - break; - - case 'L': - if (toupper(p[1]) == 'T') op = OP_LT; - if (toupper(p[1]) == 'E') op = OP_LE; - break; - - case 'A': - if (toupper(p[1]) == 'N' && toupper(p[2]) == 'D') op = OP_AND; - break; - - case 'O': if (toupper(p[1]) == 'R') op = OP_OR; break; - - case 'X': - if (toupper(p[1]) == 'O' && toupper(p[2]) == 'R') op = OP_XOR; - break; - } - - // Advance pointer - switch (op) { - case OP_INVALID: break; - case OP_MINUS: case OP_MUL: case OP_DIV: case OP_ADD: - case OP_SUB: *_p += 1; break; - case OP_EXP: case OP_EQ: case OP_NE: case OP_GT: case OP_GE: case OP_LT: - case OP_LE: case OP_OR: *_p += 2; break; - case OP_MOD: case OP_AND: case OP_XOR: *_p += 3; break; - } - - return op; -} - - -static float _apply_binary(op_t op, float left, float right) { - switch (op) { - case OP_INVALID: case OP_MINUS: return 0; - - case OP_EXP: return pow(left, right); - - case OP_MUL: return left * right; - case OP_DIV: return left / right; - case OP_MOD: return fmod(left, right); - - case OP_ADD: return left + right; - case OP_SUB: return left - right; - - case OP_EQ: return left == right; - case OP_NE: return left != right; - case OP_GT: return left > right; - case OP_GE: return left >= right; - case OP_LT: return left > right; - case OP_LE: return left <= right; - - case OP_AND: return left && right; - case OP_OR: return left || right; - case OP_XOR: return (bool)left ^ (bool)right; - } - - return 0; -} - - -static void _val_push(float val) { - if (parser.valPtr < GCODE_MAX_VALUE_DEPTH) parser.vals[parser.valPtr++] = val; - else parser.error = STAT_EXPR_VALUE_STACK_OVERFLOW; -} - - -static float _val_pop() { - if (parser.valPtr) return parser.vals[--parser.valPtr]; - parser.error = STAT_EXPR_VALUE_STACK_UNDERFLOW; - return 0; -} - - -static bool _op_empty() {return !parser.opPtr;} - - -static void _op_push(op_t op) { - if (parser.opPtr < GCODE_MAX_OPERATOR_DEPTH) parser.ops[parser.opPtr++] = op; - else parser.error = STAT_EXPR_OP_STACK_OVERFLOW; -} - - -static op_t _op_pop() { - if (parser.opPtr) return parser.ops[--parser.opPtr]; - parser.error = STAT_EXPR_OP_STACK_UNDERFLOW; - return OP_INVALID; -} - - -static op_t _op_top() { - if (parser.opPtr) return parser.ops[parser.opPtr - 1]; - parser.error = STAT_EXPR_OP_STACK_UNDERFLOW; - return OP_INVALID; -} - - -static void _op_apply() { - op_t op = _op_pop(); - - if (op == OP_MINUS) _val_push(-_val_pop()); - - else { - float right = _val_pop(); - float left = _val_pop(); - - _val_push(_apply_binary(op, left, right)); - } -} - - -// Parse expressions with Dijkstra's Shunting Yard Algorithm -float parse_gcode_expression(char **p) { - bool unary = true; // Used to detect unary minus - - while (!parser.error && **p) { - switch (**p) { - case ' ': case '\n': case '\r': case '\t': (*p)++; break; - case '#': _val_push(_parse_gcode_var(p)); unary = false; break; - case '[': _op_push(OP_INVALID); (*p)++; unary = true; break; - - case ']': - (*p)++; - - while (!parser.error && _op_top() != OP_INVALID) - _op_apply(); - - _op_pop(); // Pop opening bracket - if (_op_empty() && parser.valPtr == 1) return _val_pop(); - unary = false; - break; - - default: - if (isdigit(**p) || **p == '.') { - _val_push(parse_gcode_number(p)); - unary = false; - - } else if (isalpha(**p)) { - _val_push(_parse_gcode_func(p)); - unary = false; - - } else { - op_t op = _parse_gcode_op(p); - - if (unary && op == OP_ADD) continue; // Ignore it - if (unary && op == OP_SUB) {_op_push(OP_MINUS); continue;} - - if (op == OP_INVALID) parser.error = STAT_INVALID_OR_MALFORMED_COMMAND; - else { - int precedence = _op_precedence(op); - - while (!parser.error && !_op_empty() && - precedence <= _op_precedence(_op_top())) - _op_apply(); - - _op_push(op); - unary = true; - } - } - } - } - - return _val_pop(); -} diff --git a/src/avr/src/gcode_expr.h b/src/avr/src/gcode_expr.h deleted file mode 100644 index d7a963a..0000000 --- a/src/avr/src/gcode_expr.h +++ /dev/null @@ -1,33 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -float parse_gcode_number(char **p); -float parse_gcode_expression(char **p); diff --git a/src/avr/src/gcode_parser.c b/src/avr/src/gcode_parser.c deleted file mode 100644 index 97fbe40..0000000 --- a/src/avr/src/gcode_parser.c +++ /dev/null @@ -1,518 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "gcode_parser.h" - -#include "gcode_expr.h" -#include "machine.h" -#include "plan/arc.h" -#include "axis.h" -#include "util.h" - -#include -#include -#include -#include -#include -#include - - -parser_t parser = {{0}}; - - -#define SET_MODAL(m, parm, val) \ - {parser.gn.parm = val; parser.gf.parm = true; parser.modals[m] += 1; break;} -#define SET_NON_MODAL(parm, val) \ - {parser.gn.parm = val; parser.gf.parm = true; break;} -#define EXEC_FUNC(f, parm) if (parser.gf.parm) f(parser.gn.parm) - - -// NOTE Nested comments are not allowed. E.g. (msg (hello)) -static char *_parse_gcode_comment(char *p) { - char *msg = 0; - - p++; // Skip leading paren - - while (isspace(*p)) p++; // skip whitespace - - // Look for "(MSG" - if (tolower(*(p + 0)) == 'm' && - tolower(*(p + 1)) == 's' && - tolower(*(p + 2)) == 'g') { - p += 3; - while (isspace(*p)) p++; // skip whitespace - if (*p && *p != ')') msg = p; - } - - // Find end - while (*p && *p != ')') p++; - *p = 0; // Terminate string - - // Queue message - if (msg) mach_message(msg); - - return p; -} - - -static stat_t _parse_gcode_value(char **p, float *value) { - while (isspace(**p)) (*p)++; // skip whitespace - - if (**p == '[') *value = parse_gcode_expression(p); - else *value = parse_gcode_number(p); - - return parser.error; -} - - -/// Isolate the decimal point value as an integer -static uint8_t _point(float value) {return value * 10 - trunc(value) * 10;} - - -#if 0 -static bool _axis_changed() { - for (int axis = 0; axis < AXES; axis++) - if (parser.gf.target[axis]) return true; - return false; -} -#endif - - -/// 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 (parser.modals[MODAL_GROUP_G0] && parser.modals[MODAL_GROUP_G1]) - return STAT_MODAL_GROUP_VIOLATION; - -#if 0 // TODO This check fails for arcs which may have offsets but no axis word - // look for commands that require an axis word to be present - if (parser.modals[MODAL_GROUP_G0] || parser.modals[MODAL_GROUP_G1]) - if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING; -#endif - - return STAT_OK; -} - - -/* Execute parsed block - * - * Conditionally (based on whether a flag is set in gf) call the - * 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) - * 4. set spindle speed (S) - * 4a. set spindle override rate (M51) - * 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) - * 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, G90.1, G91.1) - * 18. set retract mode (G98, G99) - * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) // TODO update this - * 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 machine functions (which does the unit conversions) - */ -static stat_t _execute_gcode_block() { - stat_t status = STAT_OK; - - mach_set_line(parser.gn.line); - EXEC_FUNC(mach_set_feed_mode, feed_mode); - EXEC_FUNC(mach_set_feed_rate, feed_rate); - EXEC_FUNC(mach_feed_override_enable, feed_override_enable); - EXEC_FUNC(mach_set_spindle_speed, spindle_speed); - EXEC_FUNC(mach_spindle_override_enable, spindle_override_enable); - EXEC_FUNC(mach_select_tool, tool); - EXEC_FUNC(mach_change_tool, tool_change); - EXEC_FUNC(mach_set_spindle_mode, spindle_mode); - EXEC_FUNC(mach_mist_coolant_control, mist_coolant); - EXEC_FUNC(mach_flood_coolant_control, flood_coolant); - EXEC_FUNC(mach_override_enables, override_enables); - - if (parser.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell - RITORNO(mach_dwell(parser.gn.parameter)); - - EXEC_FUNC(mach_set_plane, plane); - EXEC_FUNC(mach_set_units, units); - //--> cutter radius compensation goes here - //--> cutter length compensation goes here - EXEC_FUNC(mach_set_coord_system, coord_system); - EXEC_FUNC(mach_set_path_mode, path_mode); - EXEC_FUNC(mach_set_distance_mode, distance_mode); - EXEC_FUNC(mach_set_arc_distance_mode, arc_distance_mode); - //--> set retract mode goes here - - switch (parser.gn.next_action) { - case NEXT_ACTION_SET_G28_POSITION: // G28.1 - mach_set_g28_position(); - break; - case NEXT_ACTION_GOTO_G28_POSITION: // G28 - status = mach_goto_g28_position(parser.gn.target, parser.gf.target); - break; - case NEXT_ACTION_SET_G30_POSITION: // G30.1 - mach_set_g30_position(); - break; - case NEXT_ACTION_GOTO_G30_POSITION: // G30 - status = mach_goto_g30_position(parser.gn.target, parser.gf.target); - break; - case NEXT_ACTION_CLEAR_HOME: // G28.2 - mach_clear_home(parser.gf.target); - break; - case NEXT_ACTION_SET_HOME: // G28.3 - mach_set_home(parser.gn.target, parser.gf.target); - break; - case NEXT_ACTION_SET_COORD_DATA: - mach_set_coord_offsets(parser.gn.parameter, parser.gn.target, - parser.gf.target); - break; - case NEXT_ACTION_SET_ORIGIN_OFFSETS: - mach_set_origin_offsets(parser.gn.target, parser.gf.target); - break; - case NEXT_ACTION_RESET_ORIGIN_OFFSETS: - mach_reset_origin_offsets(); - break; - case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: - mach_suspend_origin_offsets(); - break; - case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: - mach_resume_origin_offsets(); - break; - case NEXT_ACTION_DWELL: break; // Handled above - - case NEXT_ACTION_DEFAULT: - // apply override setting to gm struct - mach_set_absolute_mode(parser.gn.absolute_mode); - - switch (parser.gn.motion_mode) { - case MOTION_MODE_CANCEL_MOTION_MODE: - mach_set_motion_mode(parser.gn.motion_mode); - break; - case MOTION_MODE_RAPID: - status = mach_rapid(parser.gn.target, parser.gf.target); - break; - case MOTION_MODE_FEED: - status = mach_feed(parser.gn.target, parser.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 = mach_arc_feed(parser.gn.target, parser.gf.target, - parser.gn.arc_offset, parser.gf.arc_offset, - parser.gn.arc_radius, parser.gf.arc_radius, - parser.gn.parameter, parser.gf.parameter, - parser.modals[MODAL_GROUP_G1], - parser.gn.motion_mode); - break; - case MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR: // G38.2 - status = mach_probe(parser.gn.target, parser.gf.target, - parser.gn.motion_mode); - break; - case MOTION_MODE_STRAIGHT_PROBE_CLOSE: // G38.3 - status = mach_probe(parser.gn.target, parser.gf.target, - parser.gn.motion_mode); - break; - case MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR: // G38.4 - status = mach_probe(parser.gn.target, parser.gf.target, - parser.gn.motion_mode); - break; - case MOTION_MODE_STRAIGHT_PROBE_OPEN: // G38.5 - status = mach_probe(parser.gn.target, parser.gf.target, - parser.gn.motion_mode); - break; - case MOTION_MODE_SEEK_CLOSE_ERR: // G38.6 - status = mach_seek(parser.gn.target, parser.gf.target, - parser.gn.motion_mode); - break; - case MOTION_MODE_SEEK_CLOSE: // G38.7 - status = mach_seek(parser.gn.target, parser.gf.target, - parser.gn.motion_mode); - break; - case MOTION_MODE_SEEK_OPEN_ERR: // G38.8 - status = mach_seek(parser.gn.target, parser.gf.target, - parser.gn.motion_mode); - break; - case MOTION_MODE_SEEK_OPEN: // G38.9 - status = mach_seek(parser.gn.target, parser.gf.target, - parser.gn.motion_mode); - break; - default: break; // Should not get here - } - } - - // un-set absolute override once the move is planned - mach_set_absolute_mode(false); - - // do the program stops and ends : M0, M1, M2, M30, M60 - if (parser.gf.program_flow) - switch (parser.gn.program_flow) { - case PROGRAM_STOP: mach_program_stop(); break; - case PROGRAM_OPTIONAL_STOP: mach_optional_program_stop(); break; - case PROGRAM_PALLET_CHANGE_STOP: mach_pallet_change_stop(); break; - case PROGRAM_END: mach_program_end(); break; - } - - return status; -} - - -/// Load the state values in gn (next model state) and set flags in gf (model -/// state flags). -static stat_t _process_gcode_word(char letter, float value) { - switch (letter) { - case 'G': - switch ((uint8_t)value) { - case 0: - SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_RAPID); - case 1: - SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_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, plane, PLANE_XY); - case 18: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_XZ); - case 19: SET_MODAL(MODAL_GROUP_G2, plane, PLANE_YZ); - case 20: SET_MODAL(MODAL_GROUP_G6, units, INCHES); - case 21: SET_MODAL(MODAL_GROUP_G6, units, 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 2: SET_NON_MODAL(next_action, NEXT_ACTION_CLEAR_HOME); - case 3: SET_NON_MODAL(next_action, NEXT_ACTION_SET_HOME); - default: return STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - 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); - default: return STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 38: - switch (_point(value)) { - case 2: SET_MODAL(MODAL_GROUP_G1, motion_mode, - MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR); - case 3: SET_MODAL(MODAL_GROUP_G1, motion_mode, - MOTION_MODE_STRAIGHT_PROBE_CLOSE); - case 4: SET_MODAL(MODAL_GROUP_G1, motion_mode, - MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR); - case 5: SET_MODAL(MODAL_GROUP_G1, motion_mode, - MOTION_MODE_STRAIGHT_PROBE_OPEN); - case 6: SET_MODAL(MODAL_GROUP_G1, motion_mode, - MOTION_MODE_SEEK_CLOSE_ERR); - case 7: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_SEEK_CLOSE); - case 8: SET_MODAL(MODAL_GROUP_G1, motion_mode, - MOTION_MODE_SEEK_OPEN_ERR); - case 9: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_SEEK_OPEN); - default: return STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 40: break; // ignore cancel cutter radius compensation - case 49: break; // ignore cancel tool length offset comp. - case 53: SET_NON_MODAL(absolute_mode, true); - case 54: SET_MODAL(MODAL_GROUP_G12, coord_system, G54); - case 55: SET_MODAL(MODAL_GROUP_G12, coord_system, G55); - case 56: SET_MODAL(MODAL_GROUP_G12, coord_system, G56); - case 57: SET_MODAL(MODAL_GROUP_G12, coord_system, G57); - case 58: SET_MODAL(MODAL_GROUP_G12, coord_system, G58); - case 59: SET_MODAL(MODAL_GROUP_G12, coord_system, G59); - case 61: - switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_PATH); - case 1: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_EXACT_STOP); - default: return STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 64: SET_MODAL(MODAL_GROUP_G13, path_mode, PATH_CONTINUOUS); - case 80: SET_MODAL(MODAL_GROUP_G1, motion_mode, - MOTION_MODE_CANCEL_MOTION_MODE); - case 90: - switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, ABSOLUTE_MODE); - case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, ABSOLUTE_MODE); - default: return STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 91: - switch (_point(value)) { - case 0: SET_MODAL(MODAL_GROUP_G3, distance_mode, INCREMENTAL_MODE); - case 1: SET_MODAL(MODAL_GROUP_G3, arc_distance_mode, INCREMENTAL_MODE); - default: return STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 92: - switch (_point(value)) { - 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); - default: return STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 93: SET_MODAL(MODAL_GROUP_G5, feed_mode, INVERSE_TIME_MODE); - case 94: SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_MINUTE_MODE); - // case 95: - // SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_REVOLUTION_MODE); - // case 96: // Spindle Constant Surface Speed (not currently supported) - case 97: break; // Spindle RPM mode (only mode curently supported) - default: return STAT_GCODE_COMMAND_UNSUPPORTED; - } - break; - - case 'M': - switch ((uint8_t)value) { - case 0: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_STOP); - case 1: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_OPTIONAL_STOP); - case 60: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_PALLET_CHANGE_STOP); - case 2: case 30: - SET_MODAL(MODAL_GROUP_M4, program_flow, PROGRAM_END); - case 3: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CW); - case 4: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_CCW); - case 5: SET_MODAL(MODAL_GROUP_M7, spindle_mode, SPINDLE_OFF); - case 6: SET_NON_MODAL(tool_change, true); - case 7: SET_MODAL(MODAL_GROUP_M8, mist_coolant, true); - case 8: SET_MODAL(MODAL_GROUP_M8, flood_coolant, true); - case 9: SET_MODAL(MODAL_GROUP_M8, flood_coolant, false); // Also mist - 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_override_enable, true); - case 51: SET_MODAL(MODAL_GROUP_M9, spindle_override_enable, true); - default: return STAT_MCODE_COMMAND_UNSUPPORTED; - } - break; - - case 'T': SET_NON_MODAL(tool, (uint8_t)trunc(value)); - case 'F': SET_NON_MODAL(feed_rate, value); - // 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 'Z': SET_NON_MODAL(target[AXIS_Z], value); - case 'A': SET_NON_MODAL(target[AXIS_A], value); - case 'B': SET_NON_MODAL(target[AXIS_B], value); - case 'C': SET_NON_MODAL(target[AXIS_C], value); - // case 'U': SET_NON_MODAL(target[AXIS_U], value); // reserved - // case 'V': SET_NON_MODAL(target[AXIS_V], value); // reserved - // case 'W': SET_NON_MODAL(target[AXIS_W], value); // reserved - case 'I': SET_NON_MODAL(arc_offset[0], 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(line, (uint32_t)value); // line number - case 'L': break; // not used for anything - case 0: break; - default: return STAT_GCODE_COMMAND_UNSUPPORTED; - } - - return STAT_OK; -} - - -/// Parse a block (line) of gcode -/// Top level of gcode parser. Normalizes block and looks for special cases -stat_t gc_gcode_parser(char *block) { -#ifdef DEBUG - printf("GCODE: %s\n", block); -#endif - - // Delete block if it starts with / - if (*block == '/') return STAT_NOOP; - - // Set initial state for new block - // 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 - memset(&parser, 0, sizeof(parser)); // clear all parser values - - // get motion mode from previous block - parser.gn.motion_mode = mach_get_motion_mode(); - - // Parse words - for (char *p = block; *p;) { - switch (*p) { - case ' ': case '\t': case '\r': case '\n': p++; break; // Skip whitespace - case '(': p = _parse_gcode_comment(p); break; - case ';': *p = 0; break; // Comment - - default: { - char letter = toupper(*p++); - float value = 0; - if (!isalpha(letter)) return STAT_INVALID_OR_MALFORMED_COMMAND; - RITORNO(_parse_gcode_value(&p, &value)); - RITORNO(_process_gcode_word(letter, value)); - } - } - } - - RITORNO(_validate_gcode_block()); - - return _execute_gcode_block(); -} diff --git a/src/avr/src/gcode_parser.h b/src/avr/src/gcode_parser.h deleted file mode 100644 index dc880ef..0000000 --- a/src/avr/src/gcode_parser.h +++ /dev/null @@ -1,87 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include "status.h" -#include "machine.h" - - -typedef enum { // Used for detecting gcode errors. See NIST section 3.4 - MODAL_GROUP_G0, // {G10,G28,G28.1,G92} non-modal axis commands - MODAL_GROUP_G1, // {G0,G1,G2,G3,G80} motion - MODAL_GROUP_G2, // {G17,G18,G19} plane selection - MODAL_GROUP_G3, // {G90,G91} distance mode - MODAL_GROUP_G5, // {G93,G94} feed rate mode - MODAL_GROUP_G6, // {G20,G21} units - MODAL_GROUP_G7, // {G40,G41,G42} cutter radius compensation - MODAL_GROUP_G8, // {G43,G49} tool length offset - MODAL_GROUP_G9, // {G98,G99} return mode in canned cycles - MODAL_GROUP_G12, // {G54,G55,G56,G57,G58,G59} coordinate system selection - MODAL_GROUP_G13, // {G61,G61.1,G64} path control mode - MODAL_GROUP_M4, // {M0,M1,M2,M30,M60} stopping - MODAL_GROUP_M6, // {M6} tool change - MODAL_GROUP_M7, // {M3,M4,M5} spindle turning - MODAL_GROUP_M8, // {M7,M8,M9} coolant - MODAL_GROUP_M9, // {M48,M49} speed/feed override switches -} modal_group_t; - -#define MODAL_GROUP_COUNT (MODAL_GROUP_M9 + 1) - - -typedef enum { - OP_INVALID, - OP_MINUS, - OP_EXP, - OP_MUL, OP_DIV, OP_MOD, - OP_ADD, OP_SUB, - OP_EQ, OP_NE, OP_GT,OP_GE, OP_LT, OP_LE, - OP_AND, OP_OR, OP_XOR, -} op_t; - - -typedef struct { - gcode_state_t gn; // gcode input values - gcode_flags_t gf; // gcode input flags - - uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block - - op_t ops[GCODE_MAX_OPERATOR_DEPTH]; - float vals[GCODE_MAX_VALUE_DEPTH]; - int opPtr; - int valPtr; - - stat_t error; -} parser_t; - - -extern parser_t parser; - - -stat_t gc_gcode_parser(char *block); diff --git a/src/avr/src/gcode_state.c b/src/avr/src/gcode_state.c deleted file mode 100644 index 7807e56..0000000 --- a/src/avr/src/gcode_state.c +++ /dev/null @@ -1,179 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "gcode_state.h" - - -static const char INVALID_PGMSTR[] PROGMEM = "INVALID"; - -static const char INCHES_PGMSTR[] PROGMEM = "IN"; -static const char MILLIMETERS_PGMSTR[] PROGMEM = "MM"; -static const char DEGREES_PGMSTR[] PROGMEM = "DEG"; - -static const char INVERSE_TIME_MODE_PGMSTR[] PROGMEM = "INVERSE TIME"; -static const char UNITS_PER_MINUTE_MODE_PGMSTR[] PROGMEM = "PER MIN"; -static const char UNITS_PER_REVOLUTION_MODE_PGMSTR[] PROGMEM = "PER REV"; - -static const char PLANE_XY_PGMSTR[] PROGMEM = "XY"; -static const char PLANE_XZ_PGMSTR[] PROGMEM = "XZ"; -static const char PLANE_YZ_PGMSTR[] PROGMEM = "YZ"; - -static const char ABSOLUTE_COORDS_PGMSTR[] PROGMEM = "ABS"; -static const char G54_PGMSTR[] PROGMEM = "G54"; -static const char G55_PGMSTR[] PROGMEM = "G55"; -static const char G56_PGMSTR[] PROGMEM = "G56"; -static const char G57_PGMSTR[] PROGMEM = "G57"; -static const char G58_PGMSTR[] PROGMEM = "G58"; -static const char G59_PGMSTR[] PROGMEM = "G59"; - -static const char PATH_EXACT_PATH_PGMSTR[] PROGMEM = "EXACT PATH"; -static const char PATH_EXACT_STOP_PGMSTR[] PROGMEM = "EXACT STOP"; -static const char PATH_CONTINUOUS_PGMSTR[] PROGMEM = "CONTINUOUS"; - -static const char ABSOLUTE_MODE_PGMSTR[] PROGMEM = "ABSOLUTE"; -static const char INCREMENTAL_MODE_PGMSTR[] PROGMEM = "INCREMENTAL"; - - -PGM_P gs_get_units_pgmstr(units_t mode) { - switch (mode) { - case INCHES: return INCHES_PGMSTR; - case MILLIMETERS: return MILLIMETERS_PGMSTR; - case DEGREES: return DEGREES_PGMSTR; - } - - return INVALID_PGMSTR; -} - - -units_t gs_parse_units(const char *s) { - if (!strcmp_P(s, INCHES_PGMSTR)) return INCHES; - if (!strcmp_P(s, MILLIMETERS_PGMSTR)) return MILLIMETERS; - if (!strcmp_P(s, DEGREES_PGMSTR)) return DEGREES; - return -1; -} - - -PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode) { - switch (mode) { - case INVERSE_TIME_MODE: return INVERSE_TIME_MODE_PGMSTR; - case UNITS_PER_MINUTE_MODE: return UNITS_PER_MINUTE_MODE_PGMSTR; - case UNITS_PER_REVOLUTION_MODE: return UNITS_PER_REVOLUTION_MODE_PGMSTR; - } - - return INVALID_PGMSTR; -} - - -feed_mode_t gs_parse_feed_mode(const char *s) { - if (!strcmp_P(s, INVERSE_TIME_MODE_PGMSTR)) return INVERSE_TIME_MODE; - if (!strcmp_P(s, UNITS_PER_MINUTE_MODE_PGMSTR)) return UNITS_PER_MINUTE_MODE; - if (!strcmp_P(s, UNITS_PER_REVOLUTION_MODE_PGMSTR)) - return UNITS_PER_REVOLUTION_MODE; - return -1; -} - - -PGM_P gs_get_plane_pgmstr(plane_t plane) { - switch (plane) { - case PLANE_XY: return PLANE_XY_PGMSTR; - case PLANE_XZ: return PLANE_XZ_PGMSTR; - case PLANE_YZ: return PLANE_YZ_PGMSTR; - } - - return INVALID_PGMSTR; -} - - -plane_t gs_parse_plane(const char *s) { - if (!strcmp_P(s, PLANE_XY_PGMSTR)) return PLANE_XY; - if (!strcmp_P(s, PLANE_XZ_PGMSTR)) return PLANE_XZ; - if (!strcmp_P(s, PLANE_YZ_PGMSTR)) return PLANE_YZ; - return -1; -} - - -PGM_P gs_get_coord_system_pgmstr(coord_system_t cs) { - switch (cs) { - case ABSOLUTE_COORDS: return ABSOLUTE_COORDS_PGMSTR; - case G54: return G54_PGMSTR; - case G55: return G55_PGMSTR; - case G56: return G56_PGMSTR; - case G57: return G57_PGMSTR; - case G58: return G58_PGMSTR; - case G59: return G59_PGMSTR; - } - - return INVALID_PGMSTR; -} - - -coord_system_t gs_parse_coord_system(const char *s) { - if (!strcmp_P(s, ABSOLUTE_COORDS_PGMSTR)) return ABSOLUTE_COORDS; - if (!strcmp_P(s, G54_PGMSTR)) return G54; - if (!strcmp_P(s, G55_PGMSTR)) return G55; - if (!strcmp_P(s, G56_PGMSTR)) return G56; - if (!strcmp_P(s, G57_PGMSTR)) return G57; - if (!strcmp_P(s, G58_PGMSTR)) return G58; - if (!strcmp_P(s, G59_PGMSTR)) return G59; - return -1; -} - - -PGM_P gs_get_path_mode_pgmstr(path_mode_t mode) { - switch (mode) { - case PATH_EXACT_PATH: return PATH_EXACT_PATH_PGMSTR; - case PATH_EXACT_STOP: return PATH_EXACT_STOP_PGMSTR; - case PATH_CONTINUOUS: return PATH_CONTINUOUS_PGMSTR; - } - - return INVALID_PGMSTR; -} - - -path_mode_t gs_parse_path_mode(const char *s) { - if (!strcmp_P(s, PATH_EXACT_PATH_PGMSTR)) return PATH_EXACT_PATH; - if (!strcmp_P(s, PATH_EXACT_STOP_PGMSTR)) return PATH_EXACT_STOP; - if (!strcmp_P(s, PATH_CONTINUOUS_PGMSTR)) return PATH_CONTINUOUS; - return -1; -} - - -PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode) { - switch (mode) { - case ABSOLUTE_MODE: return ABSOLUTE_MODE_PGMSTR; - case INCREMENTAL_MODE: return INCREMENTAL_MODE_PGMSTR; - } - - return INVALID_PGMSTR; -} - - -distance_mode_t gs_parse_distance_mode(const char *s) { - if (!strcmp_P(s, ABSOLUTE_MODE_PGMSTR)) return ABSOLUTE_MODE; - if (!strcmp_P(s, INCREMENTAL_MODE_PGMSTR)) return INCREMENTAL_MODE; - return -1; -} diff --git a/src/avr/src/gcode_state.def b/src/avr/src/gcode_state.def deleted file mode 100644 index 5d73cf4..0000000 --- a/src/avr/src/gcode_state.def +++ /dev/null @@ -1,69 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -MEMBER(uint32_t, line) // Gcode block line number - -MEMBER(uint8_t, tool) // T - sets this value - -MEMBER(float, feed_rate) // F - mm/min or inverse time mode -MEMBER(feed_mode_t, feed_mode) -MEMBER(float, feed_override) // 1.0000 x F feed rate -MEMBER(bool, feed_override_enable) // M48, M49 - -MEMBER(float, spindle_speed) // in RPM -MEMBER(spindle_mode_t, spindle_mode) -MEMBER(float, spindle_override) // 1.0000 x S spindle speed -MEMBER(bool, spindle_override_enable) // true = override enabled - -MEMBER(motion_mode_t, motion_mode) // Group 1 modal motion -MEMBER(plane_t, plane) // G17, G18, G19 -MEMBER(units_t, units) // G20, G21 -MEMBER(coord_system_t, coord_system) // G54-G59 - select coord system 1-9 -MEMBER(bool, absolute_mode) // G53 move in machine coordinates -MEMBER(path_mode_t, path_mode) // G61 -MEMBER(distance_mode_t, distance_mode) // G91 -MEMBER(distance_mode_t, arc_distance_mode) // G91.1 - -MEMBER(bool, mist_coolant) // mist on (M7), off (M9) -MEMBER(bool, flood_coolant) // mist on (M8), off (M9) - -MEMBER(next_action_t, next_action) // G group 1 moves & non-modals -MEMBER(program_flow_t, program_flow) // used only by the gcode_parser - -// TODO unimplemented gcode parameters -// MEMBER(float cutter_radius) // D - cutter radius compensation (0 is off) -// MEMBER(float cutter_length) // H - cutter length compensation (0 is off) - -// Used for input only -MEMBER(float, target[AXES]) // XYZABC where the move should go -MEMBER(bool, override_enables) // feed and spindle enable -MEMBER(bool, tool_change) // M6 tool change flag - -MEMBER(float, parameter) // P - dwell & G10 coord select -MEMBER(float, arc_radius) // R - in arc radius mode -MEMBER(float, arc_offset[3]) // IJK - used by arc commands diff --git a/src/avr/src/gcode_state.h b/src/avr/src/gcode_state.h deleted file mode 100644 index 4dc2665..0000000 --- a/src/avr/src/gcode_state.h +++ /dev/null @@ -1,206 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "config.h" -#include "pgmspace.h" - -#include -#include - - -/* The difference between next_action_t and motion_mode_t is that - * next_action_t is used by the current block, and may carry non-modal - * commands, whereas motion_mode_t persists across blocks as G modal group 1 - */ - -typedef enum { - NEXT_ACTION_DEFAULT, // Must be zero (invokes motion modes) - NEXT_ACTION_DWELL, // G4 - NEXT_ACTION_SET_COORD_DATA, // G10 - NEXT_ACTION_GOTO_G28_POSITION, // G28 go to machine position - NEXT_ACTION_SET_G28_POSITION, // G28.1 set position in abs coordinates - NEXT_ACTION_CLEAR_HOME, // G28.3 clear axis home - NEXT_ACTION_SET_HOME, // G28.3 set axis home position - NEXT_ACTION_GOTO_G30_POSITION, // G30 - NEXT_ACTION_SET_G30_POSITION, // G30.1 - NEXT_ACTION_SET_ORIGIN_OFFSETS, // G92 - NEXT_ACTION_RESET_ORIGIN_OFFSETS, // G92.1 - NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS, // G92.2 - NEXT_ACTION_RESUME_ORIGIN_OFFSETS, // G92.3 -} next_action_t; - - -typedef enum { // G Modal Group 1 - MOTION_MODE_RAPID, // G0 - rapid - MOTION_MODE_FEED, // G1 - straight feed - MOTION_MODE_CW_ARC, // G2 - clockwise arc feed - MOTION_MODE_CCW_ARC, // G3 - counter-clockwise arc feed - MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR, // G38.2 - MOTION_MODE_STRAIGHT_PROBE_CLOSE, // G38.3 - MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR, // G38.4 - MOTION_MODE_STRAIGHT_PROBE_OPEN, // G38.5 - MOTION_MODE_SEEK_CLOSE_ERR, // G38.6 - MOTION_MODE_SEEK_CLOSE, // G38.7 - MOTION_MODE_SEEK_OPEN_ERR, // G38.8 - MOTION_MODE_SEEK_OPEN, // G38.9 - MOTION_MODE_CANCEL_MOTION_MODE, // G80 - MOTION_MODE_CANNED_CYCLE_81, // G81 - drilling - MOTION_MODE_CANNED_CYCLE_82, // G82 - drilling with dwell - MOTION_MODE_CANNED_CYCLE_83, // G83 - peck drilling - MOTION_MODE_CANNED_CYCLE_84, // G84 - right hand tapping - MOTION_MODE_CANNED_CYCLE_85, // G85 - boring, no dwell, feed out - 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, man out - MOTION_MODE_CANNED_CYCLE_89, // G89 - boring, dwell, feed out -} motion_mode_t; - - -typedef enum { // plane - translates to: - // axis_0 axis_1 axis_2 - PLANE_XY, // G17 X Y Z - PLANE_XZ, // G18 X Z Y - PLANE_YZ, // G19 Y Z X -} plane_t; - - -typedef enum { - INCHES, // G20 - MILLIMETERS, // G21 - DEGREES, // ABC axes (this value used for displays only) -} units_t; - - -typedef enum { - ABSOLUTE_COORDS, // machine coordinate system - G54, G55, G56, G57, G58, G59, -} coord_system_t; - - -/// G Modal Group 13 -typedef enum { - PATH_EXACT_PATH, // G61 hits corners but stops only if needed - PATH_EXACT_STOP, // G61.1 stops at all corners - PATH_CONTINUOUS, // G64 and typically the default mode -} path_mode_t; - - -typedef enum { - ABSOLUTE_MODE, // G90 - INCREMENTAL_MODE, // G91 -} distance_mode_t; - - -typedef enum { - UNITS_PER_MINUTE_MODE, // G94 - INVERSE_TIME_MODE, // G93 - UNITS_PER_REVOLUTION_MODE, // G95 (unimplemented) -} feed_mode_t; - - -typedef enum { - ORIGIN_OFFSET_SET, // G92 - set origin offsets - ORIGIN_OFFSET_CANCEL, // G92.1 - zero out origin offsets - ORIGIN_OFFSET_SUSPEND, // G92.2 - do not apply offsets, but preserve values - ORIGIN_OFFSET_RESUME, // G92.3 - resume application of the suspended offsets -} origin_offset_t; - - -typedef enum { - PROGRAM_STOP, - PROGRAM_OPTIONAL_STOP, - PROGRAM_PALLET_CHANGE_STOP, - PROGRAM_END, -} program_flow_t; - - -/// spindle state settings -typedef enum { - SPINDLE_OFF, - SPINDLE_CW, - SPINDLE_CCW, -} spindle_mode_t; - - -/// mist and flood coolant states -typedef enum { - COOLANT_OFF, // all coolant off - COOLANT_ON, // request coolant on or indicate both coolants are on - COOLANT_MIST, // indicates mist coolant on - COOLANT_FLOOD, // indicates flood coolant on -} coolant_state_t; - - -/* Gcode model - * - * - mach.gm is the core Gcode model state. It keeps the internal gcode - * state model in normalized, canonical form. All values are unit - * converted (to mm) and in the machine coordinate system - * (absolute coordinate system). Gm is owned by the machine layer and - * should be accessed only through mach_*() routines. - * - * - parser.gn is used by the gcode parser and is re-initialized for - * each gcode block. It accepts data in the new gcode block in the - * formats present in the block (pre-normalized forms). During - * initialization some state elements are necessarily restored - * from gm. - * - * - parser.gf is used by the gcode parser to hold flags for any data that has - * changed in gn during the parse. - */ - - -/// Gcode model state -typedef struct { -#define MEMBER(TYPE, VAR) TYPE VAR; -#include "gcode_state.def" -#undef MEMBER -} gcode_state_t; - - -typedef struct { -#define MEMBER(TYPE, VAR) bool VAR; -#include "gcode_state.def" -#undef MEMBER -} gcode_flags_t; - - -PGM_P gs_get_units_pgmstr(units_t mode); -units_t gs_parse_units(const char *units); -PGM_P gs_get_feed_mode_pgmstr(feed_mode_t mode); -feed_mode_t gs_parse_feed_mode(const char *mode); -PGM_P gs_get_plane_pgmstr(plane_t plane); -plane_t gs_parse_plane(const char *plane); -PGM_P gs_get_coord_system_pgmstr(coord_system_t cs); -coord_system_t gs_parse_coord_system(const char *cs); -PGM_P gs_get_path_mode_pgmstr(path_mode_t mode); -path_mode_t gs_parse_path_mode(const char *mode); -PGM_P gs_get_distance_mode_pgmstr(distance_mode_t mode); -distance_mode_t gs_parse_distance_mode(const char *mode); diff --git a/src/avr/src/huanyang.c b/src/avr/src/huanyang.c index f43bf11..6a22951 100644 --- a/src/avr/src/huanyang.c +++ b/src/avr/src/huanyang.c @@ -111,7 +111,6 @@ typedef struct { bool connected; bool changed; - spindle_mode_t mode; float speed; float actual_freq; @@ -223,13 +222,10 @@ static bool _update(int index) { // Setup next command uint8_t var; switch (index) { - case 0: { // Update mode - uint8_t state; - switch (ha.mode) { - case SPINDLE_CW: state = HUANYANG_FORWARD; break; - case SPINDLE_CCW: state = HUANYANG_REVERSE; break; - default: state = HUANYANG_STOP; break; - } + case 0: { // Update direction + uint8_t state = HUANYANG_STOP; + if (0 < ha.speed) state = HUANYANG_FORWARD; + else if (ha.speed < 0) state = HUANYANG_REVERSE; _set_command1(HUANYANG_CTRL_WRITE, state); @@ -430,11 +426,9 @@ void huanyang_init() { } -void huanyang_set(spindle_mode_t mode, float speed) { - if (ha.mode != mode || ha.speed != speed) { - if (ha.debug) STATUS_DEBUG("huanyang: mode=%d, speed=%0.2f", mode, speed); - - ha.mode = mode; +void huanyang_set(float speed) { + if (ha.speed != speed) { + if (ha.debug) STATUS_DEBUG("huanyang: speed=%0.2f", speed); ha.speed = speed; ha.changed = true; } @@ -454,7 +448,6 @@ void huanyang_reset() { // Save settings uint8_t id = ha.id; - spindle_mode_t mode = ha.mode; float speed = ha.speed; bool debug = ha.debug; @@ -463,7 +456,6 @@ void huanyang_reset() { // Restore settings ha.id = id; - ha.mode = mode; ha.speed = speed; ha.debug = debug; ha.changed = true; @@ -503,7 +495,7 @@ void huanyang_rtc_callback() { void huanyang_stop() { - huanyang_set(SPINDLE_OFF, 0); + huanyang_set(0); huanyang_reset(); } diff --git a/src/avr/src/huanyang.h b/src/avr/src/huanyang.h index 1e52258..e2e3ec8 100644 --- a/src/avr/src/huanyang.h +++ b/src/avr/src/huanyang.h @@ -27,11 +27,11 @@ #pragma once -#include "machine.h" +#include "spindle.h" void huanyang_init(); -void huanyang_set(spindle_mode_t mode, float speed); +void huanyang_set(float speed); void huanyang_reset(); void huanyang_rtc_callback(); void huanyang_stop(); diff --git a/src/avr/src/plan/jog.c b/src/avr/src/jog.c similarity index 88% rename from src/avr/src/plan/jog.c rename to src/avr/src/jog.c index cdb8319..e8ae33d 100644 --- a/src/avr/src/plan/jog.c +++ b/src/avr/src/jog.c @@ -28,13 +28,10 @@ #include "jog.h" #include "axis.h" -#include "planner.h" -#include "buffer.h" -#include "line.h" -#include "velocity_curve.h" -#include "runtime.h" -#include "machine.h" +#include "util.h" +#include "exec.h" #include "state.h" +#include "queue.h" #include "config.h" #include @@ -190,10 +187,10 @@ static bool _soft_limit(int axis, float V, float A) { if (min == max) return false; // Check if we need to stop to avoid exceeding a limit - float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER; + float jerk = axis_get_jerk_max(axis); float deccelDist = _compute_deccel_dist(V, A, jerk); - float position = mp_runtime_get_axis_position(axis); + float position = exec_get_axis_position(axis); if (a->velocity < 0 && position <= min + deccelDist) return true; if (0 < a->velocity && max - deccelDist <= position) return true; @@ -228,7 +225,7 @@ static float _compute_axis_velocity(int axis) { } // Compute axis max jerk - float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER; + float jerk = axis_get_jerk_max(axis); // Compute next accel a->accel = _next_accel(V, Vt, a->accel, jerk); @@ -237,7 +234,7 @@ static float _compute_axis_velocity(int axis) { } -static stat_t _exec_jog(mp_buffer_t *bf) { +stat_t jog_exec() { // Load next velocity jr.done = true; @@ -261,9 +258,9 @@ static stat_t _exec_jog(mp_buffer_t *bf) { // Check if we are done if (jr.done) { // Update machine position - mach_set_position_from_runtime(); - mp_set_cycle(CYCLE_MACHINING); // Default cycle - mp_pause_queue(false); + //mach_set_position_from_runtime(); + state_set_cycle(CYCLE_MACHINING); // Default cycle + state_pause_queue(false); return STAT_NOOP; // Done, no move executed } @@ -271,15 +268,15 @@ static stat_t _exec_jog(mp_buffer_t *bf) { // Compute target from velocity float target[AXES]; for (int axis = 0; axis < AXES; axis++) { - target[axis] = mp_runtime_get_axis_position(axis) + + target[axis] = exec_get_axis_position(axis) + jr.axes[axis].velocity * SEGMENT_TIME; target[axis] = _limit_position(axis, target[axis]); } // Set velocity and target - mp_runtime_set_velocity(sqrt(velocity_sqr)); - stat_t status = mp_runtime_move_to_target(SEGMENT_TIME, target); + exec_set_velocity(sqrt(velocity_sqr)); + stat_t status = exec_move_to_target(SEGMENT_TIME, target); if (status != STAT_OK) return status; return STAT_EAGAIN; @@ -287,8 +284,8 @@ static stat_t _exec_jog(mp_buffer_t *bf) { uint8_t command_jog(int argc, char *argv[]) { - if (mp_get_cycle() != CYCLE_JOGGING && - (mp_get_state() != STATE_READY || mp_get_cycle() != CYCLE_MACHINING)) + if (state_get_cycle() != CYCLE_JOGGING && + (state_get() != STATE_READY || state_get_cycle() != CYCLE_MACHINING)) return STAT_NOOP; float velocity[AXES]; @@ -298,17 +295,17 @@ uint8_t command_jog(int argc, char *argv[]) { else velocity[axis] = 0; // Reset - if (mp_get_cycle() != CYCLE_JOGGING) memset(&jr, 0, sizeof(jr)); + if (state_get_cycle() != CYCLE_JOGGING) memset(&jr, 0, sizeof(jr)); jr.writing = true; for (int axis = 0; axis < AXES; axis++) jr.axes[axis].next = velocity[axis]; jr.writing = false; - if (mp_get_cycle() != CYCLE_JOGGING) { - mp_set_cycle(CYCLE_JOGGING); - mp_pause_queue(true); - mp_queue_push_nonstop(_exec_jog, -1); + if (state_get_cycle() != CYCLE_JOGGING) { + state_set_cycle(CYCLE_JOGGING); + state_pause_queue(true); + queue_push(ACTION_JOG); } return STAT_OK; diff --git a/src/avr/src/plan/jog.h b/src/avr/src/jog.h similarity index 96% rename from src/avr/src/plan/jog.h rename to src/avr/src/jog.h index f2bfa69..8d64446 100644 --- a/src/avr/src/plan/jog.h +++ b/src/avr/src/jog.h @@ -26,3 +26,9 @@ \******************************************************************************/ #pragma once + + +#include "status.h" + + +stat_t jog_exec(); diff --git a/src/avr/src/machine.c b/src/avr/src/machine.c deleted file mode 100644 index 5c5be2c..0000000 --- a/src/avr/src/machine.c +++ /dev/null @@ -1,934 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* This code is a loose implementation of Kramer, Proctor and Messina's - * machining functions as described in the NIST RS274/NGC v3 - * - * The 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. - * - * Synchronizing command execution: - * - * "Synchronous commands" are commands that affect the runtime and 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 mach_xxx_xxx() function which will do any input validation and - * return an error if it detects one. - * - * - The mach_ function calls mp_queue_push(). Arguments are a callback to - * the _exec_...() function, which is the runtime execution routine, and any - * arguments that are needed by the runtime. - * - * - mp_queue_push() 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. - * - * 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 "machine.h" - -#include "config.h" -#include "axis.h" -#include "gcode_parser.h" -#include "spindle.h" -#include "coolant.h" - -#include "plan/planner.h" -#include "plan/runtime.h" -#include "plan/dwell.h" -#include "plan/arc.h" -#include "plan/line.h" -#include "plan/state.h" - - -typedef struct { // struct to manage mach globals and cycles - float offset[COORDS + 1][AXES]; // coordinate systems & offsets G53-G59 - float origin_offset[AXES]; // G92 offsets - bool origin_offset_enable; // G92 offsets enabled / disabled - - float position[AXES]; // model position - float g28_position[AXES]; // stored machine position for G28 - float g30_position[AXES]; // stored machine position for G30 - - gcode_state_t gm; // core gcode model state -} machine_t; - - -static machine_t mach = { - .gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE}, -}; - - -// Machine State functions -uint32_t mach_get_line() {return mach.gm.line;} -float mach_get_feed_rate() {return mach.gm.feed_rate;} -feed_mode_t mach_get_feed_mode() {return mach.gm.feed_mode;} - - -bool mach_is_inverse_time_mode() { - return mach.gm.feed_mode == INVERSE_TIME_MODE; -} - - -float mach_get_feed_override() { - return mach.gm.feed_override_enable ? mach.gm.feed_override : 1; -} - - -float mach_get_spindle_override() { - return mach.gm.spindle_override_enable ? mach.gm.spindle_override : 1; -} - - -motion_mode_t mach_get_motion_mode() {return mach.gm.motion_mode;} -bool mach_is_rapid() {return mach.gm.motion_mode == MOTION_MODE_RAPID;} -plane_t mach_get_plane() {return mach.gm.plane;} -units_t mach_get_units() {return mach.gm.units;} -coord_system_t mach_get_coord_system() {return mach.gm.coord_system;} -bool mach_get_absolute_mode() {return mach.gm.absolute_mode;} -path_mode_t mach_get_path_mode() {return mach.gm.path_mode;} -bool mach_is_exact_stop() {return mach.gm.path_mode == PATH_EXACT_STOP;} -bool mach_in_absolute_mode() {return mach.gm.distance_mode == ABSOLUTE_MODE;} -distance_mode_t mach_get_distance_mode() {return mach.gm.distance_mode;} -distance_mode_t mach_get_arc_distance_mode() {return mach.gm.arc_distance_mode;} - - -void mach_set_motion_mode(motion_mode_t motion_mode) { - mach.gm.motion_mode = motion_mode; -} - - -/// Spindle speed callback from planner queue -static stat_t _exec_spindle_speed(mp_buffer_t *bf) { - spindle_set_speed(bf->value); - return STAT_NOOP; // No move queued -} - - -/// Queue the S parameter to the planner buffer -void mach_set_spindle_speed(float speed) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = speed * mach_get_spindle_override(); - mp_queue_push_nonstop(_exec_spindle_speed, mach_get_line()); -} - - -/// execute the spindle command (called from planner) -static stat_t _exec_spindle_mode(mp_buffer_t *bf) { - spindle_set_mode(bf->value); - return STAT_NOOP; // No move queued -} - - -/// Queue the spindle command to the planner buffer -void mach_set_spindle_mode(spindle_mode_t mode) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = mode; - mp_queue_push_nonstop(_exec_spindle_mode, mach_get_line()); -} - - -void mach_set_absolute_mode(bool absolute_mode) { - mach.gm.absolute_mode = absolute_mode; -} - - -void mach_set_line(uint32_t line) {mach.gm.line = line;} - - -/* Coordinate systems and offsets - * - * Functions to get, set and report coordinate systems and work offsets - * These functions are not part of the NIST defined functions - * - * Notes on Coordinate System and Offset functions - * - * All positional information in the machine is kept as - * absolute coords and in canonical units (mm). The offsets are only - * used to translate in and out of canonical form during - * interpretation and response. - * - * Managing the coordinate systems & offsets is somewhat complicated. - * The following affect offsets: - * - coordinate system selected. 1-9 correspond to G54-G59 - * - absolute override: forces current move to be interpreted in machine - * coordinates: G53 (system 0) - * - G92 offsets are added "on top of" the coord system offsets -- - * if origin_offset_enable - * - G28 and G30 moves; these are run in absolute coordinates - * - * The offsets themselves are considered static, are kept in mach, and are - * supposed to be persistent. - * - * To reduce complexity and data load the following is done: - * - Full data for coordinates/offsets is only accessible by the - * machine, not the downstream - * - Resolved set of coord and G92 offsets, with per-move exceptions can - * be captured as "work_offsets" - * - The core gcode context (gm) only knows about the active coord system - * and the work offsets - */ - -/* Return the currently active coordinate offset for an axis - * - * Takes G5x, G92 and absolute override into account to return the - * active offset for this move - * - * This function is typically used to evaluate and set offsets. - */ -float mach_get_active_coord_offset(uint8_t axis) { - // no offset in absolute override mode - if (mach.gm.absolute_mode) return 0; - float offset = mach.offset[mach.gm.coord_system][axis]; - - if (mach.origin_offset_enable) - offset += mach.origin_offset[axis]; // includes G5x and G92 components - - return offset; -} - - -static stat_t _exec_update_work_offsets(mp_buffer_t *bf) { - mp_runtime_set_work_offsets(bf->target); - return STAT_NOOP; // No move queued -} - - -// Capture coord offsets from the model into absolute values -void mach_update_work_offsets() { - static float work_offset[AXES] = {0}; - bool same = true; - - for (int axis = 0; axis < AXES; axis++) { - float offset = mach_get_active_coord_offset(axis); - - if (offset != work_offset[axis]) { - work_offset[axis] = offset; - same = false; - } - } - - if (!same) { - mp_buffer_t *bf = mp_queue_get_tail(); - copy_vector(bf->target, work_offset); - mp_queue_push_nonstop(_exec_update_work_offsets, mach_get_line()); - } -} - - -const float *mach_get_position() {return mach.position;} - - -void mach_set_position(const float position[]) { - copy_vector(mach.position, position); -} - - -/*** Get position of axis in absolute coordinates - * - * NOTE: Machine position is always returned in mm mode. No unit conversion - * is performed. - */ -float mach_get_axis_position(uint8_t axis) {return mach.position[axis];} - - -/* Set the position of a single axis in the model, planner and runtime - * - * This command sets an axis/axes to a position provided as an argument. - * This is useful for setting origins for probing, and other operations. - * - * !!!!! DO NOT CALL THIS FUNCTION WHILE IN A MACHINING CYCLE !!!!! - * - * More specifically, do not call this function if there are any moves - * in the planner or if the runtime is moving. The system must be - * quiescent or you will introduce positional errors. This is true - * because the planned / running moves have a different reference - * frame than the one you are now going to set. These functions should - * only be called during initialization sequences and during cycles - * when you know there are no more moves in the planner and that all motion - * has stopped. - */ -void mach_set_axis_position(unsigned axis, float position) { - //if (!mp_is_quiescent()) ALARM(STAT_MACH_NOT_QUIESCENT); - if (AXES <= axis) return; - - // TODO should set work position, accounting for offsets - - mach.position[axis] = position; - mp_set_axis_position(axis, position); - mp_runtime_set_axis_position(axis, position); - mp_runtime_set_steps_from_position(); -} - - -/// Do not call this function while machine is moving or queue is not empty -void mach_set_position_from_runtime() { - for (int axis = 0; axis < AXES; axis++) { - mach.position[axis] = mp_runtime_get_work_position(axis); - mp_set_axis_position(axis, mach.position[axis]); - } -} - - -/*** Calculate target vector - * - * This is a core routine. It handles: - * - conversion of linear units to internal canonical form (mm) - * - conversion of relative mode to absolute (internal canonical form) - * - translation of work coordinates to machine coordinates (internal - * canonical form) - * - application of axis radius mode - * - * Target coordinates are provided in @param values. - * Axes that need processing are signaled in @param flags. - */ -void mach_calc_target(float target[], const float values[], - const bool flags[], bool absolute) { - for (int axis = 0; axis < AXES; axis++) { - target[axis] = mach.position[axis]; - if (!flags[axis] || !axis_is_enabled(axis)) continue; - - target[axis] = absolute ? mach_get_active_coord_offset(axis) : - mach.position[axis]; - - float radius = axis_get_radius(axis); - if (radius) // Handle radius mode if radius is non-zero - target[axis] += TO_MM(values[axis]) * 360 / (2 * M_PI * radius); - - // For ABC axes no mm conversion - it's already in degrees - // TODO This should depend on the axis mode - else if (AXIS_Z < axis) target[axis] += values[axis]; - else target[axis] += TO_MM(values[axis]); - } -} - - -/*** Return error code if soft limit is exceeded - * - * Tests for soft limit for any axis if min and max are different values. You - * can set min and max to 0 to disable soft limits for an axis. - */ -stat_t mach_test_soft_limits(float target[]) { - for (int axis = 0; axis < AXES; axis++) { - if (!axis_is_enabled(axis) || !axis_get_homed(axis)) continue; - - float min = axis_get_travel_min(axis); - float max = axis_get_travel_max(axis); - - // min == max means no soft limits - if (fp_EQ(min, max)) continue; - - if (target[axis] < min || max < target[axis]) - return STAT_SOFT_LIMIT_EXCEEDED; - } - - return STAT_OK; -} - - -/* Machining functions - * - * Values are passed in pre-unit_converted state (from gn structure) - * All operations occur on gm (current model state) - * - * These are organized by section number (x.x.x) in the order they are - * found in NIST RS274 NGCv3 - */ - -// Initialization and Termination (4.3.2) - -void machine_init() { - // Set gcode defaults - mach_set_units(GCODE_DEFAULT_UNITS); - mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); - mach_set_plane(GCODE_DEFAULT_PLANE); - mach_set_path_mode(GCODE_DEFAULT_PATH_CONTROL); - mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE); - mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE); - mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // always the default - - // Sub-system inits - spindle_init(); - coolant_init(); -} - - -// Representation (4.3.3) -// -// Affect the Gcode model only (asynchronous) -// These functions assume input validation occurred upstream. - -/// G17, G18, G19 select axis plane -void mach_set_plane(plane_t plane) { - if (plane != (plane_t)-1) mach.gm.plane = plane; -} - - -/// G20, G21 -void mach_set_units(units_t mode) { - if (mode != (units_t)-1) mach.gm.units = mode; -} - - -/// G90, G91 -void mach_set_distance_mode(distance_mode_t mode) { - if (mode != (distance_mode_t)-1) mach.gm.distance_mode = mode; -} - - -/// G90.1, G91.1 -void mach_set_arc_distance_mode(distance_mode_t mode) { - if (mode != (distance_mode_t)-1) mach.gm.arc_distance_mode = mode; -} - - -/* G10 L2 Pn, delayed persistence - * - * This function applies the offset to the GM model. - */ -void mach_set_coord_offsets(coord_system_t coord_system, float offset[], - bool flags[]) { - if (coord_system < G54 || G59 < coord_system) return; - - for (int axis = 0; axis < AXES; axis++) - if (flags[axis]) - mach.offset[coord_system][axis] = TO_MM(offset[axis]); -} - - -/// G54-G59 -void mach_set_coord_system(coord_system_t cs) { - if (cs != (coord_system_t)-1) mach.gm.coord_system = cs; -} - - -// G28.3 functions and support -static stat_t _exec_home(mp_buffer_t *bf) { - const float *target = bf->target; - const float *flags = bf->unit; - - for (int axis = 0; axis < AXES; axis++) - if (flags[axis]) { - mp_runtime_set_axis_position(axis, target[axis]); - axis_set_homed(axis, true); - } - - mp_runtime_set_steps_from_position(); - - return STAT_NOOP; // No move queued -} - - -/* G28.3 - model, planner and queue to runtime - * - * Takes a vector of origins (presumably 0's, but not necessarily) and - * applies them to all axes where the corresponding position in the - * flags vector is true (1). - * - * This is a 2 step process. The model and planner contexts are set - * immediately, the runtime command is queued and synchronized with - * the planner queue. This includes the runtime position and the step - * recording done by the encoders. - */ -void mach_set_home(float origin[], bool flags[]) { - mp_buffer_t *bf = mp_queue_get_tail(); - - // Compute target position - mach_calc_target(bf->target, origin, flags, true); - - for (int axis = 0; axis < AXES; axis++) - if (flags[axis] && isfinite(origin[axis])) { - bf->target[axis] -= mach_get_active_coord_offset(axis); - mach.position[axis] = bf->target[axis]; - mp_set_axis_position(axis, bf->target[axis]); // set mm position - bf->unit[axis] = true; - - } else bf->unit[axis] = false; - - // Synchronized update of runtime position - mp_queue_push_nonstop(_exec_home, mach_get_line()); -} - - -void mach_clear_home(bool flags[]) { - for (int axis = 0; axis < AXES; axis++) - if (flags[axis]) axis_set_homed(axis, false); -} - - -/* G92's behave according to NIST 3.5.18 & LinuxCNC G92 - * http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G92-G92.1-G92.2-G92.3 - */ - -/// G92 -void mach_set_origin_offsets(float offset[], bool flags[]) { - mach.origin_offset_enable = true; - - for (int axis = 0; axis < AXES; axis++) - if (flags[axis]) - mach.origin_offset[axis] = mach.position[axis] - - mach.offset[mach.gm.coord_system][axis] - TO_MM(offset[axis]); - - mach_update_work_offsets(); // update resolved offsets -} - - -/// G92.1 -void mach_reset_origin_offsets() { - mach.origin_offset_enable = false; - - for (int axis = 0; axis < AXES; axis++) - mach.origin_offset[axis] = 0; - - mach_update_work_offsets(); // update resolved offsets -} - - -/// G92.2 -void mach_suspend_origin_offsets() { - mach.origin_offset_enable = false; - mach_update_work_offsets(); // update resolved offsets -} - - -/// G92.3 -void mach_resume_origin_offsets() { - mach.origin_offset_enable = true; - mach_update_work_offsets(); // update resolved offsets -} - - -stat_t mach_plan_line(float target[], switch_id_t sw) { - buffer_flags_t flags = 0; - - switch (mach_get_motion_mode()) { - case MOTION_MODE_STRAIGHT_PROBE_CLOSE_ERR: - case MOTION_MODE_SEEK_CLOSE_ERR: - flags |= BUFFER_SEEK_CLOSE | BUFFER_SEEK_ERROR | BUFFER_EXACT_STOP; - break; - - case MOTION_MODE_STRAIGHT_PROBE_CLOSE: - case MOTION_MODE_SEEK_CLOSE: - flags |= BUFFER_SEEK_CLOSE | BUFFER_EXACT_STOP; - break; - - case MOTION_MODE_STRAIGHT_PROBE_OPEN_ERR: - case MOTION_MODE_SEEK_OPEN_ERR: - flags |= BUFFER_SEEK_OPEN | BUFFER_SEEK_ERROR | BUFFER_EXACT_STOP; - break; - - case MOTION_MODE_STRAIGHT_PROBE_OPEN: - case MOTION_MODE_SEEK_OPEN: - flags |= BUFFER_SEEK_OPEN | BUFFER_EXACT_STOP; - break; - - default: break; - } - - if (mach_is_rapid()) flags |= BUFFER_RAPID; - if (mach_is_inverse_time_mode()) flags |= BUFFER_INVERSE_TIME; - if (mach_is_exact_stop()) flags |= BUFFER_EXACT_STOP; - - return mp_aline(target, flags, sw, mach.gm.feed_rate, - mach_get_feed_override(), mach_get_line()); -} - - -// Free Space Motion (4.3.4) -static stat_t _feed(float values[], bool flags[], switch_id_t sw) { - // Trap inverse time mode wo/ feed rate - if (!mach_is_rapid() && mach.gm.feed_mode == INVERSE_TIME_MODE && - !parser.gf.feed_rate) - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - - // Compute target position - float target[AXES]; - mach_calc_target(target, values, flags, mach_in_absolute_mode()); - - // test soft limits - stat_t status = mach_test_soft_limits(target); - if (status != STAT_OK) return ALARM(status); - - // prep and plan the move - mach_update_work_offsets(); // update resolved offsets - RITORNO(mach_plan_line(target, sw)); - copy_vector(mach.position, target); // update model position - - return STAT_OK; -} - - -/// G0 linear rapid -stat_t mach_rapid(float values[], bool flags[]) { - mach_set_motion_mode(MOTION_MODE_RAPID); - return _feed(values, flags, 0); -} - - -/// G28.1 -void mach_set_g28_position() {copy_vector(mach.g28_position, mach.position);} - - -/// G28 -stat_t mach_goto_g28_position(float target[], bool flags[]) { - mach_set_absolute_mode(true); - - // move through intermediate point, or skip - mach_rapid(target, flags); - - // execute actual stored move - bool f[] = {true, true, true, true, true, true}; - return mach_rapid(mach.g28_position, f); -} - - -/// G30.1 -void mach_set_g30_position() {copy_vector(mach.g30_position, mach.position);} - - -/// G30 -stat_t mach_goto_g30_position(float target[], bool flags[]) { - mach_set_absolute_mode(true); - - // move through intermediate point, or skip - mach_rapid(target, flags); - - // execute actual stored move - bool f[] = {true, true, true, true, true, true}; - return mach_rapid(mach.g30_position, f); -} - - -stat_t mach_probe(float values[], bool flags[], motion_mode_t mode) { - mach_set_motion_mode(mode); - return _feed(values, flags, SW_PROBE); -} - - -stat_t _exec_set_seek_position(mp_buffer_t *bf) { - mach_set_position_from_runtime(); - mp_pause_queue(false); - return STAT_NOOP; // No move queued -} - - -stat_t mach_seek(float target[], bool flags[], motion_mode_t mode) { - mach_set_motion_mode(mode); - - switch_id_t sw = SW_PROBE; - - for (int axis = 0; axis < AXES; axis++) - if (flags[axis] && isfinite(target[axis])) { - // Convert to incremental move - if (mach.gm.distance_mode == ABSOLUTE_MODE) - target[axis] += mach.position[axis]; - - if (!axis_is_enabled(axis)) return STAT_SEEK_AXIS_DISABLED; - if (sw != SW_PROBE) return STAT_SEEK_MULTIPLE_AXES; - if (fp_EQ(target[axis], mach.position[axis])) return STAT_SEEK_ZERO_MOVE; - - bool min = target[axis] < mach.position[axis]; - - if (mode == MOTION_MODE_SEEK_OPEN_ERR || - mode == MOTION_MODE_SEEK_OPEN) min = !min; - - switch (axis) { - case AXIS_X: sw = min ? SW_MIN_X : SW_MAX_X; break; - case AXIS_Y: sw = min ? SW_MIN_Y : SW_MAX_Y; break; - case AXIS_Z: sw = min ? SW_MIN_Z : SW_MAX_Z; break; - case AXIS_A: sw = min ? SW_MIN_A : SW_MAX_A; break; - } - - if (!switch_is_enabled(sw)) return STAT_SEEK_SWITCH_DISABLED; - } - - if (sw == SW_PROBE) return STAT_SEEK_MISSING_AXIS; - - RITORNO(_feed(target, flags, sw)); - - mp_pause_queue(true); - mp_queue_push_nonstop(_exec_set_seek_position, mach_get_line()); - - return STAT_OK; -} - - -// Machining Attributes (4.3.5) - -/// F parameter -/// Normalize feed rate to mm/min or to minutes if in inverse time mode -void mach_set_feed_rate(float feed_rate) { - if (mach.gm.feed_mode == INVERSE_TIME_MODE) - // normalize to minutes (active for this gcode block only) - mach.gm.feed_rate = feed_rate ? 1 / feed_rate : 0; // Avoid div by zero - - else mach.gm.feed_rate = TO_MM(feed_rate); -} - - -/// G93, G94 -void mach_set_feed_mode(feed_mode_t mode) { - if (mode == (feed_mode_t)-1 || mach.gm.feed_mode == mode) return; - mach.gm.feed_rate = 0; // Force setting feed rate after changing modes - mach.gm.feed_mode = mode; -} - - -/// G61, G61.1, G64 -void mach_set_path_mode(path_mode_t mode) { - if (mode != (path_mode_t)-1) mach.gm.path_mode = mode; -} - - -// Machining Functions (4.3.6) see also arc.c - -/// G1 -stat_t mach_feed(float values[], bool flags[]) { - mach_set_motion_mode(MOTION_MODE_FEED); - return _feed(values, flags, 0); -} - - -/// G4, P parameter (seconds) -stat_t mach_dwell(float seconds) {return mp_dwell(seconds, mach_get_line());} - - -// Spindle Functions (4.3.7) see spindle.c, spindle.h - -// Tool Functions (4.3.8) - -/// T parameter -void mach_select_tool(uint8_t tool) {mach.gm.tool = tool;} - - -static stat_t _exec_change_tool(mp_buffer_t *bf) { - mp_runtime_set_tool(bf->value); - mp_set_hold_reason(HOLD_REASON_TOOL_CHANGE); - mp_state_holding(); - return STAT_NOOP; // No move queued -} - - -/// M6 -void mach_change_tool(bool x) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = mach.gm.tool; - mp_queue_push(_exec_change_tool, mach_get_line()); -} - - -// Miscellaneous Functions (4.3.9) -static stat_t _exec_mist_coolant(mp_buffer_t *bf) { - coolant_set_mist(bf->value); - return STAT_NOOP; // No move queued -} - - -/// M7 -void mach_mist_coolant_control(bool mist_coolant) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = mist_coolant; - mp_queue_push_nonstop(_exec_mist_coolant, mach_get_line()); -} - - -static stat_t _exec_flood_coolant(mp_buffer_t *bf) { - coolant_set_flood(bf->value); - if (!bf->value) coolant_set_mist(false); // M9 special function - return STAT_NOOP; // No move queued -} - - -/// M8, M9 -void mach_flood_coolant_control(bool flood_coolant) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = flood_coolant; - mp_queue_push_nonstop(_exec_flood_coolant, mach_get_line()); -} - - -/* Override enables are kind of a mess in Gcode. This is an attempt to sort - * them out. See - * http://www.linuxcnc.org/docs/2.4/html/gcode_main.html#sec:M50:-Feed-Override - */ - -void mach_set_feed_override(float value) { - mach.gm.feed_override = value; - mach.gm.feed_override_enable = !fp_ZERO(value); -} - - -void mach_set_spindle_override(float value) { - mach.gm.spindle_override = value; - mach.gm.spindle_override_enable = !fp_ZERO(value); -} - - -/// M48, M49 -void mach_override_enables(bool flag) { - mach.gm.feed_override_enable = flag; - mach.gm.spindle_override_enable = flag; -} - - -/// M50 -void mach_feed_override_enable(bool flag) { - if (parser.gf.parameter && fp_ZERO(parser.gn.parameter)) - mach.gm.feed_override_enable = false; - else { - if (parser.gf.parameter) mach.gm.feed_override = parser.gf.parameter; - mach.gm.feed_override_enable = true; - } -} - - -/// M51 -void mach_spindle_override_enable(bool flag) { - if (parser.gf.parameter && fp_ZERO(parser.gn.parameter)) - mach.gm.spindle_override_enable = false; - else { - if (parser.gf.parameter) mach.gm.spindle_override = parser.gf.parameter; - mach.gm.spindle_override_enable = true; - } -} - - -void mach_message(const char *message) { - status_message_P(0, STAT_LEVEL_INFO, STAT_OK, PSTR("%s"), message); -} - - -/* Program Functions (4.3.10) - * - * This group implements stop, start, end, and hold. - * It is extended beyond the NIST spec to handle various situations. - * - * mach_program_stop and mach_optional_program_stop are synchronous Gcode - * commands that are received through the interpreter. They cause all motion - * to stop at the end of the current command, including spindle motion. - * - * Note that the stop occurs at the end of the immediately preceding command - * (i.e. the stop is queued behind the last command). - * - * mach_program_end is a stop that also resets the machine to initial state - */ - - -static stat_t _exec_program_stop(mp_buffer_t *bf) { - // Machine should be stopped at this point. Go into hold so that a start is - // needed before executing further instructions. - mp_set_hold_reason(HOLD_REASON_PROGRAM_PAUSE); - mp_state_holding(); - return STAT_NOOP; // No move queued -} - - -/// M0 Queue a program stop -void mach_program_stop() { - mp_queue_push(_exec_program_stop, mach_get_line()); -} - - -static stat_t _exec_optional_program_stop(mp_buffer_t *bf) { - mp_state_optional_pause(); // Pause here if it was requested by the user - return STAT_NOOP; // No move queued -} - - -/// M1 -void mach_optional_program_stop() { - mp_queue_push(_exec_optional_program_stop, mach_get_line()); -} - - -static stat_t _exec_pallet_change_stop(mp_buffer_t *bf) { - // Emit pallet change signal - mp_set_hold_reason(HOLD_REASON_PALLET_CHANGE); - mp_state_holding(); - return STAT_NOOP; // No move queued -} - - -/// M60 -void mach_pallet_change_stop() { - mp_queue_push(_exec_pallet_change_stop, mach_get_line()); -} - - -/*** mach_program_end() implements M2 and M30. End behaviors are defined by - * NIST 3.6.1 are: - * - * 1. Axis offsets are set to zero (like G92.2) and origin offsets are set - * to the default (like G54) - * 2. Selected plane is set to PLANE_XY (like G17) - * 3. Distance mode is set to MODE_ABSOLUTE (like G90) - * 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94) - * 5. Feed and speed overrides are set to ON (like M48) - * 6. Cutter compensation is turned off (like G40) - * 7. The spindle is stopped (like M5) - * 8. The current motion mode is set to G_1 (like G1) - * 9. Coolant is turned off (like M9) - * - * mach_program_end() implements things slightly differently: - * - * 1. Axis offsets are set to G92.1 CANCEL offsets - * (instead of using G92.2 SUSPEND Offsets) - * Set default coordinate system - * 2. Selected plane is set to default plane - * 3. Distance mode is set to MODE_ABSOLUTE (like G90) - * 4. Feed rate mode is set to UNITS_PER_MINUTE (like G94) - * 5. Not implemented - * 6. Not implemented - * 7. The spindle is stopped (like M5) - * 8. Motion mode is canceled like G80 (not set to G1) - * 9. Coolant is turned off (like M9) - * + Default INCHES or MM units mode is restored - */ - - -/// M2, M30 -void mach_program_end() { - mach_reset_origin_offsets(); // G92.1 - we do G91.1 instead of G92.2 - mach_set_coord_system(GCODE_DEFAULT_COORD_SYSTEM); - mach_set_plane(GCODE_DEFAULT_PLANE); - mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE); - mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE); - mach_set_spindle_mode(SPINDLE_OFF); // M5 - mach_flood_coolant_control(false); // M9 - mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // G94 - mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); -} diff --git a/src/avr/src/machine.h b/src/avr/src/machine.h deleted file mode 100644 index 51e0480..0000000 --- a/src/avr/src/machine.h +++ /dev/null @@ -1,142 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include "config.h" -#include "status.h" -#include "gcode_state.h" -#include "switch.h" - - -#define TO_MM(a) (mach_get_units() == INCHES ? (a) * MM_PER_INCH : a) - -// Model state getters and setters -uint32_t mach_get_line(); -float mach_get_feed_rate(); -bool mach_is_inverse_time_mode(); -feed_mode_t mach_get_feed_mode(); -float mach_get_feed_override(); -float mach_get_spindle_override(); -motion_mode_t mach_get_motion_mode(); -bool mach_is_rapid(); -plane_t mach_get_plane(); -units_t mach_get_units(); -coord_system_t mach_get_coord_system(); -bool mach_get_absolute_mode(); -path_mode_t mach_get_path_mode(); -bool mach_is_exact_stop(); -bool mach_in_absolute_mode(); -distance_mode_t mach_get_distance_mode(); -distance_mode_t mach_get_arc_distance_mode(); - -void mach_set_motion_mode(motion_mode_t motion_mode); -void mach_set_spindle_mode(spindle_mode_t spindle_mode); -void mach_set_spindle_speed(float speed); -void mach_set_absolute_mode(bool absolute_mode); -void mach_set_line(uint32_t line); - -// Coordinate systems and offsets -float mach_get_active_coord_offset(uint8_t axis); -void mach_update_work_offsets(); -const float *mach_get_position(); -void mach_set_position(const float position[]); -float mach_get_axis_position(uint8_t axis); -void mach_set_axis_position(unsigned axis, float position); -void mach_set_position_from_runtime(); - -// Critical helpers -void mach_calc_target(float target[], const float values[], const bool flags[], - bool absolute); -stat_t mach_test_soft_limits(float target[]); - -// machining functions defined by NIST [organized by NIST Gcode doc] - -// Initialization and termination (4.3.2) -void machine_init(); - -// Representation (4.3.3) -void mach_set_plane(plane_t plane); -void mach_set_units(units_t mode); -void mach_set_distance_mode(distance_mode_t mode); -void mach_set_arc_distance_mode(distance_mode_t mode); -void mach_set_coord_offsets(coord_system_t coord_system, float offset[], - bool flags[]); -void mach_set_coord_system(coord_system_t coord_system); - -void mach_set_home(float origin[], bool flags[]); -void mach_clear_home(bool flags[]); - -void mach_set_origin_offsets(float offset[], bool flags[]); -void mach_reset_origin_offsets(); -void mach_suspend_origin_offsets(); -void mach_resume_origin_offsets(); - -// Free Space Motion (4.3.4) -stat_t mach_plan_line(float target[], switch_id_t sw); -stat_t mach_rapid(float target[], bool flags[]); -void mach_set_g28_position(); -stat_t mach_goto_g28_position(float target[], bool flags[]); -void mach_set_g30_position(); -stat_t mach_goto_g30_position(float target[], bool flags[]); -stat_t mach_probe(float target[], bool flags[], motion_mode_t mode); -stat_t mach_seek(float target[], bool flags[], motion_mode_t mode); - -// Machining Attributes (4.3.5) -void mach_set_feed_rate(float feed_rate); -void mach_set_feed_mode(feed_mode_t mode); -void mach_set_path_mode(path_mode_t mode); - -// Machining Functions (4.3.6) see arc.h -stat_t mach_feed(float target[], bool flags[]); -stat_t mach_dwell(float seconds); - -// Spindle Functions (4.3.7) see spindle.h - -// Tool Functions (4.3.8) -void mach_select_tool(uint8_t tool); -void mach_change_tool(bool x); - -// Miscellaneous Functions (4.3.9) -void mach_mist_coolant_control(bool mist_coolant); -void mach_flood_coolant_control(bool flood_coolant); - -void mach_set_feed_override(float override); -void mach_set_spindle_override(float override); -void mach_override_enables(bool flag); -void mach_feed_override_enable(bool flag); -void mach_spindle_override_enable(bool flag); - -void mach_message(const char *message); - -// Program Functions (4.3.10) -void mach_program_stop(); -void mach_optional_program_stop(); -void mach_pallet_change_stop(); -void mach_program_end(); diff --git a/src/avr/src/main.c b/src/avr/src/main.c index 3668c55..642b391 100644 --- a/src/avr/src/main.c +++ b/src/avr/src/main.c @@ -28,7 +28,6 @@ \******************************************************************************/ #include "hardware.h" -#include "machine.h" #include "stepper.h" #include "motor.h" #include "switch.h" @@ -43,10 +42,8 @@ #include "pgmspace.h" #include "outputs.h" #include "lcd.h" - -#include "plan/planner.h" -#include "plan/arc.h" -#include "plan/state.h" +#include "exec.h" +#include "state.h" #include @@ -68,8 +65,7 @@ int main() { stepper_init(); // steppers motor_init(); // motors switch_init(); // switches - mp_init(); // motion planning - machine_init(); // gcode machine + exec_init(); // motion exec vars_init(); // configuration variables estop_init(); // emergency stop handler command_init(); @@ -83,10 +79,7 @@ int main() { // Main loop while (true) { hw_reset_handler(); // handle hard reset requests - if (!estop_triggered()) { - mp_state_callback(); - mach_arc_callback(); // arc generation runs - } + if (!estop_triggered()) state_callback(); command_callback(); // process next command report_callback(); // report changes wdt_reset(); diff --git a/src/avr/src/messages.def b/src/avr/src/messages.def index c69933b..b002b2c 100644 --- a/src/avr/src/messages.def +++ b/src/avr/src/messages.def @@ -25,105 +25,22 @@ \******************************************************************************/ -// OS, communications and low-level status STAT_MSG(OK, "OK") STAT_MSG(EAGAIN, "Run command again") STAT_MSG(NOOP, "No op") -STAT_MSG(COMPLETE, "Complete") -STAT_MSG(BUSY, "Busy") -STAT_MSG(NO_SUCH_DEVICE, "No such device") -STAT_MSG(BUFFER_FULL, "Buffer full") -STAT_MSG(BUFFER_FULL_FATAL, "Buffer full - fatal") -STAT_MSG(EEPROM_DATA_INVALID, "EEPROM data invalid") -STAT_MSG(STEP_CHECK_FAILED, "Step check failed") -STAT_MSG(MACH_NOT_QUIESCENT, "Cannot perform action while machine active") +STAT_MSG(PAUSE, "Pause") STAT_MSG(INTERNAL_ERROR, "Internal error") - -STAT_MSG(MOTOR_STALLED, "Motor stalled") -STAT_MSG(MOTOR_OVER_TEMP, "Motor over temperature") -STAT_MSG(MOTOR_OVER_CURRENT, "Motor over temperature") -STAT_MSG(MOTOR_DRIVER_FAULT, "Motor driver fault") -STAT_MSG(MOTOR_UNDER_VOLTAGE, "Motor under voltage") - -STAT_MSG(PREP_LINE_MOVE_TIME_INFINITE, "Move time is infinite") -STAT_MSG(PREP_LINE_MOVE_TIME_NAN, "Move time is NAN") -STAT_MSG(MOVE_TARGET_INFINITE, "Move target is infinite") -STAT_MSG(MOVE_TARGET_NAN, "Move target is NAN") -STAT_MSG(EXCESSIVE_MOVE_ERROR, "Excessive move error") - STAT_MSG(ESTOP_USER, "User triggered EStop") STAT_MSG(ESTOP_SWITCH, "Switch triggered EStop") - -// Generic data input errors STAT_MSG(UNRECOGNIZED_NAME, "Unrecognized command or variable name") -STAT_MSG(INVALID_OR_MALFORMED_COMMAND, "Invalid or malformed command") +STAT_MSG(INVALID_COMMAND, "Invalid command") +STAT_MSG(INVALID_ARGUMENTS, "Invalid argument(s) to command") STAT_MSG(TOO_MANY_ARGUMENTS, "Too many arguments to command") STAT_MSG(TOO_FEW_ARGUMENTS, "Too few arguments to command") -STAT_MSG(BAD_NUMBER_FORMAT, "Bad number format") -STAT_MSG(PARAMETER_IS_READ_ONLY, "Parameter is read-only") -STAT_MSG(PARAMETER_CANNOT_BE_READ, "Parameter cannot be read") STAT_MSG(COMMAND_NOT_ACCEPTED, "Command not accepted at this time") -STAT_MSG(INPUT_EXCEEDS_MAX_LENGTH, "Input exceeds max length") -STAT_MSG(INPUT_LESS_THAN_MIN_VALUE, "Input less than minimum value") -STAT_MSG(INPUT_EXCEEDS_MAX_VALUE, "Input exceeds maximum value") -STAT_MSG(INPUT_VALUE_RANGE_ERROR, "Input value range error") - -// Gcode errors & warnings (Most originate from NIST) -STAT_MSG(MODAL_GROUP_VIOLATION, "Modal group violation") -STAT_MSG(GCODE_COMMAND_UNSUPPORTED, "Invalid or unsupported G-Code command") -STAT_MSG(MCODE_COMMAND_UNSUPPORTED, "M code unsupported") -STAT_MSG(GCODE_AXIS_IS_MISSING, "Axis word missing") -STAT_MSG(GCODE_FEEDRATE_NOT_SPECIFIED, "Feedrate not specified") -STAT_MSG(ARC_SPECIFICATION_ERROR, "Arc specification error") -STAT_MSG(ARC_AXIS_MISSING_FOR_SELECTED_PLANE, "Arc missing axis") -STAT_MSG(ARC_RADIUS_OUT_OF_TOLERANCE, "Arc radius arc out of tolerance") -STAT_MSG(ARC_ENDPOINT_IS_STARTING_POINT, "Arc end point is starting point") -STAT_MSG(ARC_OFFSETS_MISSING_FOR_PLANE, "arc offsets missing for plane") -STAT_MSG(P_WORD_IS_NOT_A_POSITIVE_INTEGER, "P word is not a positive integer") -STAT_MSG(EXPR_VALUE_STACK_OVERFLOW, "Expression parser value overflow") -STAT_MSG(EXPR_VALUE_STACK_UNDERFLOW, "Expression parser value underflow") -STAT_MSG(EXPR_OP_STACK_OVERFLOW, "Expression parser operator overflow") -STAT_MSG(EXPR_OP_STACK_UNDERFLOW, "Expression parser operator underflow") -STAT_MSG(GCODE_FUNC_UNSUPPORTED, "GCode function call unsupported") -STAT_MSG(GCODE_NUM_PARAM_UNSUPPORTED, "GCode numbered parameters unsupported") -STAT_MSG(GCODE_UNTERMINATED_VAR, "GCode variable reference untermiated") - -// Errors and warnings -STAT_MSG(MINIMUM_LENGTH_MOVE, "Move less than minimum length") -STAT_MSG(MINIMUM_TIME_MOVE, "Move less than minimum time") -STAT_MSG(MAXIMUM_TIME_MOVE, "Move greater than maximum time") STAT_MSG(MACHINE_ALARMED, "Machine alarmed - Command not processed") -STAT_MSG(LIMIT_SWITCH_HIT, "Limit switch hit - Shutdown occurred") -STAT_MSG(SOFT_LIMIT_EXCEEDED, "Soft limit exceeded") -STAT_MSG(INVALID_AXIS, "Invalid axis") STAT_MSG(EXPECTED_MOVE, "A move was expected but none was queued") - -// Homing -STAT_MSG(HOMING_CYCLE_FAILED, "Homing cycle failed") -STAT_MSG(HOMING_ERROR_BAD_OR_NO_AXIS, "Homing Error - Bad or no axis specified") -STAT_MSG(HOMING_ERROR_AXIS_MISCONFIGURED, "Homing Error - Axis misconfigured") -STAT_MSG(HOMING_ERROR_ZERO_SEARCH_VELOCITY, - "Homing Error - Search velocity is zero") -STAT_MSG(HOMING_ERROR_ZERO_LATCH_VELOCITY, - "Homing Error - Latch velocity is zero") -STAT_MSG(HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL, - "Homing Error - Travel min & max are the same") -STAT_MSG(HOMING_ERROR_NEGATIVE_LATCH_BACKOFF, - "Homing Error - Negative latch backoff") -STAT_MSG(HOMING_ERROR_SWITCH_MISCONFIGURATION, - "Homing Error - Homing switches misconfigured") - -// Probing -STAT_MSG(PROBE_INVALID_DEST, "Probing error - invalid destination") -STAT_MSG(MOVE_DURING_PROBE, "Probing error - move during probe") - -// Seeking -STAT_MSG(SEEK_MULTIPLE_AXES, "Seek error - multiple axes specified") -STAT_MSG(SEEK_MISSING_AXIS, "Seek error - no axis specified") -STAT_MSG(SEEK_AXIS_DISABLED, "Seek error - specified axis is disabled") -STAT_MSG(SEEK_SWITCH_DISABLED, "Seek error - target switch is disabled") -STAT_MSG(SEEK_ZERO_MOVE, "Seek error - axis move too short or zero") -STAT_MSG(SEEK_SWTICH_NOT_FOUND, "Seek error - move end before switch change") +STAT_MSG(QUEUE_FULL, "Queue full") // End of stats marker STAT_MSG(MAX, "") diff --git a/src/avr/src/motor.c b/src/avr/src/motor.c index b8a43e7..60f3e1d 100644 --- a/src/avr/src/motor.c +++ b/src/avr/src/motor.c @@ -34,12 +34,10 @@ #include "stepper.h" #include "drv8711.h" #include "estop.h" -#include "gcode_state.h" #include "axis.h" #include "util.h" #include "pgmspace.h" - -#include "plan/runtime.h" +#include "exec.h" #include @@ -176,24 +174,19 @@ bool motor_is_enabled(int motor) { int motor_get_axis(int motor) {return motors[motor].axis;} -float motor_get_steps_per_unit(int motor) {return motors[motor].steps_per_unit;} - +static int32_t _position_to_steps(int motor, float position) { + // We use half steps + return ((int32_t)round(position * motors[motor].steps_per_unit)) << 1; +} -void motor_set_position(int motor, int32_t position) { - //if (st_is_busy()) ALARM(STAT_INTERNAL_ERROR); TODO +void motor_set_position(int motor, float position) { motor_t *m = &motors[motor]; - - m->commanded = m->encoder = m->position = position << 1; // We use half steps + m->commanded = m->encoder = m->position = _position_to_steps(motor, position); m->error = 0; } -int32_t motor_get_position(int motor) { - return motors[motor].position >> 1; // Convert from half to full steps -} - - static void _update_power(int motor) { motor_t *m = &motors[motor]; @@ -289,19 +282,19 @@ void motor_load_move(int motor) { } -void motor_prep_move(int motor, float time, int32_t target) { +void motor_prep_move(int motor, float time, float target) { + ASSERT(isfinite(target)); + int32_t position = _position_to_steps(motor, target); + motor_t *m = &motors[motor]; // Validate input ASSERT(0 <= motor && motor < MOTORS); ASSERT(!m->prepped); - // We count in half steps - target = target << 1; - - // Compute travel in steps - int24_t half_steps = target - m->position; - m->position = target; + // Travel in half steps + int24_t half_steps = position - m->position; + m->position = position; // Error correction int16_t correction = abs(m->error); @@ -437,7 +430,7 @@ void set_motor_axis(int motor, uint8_t axis) { if (MOTORS <= motor || AXES <= axis || axis == motors[motor].axis) return; motors[motor].axis = axis; axis_map_motors(); - mp_runtime_set_steps_from_position(); // Reset encoder counts + exec_reset_encoder_counts(); // Reset encoder counts // Check if this is now a slave motor motors[motor].slave = false; diff --git a/src/avr/src/motor.h b/src/avr/src/motor.h index 2115bf7..e0786fa 100644 --- a/src/avr/src/motor.h +++ b/src/avr/src/motor.h @@ -46,12 +46,10 @@ void motor_init(); bool motor_is_enabled(int motor); int motor_get_axis(int motor); -float motor_get_steps_per_unit(int motor); -void motor_set_position(int motor, int32_t position); -int32_t motor_get_position(int motor); +void motor_set_position(int motor, float position); stat_t motor_rtc_callback(); void motor_end_move(int motor); void motor_load_move(int motor); -void motor_prep_move(int motor, float time, int32_t target); +void motor_prep_move(int motor, float time, float target); diff --git a/src/avr/src/plan/arc.c b/src/avr/src/plan/arc.c deleted file mode 100644 index 2cb1fcc..0000000 --- a/src/avr/src/plan/arc.c +++ /dev/null @@ -1,510 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* This module actually contains some parts that belong in the 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" - -#include "axis.h" -#include "buffer.h" -#include "line.h" -#include "gcode_parser.h" -#include "config.h" -#include "util.h" - -#include - -#include -#include -#include - - -typedef struct { - bool running; - - float target[AXES]; // XYZABC where the move should go - float position[AXES]; // end point of the current segment - - float theta; // total angle specified by arc - float radius; // Raw R value, or computed via offsets - - 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 - uint8_t linear_axis; // linear axis (normal to plane) - - uint32_t segments; // count of running segments - float segment_theta; // angular motion per segment - float segment_linear_travel; // linear motion per segment - float center_0; // center at axis 0 (e.g. X for G17) - float center_1; // center at axis 1 (e.g. Y for G17) -} arc_t; - -arc_t arc = {0}; - - -/*** Returns a naive estimate of arc execution time to inform segment - * calculation. The arc time is computed not to exceed the time taken - * in the slowest dimension in the arc plane or in linear - * travel. Maximum feed rates are compared in each dimension, but the - * comparison assumes that the arc will have at least one segment - * where the unit vector is 1 in that dimension. This is not true for - * any arbitrary arc, with the result that the time returned may be - * less than optimal. - */ -static float _estimate_arc_time(float length, float linear_travel, - float planar_travel) { - // Determine move time at requested feed rate - // Inverse feed rate is normalized to minutes - float time = mach_is_inverse_time_mode() ? - mach_get_feed_rate() : length / mach_get_feed_rate(); - - - // Apply feed override - time /= mach_get_feed_override(); - - // Downgrade the time if there is a rate-limiting axis - return max4(time, planar_travel / axis_get_velocity_max(arc.plane_axis_0), - planar_travel / axis_get_velocity_max(arc.plane_axis_1), - fabs(linear_travel) / axis_get_velocity_max(arc.linear_axis)); -} - - -/*** Compute arc center (offset) from radius. - * - * Needs to calculate the center of the circle that has the designated radius - * and passes through both the current position and the target position - * - * This method calculates the following set of equations where: - * - * [x,y] is the vector from current to target position, - * d == magnitude of that vector, - * h == hypotenuse of the triangle formed by the radius of the circle, - * the distance to the center of the travel vector. - * - * A vector perpendicular to the travel vector [-y,x] is scaled to the length - * of h [-y/d*h, x/d*h] and added to the center of the travel vector [x/2,y/2] - * to form the new point [i,j] at [x/2-y/d*h, y/2+x/d*h] which will be the - * center of the arc. - * - * d^2 == x^2 + y^2 - * h^2 == r^2 - (d/2)^2 - * i == x/2 - y/d*h - * j == y/2 + x/d*h - * O <- [i,j] - * - | - * r - | - * - | - * - | h - * - | - * [0,0] -> C -----------------+--------------- T <- [x,y] - * | <------ d/2 ---->| - * - * C - Current position - * T - Target position - * O - center of circle that pass through both C and T - * d - distance from C to T - * r - designated radius - * h - distance from center of CT to O - * - * Expanding the equations: - * - * d -> sqrt(x^2 + y^2) - * h -> sqrt(4 * r^2 - x^2 - y^2)/2 - * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2)) / sqrt(x^2 + y^2)) / 2 - * - * Which can be written: - * - * i -> (x - (y * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - * j -> (y + (x * sqrt(4 * r^2 - x^2 - y^2))/sqrt(x^2 + y^2))/2 - * - * Which we for size and speed reasons optimize to: - * - * h_x2_div_d = sqrt(4 * r^2 - x^2 - y^2)/sqrt(x^2 + y^2) - * i = (x - (y * h_x2_div_d))/2 - * j = (y + (x * h_x2_div_d))/2 - * - * Computing clockwise vs counter-clockwise motion - * - * The counter clockwise circle lies to the left of the target direction. - * When offset is positive the left hand circle will be generated - - * when it is negative the right hand circle is generated. - * - * T <-- Target position - * ^ - * Clockwise circles with | Clockwise circles with - * this center will have | this center will have - * > 180 deg of angular travel | < 180 deg of angular travel, - * \ | which is a good thing! - * \ | / - * center of arc when -> x <----- | -----> x <- center of arc when - * h_x2_div_d is positive | h_x2_div_d is negative - * | - * C <-- Current position - * - * - * Assumes arc singleton has been pre-loaded with target and position. - * Parts of this routine were originally sourced from the grbl project. - */ -static stat_t _compute_arc_offsets_from_radius(float offset[], bool clockwise) { - // Calculate the change in position along each selected axis - float x = - arc.target[arc.plane_axis_0] - mach_get_axis_position(arc.plane_axis_0); - float y = - arc.target[arc.plane_axis_1] - mach_get_axis_position(arc.plane_axis_1); - - // *** From Forrest Green - Other Machine Co, 3/27/14 - // If the distance between endpoints is greater than the arc diameter, disc - // will be negative indicating that the arc is offset into the complex plane - // beyond the reach of any real CNC. However, numerical errors can flip the - // sign of disc as it approaches zero (which happens as the arc angle - // approaches 180 degrees). To avoid mishandling these arcs we use the - // closest real solution (which will be 0 when disc <= 0). This risks - // obscuring g-code errors where the radius is actually too small (they will - // be treated as half circles), but ensures that all valid arcs end up - // reasonably close to their intended paths regardless of any numerical - // issues. - float disc = 4 * square(arc.radius) - (square(x) + square(y)); - - float h_x2_div_d = disc > 0 ? -sqrt(disc) / hypotf(x, y) : 0; - - // Invert sign of h_x2_div_d if circle is counter clockwise (see header notes) - if (!clockwise) h_x2_div_d = -h_x2_div_d; - - // Negative R is g-code-alese for "I want a circle with more than 180 degrees - // of travel" (go figure!), even though it is advised against ever generating - // such circles in a single line of g-code. By inverting the sign of - // h_x2_div_d the center of the circles is placed on the opposite side of - // the line of travel and thus we get the unadvisably long arcs as prescribed. - if (arc.radius < 0) h_x2_div_d = -h_x2_div_d; - - // Complete the operation by calculating the actual center of the arc - offset[arc.plane_axis_0] = (x - y * h_x2_div_d) / 2; - offset[arc.plane_axis_1] = (y + x * h_x2_div_d) / 2; - offset[arc.linear_axis] = 0; - - return STAT_OK; -} - - -/* Compute arc from I and J (arc center point) - * - * The theta calculation sets up an clockwise or counterclockwise arc - * from the current position to the target position around the center - * designated by the offset vector. All theta-values measured in - * radians of deviance from the positive y-axis. - * - * | <- theta == 0 - * * * * - * * * - * * * - * * O ----T <- theta_end (e.g. 90 degrees: theta_end == PI/2) - * * / - * C <- theta_start (e.g. -145 degrees: theta_start == -PI*(3/4)) - * - * Parts of this routine were originally sourced from the grbl project. - */ -static stat_t _compute_arc(bool radius_f, const float position[], - float offset[], float rotations, bool clockwise, - bool full_circle) { - // Compute radius. A non-zero radius value indicates a radius arc - if (radius_f) _compute_arc_offsets_from_radius(offset, clockwise); - else // compute start radius - arc.radius = hypotf(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]); - - // Test arc specification for correctness according to: - // http://linuxcnc.org/docs/html/gcode/gcode.html#sec:G2-G3-Arc - // "It is an error if: when the arc is projected on the selected - // plane, the distance from the current point to the center differs - // from the distance from the end point to the center by more than - // (.05 inch/.5 mm) OR ((.0005 inch/.005mm) AND .1% of radius)." - - // Compute end radius from the center of circle (offsets) to target endpoint - float end_0 = arc.target[arc.plane_axis_0] - - position[arc.plane_axis_0] - offset[arc.plane_axis_0]; - - float end_1 = arc.target[arc.plane_axis_1] - - position[arc.plane_axis_1] - offset[arc.plane_axis_1]; - - // end radius - start radius - float err = fabs(hypotf(end_0, end_1) - arc.radius); - - if (err > ARC_RADIUS_ERROR_MAX || - (err < ARC_RADIUS_ERROR_MIN && err > arc.radius * ARC_RADIUS_TOLERANCE)) - return STAT_ARC_SPECIFICATION_ERROR; - - // Calculate the theta (angle) of the current point (position) - // arc.theta is angular starting point for the arc (also needed later for - // calculating center point) - arc.theta = atan2(-offset[arc.plane_axis_0], -offset[arc.plane_axis_1]); - - // g18_correction is used to invert G18 XZ plane arcs for proper CW - // orientation - float g18_correction = mach_get_plane() == PLANE_XZ ? -1 : 1; - float angular_travel = 0; - - if (full_circle) { - // angular travel always starts as zero for full circles - angular_travel = 0; - - // handle the valid case of a full circle arc w/ P=0 - if (fp_ZERO(rotations)) rotations = 1.0; - - } else { - float theta_end = atan2(end_0, end_1); - - // Compute the angular travel - if (fp_EQ(theta_end, arc.theta)) - // very large radii arcs can have zero angular travel (thanks PartKam) - angular_travel = 0; - - else { - // make the difference positive so we have clockwise travel - if (theta_end < arc.theta) theta_end += 2 * M_PI * g18_correction; - - // compute positive angular travel - angular_travel = theta_end - arc.theta; - - // reverse travel direction if it's CCW arc - if (!clockwise) angular_travel -= 2 * M_PI * g18_correction; - } - } - - // Add in travel for rotations - if (clockwise) angular_travel += 2 * M_PI * rotations * g18_correction; - else angular_travel -= 2 * M_PI * rotations * g18_correction; - - // Calculate travel in the depth axis of the helix and compute the time it - // should take to perform the move length is the total mm of travel of - // the helix (or just a planar arc) - float linear_travel = arc.target[arc.linear_axis] - position[arc.linear_axis]; - float planar_travel = angular_travel * arc.radius; - // hypot is insensitive to +/- signs - float length = hypotf(planar_travel, linear_travel); - - // trap zero length arcs that _compute_arc can throw - if (fp_ZERO(length)) return STAT_MINIMUM_LENGTH_MOVE; - - // get an estimate of execution time to inform segment calculation - float arc_time = _estimate_arc_time(length, linear_travel, planar_travel); - - // Find the minimum number of segments that meets these constraints... - float segments_for_chordal_accuracy = - length / sqrt(4 * CHORDAL_TOLERANCE * (2 * arc.radius - CHORDAL_TOLERANCE)); - float segments_for_minimum_distance = length / ARC_SEGMENT_LENGTH; - float segments_for_minimum_time = - arc_time * MICROSECONDS_PER_MINUTE / MIN_ARC_SEGMENT_USEC; - - float segments = floor(min3(segments_for_chordal_accuracy, - segments_for_minimum_distance, - segments_for_minimum_time)); - if (segments < 1) segments = 1; // at least 1 segment - - arc.segments = (uint32_t)segments; - arc.segment_theta = angular_travel / segments; - arc.segment_linear_travel = linear_travel / segments; - arc.center_0 = position[arc.plane_axis_0] - sin(arc.theta) * arc.radius; - arc.center_1 = position[arc.plane_axis_1] - cos(arc.theta) * arc.radius; - - // initialize the linear position - arc.position[arc.linear_axis] = position[arc.linear_axis]; - - return STAT_OK; -} - - -/*** Machine entry point for arc - * - * Generates an arc by queuing line segments to the move buffer. The arc is - * approximated by generating a large number of tiny, linear segments. - */ -stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints - float offsets[], bool offsets_f[], // arc offsets - float radius, bool radius_f, // radius - float P, bool P_f, // parameter - bool modal_g1_f, - motion_mode_t motion_mode) { // defined motion mode - - // Trap some precursor cases. Since motion mode (MODAL_GROUP_G1) persists - // from the previous move it's possible for non-modal commands such as F or P - // to arrive here when no motion has actually been specified. It's also - // possible to run an arc as simple as "I25" if CW or CCW motion mode was - // already set by a previous block. Here are 2 cases to handle if CW or CCW - // motion mode was set by a previous block: - // - // Case 1: F, P or other non modal is specified but no movement is specified - // (no offsets or radius). This is OK: return STAT_OK - // - // Case 2: Movement is specified w/o a new G2 or G3 word in the (new) block. - // This is OK: continue the move - // - if (!modal_g1_f && !offsets_f[0] && !offsets_f[1] && !offsets_f[2] && - !radius_f) return STAT_OK; - - // trap missing feed rate - if (fp_ZERO(mach_get_feed_rate()) || - (mach_is_inverse_time_mode() && !parser.gf.feed_rate)) - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - - // radius must be positive and > minimum - if (radius_f && radius < MIN_ARC_RADIUS) - return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; - - // Set the arc plane for the current G17/G18/G19 setting and test arc - // specification Plane axis 0 and 1 are the arc plane, the linear axis is - // normal to the arc plane. - switch (mach_get_plane()) { - case PLANE_XY: // G17 - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Y; - arc.linear_axis = AXIS_Z; - break; - - case PLANE_XZ: // G18 - arc.plane_axis_0 = AXIS_X; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_Y; - break; - - case PLANE_YZ: // G19 - arc.plane_axis_0 = AXIS_Y; - arc.plane_axis_1 = AXIS_Z; - arc.linear_axis = AXIS_X; - break; - } - - bool clockwise = motion_mode == MOTION_MODE_CW_ARC; - - // Test if endpoints are specified in the selected plane - bool full_circle = false; - if (!values_f[arc.plane_axis_0] && !values_f[arc.plane_axis_1]) { - if (radius_f) // in radius mode arcs missing both endpoints is an error - return STAT_ARC_AXIS_MISSING_FOR_SELECTED_PLANE; - else full_circle = true; // in center format arc specifies full circle - } - - // Test radius arcs for radius tolerance - if (radius_f) { - arc.radius = TO_MM(radius); // set to internal format (mm) - if (fabs(arc.radius) < MIN_ARC_RADIUS) // radius value must be > minimum - return STAT_ARC_RADIUS_OUT_OF_TOLERANCE; - - // Test that center format absolute distance mode arcs have both offsets - } else if (mach_get_arc_distance_mode() == ABSOLUTE_MODE && - !(offsets_f[arc.plane_axis_0] && offsets_f[arc.plane_axis_1])) - return STAT_ARC_OFFSETS_MISSING_FOR_PLANE; - - // Set arc rotations - float rotations = 0; - - if (P_f) { - // If P is present it must be a positive integer - if (P < 0 || 0 < floor(P) - P) return STAT_P_WORD_IS_NOT_A_POSITIVE_INTEGER; - - rotations = P; - - } else if (full_circle) rotations = 1; // default to 1 for full circles - - // Set model target - const float *position = mach_get_position(); - mach_calc_target(arc.target, values, values_f, mach_in_absolute_mode()); - - // in radius mode it's an error for start == end - if (radius_f && fp_EQ(position[AXIS_X], arc.target[AXIS_X]) && - fp_EQ(position[AXIS_Y], arc.target[AXIS_Y]) && - fp_EQ(position[AXIS_Z], arc.target[AXIS_Z])) - return STAT_ARC_ENDPOINT_IS_STARTING_POINT; - - // now get down to the rest of the work setting up the arc for execution - mach_set_motion_mode(motion_mode); - mach_update_work_offsets(); // Update resolved offsets - arc.radius = TO_MM(radius); // set arc radius or zero - - float offset[3]; - for (int i = 0; i < 3; i++) offset[i] = TO_MM(offsets[i]); - - if (mach_get_arc_distance_mode() == ABSOLUTE_MODE) { - if (offsets_f[0]) offset[0] -= position[0]; - if (offsets_f[1]) offset[1] -= position[1]; - if (offsets_f[2]) offset[2] -= position[2]; - } - - // compute arc runtime values - RITORNO(_compute_arc - (radius_f, position, offset, rotations, clockwise, full_circle)); - - // Note, arc soft limits are not tested here - - arc.running = true; // Enable arc run in callback - mp_pause_queue(true); // Hold queue until arc is done - mach_arc_callback(); // Queue initial arc moves - mach_set_position(arc.target); // update model position - - return STAT_OK; -} - - -/*** Generate an arc - * - * 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. - */ -void mach_arc_callback() { - while (arc.running && mp_queue_get_room()) { - if (arc.segments == 1) { // Final segment - arc.position[arc.plane_axis_0] = arc.target[arc.plane_axis_0]; - arc.position[arc.plane_axis_1] = arc.target[arc.plane_axis_1]; - arc.position[arc.linear_axis] = arc.target[arc.linear_axis]; - - } else { - arc.theta += arc.segment_theta; - - arc.position[arc.plane_axis_0] = - arc.center_0 + sin(arc.theta) * arc.radius; - arc.position[arc.plane_axis_1] = - arc.center_1 + cos(arc.theta) * arc.radius; - arc.position[arc.linear_axis] += arc.segment_linear_travel; - } - - // run the line - mach_plan_line(arc.position, 0); - - if (!--arc.segments) mach_abort_arc(); - } -} - - -/// Stop arc movement without maintaining position -/// OK to call if no arc is running -void mach_abort_arc() { - arc.running = false; - mp_pause_queue(false); -} diff --git a/src/avr/src/plan/arc.h b/src/avr/src/plan/arc.h deleted file mode 100644 index 13a047f..0000000 --- a/src/avr/src/plan/arc.h +++ /dev/null @@ -1,47 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "gcode_state.h" -#include "status.h" - - -#define ARC_SEGMENT_LENGTH 0.1 // mm -#define MIN_ARC_RADIUS 0.1 - -#define MIN_ARC_SEGMENT_USEC 10000.0 // minimum arc segment time -#define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE) - - -stat_t mach_arc_feed(float target[], bool flags[], float offsets[], - bool offset_f[], float radius, bool radius_f, - float P, bool P_f, bool modal_g1_f, - motion_mode_t motion_mode); -void mach_arc_callback(); -void mach_abort_arc(); diff --git a/src/avr/src/plan/buffer.c b/src/avr/src/plan/buffer.c deleted file mode 100644 index 6a1ad0a..0000000 --- a/src/avr/src/plan/buffer.c +++ /dev/null @@ -1,240 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* Planner buffers are used to queue and operate on Gcode blocks. Each - * buffer contains one Gcode block which may be a move, M code or - * other command that must be executed synchronously with movement. - */ - -#include "buffer.h" -#include "state.h" -#include "rtc.h" -#include "util.h" - -#include -#include -#include - - -typedef struct { - uint16_t space; - mp_buffer_t *tail; - mp_buffer_t *head; - mp_buffer_t bf[PLANNER_BUFFER_POOL_SIZE]; -} buffer_pool_t; - - -buffer_pool_t mb; - - -/// Zeroes the contents of a buffer -static void _clear_buffer(mp_buffer_t *bf) { - mp_buffer_t *next = bf->next; // save pointers - mp_buffer_t *prev = bf->prev; - memset(bf, 0, sizeof(mp_buffer_t)); - bf->next = next; // restore pointers - bf->prev = prev; -} - - -static void _push() { - if (!mb.space) { - ALARM(STAT_INTERNAL_ERROR); - return; - } - - mb.tail = mb.tail->next; - mb.space--; -} - - -static void _pop() { - if (mb.space == PLANNER_BUFFER_POOL_SIZE) { - ALARM(STAT_INTERNAL_ERROR); - return; - } - - mb.head = mb.head->next; - mb.space++; -} - - -/// Initializes or resets buffers -void mp_queue_init() { - memset(&mb, 0, sizeof(mb)); // clear all values - - mb.tail = mb.head = &mb.bf[0]; // init head and tail - mb.space = PLANNER_BUFFER_POOL_SIZE; - - // Setup ring pointers - for (int i = 0; i < mb.space; i++) { - mb.bf[i].next = &mb.bf[i + 1]; - mb.bf[i].prev = &mb.bf[i - 1]; - } - - mb.bf[0].prev = &mb.bf[mb.space -1]; // Fix first->prev - mb.bf[mb.space - 1].next = &mb.bf[0]; // Fix last->next - - mp_state_idle(); -} - - -uint8_t mp_queue_get_room() { - return mb.space < PLANNER_BUFFER_HEADROOM ? - 0 : mb.space - PLANNER_BUFFER_HEADROOM; -} - - -uint8_t mp_queue_get_fill() { - return PLANNER_BUFFER_POOL_SIZE - mb.space; -} - - -bool mp_queue_is_empty() {return mb.tail == mb.head;} - - -/// Get pointer to next buffer, waiting until one is available. -mp_buffer_t *mp_queue_get_tail() { - while (!mb.space) continue; // Wait for a buffer - return mb.tail; -} - - -/*** Commit the next buffer to the queue. - * - * WARNING: The routine calling mp_queue_push() must not use the write - * buffer once it has been queued. Action may start on the buffer immediately, - * invalidating its contents - */ -void mp_queue_push(buffer_cb_t cb, uint32_t line) { - mp_buffer_validate(mb.tail); - mp_state_running(); - - mb.tail->ts = rtc_get_time(); - mb.tail->cb = cb; - mb.tail->line = line; - mb.tail->state = BUFFER_NEW; - - _push(); -} - - -mp_buffer_t *mp_queue_get_head() { - return mp_queue_is_empty() ? 0 : mb.head; -} - - -/// Clear and release buffer to pool -void mp_queue_pop() { - _clear_buffer(mb.head); - _pop(); -} - - -#ifdef DEBUG -void mp_queue_dump() { - mp_buffer_t *bf = mp_queue_get_head(); - if (!bf) return; - mp_buffer_t *bp = bf; - - do { - if (bp != bf) putchar(','); - mp_buffer_print(bp); - bp = mp_buffer_next(bp); - } while (bp != bf && bp->state != BUFFER_OFF); - - if (bp != bf) mp_buffer_print(bp); - - putchar('\n'); -} - - -void mp_buffer_print(const mp_buffer_t *bf) { - printf("{" - "\"ts\":%d," - "\"line\":%d," - "\"state\":%d," - "\"replannable\":%s," - "\"hold\":%s," - "\"value\":%0.2f," - "\"target\":[%0.2f, %0.2f, %0.2f, %0.2f]," - "\"unit\":[%0.2f, %0.2f, %0.2f, %0.2f]," - "\"length\":%0.2f," - "\"head_length\":%0.2f," - "\"body_length\":%0.2f," - "\"tail_length\":%0.2f," - "\"entry_velocity\":%0.2f," - "\"cruise_velocity\":%0.2f," - "\"exit_velocity\":%0.2f," - "\"braking_velocity\":%0.2f," - "\"entry_vmax\":%0.2f," - "\"cruise_vmax\":%0.2f," - "\"exit_vmax\":%0.2f," - "\"delta_vmax\":%0.2f," - "\"jerk\":%0.2f," - "\"cbrt_jerk\":%0.2f" - "}", bf->ts, bf->line, bf->state, - (bf->flags & BUFFER_REPLANNABLE) ? "true" : "false", - (bf->flags & BUFFER_HOLD) ? "true" : "false", - bf->value, bf->target[0], bf->target[1], - bf->target[2], bf->target[3], bf->unit[0], bf->unit[1], bf->unit[2], - bf->unit[3], bf->length, bf->head_length, bf->body_length, - bf->tail_length, bf->entry_velocity, bf->cruise_velocity, - bf->exit_velocity, bf->braking_velocity, bf->entry_vmax, - bf->cruise_vmax, bf->exit_vmax, bf->delta_vmax, bf->jerk, - bf->cbrt_jerk); -} -#endif // DEBUG - - -void mp_buffer_validate(const mp_buffer_t *bp) { - ASSERT(bp); - - if (!(bp->flags & BUFFER_LINE)) return; // Only check line buffers - - ASSERT(isfinite(bp->value)); - - ASSERT(isfinite(bp->target[0]) && isfinite(bp->target[1]) && - isfinite(bp->target[2]) && isfinite(bp->target[3])); - ASSERT(isfinite(bp->unit[0]) && isfinite(bp->unit[1]) && - isfinite(bp->unit[2]) && isfinite(bp->unit[3])); - - ASSERT(isfinite(bp->length)); - ASSERT(isfinite(bp->head_length)); - ASSERT(isfinite(bp->body_length)); - ASSERT(isfinite(bp->tail_length)); - - ASSERT(isfinite(bp->entry_velocity)); - ASSERT(isfinite(bp->cruise_velocity)); - ASSERT(isfinite(bp->exit_velocity)); - ASSERT(isfinite(bp->braking_velocity)); - - ASSERT(isfinite(bp->jerk)); - ASSERT(isfinite(bp->cbrt_jerk)); -} diff --git a/src/avr/src/plan/buffer.h b/src/avr/src/plan/buffer.h deleted file mode 100644 index de1d4d5..0000000 --- a/src/avr/src/plan/buffer.h +++ /dev/null @@ -1,119 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "machine.h" -#include "config.h" - -#include - - -typedef enum { - BUFFER_OFF, // move inactive - BUFFER_NEW, // initial value - BUFFER_INIT, // first run - BUFFER_ACTIVE, // subsequent runs - BUFFER_RESTART, // restart buffer when done -} buffer_state_t; - - -typedef enum { - BUFFER_REPLANNABLE = 1 << 0, - BUFFER_HOLD = 1 << 1, - BUFFER_SEEK_CLOSE = 1 << 2, - BUFFER_SEEK_OPEN = 1 << 3, - BUFFER_SEEK_ERROR = 1 << 4, - BUFFER_RAPID = 1 << 5, - BUFFER_INVERSE_TIME = 1 << 6, - BUFFER_EXACT_STOP = 1 << 7, - BUFFER_LINE = 1 << 8, -} buffer_flags_t; - - -// Callbacks -struct mp_buffer_t; -typedef stat_t (*buffer_cb_t)(struct mp_buffer_t *bf); - - -typedef struct mp_buffer_t { // See Planning Velocity Notes - struct mp_buffer_t *prev; // pointer to previous buffer - struct mp_buffer_t *next; // pointer to next buffer - - uint32_t ts; // Time stamp - int32_t line; // gcode block line number - buffer_cb_t cb; // callback to buffer exec function - - buffer_state_t state; // buffer state - buffer_flags_t flags; // buffer flags - switch_id_t sw; // Switch to seek - - float value; // used in dwell and other callbacks - - float target[AXES]; // XYZABC where the move should go in mm - float unit[AXES]; // unit vector for axis scaling & planning - - float length; // total length of line or helix in mm - float head_length; - float body_length; - float tail_length; - - // See notes on these variables, in mp_aline() - float entry_velocity; // entry velocity requested for the move - float cruise_velocity; // cruise velocity requested & achieved - float exit_velocity; // exit velocity requested for the move - float braking_velocity; // current value for braking velocity - - float entry_vmax; // max junction velocity at entry of this move - float cruise_vmax; // max cruise velocity requested for move - float exit_vmax; // max exit velocity possible (redundant) - float delta_vmax; // max velocity difference for this move - - float jerk; // maximum linear jerk term for this move - float cbrt_jerk; // cube root of Jm (computed & cached) -} mp_buffer_t; - - -void mp_queue_init(); - -uint8_t mp_queue_get_room(); -uint8_t mp_queue_get_fill(); - -bool mp_queue_is_empty(); - -mp_buffer_t *mp_queue_get_tail(); -void mp_queue_push(buffer_cb_t func, uint32_t line); - -mp_buffer_t *mp_queue_get_head(); -void mp_queue_pop(); - -void mp_queue_dump(); - -void mp_buffer_print(const mp_buffer_t *bp); -void mp_buffer_validate(const mp_buffer_t *bp); -static inline mp_buffer_t *mp_buffer_prev(mp_buffer_t *bp) {return bp->prev;} -static inline mp_buffer_t *mp_buffer_next(mp_buffer_t *bp) {return bp->next;} diff --git a/src/avr/src/plan/calibrate.c b/src/avr/src/plan/calibrate.c deleted file mode 100644 index 1f7c6e8..0000000 --- a/src/avr/src/plan/calibrate.c +++ /dev/null @@ -1,176 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - - -#include "calibrate.h" - -#include "buffer.h" -#include "motor.h" -#include "machine.h" -#include "planner.h" -#include "stepper.h" -#include "rtc.h" -#include "state.h" -#include "config.h" - -#include -#include -#include -#include -#include - - -#define CAL_VELOCITIES 256 -#define CAL_MIN_VELOCITY 1000 // mm/sec -#define CAL_TARGET_SG 100 -#define CAL_MAX_DELTA_SG 75 -#define CAL_WAIT_TIME 3 // ms - - -enum { - CAL_START, - CAL_ACCEL, - CAL_MEASURE, - CAL_DECEL, -}; - - -typedef struct { - bool stall_valid; - bool stalled; - bool reverse; - - uint32_t wait; - int state; - int motor; - int axis; - - float velocity; - uint16_t stallguard; -} calibrate_t; - -static calibrate_t cal = {0}; - - -static stat_t _exec_calibrate(mp_buffer_t *bf) { - const float time = SEGMENT_TIME; // In minutes - const float max_delta_v = CAL_ACCELERATION * time; - - do { - if (rtc_expired(cal.wait)) - switch (cal.state) { - case CAL_START: { - cal.axis = motor_get_axis(cal.motor); - cal.state = CAL_ACCEL; - cal.velocity = 0; - cal.stall_valid = false; - cal.stalled = false; - cal.reverse = false; - - //tmc2660_set_stallguard_threshold(cal.motor, 8); - cal.wait = rtc_get_time() + CAL_WAIT_TIME; - - break; - } - - case CAL_ACCEL: - if (CAL_MIN_VELOCITY < cal.velocity) cal.stall_valid = true; - - if (cal.velocity < CAL_MIN_VELOCITY || CAL_TARGET_SG < cal.stallguard) - cal.velocity += max_delta_v; - - if (cal.stalled) { - if (cal.reverse) { - int32_t steps = -motor_get_position(cal.motor); - float mm = (float)steps / motor_get_steps_per_unit(cal.motor); - STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm); - - //tmc2660_set_stallguard_threshold(cal.motor, 63); - - mp_set_cycle(CYCLE_MACHINING); // Default cycle - - return STAT_NOOP; // Done, no move queued - - } else { - motor_set_position(cal.motor, 0); - - cal.reverse = true; - cal.velocity = 0; - cal.stall_valid = false; - cal.stalled = false; - } - } - break; - } - } while (fp_ZERO(cal.velocity)); // Repeat if computed velocity was zero - - // Compute travel - float travel[AXES] = {0}; // In mm - travel[cal.axis] = time * cal.velocity * (cal.reverse ? -1 : 1); - - // Convert to steps - float steps[MOTORS] = {0}; - mp_kinematics(travel, steps); - - // Queue segment - st_prep_line(time, steps); - - return STAT_EAGAIN; -} - - -bool calibrate_busy() {return mp_get_cycle() == CYCLE_CALIBRATING;} - - -void calibrate_set_stallguard(int motor, uint16_t sg) { - if (cal.motor != motor) return; - - if (cal.stall_valid) { - int16_t delta = sg - cal.stallguard; - if (!sg || CAL_MAX_DELTA_SG < abs(delta)) { - cal.stalled = true; - //motor_end_move(cal.motor); - } - } - - cal.stallguard = sg; -} - - -uint8_t command_calibrate(int argc, char *argv[]) { - if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY) - return 0; - - // Start - memset(&cal, 0, sizeof(cal)); - mp_set_cycle(CYCLE_CALIBRATING); - cal.motor = 1; - - mp_queue_push_nonstop(_exec_calibrate, -1); - - return 0; -} diff --git a/src/avr/src/plan/dwell.c b/src/avr/src/plan/dwell.c deleted file mode 100644 index 2d5af43..0000000 --- a/src/avr/src/plan/dwell.c +++ /dev/null @@ -1,54 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "dwell.h" - -#include "buffer.h" -#include "machine.h" -#include "stepper.h" - - -// Dwells are performed by passing a dwell move to the stepper drivers. - - -/// Dwell execution -static stat_t _exec_dwell(mp_buffer_t *bf) { - st_prep_dwell(bf->value); // in seconds - return STAT_OK; // Done -} - - -/// Queue a dwell -stat_t mp_dwell(float seconds, int32_t line) { - mp_buffer_t *bf = mp_queue_get_tail(); - bf->value = seconds; // in seconds, not minutes - mp_queue_push(_exec_dwell, line); - - return STAT_OK; -} diff --git a/src/avr/src/plan/exec.c b/src/avr/src/plan/exec.c deleted file mode 100644 index ec1cff3..0000000 --- a/src/avr/src/plan/exec.c +++ /dev/null @@ -1,482 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "planner.h" -#include "buffer.h" -#include "axis.h" -#include "runtime.h" -#include "state.h" -#include "stepper.h" -#include "motor.h" -#include "rtc.h" -#include "util.h" -#include "velocity_curve.h" -#include "config.h" - -#include -#include -#include -#include - - -typedef struct { - float unit[AXES]; // unit vector for axis scaling & planning - float final_target[AXES]; // final target, used to correct rounding errors - float waypoint[3][AXES]; // head/body/tail endpoints for correction - - // copies of bf variables of same name - float head_length; - float body_length; - float tail_length; - float entry_velocity; - float cruise_velocity; - float exit_velocity; - - uint24_t segment_count; // count of running segments - uint24_t segment; // current segment - float segment_time; - float segment_velocity; // computed velocity for segment - float segment_start[AXES]; - float segment_delta; - float segment_dist; - bool hold_planned; // true when a feedhold has been planned - move_section_t section; // current move section - bool section_new; // true if it's a new section - bool abort; -} mp_exec_t; - - -static mp_exec_t ex = {{0}}; - -/// Common code for head and tail sections -static stat_t _exec_aline_section(float length, float Vi, float Vt) { - if (ex.section_new) { - ASSERT(isfinite(length)); - - if (fp_ZERO(length)) return STAT_NOOP; // end the section - - ASSERT(isfinite(Vi) && isfinite(Vt)); - ASSERT(0 <= Vi && 0 <= Vt); - ASSERT(Vi || Vt); - - // len / avg. velocity - const float move_time = 2 * length / (Vi + Vt); // in mins - const float segments = ceil(move_time / SEGMENT_TIME); - ex.segment_count = segments; - ex.segment_time = move_time / segments; // in mins - ex.segment = 0; - ex.segment_dist = 0; - - for (int axis = 0; axis < AXES; axis++) - ex.segment_start[axis] = mp_runtime_get_axis_position(axis); - - if (Vi == Vt) { - ex.segment_delta = length / segments; - ex.segment_velocity = Vi; - - } else ex.segment_delta = 1 / (segments + 1); - - ex.section_new = false; - } - - float target[AXES]; - ex.segment++; - - // Set target position for the segment. If the segment ends on a section - // waypoint, synchronize to the head, body or tail end. Otherwise, if not - // at section waypoint compute target from segment time and velocity. Don't - // do waypoint correction when going into a hold. - if (ex.segment == ex.segment_count && !ex.section_new && !ex.hold_planned) - copy_vector(target, ex.waypoint[ex.section]); - - else { - if (Vi == Vt) ex.segment_dist += ex.segment_delta; - else { - // Compute quintic Bezier curve - ex.segment_velocity = - velocity_curve(Vi, Vt, ex.segment * ex.segment_delta); - ex.segment_dist += ex.segment_velocity * ex.segment_time; - } - - // Avoid overshoot - if (length < ex.segment_dist) ex.segment_dist = length; - - for (int axis = 0; axis < AXES; axis++) - target[axis] = ex.segment_start[axis] + ex.unit[axis] * ex.segment_dist; - } - - mp_runtime_set_velocity(ex.segment_velocity); - RITORNO(mp_runtime_move_to_target(ex.segment_time, target)); - - // Return EAGAIN to continue or OK if this segment is done - return ex.segment < ex.segment_count ? STAT_EAGAIN : STAT_OK; -} - - -/// Callback for tail section -static stat_t _exec_aline_tail() { - ex.section = SECTION_TAIL; - return - _exec_aline_section(ex.tail_length, ex.cruise_velocity, ex.exit_velocity); -} - - -/// Callback for body section -static stat_t _exec_aline_body() { - ex.section = SECTION_BODY; - - stat_t status = - _exec_aline_section(ex.body_length, ex.cruise_velocity, ex.cruise_velocity); - - switch (status) { - case STAT_NOOP: return _exec_aline_tail(); - case STAT_OK: - ex.section = SECTION_TAIL; - ex.section_new = true; - - return STAT_EAGAIN; - - default: return status; - } -} - - -/// Callback for head section -static stat_t _exec_aline_head() { - ex.section = SECTION_HEAD; - stat_t status = - _exec_aline_section(ex.head_length, ex.entry_velocity, ex.cruise_velocity); - - switch (status) { - case STAT_NOOP: return _exec_aline_body(); - case STAT_OK: - ex.section = SECTION_BODY; - ex.section_new = true; - - return STAT_EAGAIN; - - default: return status; - } -} - - -/// Replan current move to execute hold -/// -/// Holds are initiated by the planner entering STATE_STOPPING. In which case -/// _plan_hold() is called to replan the current move towards zero. If it is -/// unable to plan to zero in the remaining length of the current move it will -/// decelerate as much as possible and then wait for the next move. Once it is -/// possible to plan to zero velocity in the current move the remaining length -/// is put into the run buffer, which is still allocated, and the run buffer -/// becomes the hold point. The hold is left by a start request in state.c. At -/// this point the remaining buffers, if any, are replanned from zero up to -/// speed. -static void _plan_hold() { - mp_buffer_t *bf = mp_queue_get_head(); // working buffer pointer - if (!bf) return; // Oops! nothing's running - - // Examine and process current buffer and compute length left for decel - float available_length = - axis_get_vector_length(ex.final_target, mp_runtime_get_position()); - // Velocity left to shed to brake to zero - float braking_velocity = ex.segment_velocity; - // Distance to brake to zero from braking_velocity, bf is OK to use here - float braking_length = mp_get_target_length(braking_velocity, 0, bf->jerk); - - // Hack to prevent Case 2 moves for perfect-fit decels. - if (available_length < braking_length && fp_ZERO(bf->exit_velocity)) - braking_length = available_length; - - // Replan to decelerate - ex.section = SECTION_TAIL; - ex.section_new = true; - ex.cruise_velocity = braking_velocity; - ex.hold_planned = true; - - // Avoid creating segments before or after the hold which are too small. - if (fabs(available_length - braking_length) < HOLD_DECELERATION_TOLERANCE) { - // Case 0: deceleration fits almost exactly - ex.exit_velocity = 0; - ex.tail_length = available_length; - - } else if (braking_length <= available_length) { - // Case 1: deceleration fits entirely into the remaining length - // Setup tail to perform the deceleration - ex.exit_velocity = 0; - ex.tail_length = braking_length; - - // Re-use bf to run the remaining block length - bf->length = available_length - braking_length; - bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); - bf->entry_vmax = 0; - bf->state = BUFFER_RESTART; // Restart the buffer when done - - } else if (HOLD_VELOCITY_TOLERANCE < braking_velocity) { - // Case 2: deceleration exceeds length remaining in buffer - // Replan to minimum (but non-zero) exit velocity - ex.tail_length = available_length; - ex.exit_velocity = - braking_velocity - mp_get_target_velocity(0, available_length, bf); - - } else { // Were're close enough - ex.tail_length = available_length; - ex.exit_velocity = 0; - } - - // Don't error if seek was stopped - bf->flags &= ~BUFFER_SEEK_ERROR; -} - - -/// Initializes move runtime with a new planner buffer -static stat_t _exec_aline_init(mp_buffer_t *bf) { -#ifdef DEBUG - printf("buffer:"); - mp_buffer_print(bf); - putchar('\n'); -#endif - - // Remove zero length lines. Short lines have already been removed. - if (fp_ZERO(bf->length)) return STAT_NOOP; - - // Initialize move - copy_vector(ex.unit, bf->unit); - copy_vector(ex.final_target, bf->target); - - ex.head_length = bf->head_length; - ex.body_length = bf->body_length; - ex.tail_length = bf->tail_length; - ex.entry_velocity = bf->entry_velocity; - ex.cruise_velocity = bf->cruise_velocity; - ex.exit_velocity = bf->exit_velocity; - - // Enforce min cruise velocity - // TODO How does cruise_velocity ever end up zero when length is non-zero? - if (ex.cruise_velocity < 10) ex.cruise_velocity = 10; - - ex.section = SECTION_HEAD; - ex.section_new = true; - ex.hold_planned = false; - - // Generate waypoints for position correction at section ends. This helps - // negate floating point errors in the forward differencing code. - for (int axis = 0; axis < AXES; axis++) { - float position = mp_runtime_get_axis_position(axis); - - ex.waypoint[SECTION_HEAD][axis] = position + ex.unit[axis] * ex.head_length; - ex.waypoint[SECTION_BODY][axis] = position + - ex.unit[axis] * (ex.head_length + ex.body_length); - ex.waypoint[SECTION_TAIL][axis] = ex.final_target[axis]; - } - - return STAT_OK; -} - - -void mp_exec_abort() {ex.abort = true;} - - -/// Aline execution routines -/// -/// Everything here fires from interrupts and must be interrupt safe -/// -/// Returns: -/// -/// STAT_OK move is done -/// STAT_EAGAIN move is not finished - has more segments to run -/// STAT_NOOP cause no stepper operation - do not load the move -/// STAT_xxxxx fatal error. Ends the move and frees the bf buffer -/// -/// This routine is called from the (LO) interrupt level. The interrupt -/// sequencing relies on the correct behavior of these routines. -/// Each call to _exec_aline() must execute and prep *one and only one* -/// segment. If the segment is not the last segment in the bf buffer the -/// _aline() returns STAT_EAGAIN. If it's the last segment it returns -/// STAT_OK. If it encounters a fatal error that would terminate the move it -/// returns a valid error code. -/// -/// Notes: -/// -/// [1] Returning STAT_OK ends the move and frees the bf buffer. -/// Returning STAT_OK at does NOT advance position meaning -/// any position error will be compensated by the next 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 many possible degenerate trapezoids where any of the 5 periods -/// may be zero length but note that either none or both of a ramping pair can -/// be zero. -/// -/// 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 -/// -/// move_time is the actual time of the move, accel_time is the time value -/// needed to compute the velocity taking the initial velocity into account. -/// move_time does not need to. -stat_t mp_exec_aline(mp_buffer_t *bf) { - stat_t status = STAT_OK; - - if (ex.abort) { - ex.abort = false; - mp_runtime_set_velocity(0); // Assume a hard stop - return STAT_NOOP; - } - - // Start a new move - if (bf->state == BUFFER_INIT) { - bf->state = BUFFER_ACTIVE; - status = _exec_aline_init(bf); - if (status != STAT_OK) return status; - } - - // If seeking, end move when switch is in target state. - if ((((bf->flags & BUFFER_SEEK_CLOSE) && switch_is_active(bf->sw)) || - ((bf->flags & BUFFER_SEEK_OPEN) && !switch_is_active(bf->sw))) && - !ex.hold_planned) { - if (!fp_ZERO(mp_runtime_get_velocity())) _plan_hold(); - else { - mp_runtime_set_velocity(0); - bf->flags &= ~BUFFER_SEEK_ERROR; - return STAT_NOOP; - } - } - - // Plan holds - if (mp_get_state() == STATE_STOPPING && !ex.hold_planned) _plan_hold(); - - // Main segment dispatch - switch (ex.section) { - case SECTION_HEAD: status = _exec_aline_head(); break; - case SECTION_BODY: status = _exec_aline_body(); break; - case SECTION_TAIL: status = _exec_aline_tail(); break; - } - - // Exiting - if (status != STAT_EAGAIN) { - // Set runtime velocity on exit - mp_runtime_set_velocity(ex.exit_velocity); - - // If seeking, switch was not found. Signal error if necessary. - if ((bf->flags & (BUFFER_SEEK_CLOSE | BUFFER_SEEK_OPEN)) && - (bf->flags & BUFFER_SEEK_ERROR)) - return STAT_SEEK_SWTICH_NOT_FOUND; - } - - return status; -} - - -/// Dequeues buffers, initializes them, executes their callbacks and cleans up. -/// -/// This is the guts of the planner runtime execution. Because this routine is -/// run in an interrupt the state changes must be carefully ordered. -stat_t mp_exec_move() { - // Check if we can run a buffer - mp_buffer_t *bf = mp_queue_get_head(); - if (mp_get_state() == STATE_ESTOPPED || mp_get_state() == STATE_HOLDING || - !bf) { - mp_runtime_set_velocity(0); - mp_runtime_set_busy(false); - if (mp_get_state() == STATE_STOPPING) mp_state_holding(); - - return STAT_NOOP; // Nothing running - } - - // Process new buffers - if (bf->state == BUFFER_NEW) { - // On restart wait a bit to give planner queue a chance to fill - if (!mp_runtime_is_busy() && mp_queue_get_fill() < PLANNER_EXEC_MIN_FILL && - !rtc_expired(bf->ts + PLANNER_EXEC_DELAY)) return STAT_NOOP; - - // Take control of buffer - bf->state = BUFFER_INIT; - bf->flags &= ~BUFFER_REPLANNABLE; - - // Update runtime - mp_runtime_set_line(bf->line); - } - - // Execute the buffer - stat_t status = bf->cb(bf); - - // Signal that we are busy only if a move was queued. This means that - // nonstop buffers, i.e. non-plan-to-zero commands, will not cause the - // runtime to enter the busy state. This causes mp_exec_move() to continue - // to wait above for the planner buffer to fill when a new stream starts - // with some nonstop buffers. If this weren't so, the code below - // which marks the next buffer not replannable would lock the first move - // buffer and cause it to be unnecessarily planned to zero. - if (status == STAT_EAGAIN || status == STAT_OK) mp_runtime_set_busy(true); - - // Process finished buffers - if (status != STAT_EAGAIN) { - // Signal that we've encountered a stopping point - if (fp_ZERO(mp_runtime_get_velocity()) && - (mp_get_state() == STATE_STOPPING || (bf->flags & BUFFER_HOLD))) - mp_state_holding(); - - // Handle buffer restarts and deallocation - if (bf->state == BUFFER_RESTART) bf->state = BUFFER_NEW; - else { - // Solves a potential race condition where the current buffer ends but - // the new buffer has not started because the current one is still - // being run by the steppers. Planning can overwrite the new buffer. - // See notes above. - mp_buffer_next(bf)->flags &= ~BUFFER_REPLANNABLE; - - mp_queue_pop(); // Release buffer - - // Enter READY state - if (mp_queue_is_empty()) mp_state_idle(); - } - } - - // Convert return status for stepper.c - switch (status) { - case STAT_NOOP: - // Tell caller to call again if there is more in the queue - return mp_queue_is_empty() ? STAT_NOOP : STAT_EAGAIN; - case STAT_EAGAIN: return STAT_OK; // A move was queued, call again later - default: return status; - } -} diff --git a/src/avr/src/plan/exec.h b/src/avr/src/plan/exec.h deleted file mode 100644 index 409f62d..0000000 --- a/src/avr/src/plan/exec.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "buffer.h" - - -stat_t mp_exec_move(); -void mp_exec_abort(); -stat_t mp_exec_aline(mp_buffer_t *bf); diff --git a/src/avr/src/plan/line.c b/src/avr/src/plan/line.c deleted file mode 100644 index 1a0a34c..0000000 --- a/src/avr/src/plan/line.c +++ /dev/null @@ -1,437 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "line.h" - -#include "axis.h" -#include "planner.h" -#include "exec.h" -#include "buffer.h" - -#include -#include - - -/* 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 simplify 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 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(dot(a, b) / (norm(a) * norm(b))); - * 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 = 0; - for (int axis = 0; axis < AXES; axis++) - costheta -= a_unit[axis] * b_unit[axis]; - - if (costheta < -0.99) return FLT_MAX; // straight line cases - if (0.99 < costheta) return 0; // reversal cases - - // Fuse the junction deviations into a vector sum - float a_delta = 0; - float b_delta = 0; - - for (int axis = 0; axis < AXES; axis++) { - a_delta += square(a_unit[axis] * JUNCTION_DEVIATION); - b_delta += square(b_unit[axis] * JUNCTION_DEVIATION); - } - - if (!a_delta || !b_delta) return 0; // One or both unit vectors are null - - float delta = (sqrt(a_delta) + sqrt(b_delta)) / 2; - float sin_half_theta = sqrt((1 - costheta) / 2); - float radius = delta * sin_half_theta / (1 - sin_half_theta); - float velocity = sqrt(radius * JUNCTION_ACCELERATION); - - return velocity; -} - - -/* Find the axis for which the jerk cannot be exceeded - the 'jerk_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. - * - * The math for time-to-decelerate T from speed S to speed E with constant - * jerk J is: - * - * T = 2 * sqrt((S - E) / J) - * - * Since E is always zero in this calculation, we can simplify: - * - * T = 2 * sqrt(S / J) - * - * 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) - * - * Since E is always zero in this calculation, we can simplify: - * - * l = S * sqrt(S / J) - * - * The final value we want to know is 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]. Using 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 - * C[n] = "axis contribution" of axis n - * - * For each axis n: - * - * C[n] = sqrt(1 / J[n]) * D[n] - * - * Keeping in mind that we only need a rank compared to the other axes, we can - * further optimize the calculations: - * - * Square the expression to remove the square root: - * - * C[n]^2 = 1 / J[n] * D[n]^2 - * - * We don't care that C is squared, so we'll use it that way. - */ -int mp_find_jerk_axis(const float axis_square[]) { - float C; - float maxC = 0; - int jerk_axis = 0; - - for (int axis = 0; axis < AXES; axis++) - if (axis_square[axis]) { // Do not use fp_ZERO here - // Squaring axis_length ensures it's positive - C = axis_square[axis] / axis_get_jerk_max(axis); - - if (maxC < C) { - maxC = C; - jerk_axis = axis; - } - } - - return jerk_axis; -} - - -/// Determine jerk value to use for the block. -static float _calc_jerk(const float axis_square[], const float unit[]) { - int axis = mp_find_jerk_axis(axis_square); - - ASSERT(isfinite(unit[axis]) && unit[axis]); - - // Finally, the selected jerk term needs to be scaled by the - // reciprocal of the absolute value of the axis's unit - // vector term. This way when the move is finally decomposed into - // its constituent axes for execution the jerk for that axis will be - // at it's maximum value. - return axis_get_jerk_max(axis) * JERK_MULTIPLIER / fabs(unit[axis]); -} - - -/// Compute cached jerk terms used by planning -static void _calc_and_cache_jerk_values(mp_buffer_t *bf) { - static float jerk = 0; - static float cbrt_jerk = 0; - - if (JERK_MATCH_PRECISION < fabs(bf->jerk - jerk)) { // Tolerance comparison - jerk = bf->jerk; - cbrt_jerk = cbrt(bf->jerk); - } - - bf->cbrt_jerk = cbrt_jerk; -} - - -static void _calc_max_velocities(mp_buffer_t *bf, float move_time, - bool exact_stop) { - ASSERT(0 < move_time && isfinite(move_time)); - - bf->cruise_vmax = bf->length / move_time; // target velocity requested - - float junction_velocity = FLT_MAX; - - mp_buffer_t *prev = mp_buffer_prev(bf); - while (prev->state != BUFFER_OFF) - if (prev->flags & BUFFER_LINE) { - junction_velocity = _get_junction_vmax(prev->unit, bf->unit); - break; - - } else prev = mp_buffer_prev(prev); - - bf->entry_vmax = min(bf->cruise_vmax, junction_velocity); - bf->delta_vmax = mp_get_target_velocity(0, bf->length, bf); - bf->exit_vmax = min(bf->cruise_vmax, (bf->entry_vmax + bf->delta_vmax)); - bf->braking_velocity = bf->delta_vmax; - - // Zero out exact stop cases - if (exact_stop) bf->entry_vmax = bf->exit_vmax = 0; -} - - -/* Compute optimal and minimum move times - * - * "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 rapids are always the minimum time. - * - * 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 - * - * 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 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 float _calc_move_time(const float axis_length[], - const float axis_square[], bool rapid, - bool inverse_time, float feed_rate, - float feed_override) { - ASSERT(0 < feed_override); - float max_time = 0; - - // Compute times for feed motion - if (!rapid) { - if (inverse_time) max_time = feed_rate; - else { - // Compute length of linear move in millimeters. Feed rate in mm/min. - max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + - axis_square[AXIS_Z]) / 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(max_time)) - max_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + - axis_square[AXIS_C]) / feed_rate; - } - } - - // Apply feed override - max_time /= feed_override; - - // Compute time required for rate-limiting axis - for (int axis = 0; axis < AXES; axis++) { - float time = fabs(axis_length[axis]) / axis_get_velocity_max(axis); - if (max_time < time) max_time = time; - } - - return max_time < SEGMENT_TIME ? SEGMENT_TIME : max_time; -} - - -/*** Plan line acceleration / deceleration - * - * This function uses constant jerk motion equations to plan acceleration and - * deceleration. 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. - * - * Notes: - * [1] All math is done in absolute coordinates using single precision floats. - * - * [2] 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. - * - * @param reed_rate is in minutes when @param inverse_time is true. - * See mach_set_feed_rate() - */ -stat_t mp_aline(const float target[], buffer_flags_t flags, switch_id_t sw, - float feed_rate, float feed_override, int32_t line) { - DEBUG_CALL("(%f, %f, %f, %f), %s%s%s, %f, %f, %d", - target[0], target[1], target[2], target[3], - (flags & BUFFER_RAPID) ? "rapid|" : "", - (flags & BUFFER_INVERSE_TIME) ? "inverse_time|" : "", - (flags & BUFFER_EXACT_STOP) ? "exact_stop|" : "", - feed_rate, feed_override, line); - - // Trap zero feed rate condition - if (!(flags & BUFFER_RAPID) && fp_ZERO(feed_rate)) - return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; - - // Compute axis and move lengths - float axis_length[AXES]; - float axis_square[AXES]; - float length_square = 0; - - for (int axis = 0; axis < AXES; axis++) { - axis_length[axis] = target[axis] - mp_get_axis_position(axis); - axis_square[axis] = square(axis_length[axis]); - length_square += axis_square[axis]; - } - - float length = sqrt(length_square); - if (fp_ZERO(length)) return STAT_OK; - - // Get a buffer. Note, new buffers are initialized to zero. - mp_buffer_t *bf = mp_queue_get_tail(); // current move pointer - - // Set buffer values - bf->length = length; - copy_vector(bf->target, target); - - // Compute unit vector - for (int axis = 0; axis < AXES; axis++) - bf->unit[axis] = axis_length[axis] / length; - - // Compute and cache jerk values - bf->jerk = _calc_jerk(axis_square, bf->unit); - _calc_and_cache_jerk_values(bf); - - // Compute move time and velocities - float time = _calc_move_time(axis_length, axis_square, flags & BUFFER_RAPID, - flags & BUFFER_INVERSE_TIME, feed_rate, - feed_override); - _calc_max_velocities(bf, time, flags & BUFFER_EXACT_STOP); - - flags |= BUFFER_LINE; - if (!(flags & BUFFER_EXACT_STOP)) flags |= BUFFER_REPLANNABLE; - - // Note, the following lines must remain in order. - bf->line = line; // Planner needs this when planning steps - bf->flags = flags; // Move flags - bf->sw = sw; // Seek switch, if any - mp_plan(bf); // Plan block list - mp_set_position(target); // Set planner position before pushing buffer - mp_queue_push(mp_exec_aline, line); // After position update - - return STAT_OK; -} diff --git a/src/avr/src/plan/planner.c b/src/avr/src/plan/planner.c deleted file mode 100644 index bf3af97..0000000 --- a/src/avr/src/plan/planner.c +++ /dev/null @@ -1,768 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -/* Planner Notes - * - * The planner works below the 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. - * - * 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 (exec.c). - * - * The Gcode model is owned by the machine and should only be - * accessed by mach_xxxx() functions. Data from the Gcode model is - * transferred to the planner by the mp_xxx() functions called by - * the 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" - -#include "axis.h" -#include "buffer.h" -#include "machine.h" -#include "stepper.h" -#include "motor.h" -#include "state.h" -#include "usart.h" - -#include -#include -#include - - -typedef struct { - float position[AXES]; // final move position for planning purposes - bool plan_steps; // if true plan one GCode line at a time -} planner_t; - - -static planner_t mp = {{0}}; - - -void mp_init() {mp_queue_init();} - - -/// Set planner position for a single axis -void mp_set_axis_position(int axis, float p) {mp.position[axis] = p;} -float mp_get_axis_position(int axis) {return mp.position[axis];} -void mp_set_position(const float p[]) {copy_vector(mp.position, p);} -void mp_set_plan_steps(bool plan_steps) {mp.plan_steps = plan_steps;} - - -/*** Flush all moves in the planner - * - * Does not affect the move currently running. 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 usually - * be directly called. Call mp_request_flush() instead. - */ -void mp_flush_planner() {mp_queue_init();} - - -/*** Performs axis mapping & conversion of length units to steps. - * - * The reason steps are returned as floats (as opposed to, say, - * uint32_t) is to accommodate fractional steps. stepper.c 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 mp_kinematics(const float travel[], float steps[]) { - // You could insert inverse kinematics transformations here - // or just use travel directly for Cartesian machines. - - // Map motors to axes and convert length units to steps - // Most of the conversion math has already been done during config in - // steps_per_unit() which takes axis travel, step angle and microsteps into - // account. - for (int motor = 0; motor < MOTORS; motor++) - if (motor_is_enabled(motor)) - steps[motor] = - travel[motor_get_axis(motor)] * motor_get_steps_per_unit(motor); - else steps[motor] = 0; -} - - -// 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 \ - (SEGMENT_TIME * (bf->cruise_velocity + bf->entry_velocity)) -#define MIN_TAIL_LENGTH \ - (SEGMENT_TIME * (bf->cruise_velocity + bf->exit_velocity)) -#define MIN_BODY_LENGTH (SEGMENT_TIME * bf->cruise_velocity) - - -/*** Calculate move acceleration / deceleration - * - * This rather brute-force and long-ish function sets section lengths and - * velocities based on the line length and velocities requested. It modifies - * the incoming bf buffer and returns accurate head, body and tail lengths, and - * accurate or reasonably approximate velocities. We care about accuracy on - * lengths, less so for velocity, as long as velocity errs on the side of too - * slow. - * - * 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 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. 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 lowered. - * The entry and exit velocities are satisfied. - * - * Degraded-Fit - The move does not have sufficient length to transition from - * the entry velocity to the exit velocity in the available length. These - * velocities are not negotiable, so a degraded solution is found. - * - * In worst cases, the move cannot be executed as the required execution - * time is less than the minimum segment time. The first degradation is to - * reduce the move to a body-only segment with an average velocity. If that - * still doesn't fit then the move velocity is reduced so it fits into a - * minimum segment. This will reduce the velocities in that region of the - * planner buffer as the moves are replanned to that worst-case move. - * - * Various cases handled (H=head, B=body, T=tail) - * - * Requested-Fit cases: - * - * HBT VeVx sufficient length exists for all parts (corner - * case: HBT') - * HB VeVx enter at full speed & decelerate (corner case: T') - * HT Ve & Vx perfect fit HT (very rare). May be symmetric or - * asymmetric - * H VeVx perfect fit T (common, results from planning) - * B Ve=Vt=Vx Velocities are close to each other and within - * matching tolerance - * - * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot: - * - * HT (Ve=Vx)Vx Ve is degraded (velocity step). Vx is met - * B" line is very short but drawable; is treated as a - * body only. - * F force fit: This block is slowed down until it can - * be executed. - * - * Note: The order of the cases/tests in the code is 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. - */ -void mp_calculate_trapezoid(mp_buffer_t *bf) { - if (!bf->length) return; - - // 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). - float naive_move_time = - 2 * bf->length / (bf->entry_velocity + bf->exit_velocity); // average - - if (isfinite(naive_move_time) && naive_move_time < SEGMENT_TIME) { - bf->cruise_velocity = bf->length / SEGMENT_TIME; - bf->exit_velocity = - max(0, min(bf->cruise_velocity, (bf->entry_velocity - bf->delta_vmax))); - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - - // Violating jerk but since it's a single segment move we don't use it. - return; - } - - // B" case: Block is short, but fits into a single body segment - if (isfinite(naive_move_time) && naive_move_time <= SEGMENT_TIME) { - bf->entry_velocity = mp_buffer_prev(bf)->exit_velocity; - - if (!fp_ZERO(bf->entry_velocity)) { - bf->cruise_velocity = bf->entry_velocity; - bf->exit_velocity = bf->entry_velocity; - - } else { - bf->cruise_velocity = bf->delta_vmax / 2; - bf->exit_velocity = bf->delta_vmax; - } - - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - - // Violating jerk 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. - if ((fabs(bf->cruise_velocity - bf->entry_velocity) < - TRAPEZOID_VELOCITY_TOLERANCE) && - (fabs(bf->cruise_velocity - bf->exit_velocity) < - TRAPEZOID_VELOCITY_TOLERANCE)) { - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - - return; - } - - // 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 - bf->body_length = 0; - float minimum_length = - mp_get_target_length(bf->entry_velocity, bf->exit_velocity, bf->jerk); - - // 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; - bf->head_length = 0; - - return; - } - - // 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; - bf->tail_length = 0; - - return; - } - } - - // Set head and tail lengths for evaluating the next cases - bf->head_length = - mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk); - bf->tail_length = - mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk); - if (bf->head_length < MIN_HEAD_LENGTH) bf->head_length = 0; - if (bf->tail_length < MIN_TAIL_LENGTH) bf->tail_length = 0; - - // Rate-limited HT and HT' cases - 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) { - 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)); - - if (bf->head_length < MIN_HEAD_LENGTH) { - // Convert this to a body-only move - bf->body_length = bf->length; - bf->head_length = 0; - bf->tail_length = 0; - - // Average the entry speed and computed best cruise-speed - bf->cruise_velocity = (bf->entry_velocity + bf->cruise_velocity) / 2; - bf->entry_velocity = bf->cruise_velocity; - bf->exit_velocity = bf->cruise_velocity; - } - - return; - } - - // Asymmetric HT' rate-limited case. This is relatively expensive but it's - // not called very often - float computed_velocity = bf->cruise_vmax; - do { - // initialize from previous iteration - bf->cruise_velocity = computed_velocity; - bf->head_length = - mp_get_target_length(bf->entry_velocity, bf->cruise_velocity, bf->jerk); - bf->tail_length = - mp_get_target_length(bf->exit_velocity, bf->cruise_velocity, bf->jerk); - - if (bf->tail_length < bf->head_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); - - } else if (bf->head_length + bf->tail_length) { - 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); - - } else break; - - } while (TRAPEZOID_ITERATION_ERROR_PERCENT < - (fabs(bf->cruise_velocity - computed_velocity) / - computed_velocity)); - - // 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->jerk); - bf->tail_length = bf->length - bf->head_length; - - if (bf->head_length < MIN_HEAD_LENGTH) { - // adjust the move to be all tail... - bf->tail_length = bf->length; - bf->head_length = 0; - } - - if (bf->tail_length < MIN_TAIL_LENGTH) { - bf->head_length = bf->length; //...or all head - bf->tail_length = 0; - } - - return; - } - - // 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 (bf->body_length < MIN_BODY_LENGTH && !fp_ZERO(bf->body_length)) { - if (!fp_ZERO(bf->head_length)) { - if (!fp_ZERO(bf->tail_length)) { // HBT reduces to HT - bf->head_length += bf->body_length / 2; - bf->tail_length += bf->body_length / 2; - - } else bf->head_length += bf->body_length; // HB reduces to H - } else bf->tail_length += bf->body_length; // BT reduces to T - - 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 - } else if (fp_ZERO(bf->head_length) && fp_ZERO(bf->tail_length)) - bf->cruise_velocity = bf->entry_velocity; -} - - -#if 0 -void mp_print_buffer(mp_buffer_t *bp) { - printf_P(PSTR("{\"msg\":\"")); - printf_P(PSTR("%d,"), bp->flags & BUFFER_REPLANNABLE); - printf_P(PSTR("0x%04lx,"), (intptr_t)bp->cb); - printf_P(PSTR("%0.2f,"), bp->length); - printf_P(PSTR("%0.2f,"), bp->head_length); - printf_P(PSTR("%0.2f,"), bp->body_length); - printf_P(PSTR("%0.2f,"), bp->tail_length); - printf_P(PSTR("%0.2f,"), bp->entry_velocity); - printf_P(PSTR("%0.2f,"), bp->cruise_velocity); - printf_P(PSTR("%0.2f,"), bp->exit_velocity); - printf_P(PSTR("%0.2f,"), bp->braking_velocity); - printf_P(PSTR("%0.2f,"), bp->entry_vmax); - printf_P(PSTR("%0.2f,"), bp->cruise_vmax); - printf_P(PSTR("%0.2f"), bp->exit_vmax); - printf_P(PSTR("\"}\n")); - - while (!usart_tx_empty()) continue; -} - - -/// Prints the entire planning queue as comma separated values embedded in -/// JSON ``msg`` entries. Used for debugging. -void mp_print_queue(mp_buffer_t *bf) { - printf_P(PSTR("{\"msg\":\"replannable,callback," - "length,head_length,body_length,tail_length," - "entry_velocity,cruise_velocity,exit_velocity,braking_velocity," - "entry_vmax,cruise_vmax,exit_vmax\"}\n")); - - mp_buffer_t *bp = bf; - while (bp) { - mp_print_buffer(bp); - bp = mp_buffer_prev(bp); - if (bp == bf || bp->state == BUFFER_OFF) break; - } -} -#endif - - -/*** Plans the entire queue - * - * The block list is the circular buffer of planner buffers (bl's). The block - * currently being planned is the "bl" 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. - * - * mp_plan() plans all blocks between and including the (effective) - * first block and the bl. It sets entry, exit and cruise v's from vmax's then - * calls trapezoid generation. - * - * Variables that must be provided in the mp_buffer_t that will be processed: - * - * bl (function arg) - end of block list (last block in time) - * bl->flags - replanable, hold, probe, etc [Note 1] - * bl->length - provides block length - * bl->entry_vmax - used during forward planning to set entry velocity - * bl->cruise_vmax - used during forward planning to set cruise velocity - * bl->exit_vmax - used during forward planning to set exit velocity - * bl->delta_vmax - used during forward planning to set exit velocity - * bl->cbrt_jerk - used during trapezoid generation - * - * Variables that will be set during processing: - * - * bl->flags - replanable - * bl->braking_velocity - set during backward planning - * bl->entry_velocity - set during forward planning - * bl->cruise_velocity - set during forward planning - * bl->exit_velocity - set during forward planning - * bl->head_length - set during trapezoid generation - * bl->body_length - set during trapezoid generation - * bl->tail_length - set during trapezoid generation - * - * Variables that are ignored but here's what you would expect them to be: - * - * bl->state - BUFFER_NEW for all blocks but the earliest - * bl->target[] - block target position - * bl->unit[] - block unit vector - * bl->jerk - source of the other jerk variables. - * - * Notes: - * - * [1] Whether or not a block is planned is controlled by the bl->flags - * BUFFER_REPLANNABLE bit. Replan flags are checked during the backwards - * pass. They prune the replan list to include only 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 (ex) buffer and re-shuffle - * the block list, re-enlisting the current bl 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. - */ -void mp_plan(mp_buffer_t *bf) { - mp_buffer_t *bp = bf; - - // Backward planning pass. Find first block and update braking velocities. - // By the end bp points to the buffer before the first block. - mp_buffer_t *next = bp; - while ((bp = mp_buffer_prev(bp)) != bf) { - if (!(bp->flags & BUFFER_REPLANNABLE)) break; - - // TODO what about non-move buffers? - bp->braking_velocity = - min(next->entry_vmax, next->braking_velocity) + bp->delta_vmax; - - next = bp; - } - - // Forward planning pass. Recompute trapezoids from the first block to bf. - mp_buffer_t *prev = bp; - while ((bp = mp_buffer_next(bp)) != bf) { - mp_buffer_t *next = mp_buffer_next(bp); - - bp->entry_velocity = prev == bf ? bp->entry_vmax : prev->exit_velocity; - bp->cruise_velocity = bp->cruise_vmax; - bp->exit_velocity = min4(bp->exit_vmax, next->entry_vmax, - next->braking_velocity, - bp->entry_velocity + bp->delta_vmax); - - if (mp.plan_steps && bp->line != next->line) { - bp->exit_velocity = 0; - bp->flags |= BUFFER_HOLD; - - } else bp->flags &= ~BUFFER_HOLD; - - mp_calculate_trapezoid(bp); - - // Test for optimally planned trapezoids by checking exit conditions - if ((fp_EQ(bp->exit_velocity, bp->exit_vmax) || - fp_EQ(bp->exit_velocity, next->entry_vmax)) || - (!(prev->flags & BUFFER_REPLANNABLE) && - fp_EQ(bp->exit_velocity, (bp->entry_velocity + bp->delta_vmax)))) - bp->flags &= ~BUFFER_REPLANNABLE; - - prev = bp; - } - - // Finish last block - bf->entry_velocity = prev->exit_velocity; - bf->cruise_velocity = bf->cruise_vmax; - bf->exit_velocity = 0; - - mp_calculate_trapezoid(bf); -} - - -void mp_replan_all() { - ASSERT(mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING); - - // Get next buffer - mp_buffer_t *bf = mp_queue_get_head(); - if (!bf) return; - - mp_buffer_t *bp = bf; - - // Mark all blocks replanable - while (true) { - bp->flags |= BUFFER_REPLANNABLE; - mp_buffer_t *next = mp_buffer_next(bp); - if (next->state == BUFFER_OFF || next == bf) break; // Avoid wrap around - bp = next; - } - - // Plan blocks - mp_plan(bp); -} - - -/// Push a non-stop command to the queue. I.e. one that does not cause the -/// planner to plan to zero. -void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line) { - mp_buffer_t *bp = mp_queue_get_tail(); - - bp->entry_vmax = bp->cruise_vmax = bp->exit_vmax = INFINITY; - bp->flags |= BUFFER_REPLANNABLE; - - mp_queue_push(cb, line); -} - - -/*** Derive accel/decel length from delta V and jerk - * - * A convenient function for determining the optimal_length (L) of a line - * given the initial velocity (Vi), final velocity (Vf) and maximum jerk (Jm). - * - * Definitions: - * - * Jm = the given maximum jerk - * T = time of the entire move - * Vi = initial velocity - * Vf = final velocity - * T = time - * As = accel at inflection point between convex & concave portions of S-curve - * Ar = ramp acceleration - * - * Formulas: - * - * T = 2 * sqrt((Vt - Vi) / Jm) - * As = (Jm * T) / 2 - * Ar = As / 2 = (Jm * T) / 4 - * - * Then the length (distance) equation is: - * - * a) L = (Vf - Vi) * T - (Ar * T^2) / 2 - * - * Substituting for Ar and T: - * - * b) L = (Vf - Vi) * 2 * sqrt((Vf - Vi) / Jm) - - * (2 * sqrt((Vf - Vi) / Jm) * (Vf - Vi)) / 2 - * - * Reducing b). See Wolfram Alpha: - * - * c) L = (Vf - Vi)^(3/2) / sqrt(Jm) - * - * Assuming Vf >= Vi [Note 2]: - * - * d) L = (Vf - Vi) * sqrt((Vf - Vi) / Jm) - * - * Notes: - * - * [1] Assuming Vi, Vf and L are positive or zero. - * [2] Cannot assume Vf >= Vi due to rounding errors and use of - * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs() - */ -float mp_get_target_length(float Vi, float Vf, float jerk) { - ASSERT(0 <= Vi); - ASSERT(0 <= Vf); - ASSERT(isfinite(jerk)); - return fp_EQ(Vi, Vf) ? 0 : fabs(Vi - Vf) * invsqrt(jerk / fabs(Vi - Vf)); -} - - -#define GET_VELOCITY_ITERATIONS 0 // must be zero or more - -/*** Derive velocity achievable from delta V and length - * - * A convenient function for determining Vf target velocity for a given - * initial velocity (Vi), length (L), and maximum jerk (Jm). Equation e) is - * b) solved for Vf. Equation f) is equation c) solved for Vf. We use f) since - * it is more simple. - * - * e) Vf = (sqrt(L) * (L / sqrt(1 / Jm))^(1/6) + - * (1 / Jm)^(1/4) * Vi) / (1 / Jm)^(1/4) - * - * f) Vf = L^(2/3) * Jm^(1/3) + Vi - * - * FYI: Here's an expression that returns the jerk for a given deltaV and L: - * - * cube(deltaV / (pow(L, 0.66666666))); - * - * 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. - * - * 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) - * - * Replacing Vf with x, and subtracting the known L we get: - * - * 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)^2) / L^2 - * - * Replacing Vf with x, and subtracting the known J we get: - * - * 0 = ((x - Vi) * (Vi + x)^2) / L^2 - J - * Z(x) = ((x - Vi) * (Vi + x)^2) / L^2 - J - * - * L doesn't resolve to the value very quickly (its graph is nearly 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: - * - * J > 0, Vi > 0, Vf > 0 - * A = sqrt((x - Vi) * J) - * B = sqrt((x - Vi) / J) - * - * L'(x) = B + (Vi + x) / (2 * J) + (Vi + x) / (2 * A) - * - * J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2 - */ -float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) { - ASSERT(0 <= Vi); - ASSERT(0 <= L); - ASSERT(isfinite(bf->jerk)); - ASSERT(isfinite(bf->cbrt_jerk)); - - if (!L) return Vi; - - // 0 iterations (a reasonable estimate) - float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate - -#if GET_VELOCITY_ITERATIONS - const float L2 = L * L; - const float Vi2 = Vi * Vi; - - for (int i = 0; i < GET_VELOCITY_ITERATIONS; i++) - // x' = x - Z(x) / J'(x) - x = x - ((x - Vi) * square(Vi + x) / L2 - bf->jerk) / - ((2 * Vi * x - Vi2 + 3 * x * x) / L2); -#endif - - return x; -} diff --git a/src/avr/src/plan/planner.h b/src/avr/src/plan/planner.h deleted file mode 100644 index bdc2c35..0000000 --- a/src/avr/src/plan/planner.h +++ /dev/null @@ -1,86 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2013 - 2015 Alden S. Hart, Jr. - Copyright (c) 2013 - 2015 Robert Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - - -#include "machine.h" // used for gcode_state_t -#include "buffer.h" -#include "util.h" -#include "config.h" - -#include - - -// Most of these factors are the result of a lot of tweaking. -// Change with caution. -#define JERK_MULTIPLIER 1000000.0 -#define JERK_MATCH_PRECISION 1000.0 // jerk precision to be considered same - -/// Error percentage for iteration convergence. As percent - 0.01 = 1% -#define TRAPEZOID_ITERATION_ERROR_PERCENT 0.1 - -/// Adaptive velocity tolerance term -#define TRAPEZOID_VELOCITY_TOLERANCE (max(2, bf->entry_velocity / 100)) - -/*** If the absolute value of the remaining deceleration length would be less - * than this value then finish the deceleration in the current move. This is - * used to avoid creating segements before or after the hold which are too - * short to process correctly. - */ -#define HOLD_DECELERATION_TOLERANCE 1 // In mm -#define HOLD_VELOCITY_TOLERANCE 60 // In mm/min - - -typedef enum { - SECTION_HEAD, // acceleration - SECTION_BODY, // cruise - SECTION_TAIL, // deceleration -} move_section_t; - - -void mp_init(); - -void mp_set_axis_position(int axis, float position); -float mp_get_axis_position(int axis); -void mp_set_position(const float p[]); -void mp_set_plan_steps(bool plan_steps); - -void mp_flush_planner(); -void mp_kinematics(const float travel[], float steps[]); - -void mp_print_buffer(mp_buffer_t *bp); - -void mp_plan(mp_buffer_t *bf); -void mp_replan_all(); - -void mp_queue_push_nonstop(buffer_cb_t cb, uint32_t line); - -float mp_get_target_length(float Vi, float Vf, float jerk); -float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf); diff --git a/src/avr/src/plan/runtime.c b/src/avr/src/plan/runtime.c deleted file mode 100644 index a9c1bf1..0000000 --- a/src/avr/src/plan/runtime.c +++ /dev/null @@ -1,171 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "planner.h" -#include "buffer.h" -#include "stepper.h" -#include "motor.h" -#include "util.h" -#include "report.h" -#include "state.h" -#include "config.h" - -#include -#include -#include -#include - - -typedef struct { - bool busy; // True if a move is running - float position[AXES]; // Current move position - float work_offset[AXES]; // Current move work offset - float velocity; // Current move velocity - - int32_t line; // Current move GCode line number - uint8_t tool; // Active tool - -#if 0 // TODO These are not currently being set - float feed; - feed_mode_t feed_mode; - float feed_override; - float spindle_override; - - plane_t plane; - units_t units; - coord_system_t coord_system; - bool absolute_mode; - path_mode_t path_mode; - distance_mode_t distance_mode; - distance_mode_t arc_distance_mode; -#endif -} mp_runtime_t; - -static mp_runtime_t rt = {0}; - - -bool mp_runtime_is_busy() {return rt.busy;} -void mp_runtime_set_busy(bool busy) {rt.busy = busy;} -int32_t mp_runtime_get_line() {return rt.line;} -void mp_runtime_set_line(int32_t line) {rt.line = line; report_request();} -uint8_t mp_runtime_get_tool() {return rt.tool;} -void mp_runtime_set_tool(uint8_t tool) {rt.tool = tool; report_request();} - - -/// Returns current segment velocity -float mp_runtime_get_velocity() {return rt.velocity;} - - -void mp_runtime_set_velocity(float velocity) { - rt.velocity = velocity; - report_request(); -} - - -/// Set encoder counts to the runtime position -void mp_runtime_set_steps_from_position() { - // Convert lengths to steps in floating point - float steps[MOTORS]; - mp_kinematics(rt.position, steps); - - for (int motor = 0; motor < MOTORS; motor++) - // Write steps to encoder register - motor_set_position(motor, steps[motor]); -} - - -/* 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: - * - * - mp_position - start and end position for planning - * - rt.position - current position of runtime segment - * - rt.steps.* - position in steps - * - * Note that position is set immediately when called and may not be - * 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 runtime position for a single axis -void mp_runtime_set_axis_position(uint8_t axis, float position) { - rt.position[axis] = position; - report_request(); -} - - -/// Returns current axis position in machine coordinates -float mp_runtime_get_axis_position(uint8_t axis) {return rt.position[axis];} -float *mp_runtime_get_position() {return rt.position;} - - -void mp_runtime_set_position(const float position[]) { - copy_vector(rt.position, position); - report_request(); -} - - -/// Returns axis position in work coordinates that were in effect at plan time -float mp_runtime_get_work_position(uint8_t axis) { - return rt.position[axis] - rt.work_offset[axis]; -} - - -/// Set offsets -void mp_runtime_set_work_offsets(float offset[]) { - copy_vector(rt.work_offset, offset); -} - - -/// Segment runner -stat_t mp_runtime_move_to_target(float time, const float target[]) { - ASSERT(isfinite(target[0]) && isfinite(target[1]) && - isfinite(target[2]) && isfinite(target[3])); - - // Convert target position to steps. - float steps[MOTORS]; - mp_kinematics(target, steps); - - // Call the stepper prep function - RITORNO(st_prep_line(time, steps)); - - // Update positions - mp_runtime_set_position(target); - - return STAT_OK; -} diff --git a/src/avr/src/plan/runtime.h b/src/avr/src/plan/runtime.h deleted file mode 100644 index a5c0fc2..0000000 --- a/src/avr/src/plan/runtime.h +++ /dev/null @@ -1,58 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -#include "status.h" - -#include - - -bool mp_runtime_is_busy(); -void mp_runtime_set_busy(bool busy); - -int32_t mp_runtime_get_line(); -void mp_runtime_set_line(int32_t line); - -uint8_t mp_runtime_get_tool(); -void mp_runtime_set_tool(uint8_t tool); - -float mp_runtime_get_velocity(); -void mp_runtime_set_velocity(float velocity); - -void mp_runtime_set_steps_from_position(); - -void mp_runtime_set_axis_position(uint8_t axis, float position); -float mp_runtime_get_axis_position(uint8_t axis); - -float *mp_runtime_get_position(); -void mp_runtime_set_position(const float position[]); - -float mp_runtime_get_work_position(uint8_t axis); -void mp_runtime_set_work_offsets(float offset[]); - -stat_t mp_runtime_move_to_target(float time, const float target[]); diff --git a/src/avr/src/plan/velocity_curve.c b/src/avr/src/plan/velocity_curve.c deleted file mode 100644 index 7ef1009..0000000 --- a/src/avr/src/plan/velocity_curve.c +++ /dev/null @@ -1,75 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "velocity_curve.h" - -#include - - -/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity -/// curve. This yields a constant pop; with pop being the sixth derivative -/// of position: -/// -/// 1st - velocity -/// 2nd - acceleration -/// 3rd - jerk -/// 4th - snap -/// 5th - crackle -/// 6th - pop -/// -/// The Bezier curve takes the form: -/// -/// f(t) = P_0(1 - t)^5 + 5P_1(1 - t)^4 t + 10P_2(1 - t)^3 t^2 + -/// 10P_3(1 - t)^2 t^3 + 5P_4(1 - t)t^4 + P_5t^5 -/// -/// Where 0 <= t <= 1, f(t) is the velocity and P_0 through P_5 are the control -/// points. In our case: -/// -/// P_0 = P_1 = P2 = Vi -/// P_3 = P_4 = P5 = Vt -/// -/// Where Vi is the initial velocity and Vt is the target velocity. -/// -/// After substitution, expanding the polynomial and collecting terms we have: -/// -/// f(t) = (Vt - Vi)(6t^5 - 15t^4 + 10t^3) + Vi -/// -/// Computing this directly using 32bit float-point on a 32MHz AtXMega processor -/// takes about 60uS or about 1,920 clocks. The code was compiled with avr-gcc -/// v4.9.2 with -O3. -float velocity_curve(float Vi, float Vt, float t) { - // If the change is small enough just do a linear velocity transition. - // TODO revisit this. - if (fabs(Vt - Vi) < 200) return Vi + (Vt - Vi) * t; - - const float t2 = t * t; - const float t3 = t2 * t; - - return (Vt - Vi) * (6 * t2 - 15 * t + 10) * t3 + Vi; -} diff --git a/src/avr/src/plan/velocity_curve.h b/src/avr/src/plan/velocity_curve.h deleted file mode 100644 index 8d1e497..0000000 --- a/src/avr/src/plan/velocity_curve.h +++ /dev/null @@ -1,32 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - Copyright (c) 2010 - 2015 Alden S. Hart, Jr. - Copyright (c) 2012 - 2015 Rob Giseburt - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#pragma once - -float velocity_curve(float Vi, float Vt, float t); diff --git a/src/avr/src/pwm_spindle.c b/src/avr/src/pwm_spindle.c index 305251a..dc9a30e 100644 --- a/src/avr/src/pwm_spindle.c +++ b/src/avr/src/pwm_spindle.c @@ -32,6 +32,8 @@ #include "estop.h" #include "outputs.h" +#include + typedef struct { uint16_t freq; // base frequency for PWM driver, in Hz @@ -56,13 +58,14 @@ static void _set_dir(bool clockwise) { } -static void _set_pwm(spindle_mode_t mode, float speed) { - if (mode == SPINDLE_OFF || speed < spindle.min_rpm || estop_triggered()) { +static void _set_pwm(float speed) { + if (speed < spindle.min_rpm || estop_triggered()) { TIMER_PWM.CTRLA = 0; OUTCLR_PIN(SPIN_PWM_PIN); _set_enable(false); return; } + _set_enable(true); // Invert PWM if (spindle.pwm_invert) PINCTRL_PIN(SPIN_PWM_PIN) |= PORT_INVEN_bm; @@ -122,14 +125,13 @@ void pwm_spindle_init() { } -void pwm_spindle_set(spindle_mode_t mode, float speed) { - if (mode != SPINDLE_OFF) _set_dir(mode == SPINDLE_CW); - _set_pwm(mode, speed); - _set_enable(mode != SPINDLE_OFF); +void pwm_spindle_set(float speed) { + if (speed) _set_dir(0 < speed); + _set_pwm(fabs(speed)); } -void pwm_spindle_stop() {pwm_spindle_set(SPINDLE_OFF, 0);} +void pwm_spindle_stop() {pwm_spindle_set(0);} // TODO these need more effort and should work with the huanyang spindle too diff --git a/src/avr/src/pwm_spindle.h b/src/avr/src/pwm_spindle.h index 10006a6..cd38bb1 100644 --- a/src/avr/src/pwm_spindle.h +++ b/src/avr/src/pwm_spindle.h @@ -27,9 +27,9 @@ #pragma once -#include "machine.h" +#include "spindle.h" void pwm_spindle_init(); -void pwm_spindle_set(spindle_mode_t mode, float speed); +void pwm_spindle_set(float speed); void pwm_spindle_stop(); diff --git a/src/avr/src/queue.c b/src/avr/src/queue.c new file mode 100644 index 0000000..7a350bc --- /dev/null +++ b/src/avr/src/queue.c @@ -0,0 +1,103 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "queue.h" + +#include + + +typedef struct { + action_t action; + + union { + float f; + int32_t i; + bool b; + struct { + int16_t l; + int16_t r; + } p; + } value; +} buffer_t; + + +#define RING_BUF_NAME q +#define RING_BUF_TYPE buffer_t +#define RING_BUF_SIZE QUEUE_SIZE +#include "ringbuf.def" + + +void queue_init() { + q_init(); +} + + +void queue_flush() {q_init();} +bool queue_is_empty() {return q_empty();} +int queue_get_room() {return q_space();} +int queue_get_fill() {return q_fill();} + + +void queue_push(action_t action) { + buffer_t b = {action}; + q_push(b); +} + + +void queue_push_float(action_t action, float value) { + buffer_t b = {action, {.f = value}}; + q_push(b); +} + + +void queue_push_int(action_t action, int32_t value) { + buffer_t b = {action, {.i = value}}; + q_push(b); +} + + +void queue_push_bool(action_t action, bool value) { + buffer_t b = {action, {.b = value}}; + q_push(b); +} + + +void queue_push_pair(action_t action, int16_t left, int16_t right) { + buffer_t b = {action}; + b.value.p.l = left; + b.value.p.r = right; + q_push(b); +} + + +void queue_pop() {q_pop();} +action_t queue_head() {return q_peek().action;} +float queue_head_float() {return q_peek().value.f;} +int32_t queue_head_int() {return q_peek().value.i;} +bool queue_head_bool() {return q_peek().value.b;} +int16_t queue_head_left() {return q_peek().value.p.l;} +int16_t queue_head_right() {return q_peek().value.p.r;} diff --git a/src/avr/src/plan/calibrate.h b/src/avr/src/queue.h similarity index 69% rename from src/avr/src/plan/calibrate.h rename to src/avr/src/queue.h index 00a9190..f375135 100644 --- a/src/avr/src/plan/calibrate.h +++ b/src/avr/src/queue.h @@ -27,9 +27,30 @@ #pragma once -#include +#include "config.h" +#include "action.h" + #include +#include + + +// Called by state.c +void queue_flush(); +bool queue_is_empty(); +int queue_get_room(); +int queue_get_fill(); + +void queue_push(action_t action); +void queue_push_float(action_t action, float value); +void queue_push_int(action_t action, int32_t value); +void queue_push_bool(action_t action, bool value); +void queue_push_pair(action_t action, int16_t left, int16_t right); +void queue_pop(); -bool calibrate_busy(); -void calibrate_set_stallguard(int motor, uint16_t sg); +action_t queue_head(); +float queue_head_float(); +int32_t queue_head_int(); +bool queue_head_bool(); +int16_t queue_head_left(); +int16_t queue_head_right(); diff --git a/src/avr/src/ringbuf.def b/src/avr/src/ringbuf.def index 5228fd0..40f57cb 100644 --- a/src/avr/src/ringbuf.def +++ b/src/avr/src/ringbuf.def @@ -89,13 +89,15 @@ typedef struct { RING_BUF_TYPE buf[RING_BUF_SIZE]; volatile RING_BUF_INDEX_TYPE head; volatile RING_BUF_INDEX_TYPE tail; + volatile RING_BUF_INDEX_TYPE tran_tail; + volatile bool transaction; } RING_BUF_STRUCT; static RING_BUF_STRUCT RING_BUF; RING_BUF_FUNC void CONCAT(RING_BUF_NAME, _init)() { - RING_BUF.head = RING_BUF.tail = 0; + RING_BUF.head = RING_BUF.tail = RING_BUF.tran_tail = RING_BUF.transaction = 0; } diff --git a/src/avr/src/spindle.c b/src/avr/src/spindle.c index d6ae802..2349895 100644 --- a/src/avr/src/spindle.c +++ b/src/avr/src/spindle.c @@ -31,6 +31,7 @@ #include "config.h" #include "pwm_spindle.h" #include "huanyang.h" +#include "pgmspace.h" typedef enum { @@ -41,7 +42,6 @@ typedef enum { typedef struct { spindle_type_t type; - spindle_mode_t mode; float speed; bool reversed; } spindle_t; @@ -56,29 +56,19 @@ void spindle_init() { } -void _spindle_set(spindle_mode_t mode, float speed) { - if (speed < 0) speed = 0; - if (mode != SPINDLE_CW && mode != SPINDLE_CCW) mode = SPINDLE_OFF; - - spindle.mode = mode; +void spindle_set_speed(float speed) { spindle.speed = speed; - if (spindle.reversed) { - if (mode == SPINDLE_CW) mode = SPINDLE_CCW; - else if (mode == SPINDLE_CCW) mode = SPINDLE_CW; - } - switch (spindle.type) { - case SPINDLE_TYPE_PWM: pwm_spindle_set(mode, speed); break; - case SPINDLE_TYPE_HUANYANG: huanyang_set(mode, speed); break; + case SPINDLE_TYPE_PWM: pwm_spindle_set(spindle_get_speed()); break; + case SPINDLE_TYPE_HUANYANG: huanyang_set(spindle_get_speed()); break; } } -void spindle_set_mode(spindle_mode_t mode) {_spindle_set(mode, spindle.speed);} -void spindle_set_speed(float speed) {_spindle_set(spindle.mode, speed);} -spindle_mode_t spindle_get_mode() {return spindle.mode;} -float spindle_get_speed() {return spindle.speed;} +float spindle_get_speed() { + return spindle.reversed ? -spindle.speed : spindle.speed; +} void spindle_stop() { @@ -94,12 +84,11 @@ uint8_t get_spindle_type() {return spindle.type;} void set_spindle_type(uint8_t value) { if (value != spindle.type) { - spindle_mode_t mode = spindle.mode; float speed = spindle.speed; - _spindle_set(SPINDLE_OFF, 0); + spindle_set_speed(0); spindle.type = value; - _spindle_set(mode, speed); + spindle_set_speed(speed); } } @@ -109,16 +98,13 @@ bool get_spin_reversed() {return spindle.reversed;} void set_spin_reversed(bool reversed) { - spindle.reversed = reversed; - _spindle_set(spindle.mode, spindle.speed); + if (spindle.reversed != reversed) { + spindle.reversed = reversed; + spindle_set_speed(-spindle.speed); + } } -PGM_P get_spin_mode() { - switch (spindle.mode) { - case SPINDLE_CW: return PSTR("Clockwise"); - case SPINDLE_CCW: return PSTR("Counterclockwise"); - default: break; - } - return PSTR("Off"); -} +// Var callbacks +void set_speed(float speed) {spindle_set_speed(speed);} +float get_speed() {return spindle_get_speed();} diff --git a/src/avr/src/spindle.h b/src/avr/src/spindle.h index 9dbe5a5..102753b 100644 --- a/src/avr/src/spindle.h +++ b/src/avr/src/spindle.h @@ -28,13 +28,10 @@ #pragma once -#include "machine.h" - +#include void spindle_init(); -void spindle_set_mode(spindle_mode_t mode); void spindle_set_speed(float speed); -spindle_mode_t spindle_get_mode(); float spindle_get_speed(); void spindle_stop(); bool spindle_is_reversed(); diff --git a/src/avr/src/plan/state.c b/src/avr/src/state.c similarity index 52% rename from src/avr/src/plan/state.c rename to src/avr/src/state.c index 29c161b..742df89 100644 --- a/src/avr/src/plan/state.c +++ b/src/avr/src/state.c @@ -28,23 +28,20 @@ \******************************************************************************/ #include "state.h" -#include "machine.h" -#include "planner.h" -#include "runtime.h" -#include "buffer.h" -#include "arc.h" + +#include "exec.h" +#include "queue.h" #include "stepper.h" #include "spindle.h" - #include "report.h" #include -typedef struct { - mp_state_t state; - mp_cycle_t cycle; - mp_hold_reason_t hold_reason; +static struct { + state_t state; + cycle_t cycle; + hold_reason_t hold_reason; bool pause; bool hold_requested; @@ -52,15 +49,13 @@ typedef struct { bool start_requested; bool resume_requested; bool optional_pause_requested; -} planner_state_t; - -static planner_state_t ps = { +} s = { .flush_requested = true, // Start out flushing }; -PGM_P mp_get_state_pgmstr(mp_state_t state) { +PGM_P state_get_pgmstr(state_t state) { switch (state) { case STATE_READY: return PSTR("READY"); case STATE_ESTOPPED: return PSTR("ESTOPPED"); @@ -73,12 +68,11 @@ PGM_P mp_get_state_pgmstr(mp_state_t state) { } -PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle) { +PGM_P state_get_cycle_pgmstr(cycle_t cycle) { switch (cycle) { case CYCLE_MACHINING: return PSTR("MACHINING"); case CYCLE_HOMING: return PSTR("HOMING"); case CYCLE_PROBING: return PSTR("PROBING"); - case CYCLE_CALIBRATING: return PSTR("CALIBRATING"); case CYCLE_JOGGING: return PSTR("JOGGING"); } @@ -86,7 +80,7 @@ PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle) { } -PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason) { +PGM_P state_get_hold_reason_pgmstr(hold_reason_t reason) { switch (reason) { case HOLD_REASON_USER_PAUSE: return PSTR("USER"); case HOLD_REASON_PROGRAM_PAUSE: return PSTR("PROGRAM"); @@ -99,110 +93,116 @@ PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason) { } -mp_state_t mp_get_state() {return ps.state;} -mp_cycle_t mp_get_cycle() {return ps.cycle;} +state_t state_get() {return s.state;} +cycle_t state_get_cycle() {return s.cycle;} -static void _set_state(mp_state_t state) { - if (ps.state == state) return; // No change - if (ps.state == STATE_ESTOPPED) return; // Can't leave EStop state - if (state == STATE_READY) mp_runtime_set_line(0); - ps.state = state; +static void _set_state(state_t state) { + if (s.state == state) return; // No change + if (s.state == STATE_ESTOPPED) return; // Can't leave EStop state + if (state == STATE_READY) exec_set_line(0); + s.state = state; report_request(); } -void mp_set_cycle(mp_cycle_t cycle) { - if (ps.cycle == cycle) return; // No change +void state_set_cycle(cycle_t cycle) { + if (s.cycle == cycle) return; // No change - if (ps.state != STATE_READY && cycle != CYCLE_MACHINING) { + if (s.state != STATE_READY && cycle != CYCLE_MACHINING) { STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S", - mp_get_cycle_pgmstr(cycle), - mp_get_state_pgmstr(ps.state)); + state_get_cycle_pgmstr(cycle), + state_get_pgmstr(s.state)); return; } - if (ps.cycle != CYCLE_MACHINING && cycle != CYCLE_MACHINING) { + if (s.cycle != CYCLE_MACHINING && cycle != CYCLE_MACHINING) { STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to cycle %S while in %S", - mp_get_cycle_pgmstr(cycle), - mp_get_cycle_pgmstr(ps.cycle)); + state_get_cycle_pgmstr(cycle), + state_get_cycle_pgmstr(s.cycle)); return; } - ps.cycle = cycle; + s.cycle = cycle; report_request(); } -mp_hold_reason_t mp_get_hold_reason() {return ps.hold_reason;} - - -void mp_set_hold_reason(mp_hold_reason_t reason) { - if (ps.hold_reason == reason) return; // No change - ps.hold_reason = reason; +void state_set_hold_reason(hold_reason_t reason) { + if (s.hold_reason == reason) return; // No change + s.hold_reason = reason; report_request(); } -bool mp_is_flushing() {return ps.flush_requested && !ps.resume_requested;} -bool mp_is_resuming() {return ps.resume_requested;} +bool state_is_flushing() {return s.flush_requested && !s.resume_requested;} +bool state_is_resuming() {return s.resume_requested;} -bool mp_is_quiescent() { - return (mp_get_state() == STATE_READY || mp_get_state() == STATE_HOLDING) && - !st_is_busy() && !mp_runtime_is_busy(); +bool state_is_quiescent() { + return (state_get() == STATE_READY || state_get() == STATE_HOLDING) && + !st_is_busy() && !exec_is_busy(); } -bool mp_is_ready() { - return mp_queue_get_room() && !mp_is_resuming() && !ps.pause; +bool state_is_ready() { + return queue_get_room() && !state_is_resuming() && !s.pause; } -void mp_pause_queue(bool x) {ps.pause = x;} +void state_pause_queue(bool x) {s.pause = x;} -void mp_state_optional_pause() { - if (ps.optional_pause_requested) { - mp_set_hold_reason(HOLD_REASON_USER_PAUSE); - mp_state_holding(); +void state_optional_pause() { + if (s.optional_pause_requested) { + state_set_hold_reason(HOLD_REASON_USER_PAUSE); + state_holding(); } } -void mp_state_holding() { +static void _set_plan_steps(bool plan_steps) {} // TODO + + +void state_holding() { _set_state(STATE_HOLDING); - mp_set_plan_steps(false); + _set_plan_steps(false); } -void mp_state_running() { - if (mp_get_state() == STATE_READY) _set_state(STATE_RUNNING); +void state_running() { + if (state_get() == STATE_READY) _set_state(STATE_RUNNING); } -void mp_state_idle() { - if (mp_get_state() == STATE_RUNNING) _set_state(STATE_READY); +void state_idle() { + if (state_get() == STATE_RUNNING) _set_state(STATE_READY); } -void mp_state_estop() { +void state_estop() { _set_state(STATE_ESTOPPED); - mp_pause_queue(false); + state_pause_queue(false); } -void mp_request_hold() {ps.hold_requested = true;} -void mp_request_start() {ps.start_requested = true;} -void mp_request_flush() {ps.flush_requested = true;} -void mp_request_resume() {if (ps.flush_requested) ps.resume_requested = true;} -void mp_request_optional_pause() {ps.optional_pause_requested = true;} +void state_request_hold() {s.hold_requested = true;} +void state_request_start() {s.start_requested = true;} +void state_request_flush() {s.flush_requested = true;} -void mp_request_step() { - mp_set_plan_steps(true); - ps.start_requested = true; +void state_request_resume() { + if (s.flush_requested) s.resume_requested = true; +} + + +void state_request_optional_pause() {s.optional_pause_requested = true;} + + +void state_request_step() { + _set_plan_steps(true); + s.start_requested = true; } @@ -218,60 +218,54 @@ void mp_request_step() { * - during motion is ignored but not reset * - during a feedhold is deferred until the feedhold enters HOLDING state. * I.e. until deceleration is complete. - * - when stopped or holding and the planner is not busy, is honored + * - when stopped or holding and the exec is not busy, is honored * * A start request received: * - during motion is ignored and reset * - during a feedhold is deferred until the feedhold enters HOLDING state. * I.e. until deceleration is complete. If a queue flush request is also * present the queue flush is done first - * - when stopped is honored and starts to run anything in the planner queue + * - when stopped is honored and starts to run anything in the queue */ -void mp_state_callback() { - if (ps.hold_requested || ps.flush_requested) { - ps.hold_requested = false; - mp_set_hold_reason(HOLD_REASON_USER_PAUSE); +void state_callback() { + if (s.hold_requested || s.flush_requested) { + s.hold_requested = false; + state_set_hold_reason(HOLD_REASON_USER_PAUSE); - if (mp_get_state() == STATE_RUNNING) _set_state(STATE_STOPPING); + if (state_get() == STATE_RUNNING) _set_state(STATE_STOPPING); } // Only flush queue when idle or holding. - if (ps.flush_requested && mp_is_quiescent()) { - mach_abort_arc(); - - if (!mp_queue_is_empty()) { - mp_flush_planner(); + if (s.flush_requested && state_is_quiescent()) { - // NOTE The following uses low-level mp calls for absolute position. - // Reset to actual machine position. Otherwise machine is set to the - // position of the last queued move. - mach_set_position_from_runtime(); - } + if (!queue_is_empty()) queue_flush(); // Stop spindle spindle_stop(); // Resume - if (ps.resume_requested) { - ps.flush_requested = ps.resume_requested = false; + if (s.resume_requested) { + s.flush_requested = s.resume_requested = false; _set_state(STATE_READY); } } // Don't start while flushing or stopping - if (ps.start_requested && !ps.flush_requested && - mp_get_state() != STATE_STOPPING) { - ps.start_requested = false; - ps.optional_pause_requested = false; + if (s.start_requested && !s.flush_requested && + state_get() != STATE_STOPPING) { + s.start_requested = false; + s.optional_pause_requested = false; - if (mp_get_state() == STATE_HOLDING) { + if (state_get() == STATE_HOLDING) { // Check if any moves are buffered - if (!mp_queue_is_empty()) { - // Always replan when coming out of a hold - mp_replan_all(); - _set_state(STATE_RUNNING); - - } else _set_state(STATE_READY); + if (!queue_is_empty()) _set_state(STATE_RUNNING); + else _set_state(STATE_READY); } } } + + +// Var callbacks +PGM_P get_state() {return state_get_pgmstr(state_get());} +PGM_P get_cycle() {return state_get_cycle_pgmstr(state_get_cycle());} +PGM_P get_hold_reason() {return state_get_hold_reason_pgmstr(s.hold_reason);} diff --git a/src/avr/src/plan/state.h b/src/avr/src/state.h similarity index 68% rename from src/avr/src/plan/state.h rename to src/avr/src/state.h index a287c42..5d42cc5 100644 --- a/src/avr/src/plan/state.h +++ b/src/avr/src/state.h @@ -40,16 +40,15 @@ typedef enum { STATE_RUNNING, STATE_STOPPING, STATE_HOLDING, -} mp_state_t; +} state_t; typedef enum { CYCLE_MACHINING, CYCLE_HOMING, CYCLE_PROBING, - CYCLE_CALIBRATING, CYCLE_JOGGING, -} mp_cycle_t; +} cycle_t; typedef enum { @@ -58,38 +57,35 @@ typedef enum { HOLD_REASON_PROGRAM_END, HOLD_REASON_PALLET_CHANGE, HOLD_REASON_TOOL_CHANGE, -} mp_hold_reason_t; - - -PGM_P mp_get_state_pgmstr(mp_state_t state); -PGM_P mp_get_cycle_pgmstr(mp_cycle_t cycle); -PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason); - -mp_state_t mp_get_state(); - -mp_cycle_t mp_get_cycle(); -void mp_set_cycle(mp_cycle_t cycle); - -mp_hold_reason_t mp_get_hold_reason(); -void mp_set_hold_reason(mp_hold_reason_t reason); - -bool mp_is_flushing(); -bool mp_is_resuming(); -bool mp_is_quiescent(); -bool mp_is_ready(); -void mp_pause_queue(bool x); - -void mp_state_optional_pause(); -void mp_state_holding(); -void mp_state_running(); -void mp_state_idle(); -void mp_state_estop(); - -void mp_request_hold(); -void mp_request_start(); -void mp_request_flush(); -void mp_request_resume(); -void mp_request_optional_pause(); -void mp_request_step(); - -void mp_state_callback(); +} hold_reason_t; + + +PGM_P state_get_pgmstr(state_t state); +PGM_P state_get_cycle_pgmstr(cycle_t cycle); +PGM_P state_get_hold_reason_pgmstr(hold_reason_t reason); + +state_t state_get(); +cycle_t state_get_cycle(); +void state_set_cycle(cycle_t cycle); +void state_set_hold_reason(hold_reason_t reason); + +bool state_is_flushing(); +bool state_is_resuming(); +bool state_is_quiescent(); +bool state_is_ready(); +void state_pause_queue(bool x); + +void state_optional_pause(); +void state_holding(); +void state_running(); +void state_idle(); +void state_estop(); + +void state_request_hold(); +void state_request_start(); +void state_request_flush(); +void state_request_resume(); +void state_request_optional_pause(); +void state_request_step(); + +void state_callback(); diff --git a/src/avr/src/stepper.c b/src/avr/src/stepper.c index 2396f07..660a51f 100644 --- a/src/avr/src/stepper.c +++ b/src/avr/src/stepper.c @@ -30,15 +30,13 @@ #include "stepper.h" #include "config.h" -#include "machine.h" -#include "plan/runtime.h" -#include "plan/exec.h" #include "motor.h" #include "hardware.h" #include "estop.h" #include "util.h" #include "cpp_magic.h" #include "report.h" +#include "exec.h" #include #include @@ -46,7 +44,7 @@ typedef enum { MOVE_TYPE_NULL, // null move - does a no-op - MOVE_TYPE_LINE, // acceleration planned line + MOVE_TYPE_LINE, // linear move MOVE_TYPE_DWELL, // delay with no movement } move_type_t; @@ -84,6 +82,12 @@ void stepper_init() { } +void st_set_position(const float position[]) { + for (int motor = 0; motor < MOTORS; motor++) + motor_set_position(motor, position[motor_get_axis(motor)]); +} + + void st_shutdown() { OUTCLR_PIN(MOTOR_ENABLE_PIN); st.dwell = 0; @@ -105,7 +109,7 @@ bool st_is_busy() {return st.busy;} /// ADC channel 0 triggered by load ISR as a "software" interrupt. ISR(STEP_LOW_LEVEL_ISR) { while (true) { - stat_t status = mp_exec_move(); + stat_t status = exec_next_action(); switch (status) { case STAT_NOOP: st.busy = false; break; // No command executed @@ -196,40 +200,23 @@ static void _load_move() { /// Step timer interrupt routine. -ISR(STEP_TIMER_ISR) { - _load_move(); -} +ISR(STEP_TIMER_ISR) {_load_move();} -/* Prepare the next move - * - * This function precomputes the next pulse segment (move) so it can - * be executed quickly in the ISR. It works in steps, rather than - * length units. All args are provided as floats which converted here - * to integer values. - * - * Args: - * @param target signed position in steps for each motor. - * Steps are fractional. Their sign indicates direction. Motors not in the - * move have 0 steps. - */ -stat_t st_prep_line(float time, const float target[]) { +void st_prep_line(float time, const float target[]) { // Trap conditions that would prevent queueing the line ASSERT(!st.move_ready); + ASSERT(isfinite(time)); // Setup segment parameters st.move_type = MOVE_TYPE_LINE; st.clock_period = round(time * STEP_TIMER_FREQ * 60); // Prepare motor moves - for (int motor = 0; motor < MOTORS; motor++) { - ASSERT(isfinite(target[motor])); - motor_prep_move(motor, time, round(target[motor])); - } + for (int motor = 0; motor < MOTORS; motor++) + motor_prep_move(motor, time, target[motor_get_axis(motor)]); st.move_queued = true; // signal prep buffer ready (do this last) - - return STAT_OK; } diff --git a/src/avr/src/stepper.h b/src/avr/src/stepper.h index 44f2d45..7b3ea9b 100644 --- a/src/avr/src/stepper.h +++ b/src/avr/src/stepper.h @@ -29,15 +29,14 @@ #pragma once -#include "status.h" - #include #include void stepper_init(); +void st_set_position(const float position[]); void st_shutdown(); void st_enable(); bool st_is_busy(); -stat_t st_prep_line(float time, const float target[]); +void st_prep_line(float time, const float target[]); void st_prep_dwell(float seconds); diff --git a/src/avr/src/usart.c b/src/avr/src/usart.c index be88d8b..89f14dc 100644 --- a/src/avr/src/usart.c +++ b/src/avr/src/usart.c @@ -160,9 +160,7 @@ void usart_set(int flag, bool enable) { } -bool usart_is_set(int flags) { - return (usart_flags & flags) == flags; -} +bool usart_is_set(int flags) {return (usart_flags & flags) == flags;} void usart_putc(char c) { @@ -176,9 +174,7 @@ void usart_putc(char c) { } -void usart_puts(const char *s) { - while (*s) usart_putc(*s++); -} +void usart_puts(const char *s) {while (*s) usart_putc(*s++);} int8_t usart_getc() { @@ -243,9 +239,7 @@ char *usart_readline() { } -int16_t usart_peek() { - return rx_buf_empty() ? -1 : rx_buf_peek(); -} +int16_t usart_peek() {return rx_buf_empty() ? -1 : rx_buf_peek();} void usart_flush() { @@ -257,26 +251,13 @@ void usart_flush() { } -void usart_rx_flush() { - rx_buf_init(); -} - - -int16_t usart_rx_space() { - return rx_buf_space(); -} +void usart_rx_flush() {rx_buf_init();} +int16_t usart_rx_space() {return rx_buf_space();} +int16_t usart_rx_fill() {return rx_buf_fill();} +int16_t usart_tx_space() {return tx_buf_space();} +int16_t usart_tx_fill() {return tx_buf_fill();} -int16_t usart_rx_fill() { - return rx_buf_fill(); -} - - -int16_t usart_tx_space() { - return tx_buf_space(); -} - - -int16_t usart_tx_fill() { - return tx_buf_fill(); -} +// Var callbacks +bool get_echo() {return usart_is_set(USART_ECHO);} +void set_echo(bool value) {return usart_set(USART_ECHO, value);} diff --git a/src/avr/src/varcb.c b/src/avr/src/varcb.c deleted file mode 100644 index 1a59471..0000000 --- a/src/avr/src/varcb.c +++ /dev/null @@ -1,141 +0,0 @@ -/******************************************************************************\ - - This file is part of the Buildbotics firmware. - - Copyright (c) 2015 - 2017 Buildbotics LLC - All rights reserved. - - This file ("the software") is free software: you can redistribute it - and/or modify it under the terms of the GNU General Public License, - version 2 as published by the Free Software Foundation. You should - have received a copy of the GNU General Public License, version 2 - along with the software. If not, see . - - The software is distributed in the hope that it will be useful, but - WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with the software. If not, see - . - - For information regarding this software email: - "Joseph Coffland" - -\******************************************************************************/ - -#include "usart.h" -#include "machine.h" -#include "spindle.h" -#include "coolant.h" -#include "plan/runtime.h" -#include "plan/state.h" - -// Axis -float get_axis_mach_coord(int axis) {return mp_runtime_get_axis_position(axis);} - - -void set_axis_mach_coord(int axis, float position) { - mach_set_axis_position(axis, position); -} - - -float get_axis_work_coord(int axis) {return mp_runtime_get_work_position(axis);} - - -// GCode getters -int32_t get_line() {return mp_runtime_get_line();} -PGM_P get_unit() {return gs_get_units_pgmstr(mach_get_units());} -float get_speed() {return spindle_get_speed();} -float get_feed() {return mach_get_feed_rate();} // TODO get runtime value -uint8_t get_tool() {return mp_runtime_get_tool();} -PGM_P get_feed_mode() {return gs_get_feed_mode_pgmstr(mach_get_feed_mode());} -PGM_P get_plane() {return gs_get_plane_pgmstr(mach_get_plane());} - - -PGM_P get_coord_system() { - return gs_get_coord_system_pgmstr(mach_get_coord_system()); -} - - -bool get_abs_override() {return mach_get_absolute_mode();} -PGM_P get_path_mode() {return gs_get_path_mode_pgmstr(mach_get_path_mode());} - - -PGM_P get_distance_mode() { - return gs_get_distance_mode_pgmstr(mach_get_distance_mode()); -} - - -PGM_P get_arc_dist_mode() { - return gs_get_distance_mode_pgmstr(mach_get_arc_distance_mode()); -} - - -float get_feed_override() {return mach_get_feed_override();} -float get_speed_override() {return mach_get_spindle_override();} -bool get_mist_coolant() {return coolant_get_mist();} -bool get_flood_coolant() {return coolant_get_flood();} - - -// GCode setters -void set_unit(const char *units) {mach_set_units(gs_parse_units(units));} -void set_speed(float speed) {spindle_set_speed(speed);} -void set_feed(float feed) {mach_set_feed_rate(feed);} - - -void set_tool(uint8_t tool) { - mp_runtime_set_tool(tool); - mach_select_tool(tool); -} - - -void set_feed_mode(const char *mode) { - mach_set_feed_mode(gs_parse_feed_mode(mode)); -} - - -void set_plane(const char *plane) {mach_set_plane(gs_parse_plane(plane));} - - -void set_coord_system(const char *cs) { - mach_set_coord_system(gs_parse_coord_system(cs)); -} - - -void set_abs_override(bool enable) {mach_set_absolute_mode(enable);} - - -void set_path_mode(const char *mode) { - mach_set_path_mode(gs_parse_path_mode(mode)); -} - - -void set_distance_mode(const char *mode) { - mach_set_distance_mode(gs_parse_distance_mode(mode)); -} - - -void set_arc_dist_mode(const char *mode) { - mach_set_arc_distance_mode(gs_parse_distance_mode(mode)); -} - - -void set_feed_override(float value) {mach_set_feed_override(value);} -void set_speed_override(float value) {mach_set_spindle_override(value);} -void set_mist_coolant(bool enable) {coolant_set_mist(enable);} -void set_flood_coolant(bool enable) {coolant_set_flood(enable);} - - -// System -float get_velocity() {return mp_runtime_get_velocity();} -bool get_echo() {return usart_is_set(USART_ECHO);} -void set_echo(bool value) {return usart_set(USART_ECHO, value);} -PGM_P get_state() {return mp_get_state_pgmstr(mp_get_state());} -PGM_P get_cycle() {return mp_get_cycle_pgmstr(mp_get_cycle());} - - -PGM_P get_hold_reason() { - return mp_get_hold_reason_pgmstr(mp_get_hold_reason()); -} diff --git a/src/avr/src/vars.c b/src/avr/src/vars.c index d9dcb12..8bda422 100644 --- a/src/avr/src/vars.c +++ b/src/avr/src/vars.c @@ -74,7 +74,7 @@ static float var_string_to_float(string s) {return 0;} // Program string static void var_print_pstring(pstring s) {printf_P(PSTR("\"%"PRPSTR"\""), s);} -static const char *var_parse_pstring(const char *value) {return value;} +//static const char *var_parse_pstring(const char *value) {return value;} static float var_pstring_to_float(pstring s) {return 0;} diff --git a/src/avr/src/vars.def b/src/avr/src/vars.def index 683abc1..c586995 100644 --- a/src/avr/src/vars.def +++ b/src/avr/src/vars.def @@ -77,7 +77,6 @@ VAR(zero_backoff, zb, float, MOTORS, 1, 1, "Homing zero backoff") // Axis VAR(axis_mach_coord, p, float, AXES, 1, 1, "Axis machine coordinate") -VAR(axis_work_coord, w, float, AXES, 0, 1, "Axis work coordinate") VAR(axis_can_home, ch, bool, AXES, 0, 1, "Is axis configured for homing") // Outputs @@ -94,7 +93,6 @@ VAR(spin_max_duty, md, float, 0, 1, 1, "Maximum PWM duty cycle") VAR(spin_up, su, float, 0, 1, 1, "Spin up velocity") VAR(spin_down, sd, float, 0, 1, 1, "Spin down velocity") VAR(spin_freq, sf, uint16_t, 0, 1, 1, "Spindle PWM frequency") -VAR(spin_mode, ss, pstring, 0, 0, 1, "Spindle mode") // PWM spindle VAR(pwm_invert, pi, bool, 0, 1, 1, "Inverted spindle PWM") @@ -112,19 +110,10 @@ VAR(huanyang_status, hs, uint8_t, 0, 0, 1, "Huanyang status flags") VAR(huanyang_debug, hb, bool, 0, 1, 1, "Huanyang debugging") VAR(huanyang_connected, he, bool, 0, 0, 1, "Huanyang connected") -// GCode -VAR(line, ln, int32_t, 0, 0, 1, "Last GCode line executed") -VAR(unit, u, pstring, 0, 1, 1, "Current unit of measure") +// Machine state +VAR(line, ln, int32_t, 0, 0, 1, "Last line executed") VAR(speed, s, float, 0, 1, 1, "Current spindle speed") -VAR(feed, f, float, 0, 1, 1, "Current feed rate") VAR(tool, t, uint8_t, 0, 1, 1, "Current tool") -VAR(feed_mode, fm, pstring, 0, 1, 1, "Current feed rate mode") -VAR(plane, pa, pstring, 0, 1, 1, "Current plane") -VAR(coord_system, cs, pstring, 0, 1, 1, "Current coordinate system") -VAR(abs_override, ao, bool, 0, 1, 1, "Absolute override enabled") -VAR(path_mode, pc, pstring, 0, 1, 1, "Current path control mode") -VAR(distance_mode, dm, pstring, 0, 1, 1, "Current distance mode") -VAR(arc_dist_mode, ad, pstring, 0, 1, 1, "Current arc distance mode") VAR(feed_override, fo, float, 0, 1, 1, "Feed rate override") VAR(speed_override, so, float, 0, 1, 1, "Spindle speed override") VAR(mist_coolant, mc, bool, 0, 1, 1, "Mist coolant enabled") @@ -132,6 +121,8 @@ VAR(flood_coolant, fc, bool, 0, 1, 1, "Flood coolant enabled") // System VAR(velocity, v, float, 0, 0, 1, "Current velocity") +VAR(acceleration, a, float, 0, 0, 1, "Current acceleration") +VAR(jerk, j, float, 0, 0, 1, "Current jerk") VAR(hw_id, id, string, 0, 0, 1, "Hardware ID") VAR(echo, ec, bool, 0, 1, 1, "Enable or disable echo") VAR(estop, es, bool, 0, 1, 1, "Emergency stop") -- 2.27.0