// State
.gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE},
- .gn = {},
- .gf = {},
+ .gn = {0},
+ .gf = {0},
};
return cm.combined_state;
}
-uint32_t cm_get_linenum() {return cm.gm.linenum;}
+uint32_t cm_get_line() {return cm.gm.line;}
cmMachineState_t cm_get_machine_state() {return cm.machine_state;}
cmCycleState_t cm_get_cycle_state() {return cm.cycle_state;}
cmMotionState_t cm_get_motion_state() {return cm.motion_state;}
}
-void cm_set_model_linenum(uint32_t linenum) {cm.gm.linenum = linenum;}
+void cm_set_model_line(uint32_t line) {cm.gm.line = line;}
/* Jerk functions
/// Alarm state; send an exception report and stop processing input
-stat_t cm_alarm(const char *location, stat_t status) {
- status_error_P(location, PSTR("ALARM"), status);
+stat_t cm_alarm(const char *location, stat_t code) {
+ status_message_P(location, STAT_LEVEL_ERROR, code, "ALARM");
estop_trigger();
- return status;
+ return code;
}
// prep and plan the move
cm_set_work_offsets(&cm.gm); // capture fully resolved offsets to the state
cm_cycle_start(); // required for homing & other cycles
+ cm.ms.line = cm.gm.line; // copy line number
mp_aline(&cm.ms); // send the move to the planner
cm_finalize_move();
/// G28
stat_t cm_goto_g28_position(float target[], float flags[]) {
cm_set_absolute_override(true);
+
// move through intermediate point, or skip
cm_straight_traverse(target, flags);
// make sure you have an available buffer
- while (!mp_get_planner_buffers_available());
+ mp_wait_for_buffer();
// execute actual stored move
float f[] = {1, 1, 1, 1, 1, 1};
/// G30
stat_t cm_goto_g30_position(float target[], float flags[]) {
cm_set_absolute_override(true);
+
// move through intermediate point, or skip
cm_straight_traverse(target, flags);
+
// make sure you have an available buffer
- while (!mp_get_planner_buffers_available());
- float f[] = {1, 1, 1, 1, 1, 1};
+ mp_wait_for_buffer();
+
// execute actual stored move
+ float f[] = {1, 1, 1, 1, 1, 1};
return cm_straight_traverse(cm.g30_position, f);
}
// prep and plan the move
cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to state
cm_cycle_start(); // required for homing & other cycles
+ cm.ms.line = cm.gm.line; // copy line number
status = mp_aline(&cm.ms); // send the move to the planner
cm_finalize_move();
}
-void cm_message(const char *message) {printf(message);}
+void cm_message(const char *message) {
+ status_message_P(0, STAT_LEVEL_INFO, STAT_OK, PSTR("%s"), message);
+}
/* Program Functions (4.3.10)
/// Process feedholds, cycle starts & queue flushes
-stat_t cm_feedhold_sequencing_callback() {
+void cm_feedhold_sequencing_callback() {
if (cm.feedhold_requested) {
if (cm.motion_state == MOTION_RUN && cm.hold_state == FEEDHOLD_OFF) {
cm_set_motion_state(MOTION_HOLD);
}
}
- bool feedhold_processing =
+ bool processing =
cm.hold_state == FEEDHOLD_SYNC ||
cm.hold_state == FEEDHOLD_PLAN ||
cm.hold_state == FEEDHOLD_DECEL;
- if (cm.cycle_start_requested && !cm.queue_flush_requested &&
- !feedhold_processing) {
+ if (cm.cycle_start_requested && !cm.queue_flush_requested && !processing) {
cm.cycle_start_requested = false;
cm.hold_state = FEEDHOLD_END_HOLD;
cm_cycle_start();
mp_end_hold();
}
-
- return STAT_OK;
}
stat_t cm_queue_flush() {
if (cm_get_runtime_busy()) return STAT_COMMAND_NOT_ACCEPTED;
- usart_rx_flush(); // flush serial queues
- mp_flush_planner(); // flush planner queue
+ mp_flush_planner(); // flush planner queue
// Note: The following uses low-level mp calls for absolute position.
for (int axis = 0; axis < AXES; axis++)
typedef struct {
+ int32_t line; // gcode block line number
float target[AXES]; // XYZABC where the move should go
float work_offset[AXES]; // offset from work coordinate system
float move_time; // optimal time for move given axis constraints
/// Gcode model state
typedef struct GCodeState {
- uint32_t linenum; // Gcode block line number
+ uint32_t line; // Gcode block line number
uint8_t tool; // Tool after T and M6
uint8_t tool_select; // T - sets this value
// Model state getters and setters
-uint32_t cm_get_linenum();
+uint32_t cm_get_line();
cmCombinedState_t cm_get_combined_state();
cmMachineState_t cm_get_machine_state();
cmCycleState_t cm_get_cycle_state();
void cm_set_spindle_speed_parameter(float speed);
void cm_set_tool_number(uint8_t tool);
void cm_set_absolute_override(bool absolute_override);
-void cm_set_model_linenum(uint32_t linenum);
+void cm_set_model_line(uint32_t line);
float cm_get_axis_jerk(uint8_t axis);
void cm_set_axis_jerk(uint8_t axis, float jerk);
void cm_request_queue_flush();
void cm_request_cycle_start();
-stat_t cm_feedhold_sequencing_callback();
+void cm_feedhold_sequencing_callback();
stat_t cm_queue_flush();
void cm_cycle_start();
// Cycles
char cm_get_axis_char(int8_t axis);
-
-// Homing cycles
-stat_t cm_homing_cycle_start();
-stat_t cm_homing_cycle_start_no_set();
-stat_t cm_homing_callback();
-
-// Probe cycles
-stat_t cm_straight_probe(float target[], float flags[]);
-stat_t cm_probe_callback();
#include "report.h"
#include "vars.h"
#include "estop.h"
+#include "homing.h"
+#include "probing.h"
#include "plan/jog.h"
#include "plan/calibrate.h"
#include "plan/buffer.h"
+#include "plan/arc.h"
#include "config.h"
#include <avr/pgmspace.h>
};
-static char *_cmd = 0;
-
-
int command_find(const char *match) {
for (int i = 0; ; i++) {
const char *name = pgm_read_word(&commands[i].name);
}
-stat_t command_hi() {
+static char *_command_next() {
// Get next command
- if (!_cmd) {
- // Read input line or return if incomplete, usart_readline() is non-blocking
- _cmd = usart_readline();
- if (!_cmd) return STAT_OK;
+ char *cmd = usart_readline();
+ if (!cmd) return 0;
- // Remove leading whitespace
- while (*_cmd && isspace(*_cmd)) _cmd++;
+ // Remove leading whitespace
+ while (*cmd && isspace(*cmd)) cmd++;
+
+ // Remove trailing whitespace
+ for (size_t len = strlen(cmd); len && isspace(cmd[len - 1]); len--)
+ cmd[len - 1] = 0;
+
+ return cmd;
+}
- // Remove trailing whitespace
- for (size_t len = strlen(_cmd); len && isspace(_cmd[len - 1]); len--)
- _cmd[len - 1] = 0;
- }
- if (usart_tx_full()) return STAT_OK;
+void command_callback() {
+ char *cmd = _command_next();
+ if (!cmd) return;
stat_t status = STAT_OK;
- switch (*_cmd) {
+ switch (*cmd) {
case 0: break; // Empty line
- case '{': status = vars_parser(_cmd); break;
- case '$': status = command_parser(_cmd); break;
- case '!': if (!_cmd[1]) cm_request_feedhold(); break;
- case '~': if (!_cmd[1]) cm_request_cycle_start(); break;
- case '%': if (!_cmd[1]) cm_request_queue_flush(); break;
- default: return STAT_OK; // Continue processing in command_lo()
+ case '{': status = vars_parser(cmd); break;
+ case '$': status = command_parser(cmd); break;
+ default:
+ if (!cmd[1])
+ switch (*cmd) {
+ case '!': cm_request_feedhold(); return;
+ case '~': cm_request_cycle_start(); return;
+ case '%': cm_request_queue_flush(); return;
+ }
+
+ if (estop_triggered()) status = STAT_MACHINE_ALARMED;
+ else if (!mp_get_planner_buffer_room()) status = STAT_BUFFER_FULL;
+ else if (cm_arc_active()) status = STAT_BUFFER_FULL;
+ else if (calibrate_busy()) status = STAT_BUSY;
+ else if (mp_jog_busy()) status = STAT_BUSY;
+ else if (cm_is_homing()) status = STAT_BUSY;
+ else if (cm_is_probing()) status = STAT_BUSY;
+ else status = gc_gcode_parser(cmd);
}
report_request();
- _cmd = 0; // Command complete
-
- return status;
-}
-
-
-stat_t command_lo() {
- if (!_cmd || mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM ||
- usart_tx_full())
- return STAT_OK; // Wait
-
- // Consume command
- char *cmd = _cmd;
- _cmd = 0;
-
- if (estop_triggered()) return STAT_MACHINE_ALARMED;
- if (calibrate_busy()) return STAT_BUSY;
- if (mp_jog_busy()) return STAT_BUSY;
- return gc_gcode_parser(cmd);
+ if (status) status_error(status);
}
#pragma once
-#include "status.h"
-
#include <stdint.h>
int command_find(const char *name);
int command_exec(int argc, char *argv[]);
-stat_t command_hi();
-stat_t command_lo();
+void command_callback();
#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
+
+
+// Planner
+/// Should be at least the number of buffers requires to support optimal
+/// planning in the case of very short lines or arc segments. Suggest 12 min.
+/// Limit is 255.
+#define PLANNER_BUFFER_POOL_SIZE 32
+
+/// Buffers to reserve in planner before processing new input line
+#define PLANNER_BUFFER_HEADROOM 4
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 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 "canonical_machine.h"
-#include "switch.h"
-#include "util.h"
-#include "report.h"
-
-#include "plan/planner.h"
-
-#include <avr/pgmspace.h>
-
-#include <stdbool.h>
-#include <math.h>
-#include <stdio.h>
-
-
-typedef stat_t (*homing_func_t)(int8_t axis);
-
-
-/// persistent homing runtime variables
-struct hmHomingSingleton {
- // controls for homing cycle
- int8_t axis; // axis currently being homed
-
- /// homing switch for current axis (index into switch flag table)
- int8_t homing_switch;
- int8_t limit_switch; // limit switch for current axis or -1 if none
-
- uint8_t homing_closed; // 0=open, 1=closed
- uint8_t limit_closed; // 0=open, 1=closed
- /// G28.4 flag. true = set coords to zero at the end of homing cycle
- uint8_t set_coordinates;
- homing_func_t func; // binding for callback function state machine
-
- // per-axis parameters
- /// set to 1 for positive (max), -1 for negative (to min);
- float direction;
- float search_travel; // signed distance to travel in search
- float search_velocity; // search speed as positive number
- float latch_velocity; // latch speed as positive number
- /// max distance to back off switch during latch phase
- float latch_backoff;
- /// distance to back off switch before setting zero
- float zero_backoff;
- /// maximum distance of switch clearing backoffs before erring out
- float max_clear_backoff;
-
- // state saved from gcode model
- uint8_t saved_units_mode; // G20,G21 global setting
- uint8_t saved_coord_system; // G54 - G59 setting
- uint8_t saved_distance_mode; // G90,G91 global setting
- uint8_t saved_feed_rate_mode; // G93,G94 global setting
- float saved_feed_rate; // F setting
- float saved_jerk; // saved and restored for each axis homed
-};
-static struct hmHomingSingleton hm;
-
-
-static stat_t _set_homing_func(homing_func_t func);
-static stat_t _homing_axis_start(int8_t axis);
-static stat_t _homing_axis_clear(int8_t axis);
-static stat_t _homing_axis_search(int8_t axis);
-static stat_t _homing_axis_latch(int8_t axis);
-static stat_t _homing_axis_zero_backoff(int8_t axis);
-static stat_t _homing_axis_set_zero(int8_t axis);
-static stat_t _homing_axis_move(int8_t axis, float target, float velocity);
-static stat_t _homing_abort(int8_t axis);
-static stat_t _homing_error_exit(int8_t axis, stat_t status);
-static stat_t _homing_finalize_exit(int8_t axis);
-static int8_t _get_next_axis(int8_t axis);
-
-
-// G28.2 homing cycle
-
-/* Homing works from a G28.2 according to the following writeup:
- *
- * https://github.com/synthetos
- * /TinyG/wiki/Homing-and-Limits-Description-and-Operation
- *
- * How does this work?
- *
- * Homing is invoked using a G28.2 command with 1 or more axes specified in the
- * command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant)
- *
- * Homing is always run in the following order - for each enabled axis:
- * Z,X,Y,A Note: B and C cannot be homed
- *
- * At the start of a homing cycle those switches configured for homing
- * (or for homing and limits) are treated as homing switches (they are modal).
- *
- * After initialization the following sequence is run for each axis to be homed:
- *
- * 0. If a homing or limit switch is closed on invocation, clear the switch
- * 1. Drive towards the homing switch at search velocity until switch is hit
- * 2. Drive away from the homing switch at latch velocity until switch opens
- * 3. Back off switch by the zero backoff distance and set zero for that axis
- *
- * Homing works as a state machine that is driven by registering a callback
- * function at hm.func() for the next state to be run. Once the axis is
- * initialized each callback basically does two things (1) start the move
- * for the current function, and (2) register the next state with hm.func().
- * When a move is started it will either be interrupted if the homing switch
- * changes state, This will cause the move to stop with a feedhold. The other
- * thing that can happen is the move will run to its full length if no switch
- * change is detected (hit or open),
- *
- * Once all moves for an axis are complete the next axis in the sequence is
- * homed.
- *
- * When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED
- * When homing completes successfully this is set to HOMING_HOMED, otherwise it
- * remains HOMING_NOT_HOMED.
- */
-/* --- Some further details ---
- *
- * Note: When coding a cycle (like this one) you get to perform one queued
- * move per entry into the continuation, then you must exit.
- *
- * Another Note: When coding a cycle (like this one) you must wait until
- * the last move has actually been queued (or has finished) before declaring
- * the cycle to be done. Otherwise there is a nasty race condition
- * that will accept the next command before the position of
- * the final move has been recorded in the Gcode model. That's what the call
- * to cm_isbusy() is about.
- */
-
-
-/// G28.2 homing cycle using limit switches
-stat_t cm_homing_cycle_start() {
- // save relevant non-axis parameters from Gcode model
- hm.saved_units_mode = cm_get_units_mode(&cm.gm);
- hm.saved_coord_system = cm_get_coord_system(&cm.gm);
- hm.saved_distance_mode = cm_get_distance_mode(&cm.gm);
- hm.saved_feed_rate_mode = cm_get_feed_rate_mode(&cm.gm);
- hm.saved_feed_rate = cm_get_feed_rate(&cm.gm);
-
- // set working values
- cm_set_units_mode(MILLIMETERS);
- cm_set_distance_mode(INCREMENTAL_MODE);
- cm_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates
- cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE);
- hm.set_coordinates = true;
-
- hm.axis = -1; // set to retrieve initial axis
- hm.func = _homing_axis_start; // bind initial processing function
- cm.cycle_state = CYCLE_HOMING;
- cm.homing_state = HOMING_NOT_HOMED;
-
- return STAT_OK;
-}
-
-stat_t cm_homing_cycle_start_no_set() {
- cm_homing_cycle_start();
- hm.set_coordinates = false; // don't update position variables at end of cycle
- return STAT_OK;
-}
-
-
-/// Main loop callback for running the homing cycle
-stat_t cm_homing_callback() {
- if (cm.cycle_state != CYCLE_HOMING)
- return STAT_NOOP; // exit if not in a homing cycle
- if (cm_get_runtime_busy()) return STAT_EAGAIN; // sync to planner move ends
- return hm.func(hm.axis); // execute the current homing move
-}
-
-
-/// A convenience for setting the next dispatch vector and exiting
-static stat_t _set_homing_func(homing_func_t func) {
- hm.func = func;
- return STAT_EAGAIN;
-}
-
-
-/// Get next axis, initialize variables, call the clear
-static stat_t _homing_axis_start(int8_t axis) {
- // get the first or next axis
- if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
- if (axis == -1) { // -1 is done
- cm.homing_state = HOMING_HOMED;
- return _set_homing_func(_homing_finalize_exit);
-
- } else if (axis == -2) // -2 is error
- return _homing_error_exit(-2, STAT_HOMING_ERROR_BAD_OR_NO_AXIS);
- }
-
- // clear the homed flag for axis to move w/o triggering soft limits
- cm.homed[axis] = false;
-
- // trap axis mis-configurations
- if (fp_ZERO(cm.a[axis].search_velocity))
- return _homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY);
- if (fp_ZERO(cm.a[axis].latch_velocity))
- return _homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY);
- if (cm.a[axis].latch_backoff < 0)
- return _homing_error_exit(axis, STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF);
-
- // calculate and test travel distance
- float travel_distance =
- fabs(cm.a[axis].travel_max - cm.a[axis].travel_min) +
- cm.a[axis].latch_backoff;
- if (fp_ZERO(travel_distance))
- return _homing_error_exit(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL);
-
- hm.axis = axis; // persist the axis
- // search velocity is always positive
- hm.search_velocity = fabs(cm.a[axis].search_velocity);
- // latch velocity is always positive
- hm.latch_velocity = fabs(cm.a[axis].latch_velocity);
-
- // determine which switch to use
- bool min_enabled = switch_is_enabled(MIN_SWITCH(axis));
- bool max_enabled = switch_is_enabled(MAX_SWITCH(axis));
-
- if (min_enabled) {
- // setup parameters homing to the minimum switch
- hm.homing_switch = MIN_SWITCH(axis); // min is the homing switch
- hm.limit_switch = MAX_SWITCH(axis); // max would be limit switch
- hm.search_travel = -travel_distance; // in negative direction
- hm.latch_backoff = cm.a[axis].latch_backoff; // in positive direction
- hm.zero_backoff = cm.a[axis].zero_backoff;
-
- } else if (max_enabled) {
- // setup parameters for positive travel (homing to the maximum switch)
- hm.homing_switch = MAX_SWITCH(axis); // max is homing switch
- hm.limit_switch = MIN_SWITCH(axis); // min would be limit switch
- hm.search_travel = travel_distance; // in positive direction
- hm.latch_backoff = -cm.a[axis].latch_backoff; // in negative direction
- hm.zero_backoff = -cm.a[axis].zero_backoff;
-
- } else {
- // if homing is disabled for the axis then skip to the next axis
- hm.limit_switch = -1; // disable the limit switch parameter
- return _set_homing_func(_homing_axis_start);
- }
-
- hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value
-
- return _set_homing_func(_homing_axis_clear); // start the clear
-}
-
-
-/// Initiate a clear to move off a switch that is thrown at the start
-static stat_t _homing_axis_clear(int8_t axis) {
- // Handle an initial switch closure by backing off the closed switch
- // NOTE: Relies on independent switches per axis (not shared)
-
- if (switch_is_active(hm.homing_switch))
- _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity);
-
- else if (switch_is_active(hm.limit_switch))
- _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
-
- return _set_homing_func(_homing_axis_search);
-}
-
-
-/// Fast search for switch, closes switch
-static stat_t _homing_axis_search(int8_t axis) {
- // use the homing jerk for search onward
- cm_set_axis_jerk(axis, cm.a[axis].jerk_homing);
- _homing_axis_move(axis, hm.search_travel, hm.search_velocity);
-
- return _set_homing_func(_homing_axis_latch);
-}
-
-
-/// Slow reverse until switch opens again
-static stat_t _homing_axis_latch(int8_t axis) {
- // verify assumption that we arrived here because of homing switch closure
- // rather than user-initiated feedhold or other disruption
- if (!switch_is_active(hm.homing_switch))
- return _set_homing_func(_homing_abort);
-
- _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity);
-
- return _set_homing_func(_homing_axis_zero_backoff);
-}
-
-
-/// backoff to zero position
-static stat_t _homing_axis_zero_backoff(int8_t axis) {
- _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity);
- return _set_homing_func(_homing_axis_set_zero);
-}
-
-
-/// set zero and finish up
-static stat_t _homing_axis_set_zero(int8_t axis) {
- if (hm.set_coordinates) {
- cm_set_position(axis, 0);
- cm.homed[axis] = true;
-
- } else // do not set axis if in G28.4 cycle
- cm_set_position(axis, mp_get_runtime_work_position(axis));
-
- cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value
-
- return _set_homing_func(_homing_axis_start);
-}
-
-
-/// helper that actually executes the above moves
-static stat_t _homing_axis_move(int8_t axis, float target, float velocity) {
- float vect[] = {};
- float flags[] = {};
-
- vect[axis] = target;
- flags[axis] = true;
- cm.gm.feed_rate = velocity;
- mp_flush_planner(); // don't use cm_request_queue_flush() here
- cm_request_cycle_start();
- RITORNO(cm_straight_feed(vect, flags));
-
- return STAT_EAGAIN;
-}
-
-
-/// End homing cycle in progress
-static stat_t _homing_abort(int8_t axis) {
- cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value
- _homing_finalize_exit(axis);
- report_request();
-
- return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED
-}
-
-
-static stat_t _homing_error_exit(int8_t axis, stat_t status) {
- // Generate the warning message.
- if (axis == -2)
- fprintf_P(stderr, PSTR("Homing error - Bad or no axis(es) specified"));
- else fprintf_P(stderr, PSTR("Homing error - %c axis settings misconfigured"),
- cm_get_axis_char(axis));
-
- _homing_finalize_exit(axis);
-
- return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED
-}
-
-
-/// Helper to finalize homing, third part of return to home
-static stat_t _homing_finalize_exit(int8_t axis) {
- mp_flush_planner(); // should be stopped, but in case of switch closure
-
- // restore to work coordinate system
- cm_set_coord_system(hm.saved_coord_system);
- cm_set_units_mode(hm.saved_units_mode);
- cm_set_distance_mode(hm.saved_distance_mode);
- cm_set_feed_rate_mode(hm.saved_feed_rate_mode);
- cm.gm.feed_rate = hm.saved_feed_rate;
- cm_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
- cm_cycle_end();
- cm.cycle_state = CYCLE_OFF;
-
- return STAT_OK;
-}
-
-
-/* Return next axis in sequence based on axis in arg
- *
- * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis
- * Returns next axis based on "axis" argument and if that axis is flagged for
- * homing in the gf struct
- * Returns -1 when all axes have been processed
- * Returns -2 if no axes are specified (Gcode calling error)
- * Homes Z first, then the rest in sequence
- *
- * Isolating this function facilitates implementing more complex and
- * user-specified axis homing orders
- */
-static int8_t _get_next_axis(int8_t axis) {
-#if (HOMING_AXES <= 4)
- switch (axis) {
- case -1: // inelegant brute force solution
- if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z;
- if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X;
- if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
- if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
- return -2; // error
-
- case AXIS_Z:
- if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X;
- if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
- if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
- break;
-
- case AXIS_X:
- if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
- if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
- break;
-
- case AXIS_Y:
- if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
- break;
- }
-
- return -1; // done
-
-#else
- switch (axis) {
- case -1:
- if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z;
- if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X;
- if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
- if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
- if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
- if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
- return -2; // error
-
- case AXIS_Z:
- if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X;
- if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
- if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
- if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
- if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
- break;
-
- case AXIS_X:
- if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
- if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
- if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
- if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
- break;
-
- case AXIS_Y:
- if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
- if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
- if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
- break;
-
- case AXIS_A:
- if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
- if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
- break;
-
- case AXIS_B:
- if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
- break;
- }
-
- return -1; // done
-#endif
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2016 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 "canonical_machine.h"
-#include "spindle.h"
-#include "switch.h"
-#include "util.h"
-
-#include "plan/planner.h"
-
-#include <avr/pgmspace.h>
-
-#include <math.h>
-#include <string.h>
-#include <stdbool.h>
-#include <stdio.h>
-
-
-#define MINIMUM_PROBE_TRAVEL 0.254
-
-
-struct pbProbingSingleton { // persistent probing runtime variables
- stat_t (*func)(); // binding for callback function state machine
-
- // state saved from gcode model
- uint8_t saved_distance_mode; // G90,G91 global setting
- uint8_t saved_coord_system; // G54 - G59 setting
- float saved_jerk[AXES]; // saved and restored for each axis
-
- // probe destination
- float start_position[AXES];
- float target[AXES];
- float flags[AXES];
-};
-
-static struct pbProbingSingleton pb;
-
-
-static stat_t _probing_init();
-static stat_t _probing_start();
-static stat_t _probing_finish();
-static stat_t _probing_finalize_exit();
-static stat_t _probing_error_exit(int8_t axis);
-
-
-/// A convenience for setting the next dispatch vector and exiting
-uint8_t _set_pb_func(uint8_t (*func)()) {
- pb.func = func;
- return STAT_EAGAIN;
-}
-
-
-/* All cm_probe_cycle_start does is prevent any new commands from
- * queueing to the planner so that the planner can move to a sop and
- * report MACHINE_PROGRAM_STOP. OK, it also queues the function
- * that's called once motion has stopped.
- *
- * Note: When coding a cycle (like this one) you get to perform one
- * queued move per entry into the continuation, then you must exit.
- *
- * Another Note: When coding a cycle (like this one) you must wait
- * until the last move has actually been queued (or has finished)
- * before declaring the cycle to be done. Otherwise there is a nasty
- * race condition in the tg_controller() that will accept the next
- * command before the position of the final move has been recorded in
- * the Gcode model. That's what the call to cm_get_runtime_busy() is
- * about.
- */
-
-
-/// G38.2 homing cycle using limit switches
-uint8_t cm_straight_probe(float target[], float flags[]) {
- // trap zero feed rate condition
- if (cm.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(cm.gm.feed_rate))
- return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
-
- // trap no axes specified
- if (fp_NOT_ZERO(flags[AXIS_X]) && fp_NOT_ZERO(flags[AXIS_Y]) &&
- fp_NOT_ZERO(flags[AXIS_Z]))
- return STAT_GCODE_AXIS_IS_MISSING;
-
- // set probe move endpoint
- copy_vector(pb.target, target); // set probe move endpoint
- copy_vector(pb.flags, flags); // set axes involved on the move
- clear_vector(cm.probe_results); // clear the old probe position.
- // NOTE: relying on probe_result will not detect a probe to 0,0,0.
-
- // wait until planner queue empties before completing initialization
- cm.probe_state = PROBE_WAITING;
- pb.func = _probing_init; // bind probing initialization function
- return STAT_OK;
-}
-
-
-/// main loop callback for running the homing cycle
-uint8_t cm_probe_callback() {
- if (cm.cycle_state != CYCLE_PROBE && cm.probe_state != PROBE_WAITING)
- return STAT_NOOP; // exit if not in a probe cycle or waiting for one
-
- if (cm_get_runtime_busy()) return STAT_EAGAIN; // sync to planner move ends
- return pb.func(); // execute the current homing move
-}
-
-/* G38.2 homing cycle using limit switches
- *
- * These initializations are required before starting the probing cycle.
- * They must be done after the planner has exhasted all current CYCLE moves as
- * they affect the runtime (specifically the switch modes). Side effects would
- * include limit switches initiating probe actions instead of just killing
- * movement
- */
-static uint8_t _probing_init() {
- // NOTE: it is *not* an error condition for the probe not to trigger.
- // it is an error for the limit or homing switches to fire, or for some other
- // configuration error.
- cm.probe_state = PROBE_FAILED;
- cm.cycle_state = CYCLE_PROBE;
-
- // initialize the axes - save the jerk settings & switch to the jerk_homing
- // settings
- for (uint8_t axis = 0; axis < AXES; axis++) {
- pb.saved_jerk[axis] = cm_get_axis_jerk(axis); // save the max jerk value
- cm_set_axis_jerk(axis, cm.a[axis].jerk_homing); // use homing jerk for probe
- pb.start_position[axis] = cm_get_absolute_position(axis);
- }
-
- // error if the probe target is too close to the current position
- if (get_axis_vector_length(pb.start_position, pb.target) <
- MINIMUM_PROBE_TRAVEL)
- _probing_error_exit(-2);
-
- // error if the probe target requires a move along the A/B/C axes
- for (uint8_t axis = 0; axis < AXES; axis++)
- if (fp_NE(pb.start_position[axis], pb.target[axis]))
- _probing_error_exit(axis);
-
- // probe in absolute machine coords
- pb.saved_coord_system = cm_get_coord_system(&cm.gm);
- pb.saved_distance_mode = cm_get_distance_mode(&cm.gm);
- cm_set_distance_mode(ABSOLUTE_MODE);
- cm_set_coord_system(ABSOLUTE_COORDS);
-
- cm_spindle_control(SPINDLE_OFF);
-
- // start the move
- return _set_pb_func(_probing_start);
-}
-
-
-static stat_t _probing_start() {
- // initial probe state, don't probe if we're already contacted!
- bool closed = switch_is_active(SW_PROBE);
-
- if (!closed) RITORNO(cm_straight_feed(pb.target, pb.flags));
-
- return _set_pb_func(_probing_finish);
-}
-
-
-static stat_t _probing_finish() {
- bool closed = switch_is_active(SW_PROBE);
- cm.probe_state = closed ? PROBE_SUCCEEDED : PROBE_FAILED;
-
- for (uint8_t axis = 0; axis < AXES; axis++) {
- // if we got here because of a feed hold keep the model position correct
- cm_set_position(axis, mp_get_runtime_work_position(axis));
-
- // store the probe results
- cm.probe_results[axis] = cm_get_absolute_position(axis);
- }
-
- return _set_pb_func(_probing_finalize_exit);
-}
-
-
-static void _probe_restore_settings() {
- // we should be stopped now, but in case of switch closure
- mp_flush_planner();
-
- // restore axis jerk
- for (uint8_t axis = 0; axis < AXES; axis++ )
- cm_set_axis_jerk(axis, pb.saved_jerk[axis]);
-
- // restore coordinate system and distance mode
- cm_set_coord_system(pb.saved_coord_system);
- cm_set_distance_mode(pb.saved_distance_mode);
-
- // update the model with actual position
- cm_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
- cm_cycle_end();
- cm.cycle_state = CYCLE_OFF;
-}
-
-
-static stat_t _probing_finalize_exit() {
- _probe_restore_settings();
- return STAT_OK;
-}
-
-
-static stat_t _probing_error_exit(int8_t axis) {
- // Generate the warning message.
- if (axis == -2)
- fprintf_P(stderr, PSTR("Probing error - invalid probe destination"));
- else fprintf_P(stderr,
- PSTR("Probing error - %c axis cannot move during probing"),
- cm_get_axis_char(axis));
-
- // clean up and exit
- _probe_restore_settings();
-
- return STAT_PROBE_CYCLE_FAILED;
-}
#include "canonical_machine.h"
#include "spindle.h"
+#include "probing.h"
+#include "homing.h"
#include "util.h"
#include <stdbool.h>
static stat_t _execute_gcode_block() {
stat_t status = STAT_OK;
- cm_set_model_linenum(cm.gn.linenum);
+ cm_set_model_line(cm.gn.line);
EXEC_FUNC(cm_set_feed_rate_mode, feed_rate_mode);
EXEC_FUNC(cm_set_feed_rate, feed_rate);
EXEC_FUNC(cm_feed_rate_override_factor, feed_rate_override_factor);
status = cm_goto_g30_position(cm.gn.target, cm.gf.target);
break;
case NEXT_ACTION_SEARCH_HOME: // G28.2
- status = cm_homing_cycle_start();
+ cm_homing_cycle_start();
break;
case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3
cm_set_absolute_origin(cm.gn.target, cm.gf.target);
break;
case NEXT_ACTION_HOMING_NO_SET: // G28.4
- status = cm_homing_cycle_start_no_set();
+ cm_homing_cycle_start_no_set();
break;
case NEXT_ACTION_STRAIGHT_PROBE: // G38.2
status = cm_straight_probe(cm.gn.target, cm.gf.target);
case 'J': SET_NON_MODAL(arc_offset[1], value);
case 'K': SET_NON_MODAL(arc_offset[2], value);
case 'R': SET_NON_MODAL(arc_radius, value);
- case 'N': SET_NON_MODAL(linenum, (uint32_t)value); // line number
+ case 'N': SET_NON_MODAL(line, (uint32_t)value); // line number
case 'L': break; // not used for anything
case 0: break;
default: status = STAT_GCODE_COMMAND_UNSUPPORTED;
/// Controller's rest handler
-stat_t hw_reset_handler() {
+void hw_reset_handler() {
if (hw.hard_reset) hw_hard_reset();
if (hw.bootloader) {
// TODO enable bootloader interrupt vectors and jump to BOOT_SECTION_START
hw.bootloader = false;
}
-
- return STAT_NOOP;
}
void hardware_init();
void hw_request_hard_reset();
void hw_hard_reset();
-stat_t hw_reset_handler();
+void hw_reset_handler();
void hw_request_bootloader();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 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 "canonical_machine.h"
+#include "switch.h"
+#include "util.h"
+#include "report.h"
+
+#include "plan/planner.h"
+
+#include <avr/pgmspace.h>
+
+#include <stdbool.h>
+#include <math.h>
+#include <stdio.h>
+
+
+typedef void (*homing_func_t)(int8_t axis);
+
+static void _homing_axis_start(int8_t axis);
+
+
+/// persistent homing runtime variables
+struct hmHomingSingleton {
+ // controls for homing cycle
+ int8_t axis; // axis currently being homed
+
+ /// homing switch for current axis (index into switch flag table)
+ int8_t homing_switch;
+ int8_t limit_switch; // limit switch for current axis or -1 if none
+
+ uint8_t homing_closed; // 0=open, 1=closed
+ uint8_t limit_closed; // 0=open, 1=closed
+ /// G28.4 flag. true = set coords to zero at the end of homing cycle
+ uint8_t set_coordinates;
+ homing_func_t func; // binding for callback function state machine
+
+ // per-axis parameters
+ /// set to 1 for positive (max), -1 for negative (to min);
+ float direction;
+ float search_travel; // signed distance to travel in search
+ float search_velocity; // search speed as positive number
+ float latch_velocity; // latch speed as positive number
+ /// max distance to back off switch during latch phase
+ float latch_backoff;
+ /// distance to back off switch before setting zero
+ float zero_backoff;
+ /// maximum distance of switch clearing backoffs before erring out
+ float max_clear_backoff;
+
+ // state saved from gcode model
+ uint8_t saved_units_mode; // G20,G21 global setting
+ uint8_t saved_coord_system; // G54 - G59 setting
+ uint8_t saved_distance_mode; // G90,G91 global setting
+ uint8_t saved_feed_rate_mode; // G93,G94 global setting
+ float saved_feed_rate; // F setting
+ float saved_jerk; // saved and restored for each axis homed
+};
+static struct hmHomingSingleton hm;
+
+
+// G28.2 homing cycle
+
+/* Homing works from a G28.2 according to the following writeup:
+ *
+ * https://github.com/synthetos
+ * /TinyG/wiki/Homing-and-Limits-Description-and-Operation
+ *
+ * How does this work?
+ *
+ * Homing is invoked using a G28.2 command with 1 or more axes specified in the
+ * command: e.g. g28.2 x0 y0 z0 (FYI: the number after each axis is irrelevant)
+ *
+ * Homing is always run in the following order - for each enabled axis:
+ * Z,X,Y,A Note: B and C cannot be homed
+ *
+ * At the start of a homing cycle those switches configured for homing
+ * (or for homing and limits) are treated as homing switches (they are modal).
+ *
+ * After initialization the following sequence is run for each axis to be homed:
+ *
+ * 0. If a homing or limit switch is closed on invocation, clear the switch
+ * 1. Drive towards the homing switch at search velocity until switch is hit
+ * 2. Drive away from the homing switch at latch velocity until switch opens
+ * 3. Back off switch by the zero backoff distance and set zero for that axis
+ *
+ * Homing works as a state machine that is driven by registering a callback
+ * function at hm.func() for the next state to be run. Once the axis is
+ * initialized each callback basically does two things (1) start the move
+ * for the current function, and (2) register the next state with hm.func().
+ * When a move is started it will either be interrupted if the homing switch
+ * changes state, This will cause the move to stop with a feedhold. The other
+ * thing that can happen is the move will run to its full length if no switch
+ * change is detected (hit or open),
+ *
+ * Once all moves for an axis are complete the next axis in the sequence is
+ * homed.
+ *
+ * When a homing cycle is initiated the homing state is set to HOMING_NOT_HOMED
+ * When homing completes successfully this is set to HOMING_HOMED, otherwise it
+ * remains HOMING_NOT_HOMED.
+ */
+/* --- Some further details ---
+ *
+ * Note: When coding a cycle (like this one) you get to perform one queued
+ * move per entry into the continuation, then you must exit.
+ *
+ * Another Note: When coding a cycle (like this one) you must wait until
+ * the last move has actually been queued (or has finished) before declaring
+ * the cycle to be done. Otherwise there is a nasty race condition
+ * that will accept the next command before the position of
+ * the final move has been recorded in the Gcode model. That's what the call
+ * to cm_isbusy() is about.
+ */
+
+
+/* Return next axis in sequence based on axis in arg
+ *
+ * Accepts "axis" arg as the current axis; or -1 to retrieve the first axis
+ * Returns next axis based on "axis" argument and if that axis is flagged for
+ * homing in the gf struct
+ * Returns -1 when all axes have been processed
+ * Returns -2 if no axes are specified (Gcode calling error)
+ * Homes Z first, then the rest in sequence
+ *
+ * Isolating this function facilitates implementing more complex and
+ * user-specified axis homing orders
+ */
+static int8_t _get_next_axis(int8_t axis) {
+ switch (axis) {
+ case -1: if (fp_TRUE(cm.gf.target[AXIS_Z])) return AXIS_Z;
+ case AXIS_Z: if (fp_TRUE(cm.gf.target[AXIS_X])) return AXIS_X;
+ case AXIS_X: if (fp_TRUE(cm.gf.target[AXIS_Y])) return AXIS_Y;
+ case AXIS_Y: if (fp_TRUE(cm.gf.target[AXIS_A])) return AXIS_A;
+ case AXIS_A: if (fp_TRUE(cm.gf.target[AXIS_B])) return AXIS_B;
+ case AXIS_B: if (fp_TRUE(cm.gf.target[AXIS_C])) return AXIS_C;
+ }
+
+ return axis == -1 ? -2 : -1; // error or done
+}
+
+
+/// Helper to finalize homing, third part of return to home
+static void _homing_finalize_exit() {
+ mp_flush_planner(); // should be stopped, but in case of switch closure
+
+ // restore to work coordinate system
+ cm_set_coord_system(hm.saved_coord_system);
+ cm_set_units_mode(hm.saved_units_mode);
+ cm_set_distance_mode(hm.saved_distance_mode);
+ cm_set_feed_rate_mode(hm.saved_feed_rate_mode);
+ cm.gm.feed_rate = hm.saved_feed_rate;
+ cm_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
+ cm_cycle_end();
+ cm.cycle_state = CYCLE_OFF;
+}
+
+
+static void _homing_error_exit(stat_t status) {
+ _homing_finalize_exit();
+ status_error(status);
+}
+
+
+/// helper that actually executes the above moves
+static void _homing_axis_move(int8_t axis, float target, float velocity) {
+ float vect[] = {};
+ float flags[] = {};
+
+ vect[axis] = target;
+ flags[axis] = true;
+ cm.gm.feed_rate = velocity;
+ mp_flush_planner(); // don't use cm_request_queue_flush() here
+ cm_request_cycle_start();
+
+ stat_t status = cm_straight_feed(vect, flags);
+ if (status) _homing_error_exit(status);
+}
+
+
+/// End homing cycle in progress
+static void _homing_abort(int8_t axis) {
+ cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value
+
+ // homing state remains HOMING_NOT_HOMED
+ _homing_error_exit(STAT_HOMING_CYCLE_FAILED);
+
+ report_request();
+}
+
+
+/// set zero and finish up
+static void _homing_axis_set_zero(int8_t axis) {
+ if (hm.set_coordinates) {
+ cm_set_position(axis, 0);
+ cm.homed[axis] = true;
+
+ } else // do not set axis if in G28.4 cycle
+ cm_set_position(axis, mp_get_runtime_work_position(axis));
+
+ cm_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value
+
+ hm.func = _homing_axis_start;
+}
+
+
+/// backoff to zero position
+static void _homing_axis_zero_backoff(int8_t axis) {
+ _homing_axis_move(axis, hm.zero_backoff, hm.search_velocity);
+ hm.func = _homing_axis_set_zero;
+}
+
+
+/// Slow reverse until switch opens again
+static void _homing_axis_latch(int8_t axis) {
+ // verify assumption that we arrived here because of homing switch closure
+ // rather than user-initiated feedhold or other disruption
+ if (!switch_is_active(hm.homing_switch)) hm.func = _homing_abort;
+
+ else {
+ _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity);
+ hm.func = _homing_axis_zero_backoff;
+ }
+}
+
+
+/// Fast search for switch, closes switch
+static void _homing_axis_search(int8_t axis) {
+ // use the homing jerk for search onward
+ cm_set_axis_jerk(axis, cm.a[axis].jerk_homing);
+ _homing_axis_move(axis, hm.search_travel, hm.search_velocity);
+ hm.func = _homing_axis_latch;
+}
+
+
+/// Initiate a clear to move off a switch that is thrown at the start
+static void _homing_axis_clear(int8_t axis) {
+ // Handle an initial switch closure by backing off the closed switch
+ // NOTE: Relies on independent switches per axis (not shared)
+
+ if (switch_is_active(hm.homing_switch))
+ _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity);
+
+ else if (switch_is_active(hm.limit_switch))
+ _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity);
+
+ hm.func = _homing_axis_search;
+}
+
+
+/// Get next axis, initialize variables, call the clear
+static void _homing_axis_start(int8_t axis) {
+ // get the first or next axis
+ if ((axis = _get_next_axis(axis)) < 0) { // axes are done or error
+ if (axis == -1) { // -1 is done
+ cm.homing_state = HOMING_HOMED;
+ _homing_finalize_exit();
+ return;
+
+ } else if (axis == -2) // -2 is error
+ return _homing_error_exit(STAT_HOMING_ERROR_BAD_OR_NO_AXIS);
+ }
+
+ // clear the homed flag for axis to move w/o triggering soft limits
+ cm.homed[axis] = false;
+
+ // trap axis mis-configurations
+ if (fp_ZERO(cm.a[axis].search_velocity))
+ return _homing_error_exit(STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY);
+ if (fp_ZERO(cm.a[axis].latch_velocity))
+ return _homing_error_exit(STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY);
+ if (cm.a[axis].latch_backoff < 0)
+ return _homing_error_exit(STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF);
+
+ // calculate and test travel distance
+ float travel_distance =
+ fabs(cm.a[axis].travel_max - cm.a[axis].travel_min) +
+ cm.a[axis].latch_backoff;
+ if (fp_ZERO(travel_distance))
+ return _homing_error_exit(STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL);
+
+ hm.axis = axis; // persist the axis
+ // search velocity is always positive
+ hm.search_velocity = fabs(cm.a[axis].search_velocity);
+ // latch velocity is always positive
+ hm.latch_velocity = fabs(cm.a[axis].latch_velocity);
+
+ // determine which switch to use
+ bool min_enabled = switch_is_enabled(MIN_SWITCH(axis));
+ bool max_enabled = switch_is_enabled(MAX_SWITCH(axis));
+
+ if (min_enabled) {
+ // setup parameters homing to the minimum switch
+ hm.homing_switch = MIN_SWITCH(axis); // min is the homing switch
+ hm.limit_switch = MAX_SWITCH(axis); // max would be limit switch
+ hm.search_travel = -travel_distance; // in negative direction
+ hm.latch_backoff = cm.a[axis].latch_backoff; // in positive direction
+ hm.zero_backoff = cm.a[axis].zero_backoff;
+
+ } else if (max_enabled) {
+ // setup parameters for positive travel (homing to the maximum switch)
+ hm.homing_switch = MAX_SWITCH(axis); // max is homing switch
+ hm.limit_switch = MIN_SWITCH(axis); // min would be limit switch
+ hm.search_travel = travel_distance; // in positive direction
+ hm.latch_backoff = -cm.a[axis].latch_backoff; // in negative direction
+ hm.zero_backoff = -cm.a[axis].zero_backoff;
+
+ } else {
+ // if homing is disabled for the axis then skip to the next axis
+ hm.limit_switch = -1; // disable the limit switch parameter
+ hm.func = _homing_axis_start;
+ return;
+ }
+
+ hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value
+ hm.func = _homing_axis_clear; // start the clear
+}
+
+
+bool cm_is_homing() {
+ return cm.cycle_state == CYCLE_HOMING;
+}
+
+
+/// G28.2 homing cycle using limit switches
+void cm_homing_cycle_start() {
+ // save relevant non-axis parameters from Gcode model
+ hm.saved_units_mode = cm_get_units_mode(&cm.gm);
+ hm.saved_coord_system = cm_get_coord_system(&cm.gm);
+ hm.saved_distance_mode = cm_get_distance_mode(&cm.gm);
+ hm.saved_feed_rate_mode = cm_get_feed_rate_mode(&cm.gm);
+ hm.saved_feed_rate = cm_get_feed_rate(&cm.gm);
+
+ // set working values
+ cm_set_units_mode(MILLIMETERS);
+ cm_set_distance_mode(INCREMENTAL_MODE);
+ cm_set_coord_system(ABSOLUTE_COORDS); // in machine coordinates
+ cm_set_feed_rate_mode(UNITS_PER_MINUTE_MODE);
+ hm.set_coordinates = true;
+
+ hm.axis = -1; // set to retrieve initial axis
+ hm.func = _homing_axis_start; // bind initial processing function
+ cm.cycle_state = CYCLE_HOMING;
+ cm.homing_state = HOMING_NOT_HOMED;
+}
+
+
+void cm_homing_cycle_start_no_set() {
+ cm_homing_cycle_start();
+ hm.set_coordinates = false; // don't update position variables at end of cycle
+}
+
+
+/// Main loop callback for running the homing cycle
+void cm_homing_callback() {
+ if (cm.cycle_state != CYCLE_HOMING || cm_get_runtime_busy()) return;
+ hm.func(hm.axis); // execute the current homing move
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 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>
+
+
+bool cm_is_homing();
+void cm_homing_cycle_start();
+void cm_homing_cycle_start_no_set();
+void cm_homing_callback();
ha.response[ha.response_length - 2];
if (computed != expected) {
- printf(PSTR("huanyang: invalid CRC, expected=0x%04u got=0x%04u\n"),
- expected, computed);
+ STATUS_WARNING("huanyang: invalid CRC, expected=0x%04u got=0x%04u",
+ expected, computed);
return false;
}
// Check return function code matches sent
if (ha.command[1] != ha.response[1]) {
- printf_P(PSTR("huanyang: invalid function code, expected=%u got=%u\n"),
- ha.command[2], ha.response[2]);
+ STATUS_WARNING("huanyang: invalid function code, expected=%u got=%u",
+ ha.command[2], ha.response[2]);
return false;
}
_set_rxc_interrupt(false);
_set_dre_interrupt(true);
- if (ha.debug) printf_P(PSTR("huanyang: retry %d\n"), ha.retry);
+ if (ha.debug) STATUS_DEBUG("huanyang: retry %d", ha.retry);
}
void huanyang_set(cmSpindleMode_t mode, float speed) {
if (ha.mode != mode || ha.speed != speed) {
- if (ha.debug)
- printf_P(PSTR("huanyang: mode=%d, speed=%0.2f\n"), mode, speed);
+ if (ha.debug) STATUS_DEBUG("huanyang: mode=%d, speed=%0.2f", mode, speed);
ha.mode = mode;
ha.speed = speed;
if (ha.last && rtc_expired(ha.last + HUANYANG_TIMEOUT)) {
if (ha.retry < HUANYANG_RETRIES) _retry_command();
else {
- if (ha.debug) printf_P(PSTR("huanyang: timedout\n"));
+ if (ha.debug) STATUS_DEBUG("huanyang: timedout");
if (ha.debug && ha.current_offset) {
- printf_P(PSTR("huanyang: sent 0x"));
+ const uint8_t buf_len = 8 * 2 + 1;
+ char sent[buf_len];
+ char received[buf_len];
- for (uint8_t i = 0; i < ha.command_length; i++)
- printf("%02x", ha.command[i]);
+ uint8_t i;
+ for (i = 0; i < ha.command_length; i++)
+ sprintf(sent + i * 2, "%02x", ha.command[i]);
+ sent[i * 2] = 0;
- printf_P(PSTR(" received 0x"));
- for (uint8_t i = 0; i < ha.current_offset; i++)
- printf("%02x", ha.response[i]);
+ for (i = 0; i < ha.current_offset; i++)
+ sprintf(received + i * 2, "%02x", ha.response[i]);
+ received[i * 2] = 0;
- printf_P(PSTR(" expected %u bytes"), ha.response_length);
-
- putchar('\n');
+ STATUS_DEBUG("huanyang: sent 0x%s received 0x%s expected %u bytes",
+ sent, received, ha.response_length);
}
huanyang_reset();
#include "report.h"
#include "command.h"
#include "estop.h"
+#include "probing.h"
+#include "homing.h"
#include "plan/planner.h"
#include "plan/buffer.h"
#include <stdbool.h>
-static bool _dispatch(stat_t (*func)()) {
- stat_t err = func();
-
- switch (err) {
- case STAT_EAGAIN: return true;
- case STAT_OK: case STAT_NOOP: break;
- default: status_error(err); break;
- }
-
- return false;
-}
-
-
-/* Main loop
- *
- * The task order is very important. Tasks are ordered by increasing dependency
- * (blocking hierarchy). Tasks that are dependent on completion of lower-level
- * tasks must be later in the list. Tasks are called repeatedly even if they
- * are not currently active.
- *
- * The DISPATCH macro calls a function and returns if not finished
- * (STAT_EAGAIN). This prevents later routines from running. Any other
- * condition - OK or ERR - drops through and runs the next routine in the list.
- */
-static void _run() {
-#define DISPATCH(func) if (_dispatch(func)) return;
-
- DISPATCH(hw_reset_handler); // handle hard reset requests
-
- DISPATCH(command_hi);
-
- DISPATCH(tmc2660_sync); // synchronize driver config
- DISPATCH(motor_power_callback); // stepper motor power sequencing
- DISPATCH(cm_feedhold_sequencing_callback); // feedhold state machine
- DISPATCH(mp_plan_hold_callback); // plan a feedhold
- DISPATCH(cm_arc_callback); // arc generation runs
- DISPATCH(cm_homing_callback); // G28.2 continuation
- DISPATCH(cm_probe_callback); // G38.2 continuation
- DISPATCH(report_callback); // report changes
-
- DISPATCH(command_lo);
-}
-
+int main() {
+ //wdt_enable(WDTO_250MS);
-static void _init() {
cli(); // disable interrupts
hardware_init(); // hardware setup - must be first
fprintf_P(stderr, PSTR("\n{\"firmware\": \"Buildbotics AVR\", "
"\"version\": \"" VERSION "\"}\n"));
-}
-
-
-int main() {
- //wdt_enable(WDTO_250MS);
-
- _init();
// main loop
while (true) {
- _run(); // single pass
+ hw_reset_handler(); // handle hard reset requests
+ cm_feedhold_sequencing_callback(); // feedhold state machine
+ mp_plan_hold_callback(); // plan a feedhold
+ cm_arc_callback(); // arc generation runs
+ cm_homing_callback(); // G28.2 continuation
+ cm_probe_callback(); // G38.2 continuation
+ command_callback(); // process next command
+ report_callback(); // report changes
+
wdt_reset();
}
STAT_MSG(BUFFER_FULL_FATAL, "Buffer full - fatal")
STAT_MSG(EEPROM_DATA_INVALID, "EEPROM data invalid")
STAT_MSG(MOTOR_ERROR, "Motor error")
+STAT_MSG(STEP_CHECK_FAILED, "Step check failed")
STAT_MSG(INTERNAL_ERROR, "Internal error")
STAT_MSG(PREP_LINE_MOVE_TIME_IS_INFINITE, "Move time is infinite")
// 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 - Homing switches misconfigured")
// Probing
-STAT_MSG(PROBE_CYCLE_FAILED, "Probe cycle failed")
+STAT_MSG(PROBE_INVALID_DEST, "Probing error - invalid destination")
+STAT_MSG(MOVE_DURING_PROBE, "Probing error - move during probe")
// End of stats marker
STAT_MSG(MAX, "")
/// Callback to manage motor power sequencing and power-down timing.
-stat_t motor_power_callback() { // called by controller
+stat_t motor_rtc_callback() { // called by controller
for (int motor = 0; motor < MOTORS; motor++)
- // Deenergize motor if disabled, in error or after timeout when not holding
- if (motors[motor].power_mode == MOTOR_DISABLED || estop_triggered() ||
- rtc_expired(motors[motor].timeout))
- _deenergize(motor);
+ // Deenergize motor if disabled or timedout
+ if (motors[motor].power_mode == MOTOR_DISABLED ||
+ rtc_expired(motors[motor].timeout)) _deenergize(motor);
return STAT_OK;
}
motors[motor].flags |= errors;
report_request();
- if (motor_error(motor)) {
- printf_P(PSTR("\nmotor %d flags="), motor);
- print_status_flags(motors[motor].flags);
- CM_ALARM(STAT_MOTOR_ERROR);
- }
+ if (motor_error(motor)) CM_ALARM(STAT_MOTOR_ERROR);
}
uint32_t clocks = seg_clocks >> (m->timer_clock - 1); // Motor timer clocks
float steps = (float)clocks / m->timer_period;
float diff = fabs(fabs(travel_steps) - steps);
- if (10 < diff)
- printf_P(PSTR("clock=%u period=%u expected=%f actual=%f diff=%f\n"),
- m->timer_clock, m->timer_period, fabs(travel_steps), steps,
- diff);
+ if (10 < diff) CM_ALARM(STAT_STEP_CHECK_FAILED);
}
// Setup the direction, compensating for polarity.
bool motor_energizing();
void motor_driver_callback(int motor);
-stat_t motor_power_callback();
+stat_t motor_rtc_callback();
void motor_error_callback(int motor, cmMotorFlags_t errors);
void motor_load_move(int motor);
} else if (offset_i) return STAT_ARC_SPECIFICATION_ERROR;
}
- // set values in the Gcode model state & copy it (linenum was already
+ // set values in the Gcode model state & copy it (line was already
// captured)
cm_set_model_target(target, flags);
// now get down to the rest of the work setting up the arc for execution
cm.gm.motion_mode = motion_mode;
cm_set_work_offsets(&cm.gm); // resolved offsets to gm
- memcpy(&arc.ms, &cm.ms, sizeof(MoveState_t));// context to arc singleton
+ cm.ms.line = cm.gm.line; // copy line number
+ memcpy(&arc.ms, &cm.ms, sizeof(MoveState_t)); // context to arc singleton
- copy_vector(arc.position, cm.position); // arc pos from gcode model
+ copy_vector(arc.position, cm.position); // arc pos from gcode model
- arc.radius = TO_MILLIMETERS(radius); // set arc radius or zero
+ arc.radius = TO_MILLIMETERS(radius); // set arc radius or zero
- arc.offset[0] = TO_MILLIMETERS(i); // offsets canonical form (mm)
+ arc.offset[0] = TO_MILLIMETERS(i); // offsets canonical form (mm)
arc.offset[1] = TO_MILLIMETERS(j);
arc.offset[2] = TO_MILLIMETERS(k);
*
* Parts of this routine were originally sourced from the grbl project.
*/
-stat_t cm_arc_callback() {
- if (arc.run_state == MOVE_OFF) return STAT_NOOP;
- if (mp_get_planner_buffers_available() < PLANNER_BUFFER_HEADROOM)
- return STAT_EAGAIN;
-
- arc.theta += arc.arc_segment_theta;
- arc.ms.target[arc.plane_axis_0] = arc.center_0 + sin(arc.theta) * arc.radius;
- arc.ms.target[arc.plane_axis_1] = arc.center_1 + cos(arc.theta) * arc.radius;
- arc.ms.target[arc.linear_axis] += arc.arc_segment_linear_travel;
- mp_aline(&arc.ms); // run the line
- copy_vector(arc.position, arc.ms.target); // update arc current pos
+void cm_arc_callback() {
+ while (arc.run_state != MOVE_OFF && mp_get_planner_buffer_room()) {
+ arc.theta += arc.arc_segment_theta;
+ arc.ms.target[arc.plane_axis_0] =
+ arc.center_0 + sin(arc.theta) * arc.radius;
+ arc.ms.target[arc.plane_axis_1] =
+ arc.center_1 + cos(arc.theta) * arc.radius;
+ arc.ms.target[arc.linear_axis] += arc.arc_segment_linear_travel;
+
+ mp_aline(&arc.ms); // run the line
+ copy_vector(arc.position, arc.ms.target); // update arc current pos
+
+ if (!--arc.arc_segment_count) arc.run_state = MOVE_OFF;
+ }
+}
- if (--arc.arc_segment_count > 0) return STAT_EAGAIN;
- arc.run_state = MOVE_OFF;
-
- return STAT_OK;
-}
+bool cm_arc_active() {return arc.run_state != MOVE_OFF;}
/// Stop arc movement without maintaining position
#pragma once
-#include "util.h"
-#include "status.h"
+#include <stdbool.h>
#define ARC_SEGMENT_LENGTH 0.1 // mm
#define MIN_ARC_SEGMENT_TIME (MIN_ARC_SEGMENT_USEC / MICROSECONDS_PER_MINUTE)
-stat_t cm_arc_callback();
+void cm_arc_callback();
+bool cm_arc_active();
void cm_abort_arc();
*/
#include "buffer.h"
+#include "report.h"
#include <string.h>
typedef struct mpBufferPool { // ring buffer for sub-moves
- uint8_t buffers_available; // running count of available buffers
+ volatile uint8_t buffers_available; // running count of available buffers
mpBuf_t *w; // get_write_buffer pointer
mpBuf_t *q; // queue_write_buffer pointer
mpBuf_t *r; // get/end_run_buffer pointer
mpBufferPool_t mb; // move buffer queue
-/// Returns # of available planner buffers
-uint8_t mp_get_planner_buffers_available() {return mb.buffers_available;}
+uint8_t mp_get_planner_buffer_room() {
+ uint16_t n = mb.buffers_available;
+ return n < PLANNER_BUFFER_HEADROOM ? 0 : n - PLANNER_BUFFER_HEADROOM;
+}
+
+
+void mp_wait_for_buffer() {
+ while (!mb.buffers_available) continue;
+}
/// buffer incr & wrap
mb.buffers_available--;
mb.w = w->nx;
+ report_request();
+
return w;
}
}
-/// Free write buffer if you decide not to commit it.
-void mp_unget_write_buffer() {
- mb.w = mb.w->pv; // queued --> write
- mb.w->buffer_state = MP_BUFFER_EMPTY; // not loading anymore
- mb.buffers_available++;
-}
-
-
/* Commit the next write buffer to the queue
* Advances write pointer & changes buffer state
*
* buffer once it has been queued. Action may start on the buffer immediately,
* invalidating its contents
*/
-void mp_commit_write_buffer(const uint8_t move_type) {
+void mp_commit_write_buffer(uint32_t line, moveType_t move_type) {
+ mb.q->ms.line = line;
mb.q->move_type = move_type;
mb.q->move_state = MOVE_NEW;
mb.q->buffer_state = MP_BUFFER_QUEUED;
mb.r->buffer_state = MP_BUFFER_PENDING; // pend next buffer
mb.buffers_available++;
+ report_request();
return mb.w == mb.r; // return true if the queue emptied
}
#pragma once
#include "canonical_machine.h"
+#include "config.h"
#include <stdbool.h>
-/* PLANNER_BUFFER_POOL_SIZE
- * Should be at least the number of buffers requires to support optimal
- * planning in the case of very short lines or arc segments.
- * Suggest 12 min. Limit is 255
- */
-#define PLANNER_BUFFER_POOL_SIZE 32
-
-/// Buffers to reserve in planner before processing new input line
-#define PLANNER_BUFFER_HEADROOM 4
-
typedef enum { // bf->move_type values
MOVE_TYPE_NULL, // null move - does a no-op
} moveType_t;
typedef enum {
- MOVE_OFF, // move inactive (MUST BE ZERO)
- MOVE_NEW, // general value if you need an initialization
- MOVE_RUN, // general run state (for non-acceleration moves)
- MOVE_SKIP_BLOCK // mark a skipped block
+ MOVE_OFF, // move inactive (MUST BE ZERO)
+ MOVE_NEW, // initial value
+ MOVE_RUN, // general run state (for non-acceleration moves)
} moveState_t;
typedef enum {
- SECTION_OFF, // section inactive
- SECTION_NEW, // uninitialized section
- SECTION_1st_HALF, // first half of S curve
- SECTION_2nd_HALF // second half of S curve or running a BODY (cruise)
+ SECTION_OFF, // section inactive
+ SECTION_NEW, // uninitialized section
+ SECTION_1st_HALF, // first half of S curve
+ SECTION_2nd_HALF // second half of S curve or running a BODY (cruise)
} sectionState_t;
// All the enums that equal zero must be zero. Don't change this
} mpBuf_t;
-uint8_t mp_get_planner_buffers_available();
+uint8_t mp_get_planner_buffer_room();
+void mp_wait_for_buffer();
void mp_init_buffers();
mpBuf_t *mp_get_write_buffer();
-void mp_unget_write_buffer();
-void mp_commit_write_buffer(const uint8_t move_type);
+void mp_commit_write_buffer(uint32_t line, moveType_t move_type);
mpBuf_t *mp_get_run_buffer();
uint8_t mp_free_run_buffer();
mpBuf_t *mp_get_first_buffer();
if (cal.reverse) {
int32_t steps = -motor_get_encoder(cal.motor);
float mm = (float)steps / motor_get_steps_per_unit(cal.motor);
- printf("%"PRIi32" steps %0.2f mm\n", steps, mm);
+ STATUS_DEBUG("%"PRIi32" steps %0.2f mm", steps, mm);
tmc2660_set_stallguard_threshold(cal.motor, 63);
mp_free_run_buffer(); // Release buffer
cal.motor = 1;
bf->bf_func = _exec_calibrate; // register callback
- mp_commit_write_buffer(MOVE_TYPE_COMMAND);
+ mp_commit_write_buffer(-1, MOVE_TYPE_COMMAND);
return 0;
}
}
// Must be final operation before exit
- mp_commit_write_buffer(MOVE_TYPE_COMMAND);
+ mp_commit_write_buffer(cm_get_line(), MOVE_TYPE_COMMAND);
}
bf->move_state = MOVE_NEW;
// must be final operation before exit
- mp_commit_write_buffer(MOVE_TYPE_DWELL);
+ mp_commit_write_buffer(cm_get_line(), MOVE_TYPE_DWELL);
return STAT_OK;
}
#include "status.h"
-stat_t mp_dwell(const float seconds);
+stat_t mp_dwell(float seconds);
// copy in the gcode model state
memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t));
bf->replannable = false;
+ report_request();
// short lines have already been removed, look for an actual zero
if (fp_ZERO(bf->length)) {
if (mr.section == SECTION_HEAD) status = _exec_aline_head();
else if (mr.section == SECTION_BODY) status = _exec_aline_body();
else if (mr.section == SECTION_TAIL) status = _exec_aline_tail();
- else if (mr.move_state == MOVE_SKIP_BLOCK) status = STAT_OK;
else return CM_ALARM(STAT_INTERNAL_ERROR); // never supposed to get here
// Feedhold processing. Refer to canonical_machine.h for state machine
/// replan block list to execute hold
-stat_t mp_plan_hold_callback() {
- if (cm.hold_state != FEEDHOLD_PLAN)
- return STAT_NOOP; // not planning a feedhold
+void mp_plan_hold_callback() {
+ if (cm.hold_state != FEEDHOLD_PLAN) return; // not planning a feedhold
mpBuf_t *bp = mp_get_run_buffer(); // working buffer pointer
- if (!bp) return STAT_NOOP; // Oops! nothing's running
+ if (!bp) return; // Oops! nothing's running
uint8_t mr_flag = true; // used to tell replan to account for mr buffer Vx
float mr_available_length; // length left in mr buffer for deceleration
mp_plan_block_list(mp_get_last_buffer(), &mr_flag);
cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit
- return STAT_OK;
+ return;
}
// Case 2: deceleration exceeds length remaining in mr buffer
_reset_replannable_list(); // replan all the blocks
mp_plan_block_list(mp_get_last_buffer(), &mr_flag);
cm.hold_state = FEEDHOLD_DECEL; // set state to decelerate and exit
-
- return STAT_OK;
}
/// End a feedhold, release the hold and restart block list
-stat_t mp_end_hold() {
+void mp_end_hold() {
if (cm.hold_state == FEEDHOLD_END_HOLD) {
cm.hold_state = FEEDHOLD_OFF;
- if (!mp_get_run_buffer()) { // 0 means nothing's running
- cm_set_motion_state(MOTION_STOP);
- return STAT_NOOP;
- }
-
- cm.motion_state = MOTION_RUN;
+ // 0 means nothing's running
+ if (!mp_get_run_buffer()) cm_set_motion_state(MOTION_STOP);
+ else cm.motion_state = MOTION_RUN;
}
-
- return STAT_OK;
}
#include "status.h"
-stat_t mp_plan_hold_callback();
-stat_t mp_end_hold();
+void mp_plan_hold_callback();
+void mp_end_hold();
// Start
jr.running = true;
bf->bf_func = _exec_jog; // register callback
- mp_commit_write_buffer(MOVE_TYPE_COMMAND);
+ mp_commit_write_buffer(-1, MOVE_TYPE_COMMAND);
}
return 0;
mp_plan_block_list(bf, &mr_flag); // replan block list
copy_vector(mm.position, bf->ms.target); // set the planner position
// commit current block (must follow the position update)
- mp_commit_write_buffer(MOVE_TYPE_ALINE);
+ mp_commit_write_buffer(ms->line, MOVE_TYPE_ALINE);
return STAT_OK;
}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 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 "canonical_machine.h"
+#include "spindle.h"
+#include "switch.h"
+#include "util.h"
+
+#include "plan/planner.h"
+
+#include <avr/pgmspace.h>
+
+#include <math.h>
+#include <string.h>
+#include <stdbool.h>
+#include <stdio.h>
+
+
+#define MINIMUM_PROBE_TRAVEL 0.254
+
+
+struct pbProbingSingleton { // persistent probing runtime variables
+ void (*func)(); // binding for callback function state machine
+
+ // state saved from gcode model
+ uint8_t saved_distance_mode; // G90,G91 global setting
+ uint8_t saved_coord_system; // G54 - G59 setting
+ float saved_jerk[AXES]; // saved and restored for each axis
+
+ // probe destination
+ float start_position[AXES];
+ float target[AXES];
+ float flags[AXES];
+};
+
+static struct pbProbingSingleton pb;
+
+
+/* All cm_probe_cycle_start does is prevent any new commands from
+ * queueing to the planner so that the planner can move to a sop and
+ * report MACHINE_PROGRAM_STOP. OK, it also queues the function
+ * that's called once motion has stopped.
+ *
+ * Note: When coding a cycle (like this one) you get to perform one
+ * queued move per entry into the continuation, then you must exit.
+ *
+ * Another Note: When coding a cycle (like this one) you must wait
+ * until the last move has actually been queued (or has finished)
+ * before declaring the cycle to be done. Otherwise there is a nasty
+ * race condition in the tg_controller() that will accept the next
+ * command before the position of the final move has been recorded in
+ * the Gcode model. That's what the call to cm_get_runtime_busy() is
+ * about.
+ */
+
+
+static void _probe_restore_settings() {
+ // we should be stopped now, but in case of switch closure
+ mp_flush_planner();
+
+ // restore axis jerk
+ for (int axis = 0; axis < AXES; axis++ )
+ cm_set_axis_jerk(axis, pb.saved_jerk[axis]);
+
+ // restore coordinate system and distance mode
+ cm_set_coord_system(pb.saved_coord_system);
+ cm_set_distance_mode(pb.saved_distance_mode);
+
+ // update the model with actual position
+ cm_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE);
+ cm_cycle_end();
+ cm.cycle_state = CYCLE_OFF;
+}
+
+
+static void _probing_error_exit(stat_t status) {
+ _probe_restore_settings(); // clean up and exit
+ status_error(status);
+}
+
+
+static void _probing_finish() {
+ bool closed = switch_is_active(SW_PROBE);
+ cm.probe_state = closed ? PROBE_SUCCEEDED : PROBE_FAILED;
+
+ for (int axis = 0; axis < AXES; axis++) {
+ // if we got here because of a feed hold keep the model position correct
+ cm_set_position(axis, mp_get_runtime_work_position(axis));
+
+ // store the probe results
+ cm.probe_results[axis] = cm_get_absolute_position(axis);
+ }
+
+ _probe_restore_settings();
+}
+
+
+static void _probing_start() {
+ // initial probe state, don't probe if we're already contacted!
+ bool closed = switch_is_active(SW_PROBE);
+
+ if (!closed) {
+ stat_t status = cm_straight_feed(pb.target, pb.flags);
+ if (status) return _probing_error_exit(status);
+ }
+
+ pb.func = _probing_finish;
+}
+
+
+/* G38.2 homing cycle using limit switches
+ *
+ * These initializations are required before starting the probing cycle.
+ * They must be done after the planner has exhasted all current CYCLE moves as
+ * they affect the runtime (specifically the switch modes). Side effects would
+ * include limit switches initiating probe actions instead of just killing
+ * movement
+ */
+static void _probing_init() {
+ // NOTE: it is *not* an error condition for the probe not to trigger.
+ // it is an error for the limit or homing switches to fire, or for some other
+ // configuration error.
+ cm.probe_state = PROBE_FAILED;
+ cm.cycle_state = CYCLE_PROBE;
+
+ // initialize the axes - save the jerk settings & switch to the jerk_homing
+ // settings
+ for (int axis = 0; axis < AXES; axis++) {
+ pb.saved_jerk[axis] = cm_get_axis_jerk(axis); // save the max jerk value
+ cm_set_axis_jerk(axis, cm.a[axis].jerk_homing); // use homing jerk for probe
+ pb.start_position[axis] = cm_get_absolute_position(axis);
+ }
+
+ // error if the probe target is too close to the current position
+ if (get_axis_vector_length(pb.start_position, pb.target) <
+ MINIMUM_PROBE_TRAVEL)
+ _probing_error_exit(STAT_PROBE_INVALID_DEST);
+
+ // error if the probe target requires a move along the A/B/C axes
+ for (int axis = 0; axis < AXES; axis++)
+ if (fp_NE(pb.start_position[axis], pb.target[axis]))
+ _probing_error_exit(STAT_MOVE_DURING_PROBE);
+
+ // probe in absolute machine coords
+ pb.saved_coord_system = cm_get_coord_system(&cm.gm);
+ pb.saved_distance_mode = cm_get_distance_mode(&cm.gm);
+ cm_set_distance_mode(ABSOLUTE_MODE);
+ cm_set_coord_system(ABSOLUTE_COORDS);
+
+ cm_spindle_control(SPINDLE_OFF);
+
+ // start the move
+ pb.func = _probing_start;
+}
+
+
+bool cm_is_probing() {
+ return cm.cycle_state == CYCLE_PROBE || cm.probe_state == PROBE_WAITING;
+}
+
+
+/// G38.2 homing cycle using limit switches
+stat_t cm_straight_probe(float target[], float flags[]) {
+ // trap zero feed rate condition
+ if (cm.gm.feed_rate_mode != INVERSE_TIME_MODE && fp_ZERO(cm.gm.feed_rate))
+ return STAT_GCODE_FEEDRATE_NOT_SPECIFIED;
+
+ // trap no axes specified
+ if (fp_NOT_ZERO(flags[AXIS_X]) && fp_NOT_ZERO(flags[AXIS_Y]) &&
+ fp_NOT_ZERO(flags[AXIS_Z]))
+ return STAT_GCODE_AXIS_IS_MISSING;
+
+ // set probe move endpoint
+ copy_vector(pb.target, target); // set probe move endpoint
+ copy_vector(pb.flags, flags); // set axes involved on the move
+ clear_vector(cm.probe_results); // clear the old probe position.
+ // NOTE: relying on probe_results will not detect a probe to (0, 0, 0).
+
+ // wait until planner queue empties before completing initialization
+ cm.probe_state = PROBE_WAITING;
+ pb.func = _probing_init; // bind probing initialization function
+
+ return STAT_OK;
+}
+
+
+/// main loop callback for running the homing cycle
+void cm_probe_callback() {
+ // sync to planner move ends
+ if (!cm_is_probing() || cm_get_runtime_busy()) return;
+
+ pb.func(); // execute the current homing move
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 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 cm_is_probing();
+stat_t cm_straight_probe(float target[], float flags[]);
+void cm_probe_callback();
}
-stat_t report_callback() {
- if (usart_tx_full()) return STAT_OK; // Wait for buffer space
+void report_callback() {
+ if (usart_tx_full()) return; // Wait for buffer space
if (report_requested && usart_tx_empty()) {
uint32_t now = rtc_get_time();
- if (now - last_report < 100) return STAT_OK;
+ if (now - last_report < 100) return;
last_report = now;
vars_report(report_full);
report_requested = report_full = false;
}
-
- return STAT_OK;
}
void report_request();
void report_request_full();
-stat_t report_callback();
+void report_callback();
#include "switch.h"
#include "huanyang.h"
+#include "motor.h"
#include <avr/io.h>
#include <avr/interrupt.h>
ISR(RTC_OVF_vect) {
ticks++;
- switch_rtc_callback(); // switch debouncing
+
+ switch_rtc_callback();
huanyang_rtc_callback();
+ if (!(ticks & 255)) motor_rtc_callback();
}
#include "status.h"
#include <stdio.h>
+#include <stdarg.h>
+
stat_t status_code; // allocate a variable for the RITORNO macro
};
-const char *status_to_pgmstr(stat_t status) {
- return pgm_read_ptr(&stat_msg[status]);
+const char *status_to_pgmstr(stat_t code) {
+ return pgm_read_ptr(&stat_msg[code]);
+}
+
+
+const char *status_level_pgmstr(status_level_t level) {
+ switch (level) {
+ case STAT_LEVEL_INFO: return PSTR("info");
+ case STAT_LEVEL_DEBUG: return PSTR("debug");
+ case STAT_LEVEL_WARNING: return PSTR("warning");
+ default: return PSTR("error");
+ }
}
-stat_t status_error(stat_t status) {
- return status_error_P(0, 0, status);
+stat_t status_error(stat_t code) {
+ return status_message_P(0, STAT_LEVEL_ERROR, code, 0);
}
-stat_t status_error_P(const char *location, const char *msg, stat_t status) {
- printf_P(PSTR("\n{\"error\": \"%S\", \"code\": %d"),
- status_to_pgmstr(status), status);
+stat_t status_message_P(const char *location, status_level_t level,
+ stat_t code, const char *msg, ...) {
+ va_list args;
+
+ // Type
+ printf_P(PSTR("\n{\"%S\": \""), status_level_pgmstr(level));
+
+ // Message
+ if (msg) {
+ va_start(args, msg);
+ vfprintf_P(stdout, msg, args);
+ va_end(args);
+
+ } else printf_P(status_to_pgmstr(code));
+
+ putchar('"');
+
+ // Code
+ if (code) printf_P(PSTR(", \"code\": %d"), code);
- if (msg) printf_P(PSTR(", \"msg\": %S"), msg);
+ // Location
if (location) printf_P(PSTR(", \"location\": %S"), location);
putchar('}');
putchar('\n');
- return status;
+ return code;
}
} stat_t;
+typedef enum {
+ STAT_LEVEL_INFO,
+ STAT_LEVEL_DEBUG,
+ STAT_LEVEL_WARNING,
+ STAT_LEVEL_ERROR,
+} status_level_t;
+
+
extern stat_t status_code;
-const char *status_to_pgmstr(stat_t status);
-stat_t status_error(stat_t status);
-stat_t status_error_P(const char *location, const char *msg, stat_t status);
+const char *status_to_pgmstr(stat_t code);
+const char *status_level_pgmstr(status_level_t level);
+stat_t status_error(stat_t code);
+stat_t status_message_P(const char *location, status_level_t level,
+ stat_t code, const char *msg, ...);
void status_help();
#define TO_STRING(x) _TO_STRING(x)
#define _TO_STRING(x) #x
#define STATUS_LOCATION PSTR(__FILE__ ":" TO_STRING(__LINE__))
-#define STATUS_ERROR(MSG, CODE) status_error_P(STATUS_LOCATION, PSTR(MSG), CODE)
+
+#define STATUS_MESSAGE(LEVEL, CODE, MSG, ...) \
+ status_message_P(STATUS_LOCATION, LEVEL, CODE, PSTR(MSG), ##__VA_ARGS__)
+
+#define STATUS_INFO(MSG, ...) \
+ STATUS_MESSAGE(STAT_LEVEL_INFO, STAT_OK, MSG, ##__VA_ARGS__)
+
+#define STATUS_DEBUG(MSG, ...) \
+ STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__)
+
+#define STATUS_WARNING(MSG, ...) \
+ STATUS_MESSAGE(STAT_LEVEL_WARNING, STAT_OK, MSG, ##__VA_ARGS__)
+
+#define STATUS_ERROR(MSG, CODE, ...) \
+ STATUS_MESSAGE(STAT_LEVEL_ERROR, CODE, MSG, ##__VA_ARGS__)
}
-stat_t tmc2660_sync() {
- for (int i = 0; i < MOTORS; i++)
- if (!tmc2660_ready(i)) return STAT_EAGAIN;
-
- return STAT_OK;
-}
-
-
void tmc2660_enable(int driver) {
tmc2660_reset(driver);
_set_current(driver, drivers[driver].drive_current);
uint8_t tmc2660_status(int driver);
void tmc2660_reset(int driver);
bool tmc2660_ready(int driver);
-stat_t tmc2660_sync();
void tmc2660_enable(int driver);
void tmc2660_disable(int driver);
void tmc2660_set_stallguard_threshold(int driver, int8_t threshold);
#include "usart.h"
#include "plan/planner.h"
+#include "plan/buffer.h"
-float get_position(int index) {
- return mp_get_runtime_absolute_position(index);
-}
-
-
-float get_velocity() {
- return mp_get_runtime_velocity();
-}
-
-
-bool get_echo() {
- return usart_is_set(USART_ECHO);
-}
-
-
-void set_echo(bool value) {
- return usart_set(USART_ECHO, value);
-}
+float get_position(int index) {return mp_get_runtime_absolute_position(index);}
+float get_velocity() {return mp_get_runtime_velocity();}
+bool get_echo() {return usart_is_set(USART_ECHO);}
+void set_echo(bool value) {return usart_set(USART_ECHO, value);}
+uint16_t get_queue() {return mp_get_planner_buffer_room();}
+int32_t get_line() {return mr.ms.line;}
// Type names
static const char bool_name [] PROGMEM = "<bool>";
#define TYPE_NAME(TYPE) static const char TYPE##_name [] PROGMEM = "<" #TYPE ">"
-MAP(TYPE_NAME, SEMI, flags_t, string, float, int8_t, uint8_t, uint16_t);
+MAP(TYPE_NAME, SEMI, flags_t, string, float, int8_t, uint8_t, uint16_t,
+ int32_t);
// String
static uint16_t var_parse_uint16_t(const char *value) {
- return strtol(value, 0, 0);
+ return strtoul(value, 0, 0);
}
}
+// int32
+static void var_print_int32_t(uint32_t x) {
+ printf_P(PSTR("%"PRIi32), x);
+}
+
+
// Var forward declarations
#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \
TYPE get_##NAME(IF(INDEX)(int index)); \
VAR(hw_id, "id", string, 0, 0, 0, "Hardware ID")
VAR(echo, "ec", bool, 0, 1, 0, "Enable or disable echo")
VAR(estop, "es", bool, 0, 1, 0, "Emergency stop")
+VAR(line, "ln", int32_t, 0, 0, 0, "Last GCode line executed")
+VAR(queue, "q", uint16_t, 0, 0, 0, "Space in planner queue")
projects / bbctrl-firmware / commitdiff