From 857defe691af6f1564e91df0c117c3546d5b9ee7 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Wed, 6 Jul 2016 04:29:34 -0700 Subject: [PATCH] Removed main loop blocking --- src/canonical_machine.c | 42 +-- src/canonical_machine.h | 18 +- src/command.c | 79 +++--- src/command.h | 5 +- src/config.h | 10 + src/gcode_parser.c | 10 +- src/hardware.c | 4 +- src/hardware.h | 2 +- src/{cycle_homing.c => homing.c} | 409 ++++++++++++----------------- src/homing.h | 36 +++ src/huanyang.c | 36 +-- src/main.c | 65 +---- src/messages.def | 5 +- src/motor.c | 20 +- src/motor.h | 2 +- src/plan/arc.c | 43 +-- src/plan/arc.h | 6 +- src/plan/buffer.c | 28 +- src/plan/buffer.h | 32 +-- src/plan/calibrate.c | 4 +- src/plan/command.c | 2 +- src/plan/dwell.c | 2 +- src/plan/dwell.h | 2 +- src/plan/exec.c | 2 +- src/plan/feedhold.c | 24 +- src/plan/feedhold.h | 4 +- src/plan/jog.c | 2 +- src/plan/line.c | 2 +- src/{cycle_probing.c => probing.c} | 172 ++++++------ src/probing.h | 37 +++ src/report.c | 8 +- src/report.h | 2 +- src/rtc.c | 5 +- src/status.c | 46 +++- src/status.h | 32 ++- src/tmc2660.c | 8 - src/tmc2660.h | 1 - src/varcb.c | 25 +- src/vars.c | 11 +- src/vars.def | 2 + 40 files changed, 599 insertions(+), 646 deletions(-) rename src/{cycle_homing.c => homing.c} (68%) create mode 100644 src/homing.h rename src/{cycle_probing.c => probing.c} (77%) create mode 100644 src/probing.h diff --git a/src/canonical_machine.c b/src/canonical_machine.c index 22bc0de..9f06ac8 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -186,8 +186,8 @@ cmSingleton_t cm = { // State .gm = {.motion_mode = MOTION_MODE_CANCEL_MOTION_MODE}, - .gn = {}, - .gf = {}, + .gn = {0}, + .gf = {0}, }; @@ -218,7 +218,7 @@ cmCombinedState_t cm_get_combined_state() { 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;} @@ -268,7 +268,7 @@ void cm_set_absolute_override(bool absolute_override) { } -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 @@ -655,10 +655,10 @@ void canonical_machine_init() { /// 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; } @@ -866,6 +866,7 @@ stat_t cm_straight_traverse(float target[], float flags[]) { // 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(); @@ -880,11 +881,12 @@ void cm_set_g28_position() {copy_vector(cm.g28_position, cm.position);} /// 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}; @@ -899,12 +901,15 @@ void cm_set_g30_position() {copy_vector(cm.g30_position, cm.position);} /// 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); } @@ -954,6 +959,7 @@ stat_t cm_straight_feed(float target[], float flags[]) { // 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(); @@ -1079,7 +1085,9 @@ void cm_spindle_override_factor(bool flag) { } -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) @@ -1129,7 +1137,7 @@ void cm_request_cycle_start() {cm.cycle_start_requested = true;} /// 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); @@ -1148,28 +1156,24 @@ stat_t cm_feedhold_sequencing_callback() { } } - 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++) diff --git a/src/canonical_machine.h b/src/canonical_machine.h index 6d07f47..dc23e38 100644 --- a/src/canonical_machine.h +++ b/src/canonical_machine.h @@ -323,6 +323,7 @@ typedef enum { 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 @@ -331,7 +332,7 @@ typedef struct { /// 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 @@ -437,7 +438,7 @@ extern cmSingleton_t cm; // canonical machine controller singleton // 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(); @@ -463,7 +464,7 @@ void cm_set_spindle_mode(cmSpindleMode_t spindle_mode); 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); @@ -553,7 +554,7 @@ void cm_request_feedhold(); 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(); @@ -565,12 +566,3 @@ void cm_program_end(); // 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(); diff --git a/src/command.c b/src/command.c index 69e8e9a..eff58c1 100644 --- a/src/command.c +++ b/src/command.c @@ -33,9 +33,12 @@ #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 @@ -73,9 +76,6 @@ static const command_t commands[] PROGMEM = { }; -static char *_cmd = 0; - - int command_find(const char *match) { for (int i = 0; ; i++) { const char *name = pgm_read_word(&commands[i].name); @@ -150,56 +150,53 @@ int command_parser(char *cmd) { } -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); } diff --git a/src/command.h b/src/command.h index 7c92c6a..e6420a3 100644 --- a/src/command.h +++ b/src/command.h @@ -28,8 +28,6 @@ #pragma once -#include "status.h" - #include @@ -48,5 +46,4 @@ typedef struct { int command_find(const char *name); int command_exec(int argc, char *argv[]); -stat_t command_hi(); -stat_t command_lo(); +void command_callback(); diff --git a/src/config.h b/src/config.h index ca84819..124c4c9 100644 --- a/src/config.h +++ b/src/config.h @@ -383,3 +383,13 @@ typedef enum { #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 diff --git a/src/gcode_parser.c b/src/gcode_parser.c index 20e243f..340ed0b 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -30,6 +30,8 @@ #include "canonical_machine.h" #include "spindle.h" +#include "probing.h" +#include "homing.h" #include "util.h" #include @@ -231,7 +233,7 @@ static stat_t _validate_gcode_block() { 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); @@ -274,13 +276,13 @@ static stat_t _execute_gcode_block() { 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); @@ -507,7 +509,7 @@ static stat_t _parse_gcode_block(char *buf) { case 'J': SET_NON_MODAL(arc_offset[1], value); case 'K': SET_NON_MODAL(arc_offset[2], value); case 'R': SET_NON_MODAL(arc_radius, value); - case 'N': SET_NON_MODAL(linenum, (uint32_t)value); // line number + case '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; diff --git a/src/hardware.c b/src/hardware.c index 95f3155..a279fdd 100644 --- a/src/hardware.c +++ b/src/hardware.c @@ -155,15 +155,13 @@ void hw_hard_reset() { /// 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; } diff --git a/src/hardware.h b/src/hardware.h index 9392de0..4287779 100644 --- a/src/hardware.h +++ b/src/hardware.h @@ -37,7 +37,7 @@ 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(); diff --git a/src/cycle_homing.c b/src/homing.c similarity index 68% rename from src/cycle_homing.c rename to src/homing.c index 74ed909..acded39 100644 --- a/src/cycle_homing.c +++ b/src/homing.c @@ -40,7 +40,9 @@ #include -typedef stat_t (*homing_func_t)(int8_t axis); +typedef void (*homing_func_t)(int8_t axis); + +static void _homing_axis_start(int8_t axis); /// persistent homing runtime variables @@ -82,20 +84,6 @@ struct hmHomingSingleton { 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: @@ -151,63 +139,151 @@ static int8_t _get_next_axis(int8_t axis); */ -/// 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); +/* 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; + } - // 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; + return axis == -1 ? -2 : -1; // error or done +} - 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; +/// 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; } -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; + +static void _homing_error_exit(stat_t status) { + _homing_finalize_exit(); + status_error(status); } -/// 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 +/// 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; } -/// 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; +/// 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 stat_t _homing_axis_start(int8_t axis) { +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; - return _set_homing_func(_homing_finalize_exit); + _homing_finalize_exit(); + return; } else if (axis == -2) // -2 is error - return _homing_error_exit(-2, STAT_HOMING_ERROR_BAD_OR_NO_AXIS); + return _homing_error_exit(STAT_HOMING_ERROR_BAD_OR_NO_AXIS); } // clear the homed flag for axis to move w/o triggering soft limits @@ -215,18 +291,18 @@ static stat_t _homing_axis_start(int8_t axis) { // trap axis mis-configurations if (fp_ZERO(cm.a[axis].search_velocity)) - return _homing_error_exit(axis, STAT_HOMING_ERROR_ZERO_SEARCH_VELOCITY); + return _homing_error_exit(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); + return _homing_error_exit(STAT_HOMING_ERROR_ZERO_LATCH_VELOCITY); if (cm.a[axis].latch_backoff < 0) - return _homing_error_exit(axis, STAT_HOMING_ERROR_NEGATIVE_LATCH_BACKOFF); + 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(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL); + return _homing_error_exit(STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL); hm.axis = axis; // persist the axis // search velocity is always positive @@ -257,214 +333,51 @@ static stat_t _homing_axis_start(int8_t axis) { } 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.func = _homing_axis_start; + return; } hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value - - return _set_homing_func(_homing_axis_clear); // start the clear + hm.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); +bool cm_is_homing() { + return cm.cycle_state == CYCLE_HOMING; } -/// 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)); +/// 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); - _homing_finalize_exit(axis); + // 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; - return STAT_HOMING_CYCLE_FAILED; // homing state remains HOMING_NOT_HOMED + 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; } -/// 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; +void cm_homing_cycle_start_no_set() { + cm_homing_cycle_start(); + hm.set_coordinates = false; // don't update position variables at end of cycle } -/* 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 +/// 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 } diff --git a/src/homing.h b/src/homing.h new file mode 100644 index 0000000..738f914 --- /dev/null +++ b/src/homing.h @@ -0,0 +1,36 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include + + +bool cm_is_homing(); +void cm_homing_cycle_start(); +void cm_homing_cycle_start_no_set(); +void cm_homing_callback(); diff --git a/src/huanyang.c b/src/huanyang.c index 932c6b4..9c67189 100644 --- a/src/huanyang.c +++ b/src/huanyang.c @@ -332,15 +332,15 @@ static bool _check_response() { 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; } @@ -379,7 +379,7 @@ static void _retry_command() { _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); } @@ -451,8 +451,7 @@ void huanyang_init() { 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; @@ -496,21 +495,24 @@ void huanyang_rtc_callback() { 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(); diff --git a/src/main.c b/src/main.c index 6ea39ec..c06e16b 100644 --- a/src/main.c +++ b/src/main.c @@ -39,6 +39,8 @@ #include "report.h" #include "command.h" #include "estop.h" +#include "probing.h" +#include "homing.h" #include "plan/planner.h" #include "plan/buffer.h" @@ -53,51 +55,9 @@ #include -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 @@ -115,17 +75,18 @@ static void _init() { 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(); } diff --git a/src/messages.def b/src/messages.def index 1c18b54..686de5b 100644 --- a/src/messages.def +++ b/src/messages.def @@ -36,6 +36,7 @@ STAT_MSG(BUFFER_FULL, "Buffer full") 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") @@ -75,6 +76,7 @@ STAT_MSG(SOFT_LIMIT_EXCEEDED, "Soft limit exceeded") // 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, @@ -87,7 +89,8 @@ STAT_MSG(HOMING_ERROR_SWITCH_MISCONFIGURATION, "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, "") diff --git a/src/motor.c b/src/motor.c index 6348232..f19eccc 100644 --- a/src/motor.c +++ b/src/motor.c @@ -297,12 +297,11 @@ void motor_driver_callback(int motor) { /// 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; } @@ -317,11 +316,7 @@ void motor_error_callback(int motor, cmMotorFlags_t errors) { 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); } @@ -443,10 +438,7 @@ void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, 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. diff --git a/src/motor.h b/src/motor.h index dbeac21..abce100 100644 --- a/src/motor.h +++ b/src/motor.h @@ -77,7 +77,7 @@ void motor_reset(int motor); 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); diff --git a/src/plan/arc.c b/src/plan/arc.c index d62d18b..7782ae9 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -426,7 +426,7 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints } 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); @@ -439,13 +439,14 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints // 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); @@ -476,24 +477,24 @@ stat_t cm_arc_feed(float target[], float flags[], // arc endpoints * * 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 diff --git a/src/plan/arc.h b/src/plan/arc.h index 2d05be3..e1a8ff0 100644 --- a/src/plan/arc.h +++ b/src/plan/arc.h @@ -28,8 +28,7 @@ #pragma once -#include "util.h" -#include "status.h" +#include #define ARC_SEGMENT_LENGTH 0.1 // mm @@ -39,5 +38,6 @@ #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(); diff --git a/src/plan/buffer.c b/src/plan/buffer.c index 38b0cdf..974dd0b 100644 --- a/src/plan/buffer.c +++ b/src/plan/buffer.c @@ -50,12 +50,13 @@ */ #include "buffer.h" +#include "report.h" #include 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 @@ -66,8 +67,15 @@ typedef struct mpBufferPool { // ring buffer for sub-moves 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 @@ -111,6 +119,8 @@ mpBuf_t *mp_get_write_buffer() { mb.buffers_available--; mb.w = w->nx; + report_request(); + return w; } @@ -118,14 +128,6 @@ mpBuf_t *mp_get_write_buffer() { } -/// 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 * @@ -133,7 +135,8 @@ void mp_unget_write_buffer() { * 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; @@ -173,6 +176,7 @@ uint8_t mp_free_run_buffer() { // EMPTY current run buf & adv to next 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 } diff --git a/src/plan/buffer.h b/src/plan/buffer.h index cc5e481..59da843 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -28,19 +28,10 @@ #pragma once #include "canonical_machine.h" +#include "config.h" #include -/* 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 @@ -50,17 +41,16 @@ typedef enum { // bf->move_type values } 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 @@ -120,11 +110,11 @@ typedef struct mpBuffer { // See Planning Velocity Notes } 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(); diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index da06e33..1909b8e 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -108,7 +108,7 @@ static stat_t _exec_calibrate(mpBuf_t *bf) { 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 @@ -178,7 +178,7 @@ uint8_t command_calibrate(int argc, char *argv[]) { 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; } diff --git a/src/plan/command.c b/src/plan/command.c index 9a67ba2..0bbd28e 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -78,7 +78,7 @@ void mp_queue_command(cm_exec_t cm_exec, float *value, float *flag) { } // Must be final operation before exit - mp_commit_write_buffer(MOVE_TYPE_COMMAND); + mp_commit_write_buffer(cm_get_line(), MOVE_TYPE_COMMAND); } diff --git a/src/plan/dwell.c b/src/plan/dwell.c index 281d283..d543471 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -60,7 +60,7 @@ stat_t mp_dwell(float seconds) { 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; } diff --git a/src/plan/dwell.h b/src/plan/dwell.h index 36ea82d..8be10df 100644 --- a/src/plan/dwell.h +++ b/src/plan/dwell.h @@ -29,4 +29,4 @@ #include "status.h" -stat_t mp_dwell(const float seconds); +stat_t mp_dwell(float seconds); diff --git a/src/plan/exec.c b/src/plan/exec.c index 2159bab..4fc1af7 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -708,6 +708,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { // 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)) { @@ -762,7 +763,6 @@ stat_t mp_exec_aline(mpBuf_t *bf) { 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 diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index f14a566..b38a071 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -120,12 +120,11 @@ static float _compute_next_segment_velocity() { /// 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 @@ -169,7 +168,7 @@ stat_t mp_plan_hold_callback() { 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 @@ -222,23 +221,16 @@ stat_t mp_plan_hold_callback() { _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; } diff --git a/src/plan/feedhold.h b/src/plan/feedhold.h index c02c366..1fbb6e8 100644 --- a/src/plan/feedhold.h +++ b/src/plan/feedhold.h @@ -29,5 +29,5 @@ #include "status.h" -stat_t mp_plan_hold_callback(); -stat_t mp_end_hold(); +void mp_plan_hold_callback(); +void mp_end_hold(); diff --git a/src/plan/jog.c b/src/plan/jog.c index f641abe..c511196 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -142,7 +142,7 @@ uint8_t command_jog(int argc, char *argv[]) { // 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; diff --git a/src/plan/line.c b/src/plan/line.c index 3768553..bb25aa4 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -366,7 +366,7 @@ stat_t mp_aline(MoveState_t *ms) { 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; } diff --git a/src/cycle_probing.c b/src/probing.c similarity index 77% rename from src/cycle_probing.c rename to src/probing.c index c62fb17..eae282f 100644 --- a/src/cycle_probing.c +++ b/src/probing.c @@ -45,7 +45,7 @@ struct pbProbingSingleton { // persistent probing runtime variables - stat_t (*func)(); // binding for callback function state machine + void (*func)(); // binding for callback function state machine // state saved from gcode model uint8_t saved_distance_mode; // G90,G91 global setting @@ -61,20 +61,6 @@ struct pbProbingSingleton { // persistent probing runtime variables 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 @@ -93,39 +79,60 @@ uint8_t _set_pb_func(uint8_t (*func)()) { */ -/// 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; +static void _probe_restore_settings() { + // we should be stopped now, but in case of switch closure + mp_flush_planner(); - // 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; + // restore axis jerk + for (int axis = 0; axis < AXES; axis++ ) + cm_set_axis_jerk(axis, pb.saved_jerk[axis]); - // 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. + // restore coordinate system and distance mode + cm_set_coord_system(pb.saved_coord_system); + cm_set_distance_mode(pb.saved_distance_mode); - // wait until planner queue empties before completing initialization - cm.probe_state = PROBE_WAITING; - pb.func = _probing_init; // bind probing initialization function - return STAT_OK; + // update the model with actual position + cm_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); + cm_cycle_end(); + cm.cycle_state = CYCLE_OFF; } -/// 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 +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 (cm_get_runtime_busy()) return STAT_EAGAIN; // sync to planner move ends - return pb.func(); // execute the current homing move + 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. @@ -134,7 +141,7 @@ uint8_t cm_probe_callback() { * include limit switches initiating probe actions instead of just killing * movement */ -static uint8_t _probing_init() { +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. @@ -143,7 +150,7 @@ static uint8_t _probing_init() { // initialize the axes - save the jerk settings & switch to the jerk_homing // settings - for (uint8_t axis = 0; axis < AXES; axis++) { + 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); @@ -152,12 +159,12 @@ static uint8_t _probing_init() { // 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); + _probing_error_exit(STAT_PROBE_INVALID_DEST); // error if the probe target requires a move along the A/B/C axes - for (uint8_t axis = 0; axis < AXES; axis++) + for (int axis = 0; axis < AXES; axis++) if (fp_NE(pb.start_position[axis], pb.target[axis])) - _probing_error_exit(axis); + _probing_error_exit(STAT_MOVE_DURING_PROBE); // probe in absolute machine coords pb.saved_coord_system = cm_get_coord_system(&cm.gm); @@ -168,71 +175,44 @@ static uint8_t _probing_init() { cm_spindle_control(SPINDLE_OFF); // start the move - return _set_pb_func(_probing_start); + 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); +bool cm_is_probing() { + return cm.cycle_state == CYCLE_PROBE || cm.probe_state == PROBE_WAITING; } -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]); +/// 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; - // restore coordinate system and distance mode - cm_set_coord_system(pb.saved_coord_system); - cm_set_distance_mode(pb.saved_distance_mode); + // 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; - // update the model with actual position - cm_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); - cm_cycle_end(); - cm.cycle_state = CYCLE_OFF; -} + // 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 -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(); +/// 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; - return STAT_PROBE_CYCLE_FAILED; + pb.func(); // execute the current homing move } diff --git a/src/probing.h b/src/probing.h new file mode 100644 index 0000000..d79e1d4 --- /dev/null +++ b/src/probing.h @@ -0,0 +1,37 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +#include + + +bool cm_is_probing(); +stat_t cm_straight_probe(float target[], float flags[]); +void cm_probe_callback(); diff --git a/src/report.c b/src/report.c index 417ae6d..b39bfd1 100644 --- a/src/report.c +++ b/src/report.c @@ -52,17 +52,15 @@ void report_request_full() { } -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; } diff --git a/src/report.h b/src/report.h index b05360a..c66f05a 100644 --- a/src/report.h +++ b/src/report.h @@ -32,4 +32,4 @@ void report_request(); void report_request_full(); -stat_t report_callback(); +void report_callback(); diff --git a/src/rtc.c b/src/rtc.c index 4f7f016..73f5947 100644 --- a/src/rtc.c +++ b/src/rtc.c @@ -30,6 +30,7 @@ #include "switch.h" #include "huanyang.h" +#include "motor.h" #include #include @@ -42,8 +43,10 @@ static uint32_t ticks; ISR(RTC_OVF_vect) { ticks++; - switch_rtc_callback(); // switch debouncing + + switch_rtc_callback(); huanyang_rtc_callback(); + if (!(ticks & 255)) motor_rtc_callback(); } diff --git a/src/status.c b/src/status.c index 02bf2f5..80ed0ec 100644 --- a/src/status.c +++ b/src/status.c @@ -28,6 +28,8 @@ #include "status.h" #include +#include + stat_t status_code; // allocate a variable for the RITORNO macro @@ -42,27 +44,53 @@ static const char *const stat_msg[] PROGMEM = { }; -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; } diff --git a/src/status.h b/src/status.h index b320591..e2e7df9 100644 --- a/src/status.h +++ b/src/status.h @@ -43,15 +43,39 @@ typedef enum { } 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__) diff --git a/src/tmc2660.c b/src/tmc2660.c index 57eb0e5..54082be 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -382,14 +382,6 @@ bool tmc2660_ready(int motor) { } -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); diff --git a/src/tmc2660.h b/src/tmc2660.h index 625308f..3758ab6 100644 --- a/src/tmc2660.h +++ b/src/tmc2660.h @@ -39,7 +39,6 @@ void tmc2660_init(); 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); diff --git a/src/varcb.c b/src/varcb.c index 7cd4a85..acb2831 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -28,23 +28,12 @@ #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;} diff --git a/src/vars.c b/src/vars.c index 03e21c7..06e7878 100644 --- a/src/vars.c +++ b/src/vars.c @@ -56,7 +56,8 @@ static const char indexed_code_fmt[] PROGMEM = "\"%c%s\":"; // Type names static const char bool_name [] PROGMEM = ""; #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 @@ -171,7 +172,7 @@ static void var_print_uint16_t(uint16_t x) { static uint16_t var_parse_uint16_t(const char *value) { - return strtol(value, 0, 0); + return strtoul(value, 0, 0); } @@ -185,6 +186,12 @@ static void eeprom_update_uint16_t(uint16_t *addr, uint16_t value) { } +// 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)); \ diff --git a/src/vars.def b/src/vars.def index 633b537..e22393d 100644 --- a/src/vars.def +++ b/src/vars.def @@ -96,3 +96,5 @@ VAR(velocity, "v", float, 0, 0, 0, "Current velocity") 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") -- 2.27.0