--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "action.h"
+
+#include "queue.h"
+
+#include <stdlib.h>
+#include <ctype.h>
+#include <string.h>
+
+
+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;
+}
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "status.h"
+
+
+typedef enum {
+ ACTION_SCURVE = 's',
+ ACTION_DATA = 'd',
+ ACTION_VELOCITY = 'v',
+
+ 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);
#include "axis.h"
#include "motor.h"
-#include "plan/planner.h"
+#include "switch.h"
+#include "util.h"
#include <math.h>
#include <string.h>
*
* 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)
enum {
AXIS_X, AXIS_Y, AXIS_Z,
AXIS_A, AXIS_B, AXIS_C,
- AXIS_U, AXIS_V, AXIS_W // reserved
};
#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 <avr/wdt.h>
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;
}
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], '=');
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
uint8_t command_resume(int argc, char *argv[]) {
- mp_request_resume();
+ state_request_resume();
return 0;
}
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")
#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
*
* 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
// 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)
#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
#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
// 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
void coolant_init();
void coolant_set_mist(bool x);
void coolant_set_flood(bool x);
-bool coolant_get_mist();
-bool coolant_get_flood();
#include "report.h"
#include "hardware.h"
#include "config.h"
-
-#include "plan/planner.h"
-#include "plan/state.h"
+#include "state.h"
+#include "exec.h"
#include <avr/eeprom.h>
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);
spindle_stop();
// Set machine state
- mp_state_estop();
+ state_estop();
// Save reason
_set_reason(reason);
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#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 <string.h>
+#include <float.h>
+
+
+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;
+}
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "pgmspace.h"
+#include "status.h"
+
+#include <stdbool.h>
+
+
+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();
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "gcode_expr.h"
-
-#include "gcode_parser.h"
-#include "vars.h"
-
-#include <math.h>
-#include <ctype.h>
-#include <stdlib.h>
-
-
-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();
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-float parse_gcode_number(char **p);
-float parse_gcode_expression(char **p);
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "gcode_parser.h"
-
-#include "gcode_expr.h"
-#include "machine.h"
-#include "plan/arc.h"
-#include "axis.h"
-#include "util.h"
-
-#include <stdbool.h>
-#include <string.h>
-#include <ctype.h>
-#include <stdlib.h>
-#include <math.h>
-#include <stdio.h>
-
-
-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();
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#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);
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#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;
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-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
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "config.h"
-#include "pgmspace.h"
-
-#include <stdint.h>
-#include <stdbool.h>
-
-
-/* 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);
bool connected;
bool changed;
- spindle_mode_t mode;
float speed;
float actual_freq;
// 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);
}
-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;
}
// Save settings
uint8_t id = ha.id;
- spindle_mode_t mode = ha.mode;
float speed = ha.speed;
bool debug = ha.debug;
// Restore settings
ha.id = id;
- ha.mode = mode;
ha.speed = speed;
ha.debug = debug;
ha.changed = true;
void huanyang_stop() {
- huanyang_set(SPINDLE_OFF, 0);
+ huanyang_set(0);
huanyang_reset();
}
#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();
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "jog.h"
+
+#include "axis.h"
+#include "util.h"
+#include "exec.h"
+#include "state.h"
+#include "queue.h"
+#include "config.h"
+
+#include <stdbool.h>
+#include <math.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+
+typedef struct {
+ float delta;
+ float t;
+ bool changed;
+
+ int sign;
+ float velocity;
+ float accel;
+ float next;
+ float initial;
+ float target;
+} jog_axis_t;
+
+
+typedef struct {
+ bool writing;
+ bool done;
+
+ float Vi;
+ float Vt;
+
+ jog_axis_t axes[AXES];
+} jog_runtime_t;
+
+
+static jog_runtime_t jr;
+
+
+static bool _axis_velocity_target(int axis) {
+ jog_axis_t *a = &jr.axes[axis];
+
+ float Vn = a->next * axis_get_velocity_max(axis);
+ float Vi = a->velocity;
+ float Vt = a->target;
+
+ if (JOG_MIN_VELOCITY < fabs(Vn)) jr.done = false; // Still jogging
+
+ if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0))
+ Vn = 0; // Plan to zero on sign change
+
+ if (fabs(Vn) < JOG_MIN_VELOCITY) Vn = 0;
+
+ if (Vt == Vn) return false; // No change
+
+ a->target = Vn;
+ if (Vn) a->sign = Vn < 0 ? -1 : 1;
+
+ return true; // Velocity changed
+}
+
+
+#if 0
+// Numeric version
+static float _compute_deccel_dist(float vel, float accel, float jerk) {
+ float dist = 0;
+ float Ad = jerk * SEGMENT_TIME; // Delta accel
+
+ while (true) {
+ // Compute next accel
+ float At2 = -jerk * vel;
+ if (accel * accel < At2) accel += Ad;
+ else accel -= Ad;
+
+ // Compute next velocity
+ vel += accel * SEGMENT_TIME;
+ if (vel <= 0) break;
+
+ // Compute distance traveled
+ dist += vel * SEGMENT_TIME;
+ }
+
+ return dist;
+}
+
+
+#else
+// Analytical version
+static float _compute_deccel_dist(float vel, float accel, float jerk) {
+ float dist = 0;
+
+ // Compute distance to decrease accel to zero
+ if (0 < accel) {
+ // s(a/j) = v * a / j + 2 * a^3 / (3 * j^2)
+ dist += vel * accel / jerk + 2 * accel * accel * accel / (3 * jerk * jerk);
+ // v(a/j) = a^2 / 2j + v
+ vel += accel * accel / (2 * jerk);
+ accel = 0;
+ }
+
+ // Compute max deccel given accel, vel and jerk
+ float maxDeccel = -sqrt(0.5 * accel * accel + vel * jerk);
+
+ // Compute distance to max deccel
+ if (maxDeccel < accel) {
+ float t = (maxDeccel - accel) / -jerk;
+ dist += vel * t + accel * t * t / 2 - jerk * t * t * t / 6;
+ vel += accel * t - jerk * t * t / 2;
+ accel = maxDeccel;
+ }
+
+ // Compute distance to zero vel
+ float t = -accel / jerk;
+ dist += vel * t + accel * t * t / 2 + jerk * t * t * t / 6;
+
+ return dist;
+}
+#endif
+
+
+static float _limit_position(int axis, float p) {
+ jog_axis_t *a = &jr.axes[axis];
+
+ // Check if axis is homed
+ if (!axis_get_homed(axis)) return p;
+
+ // Check if limits are enabled
+ float min = axis_get_travel_min(axis);
+ float max = axis_get_travel_max(axis);
+ if (min == max) return p;
+
+ if (a->velocity < 0 && p < min) {
+ a->velocity = 0;
+ return min;
+ }
+
+ if (0 < a->velocity && max < p) {
+ a->velocity = 0;
+ return max;
+ }
+
+ return p;
+}
+
+
+static bool _soft_limit(int axis, float V, float A) {
+ jog_axis_t *a = &jr.axes[axis];
+
+ // Check if axis is homed
+ if (!axis_get_homed(axis)) return false;
+
+ // Check if limits are enabled
+ float min = axis_get_travel_min(axis);
+ float max = axis_get_travel_max(axis);
+ if (min == max) return false;
+
+ // Check if we need to stop to avoid exceeding a limit
+ float jerk = axis_get_jerk_max(axis);
+ float deccelDist = _compute_deccel_dist(V, A, jerk);
+
+ 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;
+
+ return false;
+}
+
+
+static float _next_accel(float Vi, float Vt, float Ai, float jerk) {
+ float At = sqrt(jerk * fabs(Vt - Vi)) * (Vt < Vi ? -1 : 1); // Target accel
+ float Ad = jerk * SEGMENT_TIME; // Delta accel
+
+ if (Ai < At) return (Ai < At + Ad) ? At : (Ai + Ad);
+ if (At < Ai) return (Ai - Ad < At) ? At : (Ai - Ad);
+
+ return Ai;
+}
+
+
+static float _compute_axis_velocity(int axis) {
+ jog_axis_t *a = &jr.axes[axis];
+
+ float V = fabs(a->velocity);
+ float Vt = fabs(a->target);
+
+ // Apply soft limits
+ if (_soft_limit(axis, V, a->accel)) Vt = JOG_MIN_VELOCITY;
+
+ // Check if velocity has reached its target
+ if (fp_EQ(V, Vt)) {
+ a->accel = 0;
+ return Vt;
+ }
+
+ // Compute axis max jerk
+ float jerk = axis_get_jerk_max(axis);
+
+ // Compute next accel
+ a->accel = _next_accel(V, Vt, a->accel, jerk);
+
+ return V + a->accel * SEGMENT_TIME;
+}
+
+
+stat_t jog_exec() {
+ // Load next velocity
+ jr.done = true;
+
+ if (!jr.writing)
+ for (int axis = 0; axis < AXES; axis++) {
+ if (!axis_is_enabled(axis)) continue;
+ jr.axes[axis].changed = _axis_velocity_target(axis);
+ }
+
+ float velocity_sqr = 0;
+
+ // Compute per axis velocities
+ for (int axis = 0; axis < AXES; axis++) {
+ if (!axis_is_enabled(axis)) continue;
+ float V = _compute_axis_velocity(axis);
+ if (JOG_MIN_VELOCITY < V) jr.done = false;
+ velocity_sqr += square(V);
+ jr.axes[axis].velocity = V * jr.axes[axis].sign;
+ }
+
+ // Check if we are done
+ if (jr.done) {
+ // Update machine position
+ //mach_set_position_from_runtime();
+ state_set_cycle(CYCLE_MACHINING); // Default cycle
+ state_pause_queue(false);
+
+ return STAT_NOOP; // Done, no move executed
+ }
+
+ // Compute target from velocity
+ float target[AXES];
+ for (int axis = 0; axis < AXES; 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
+ 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;
+}
+
+
+uint8_t command_jog(int argc, char *argv[]) {
+ if (state_get_cycle() != CYCLE_JOGGING &&
+ (state_get() != STATE_READY || state_get_cycle() != CYCLE_MACHINING))
+ return STAT_NOOP;
+
+ float velocity[AXES];
+
+ for (int axis = 0; axis < AXES; axis++)
+ if (axis < argc - 1) velocity[axis] = atof(argv[axis + 1]);
+ else velocity[axis] = 0;
+
+ // Reset
+ 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 (state_get_cycle() != CYCLE_JOGGING) {
+ state_set_cycle(CYCLE_JOGGING);
+ state_pause_queue(true);
+ queue_push(ACTION_JOG);
+ }
+
+ return STAT_OK;
+}
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+
+#include "status.h"
+
+
+stat_t jog_exec();
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* 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);
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#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();
\******************************************************************************/
#include "hardware.h"
-#include "machine.h"
#include "stepper.h"
#include "motor.h"
#include "switch.h"
#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 <avr/wdt.h>
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();
// 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();
\******************************************************************************/
-// 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, "")
#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 <util/delay.h>
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];
}
-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);
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;
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);
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* 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 <plan/state.h>
-
-#include <math.h>
-#include <stdbool.h>
-#include <string.h>
-
-
-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);
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#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();
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* 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 <string.h>
-#include <math.h>
-#include <stdio.h>
-
-
-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));
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "machine.h"
-#include "config.h"
-
-#include <stdbool.h>
-
-
-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;}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-
-#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 <stdint.h>
-#include <stdio.h>
-#include <string.h>
-#include <stdlib.h>
-#include <inttypes.h>
-
-
-#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;
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include <stdbool.h>
-#include <stdint.h>
-
-
-bool calibrate_busy();
-void calibrate_set_stallguard(int motor, uint16_t sg);
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#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;
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdint.h>
-
-
-stat_t mp_dwell(float seconds, int32_t line);
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#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 <string.h>
-#include <stdbool.h>
-#include <math.h>
-#include <stdio.h>
-
-
-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;
- }
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "buffer.h"
-
-
-stat_t mp_exec_move();
-void mp_exec_abort();
-stat_t mp_exec_aline(mp_buffer_t *bf);
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#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 "state.h"
-#include "config.h"
-
-#include <stdbool.h>
-#include <math.h>
-#include <string.h>
-#include <stdio.h>
-#include <stdlib.h>
-
-
-typedef struct {
- float delta;
- float t;
- bool changed;
-
- int sign;
- float velocity;
- float accel;
- float next;
- float initial;
- float target;
-} jog_axis_t;
-
-
-typedef struct {
- bool writing;
- bool done;
-
- float Vi;
- float Vt;
-
- jog_axis_t axes[AXES];
-} jog_runtime_t;
-
-
-static jog_runtime_t jr;
-
-
-static bool _axis_velocity_target(int axis) {
- jog_axis_t *a = &jr.axes[axis];
-
- float Vn = a->next * axis_get_velocity_max(axis);
- float Vi = a->velocity;
- float Vt = a->target;
-
- if (JOG_MIN_VELOCITY < fabs(Vn)) jr.done = false; // Still jogging
-
- if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0))
- Vn = 0; // Plan to zero on sign change
-
- if (fabs(Vn) < JOG_MIN_VELOCITY) Vn = 0;
-
- if (Vt == Vn) return false; // No change
-
- a->target = Vn;
- if (Vn) a->sign = Vn < 0 ? -1 : 1;
-
- return true; // Velocity changed
-}
-
-
-#if 0
-// Numeric version
-static float _compute_deccel_dist(float vel, float accel, float jerk) {
- float dist = 0;
- float Ad = jerk * SEGMENT_TIME; // Delta accel
-
- while (true) {
- // Compute next accel
- float At2 = -jerk * vel;
- if (accel * accel < At2) accel += Ad;
- else accel -= Ad;
-
- // Compute next velocity
- vel += accel * SEGMENT_TIME;
- if (vel <= 0) break;
-
- // Compute distance traveled
- dist += vel * SEGMENT_TIME;
- }
-
- return dist;
-}
-
-
-#else
-// Analytical version
-static float _compute_deccel_dist(float vel, float accel, float jerk) {
- float dist = 0;
-
- // Compute distance to decrease accel to zero
- if (0 < accel) {
- // s(a/j) = v * a / j + 2 * a^3 / (3 * j^2)
- dist += vel * accel / jerk + 2 * accel * accel * accel / (3 * jerk * jerk);
- // v(a/j) = a^2 / 2j + v
- vel += accel * accel / (2 * jerk);
- accel = 0;
- }
-
- // Compute max deccel given accel, vel and jerk
- float maxDeccel = -sqrt(0.5 * accel * accel + vel * jerk);
-
- // Compute distance to max deccel
- if (maxDeccel < accel) {
- float t = (maxDeccel - accel) / -jerk;
- dist += vel * t + accel * t * t / 2 - jerk * t * t * t / 6;
- vel += accel * t - jerk * t * t / 2;
- accel = maxDeccel;
- }
-
- // Compute distance to zero vel
- float t = -accel / jerk;
- dist += vel * t + accel * t * t / 2 + jerk * t * t * t / 6;
-
- return dist;
-}
-#endif
-
-
-static float _limit_position(int axis, float p) {
- jog_axis_t *a = &jr.axes[axis];
-
- // Check if axis is homed
- if (!axis_get_homed(axis)) return p;
-
- // Check if limits are enabled
- float min = axis_get_travel_min(axis);
- float max = axis_get_travel_max(axis);
- if (min == max) return p;
-
- if (a->velocity < 0 && p < min) {
- a->velocity = 0;
- return min;
- }
-
- if (0 < a->velocity && max < p) {
- a->velocity = 0;
- return max;
- }
-
- return p;
-}
-
-
-static bool _soft_limit(int axis, float V, float A) {
- jog_axis_t *a = &jr.axes[axis];
-
- // Check if axis is homed
- if (!axis_get_homed(axis)) return false;
-
- // Check if limits are enabled
- float min = axis_get_travel_min(axis);
- float max = axis_get_travel_max(axis);
- 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 deccelDist = _compute_deccel_dist(V, A, jerk);
-
- float position = mp_runtime_get_axis_position(axis);
- if (a->velocity < 0 && position <= min + deccelDist) return true;
- if (0 < a->velocity && max - deccelDist <= position) return true;
-
- return false;
-}
-
-
-static float _next_accel(float Vi, float Vt, float Ai, float jerk) {
- float At = sqrt(jerk * fabs(Vt - Vi)) * (Vt < Vi ? -1 : 1); // Target accel
- float Ad = jerk * SEGMENT_TIME; // Delta accel
-
- if (Ai < At) return (Ai < At + Ad) ? At : (Ai + Ad);
- if (At < Ai) return (Ai - Ad < At) ? At : (Ai - Ad);
-
- return Ai;
-}
-
-
-static float _compute_axis_velocity(int axis) {
- jog_axis_t *a = &jr.axes[axis];
-
- float V = fabs(a->velocity);
- float Vt = fabs(a->target);
-
- // Apply soft limits
- if (_soft_limit(axis, V, a->accel)) Vt = JOG_MIN_VELOCITY;
-
- // Check if velocity has reached its target
- if (fp_EQ(V, Vt)) {
- a->accel = 0;
- return Vt;
- }
-
- // Compute axis max jerk
- float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
-
- // Compute next accel
- a->accel = _next_accel(V, Vt, a->accel, jerk);
-
- return V + a->accel * SEGMENT_TIME;
-}
-
-
-static stat_t _exec_jog(mp_buffer_t *bf) {
- // Load next velocity
- jr.done = true;
-
- if (!jr.writing)
- for (int axis = 0; axis < AXES; axis++) {
- if (!axis_is_enabled(axis)) continue;
- jr.axes[axis].changed = _axis_velocity_target(axis);
- }
-
- float velocity_sqr = 0;
-
- // Compute per axis velocities
- for (int axis = 0; axis < AXES; axis++) {
- if (!axis_is_enabled(axis)) continue;
- float V = _compute_axis_velocity(axis);
- if (JOG_MIN_VELOCITY < V) jr.done = false;
- velocity_sqr += square(V);
- jr.axes[axis].velocity = V * jr.axes[axis].sign;
- }
-
- // 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);
-
- return STAT_NOOP; // Done, no move executed
- }
-
- // Compute target from velocity
- float target[AXES];
- for (int axis = 0; axis < AXES; axis++) {
- target[axis] = mp_runtime_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);
- if (status != STAT_OK) return status;
-
- return STAT_EAGAIN;
-}
-
-
-uint8_t command_jog(int argc, char *argv[]) {
- if (mp_get_cycle() != CYCLE_JOGGING &&
- (mp_get_state() != STATE_READY || mp_get_cycle() != CYCLE_MACHINING))
- return STAT_NOOP;
-
- float velocity[AXES];
-
- for (int axis = 0; axis < AXES; axis++)
- if (axis < argc - 1) velocity[axis] = atof(argv[axis + 1]);
- else velocity[axis] = 0;
-
- // Reset
- if (mp_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);
- }
-
- return STAT_OK;
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "line.h"
-
-#include "axis.h"
-#include "planner.h"
-#include "exec.h"
-#include "buffer.h"
-
-#include <stdio.h>
-#include <float.h>
-
-
-/* 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;
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-#include "buffer.h"
-
-#include <stdbool.h>
-#include <stdint.h>
-
-
-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[]);
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-/* 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 <string.h>
-#include <stdbool.h>
-#include <stdio.h>
-
-
-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 Ve<Vt>Vx sufficient length exists for all parts (corner
- * case: HBT')
- * HB Ve<Vt=Vx head accelerates to cruise - exits at full speed
- * (corner case: H')
- * BT Ve=Vt>Vx enter at full speed & decelerate (corner case: T')
- * HT Ve & Vx perfect fit HT (very rare). May be symmetric or
- * asymmetric
- * H Ve<Vx perfect fit H (common, results from planning)
- * T Ve>Vx perfect fit T (common, results from planning)
- * B Ve=Vt=Vx Velocities are close to each other and within
- * matching tolerance
- *
- * Rate-Limited cases - Ve and Vx can be satisfied but Vt cannot:
- *
- * HT (Ve=Vx)<Vt symmetric case. Split the length and compute Vt.
- * HT' (Ve!=Vx)<Vt asymmetric case. Find H and T by successive
- * approximation.
- * HBT' body length < min body length treated as an HT case
- * H' body length < min body length subsume body into head length
- * T' body length < min body length subsume body into tail length
- *
- * Degraded fit cases - line is too short to satisfy both Ve and Vx:
- *
- * H" Ve<Vx Ve is degraded (velocity step). Vx is met
- * T" Ve>Vx Ve is degraded (velocity step). Vx is met
- * B" <short> line is very short but drawable; is treated as a
- * body only.
- * F <too short> force fit: This block is slowed down until it can
- * be executed.
- *
- * Note: The order of the cases/tests in the code is 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;
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-
-#include "machine.h" // used for gcode_state_t
-#include "buffer.h"
-#include "util.h"
-#include "config.h"
-
-#include <stdbool.h>
-
-
-// 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);
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#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 <string.h>
-#include <stdbool.h>
-#include <math.h>
-#include <stdio.h>
-
-
-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;
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "status.h"
-
-#include <stdbool.h>
-
-
-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[]);
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "state.h"
-#include "machine.h"
-#include "planner.h"
-#include "runtime.h"
-#include "buffer.h"
-#include "arc.h"
-#include "stepper.h"
-#include "spindle.h"
-
-#include "report.h"
-
-#include <stdbool.h>
-
-
-typedef struct {
- mp_state_t state;
- mp_cycle_t cycle;
- mp_hold_reason_t hold_reason;
- bool pause;
-
- bool hold_requested;
- bool flush_requested;
- bool start_requested;
- bool resume_requested;
- bool optional_pause_requested;
-} planner_state_t;
-
-
-static planner_state_t ps = {
- .flush_requested = true, // Start out flushing
-};
-
-
-PGM_P mp_get_state_pgmstr(mp_state_t state) {
- switch (state) {
- case STATE_READY: return PSTR("READY");
- case STATE_ESTOPPED: return PSTR("ESTOPPED");
- case STATE_RUNNING: return PSTR("RUNNING");
- case STATE_STOPPING: return PSTR("STOPPING");
- case STATE_HOLDING: return PSTR("HOLDING");
- }
-
- return PSTR("INVALID");
-}
-
-
-PGM_P mp_get_cycle_pgmstr(mp_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");
- }
-
- return PSTR("INVALID");
-}
-
-
-PGM_P mp_get_hold_reason_pgmstr(mp_hold_reason_t reason) {
- switch (reason) {
- case HOLD_REASON_USER_PAUSE: return PSTR("USER");
- case HOLD_REASON_PROGRAM_PAUSE: return PSTR("PROGRAM");
- case HOLD_REASON_PROGRAM_END: return PSTR("END");
- case HOLD_REASON_PALLET_CHANGE: return PSTR("PALLET");
- case HOLD_REASON_TOOL_CHANGE: return PSTR("TOOL");
- }
-
- return PSTR("INVALID");
-}
-
-
-mp_state_t mp_get_state() {return ps.state;}
-mp_cycle_t mp_get_cycle() {return ps.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;
- report_request();
-}
-
-
-void mp_set_cycle(mp_cycle_t cycle) {
- if (ps.cycle == cycle) return; // No change
-
- if (ps.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));
- return;
- }
-
- if (ps.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));
- return;
- }
-
- ps.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;
- report_request();
-}
-
-
-bool mp_is_flushing() {return ps.flush_requested && !ps.resume_requested;}
-bool mp_is_resuming() {return ps.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 mp_is_ready() {
- return mp_queue_get_room() && !mp_is_resuming() && !ps.pause;
-}
-
-
-void mp_pause_queue(bool x) {ps.pause = x;}
-
-
-void mp_state_optional_pause() {
- if (ps.optional_pause_requested) {
- mp_set_hold_reason(HOLD_REASON_USER_PAUSE);
- mp_state_holding();
- }
-}
-
-
-void mp_state_holding() {
- _set_state(STATE_HOLDING);
- mp_set_plan_steps(false);
-}
-
-
-void mp_state_running() {
- if (mp_get_state() == STATE_READY) _set_state(STATE_RUNNING);
-}
-
-
-void mp_state_idle() {
- if (mp_get_state() == STATE_RUNNING) _set_state(STATE_READY);
-}
-
-
-void mp_state_estop() {
- _set_state(STATE_ESTOPPED);
- mp_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 mp_request_step() {
- mp_set_plan_steps(true);
- ps.start_requested = true;
-}
-
-
-/*** Feedholds, queue flushes and starts are all related. Request functions
- * set flags. The callback interprets the flags according to these rules:
- *
- * A hold request received:
- * - during motion is honored
- * - during a feedhold is ignored and reset
- * - when already stopped is ignored and reset
- *
- * A flush request received:
- * - 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
- *
- * 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
- */
-void mp_state_callback() {
- if (ps.hold_requested || ps.flush_requested) {
- ps.hold_requested = false;
- mp_set_hold_reason(HOLD_REASON_USER_PAUSE);
-
- if (mp_get_state() == 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();
-
- // 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();
- }
-
- // Stop spindle
- spindle_stop();
-
- // Resume
- if (ps.resume_requested) {
- ps.flush_requested = ps.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 (mp_get_state() == 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);
- }
- }
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-#include "pgmspace.h"
-
-#include <stdbool.h>
-
-
-typedef enum {
- STATE_READY,
- STATE_ESTOPPED,
- STATE_RUNNING,
- STATE_STOPPING,
- STATE_HOLDING,
-} mp_state_t;
-
-
-typedef enum {
- CYCLE_MACHINING,
- CYCLE_HOMING,
- CYCLE_PROBING,
- CYCLE_CALIBRATING,
- CYCLE_JOGGING,
-} mp_cycle_t;
-
-
-typedef enum {
- HOLD_REASON_USER_PAUSE,
- HOLD_REASON_PROGRAM_PAUSE,
- 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();
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#include "velocity_curve.h"
-
-#include <math.h>
-
-
-/// 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;
-}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#pragma once
-
-float velocity_curve(float Vi, float Vt, float t);
#include "estop.h"
#include "outputs.h"
+#include <math.h>
+
typedef struct {
uint16_t freq; // base frequency for PWM driver, in Hz
}
-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;
}
-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
#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();
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "queue.h"
+
+#include <string.h>
+
+
+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;}
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "config.h"
+#include "action.h"
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+// 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();
+
+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();
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;
}
#include "config.h"
#include "pwm_spindle.h"
#include "huanyang.h"
+#include "pgmspace.h"
typedef enum {
typedef struct {
spindle_type_t type;
- spindle_mode_t mode;
float speed;
bool reversed;
} spindle_t;
}
-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() {
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);
}
}
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();}
#pragma once
-#include "machine.h"
-
+#include <stdbool.h>
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();
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "state.h"
+
+#include "exec.h"
+#include "queue.h"
+#include "stepper.h"
+#include "spindle.h"
+#include "report.h"
+
+#include <stdbool.h>
+
+
+static struct {
+ state_t state;
+ cycle_t cycle;
+ hold_reason_t hold_reason;
+ bool pause;
+
+ bool hold_requested;
+ bool flush_requested;
+ bool start_requested;
+ bool resume_requested;
+ bool optional_pause_requested;
+
+} s = {
+ .flush_requested = true, // Start out flushing
+};
+
+
+PGM_P state_get_pgmstr(state_t state) {
+ switch (state) {
+ case STATE_READY: return PSTR("READY");
+ case STATE_ESTOPPED: return PSTR("ESTOPPED");
+ case STATE_RUNNING: return PSTR("RUNNING");
+ case STATE_STOPPING: return PSTR("STOPPING");
+ case STATE_HOLDING: return PSTR("HOLDING");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+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_JOGGING: return PSTR("JOGGING");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+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");
+ case HOLD_REASON_PROGRAM_END: return PSTR("END");
+ case HOLD_REASON_PALLET_CHANGE: return PSTR("PALLET");
+ case HOLD_REASON_TOOL_CHANGE: return PSTR("TOOL");
+ }
+
+ return PSTR("INVALID");
+}
+
+
+state_t state_get() {return s.state;}
+cycle_t state_get_cycle() {return s.cycle;}
+
+
+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 state_set_cycle(cycle_t cycle) {
+ if (s.cycle == cycle) return; // No change
+
+ if (s.state != STATE_READY && cycle != CYCLE_MACHINING) {
+ STATUS_ERROR(STAT_INTERNAL_ERROR, "Cannot transition to %S while %S",
+ state_get_cycle_pgmstr(cycle),
+ state_get_pgmstr(s.state));
+ return;
+ }
+
+ if (s.cycle != CYCLE_MACHINING && cycle != CYCLE_MACHINING) {
+ STATUS_ERROR(STAT_INTERNAL_ERROR,
+ "Cannot transition to cycle %S while in %S",
+ state_get_cycle_pgmstr(cycle),
+ state_get_cycle_pgmstr(s.cycle));
+ return;
+ }
+
+ s.cycle = cycle;
+ report_request();
+}
+
+
+void state_set_hold_reason(hold_reason_t reason) {
+ if (s.hold_reason == reason) return; // No change
+ s.hold_reason = reason;
+ report_request();
+}
+
+
+bool state_is_flushing() {return s.flush_requested && !s.resume_requested;}
+bool state_is_resuming() {return s.resume_requested;}
+
+
+bool state_is_quiescent() {
+ return (state_get() == STATE_READY || state_get() == STATE_HOLDING) &&
+ !st_is_busy() && !exec_is_busy();
+}
+
+
+bool state_is_ready() {
+ return queue_get_room() && !state_is_resuming() && !s.pause;
+}
+
+
+void state_pause_queue(bool x) {s.pause = x;}
+
+
+void state_optional_pause() {
+ if (s.optional_pause_requested) {
+ state_set_hold_reason(HOLD_REASON_USER_PAUSE);
+ state_holding();
+ }
+}
+
+
+static void _set_plan_steps(bool plan_steps) {} // TODO
+
+
+void state_holding() {
+ _set_state(STATE_HOLDING);
+ _set_plan_steps(false);
+}
+
+
+void state_running() {
+ if (state_get() == STATE_READY) _set_state(STATE_RUNNING);
+}
+
+
+void state_idle() {
+ if (state_get() == STATE_RUNNING) _set_state(STATE_READY);
+}
+
+
+void state_estop() {
+ _set_state(STATE_ESTOPPED);
+ state_pause_queue(false);
+}
+
+
+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 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;
+}
+
+
+/*** Feedholds, queue flushes and starts are all related. Request functions
+ * set flags. The callback interprets the flags according to these rules:
+ *
+ * A hold request received:
+ * - during motion is honored
+ * - during a feedhold is ignored and reset
+ * - when already stopped is ignored and reset
+ *
+ * A flush request received:
+ * - 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 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 queue
+ */
+void state_callback() {
+ if (s.hold_requested || s.flush_requested) {
+ s.hold_requested = false;
+ state_set_hold_reason(HOLD_REASON_USER_PAUSE);
+
+ if (state_get() == STATE_RUNNING) _set_state(STATE_STOPPING);
+ }
+
+ // Only flush queue when idle or holding.
+ if (s.flush_requested && state_is_quiescent()) {
+
+ if (!queue_is_empty()) queue_flush();
+
+ // Stop spindle
+ spindle_stop();
+
+ // Resume
+ if (s.resume_requested) {
+ s.flush_requested = s.resume_requested = false;
+ _set_state(STATE_READY);
+ }
+ }
+
+ // Don't start while flushing or stopping
+ if (s.start_requested && !s.flush_requested &&
+ state_get() != STATE_STOPPING) {
+ s.start_requested = false;
+ s.optional_pause_requested = false;
+
+ if (state_get() == STATE_HOLDING) {
+ // Check if any moves are buffered
+ 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);}
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include "pgmspace.h"
+
+#include <stdbool.h>
+
+
+typedef enum {
+ STATE_READY,
+ STATE_ESTOPPED,
+ STATE_RUNNING,
+ STATE_STOPPING,
+ STATE_HOLDING,
+} state_t;
+
+
+typedef enum {
+ CYCLE_MACHINING,
+ CYCLE_HOMING,
+ CYCLE_PROBING,
+ CYCLE_JOGGING,
+} cycle_t;
+
+
+typedef enum {
+ HOLD_REASON_USER_PAUSE,
+ HOLD_REASON_PROGRAM_PAUSE,
+ HOLD_REASON_PROGRAM_END,
+ HOLD_REASON_PALLET_CHANGE,
+ HOLD_REASON_TOOL_CHANGE,
+} 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();
#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 <string.h>
#include <stdio.h>
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;
}
+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;
/// 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
/// 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;
}
#pragma once
-#include "status.h"
-
#include <stdbool.h>
#include <stdint.h>
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);
}
-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) {
}
-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() {
}
-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() {
}
-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);}
+++ /dev/null
-/******************************************************************************\
-
- 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 <http://www.gnu.org/licenses/>.
-
- 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
- <http://www.gnu.org/licenses/>.
-
- For information regarding this software email:
- "Joseph Coffland" <joseph@buildbotics.com>
-
-\******************************************************************************/
-
-#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());
-}
// 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;}
// 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
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")
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")
// 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")
projects / bbctrl-firmware / commitdiff