From 148c9fb35a9be94bb2ff7faa2f27a2723ca954d5 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sun, 10 Apr 2016 16:21:44 -0700 Subject: [PATCH] Disable echo, velocity vs. sg threshold calibration routine --- src/command.c | 20 +- src/command.def | 1 + src/command.h | 3 + src/gcode_parser.c | 395 +++++++++---------- src/motor.c | 27 +- src/motor.h | 3 + src/plan/buffer.h | 1 - src/plan/calibrate.c | 207 ++++++++++ src/plan/calibrate.h | 35 ++ src/plan/exec.c | 895 ++++++++++++++++++++++--------------------- src/plan/jog.c | 28 +- src/plan/jog.h | 4 +- src/plan/planner.c | 2 +- src/plan/planner.h | 2 - src/report.c | 12 - src/stepper.c | 9 +- src/stepper.h | 1 - src/tmc2660.c | 13 +- src/tmc2660.h | 2 + src/usart.c | 9 +- src/usart.h | 3 +- src/varcb.c | 50 +++ src/vars.c | 76 ++-- src/vars.def | 3 +- 24 files changed, 1057 insertions(+), 744 deletions(-) create mode 100644 src/plan/calibrate.c create mode 100644 src/plan/calibrate.h create mode 100644 src/varcb.c diff --git a/src/command.c b/src/command.c index 33f10c7..4bd65da 100644 --- a/src/command.c +++ b/src/command.c @@ -33,13 +33,13 @@ #include "report.h" #include "vars.h" #include "plan/jog.h" +#include "plan/calibrate.h" #include "config.h" #include #include #include -#include #include #include @@ -170,8 +170,9 @@ int command_eval(char *cmd) { case '{': return vars_parser(cmd); case '$': return command_parser(cmd); default: - if (!mp_jog_busy()) return gc_gcode_parser(cmd); - return STAT_OK; + if (calibrate_busy()) return STAT_OK; + if (mp_jog_busy()) return STAT_OK; + return gc_gcode_parser(cmd); } } @@ -247,16 +248,3 @@ uint8_t command_clear(int argc, char *argv[]) { vars_clear(); return 0; } - - -uint8_t command_jog(int argc, char *argv[]) { - float velocity[AXES]; - - for (int axis = 0; axis < AXES; axis++) - if (axis < argc - 1) velocity[axis] = atof(argv[axis + 1]); - else velocity[axis] = 0; - - mp_jog(velocity); - - return 0; -} diff --git a/src/command.def b/src/command.def index 534eed5..91d3240 100644 --- a/src/command.def +++ b/src/command.def @@ -35,3 +35,4 @@ CMD(restore, 0, 0, "Restore settings") CMD(clear, 0, 0, "Clear saved settings") CMD(jog, 1, 4, "Jog") CMD(mreset, 0, 1, "Reset motor") +CMD(calibrate, 0, 0, "Calibrate motors") diff --git a/src/command.h b/src/command.h index 4641f12..a110f62 100644 --- a/src/command.h +++ b/src/command.h @@ -30,6 +30,9 @@ #include "status.h" +#include + + #define MAX_ARGS 16 typedef uint8_t (*command_cb_t)(int argc, char *argv[]); diff --git a/src/gcode_parser.c b/src/gcode_parser.c index 47d75cb..e5d7750 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -39,53 +39,16 @@ #include -struct gcodeParserSingleton { // struct to manage globals - uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block -}; struct gcodeParserSingleton gp; - -// local helper functions and macros -static void _normalize_gcode_block(char *str, char **com, char **msg, - uint8_t *block_delete_flag); -static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value); -static stat_t _point(float value); -static stat_t _validate_gcode_block(); -/// Parse the block into the GN/GF structs -static stat_t _parse_gcode_block(char *line); -static stat_t _execute_gcode_block(); // Execute the gcode block - #define SET_MODAL(m, parm, val) \ - {cm.gn.parm = val; cm.gf.parm = 1; gp.modals[m] += 1; break;} + {cm.gn.parm = val; cm.gf.parm = 1; modals[m] += 1; break;} #define SET_NON_MODAL(parm, val) {cm.gn.parm = val; cm.gf.parm = 1; break;} #define EXEC_FUNC(f, v) if ((uint8_t)cm.gf.v) f(cm.gn.v) -/// Parse a block (line) of gcode -/// Top level of gcode parser. Normalizes block and looks for special cases -stat_t gc_gcode_parser(char *block) { - char *str = block; // gcode command or 0 string - char none = 0; - char *com = &none; // gcode comment or 0 string - char *msg = &none; // gcode message or 0 string - uint8_t block_delete_flag; - - // don't process Gcode blocks if in alarmed state - if (cm.machine_state == MACHINE_ALARM) return STAT_MACHINE_ALARMED; - - _normalize_gcode_block(str, &com, &msg, &block_delete_flag); - - // Block delete omits the line if a / char is present in the first space - // For now this is unconditional and will always delete - if (block_delete_flag) return STAT_NOOP; - - // queue a "(MSG" response - if (*msg) cm_message(msg); // queue the message - - return _parse_gcode_block(block); -} +static uint8_t modals[MODAL_GROUP_COUNT]; // collects modal groups in a block -/* - * Normalize a block (line) of gcode in place +/* Normalize a block (line) of gcode in place * * Normalization functions: * - convert all letters to upper case @@ -126,19 +89,18 @@ stat_t gc_gcode_parser(char *block) { static void _normalize_gcode_block(char *str, char **com, char **msg, uint8_t *block_delete_flag) { - char *rd = str; // read pointer - char *wr = str; // write pointer + char *rd = str; // read pointer + char *wr = str; // write pointer // mark block deletes - if (*rd == '/') { *block_delete_flag = true; } - else { *block_delete_flag = false; } + *block_delete_flag = *rd == '/'; // normalize the command block & find the comment (if any) for (; *wr; rd++) if (!*rd) *wr = 0; else if (*rd == '(' || *rd == ';') {*wr = 0; *com = rd + 1;} - else if (isalnum((char)*rd) || strchr("-.", *rd)) // all valid characters - *wr++ = (char)toupper((char)*rd); + else if (isalnum(*rd) || strchr("-.", *rd)) // all valid characters + *wr++ = toupper(*rd); // Perform Octal stripping - remove invalid leading zeros in number strings rd = str; @@ -195,16 +157,15 @@ static stat_t _get_next_gcode_word(char **pstr, char *letter, float *value) { *value = strtod(*pstr, &end); // more robust test then checking for value == 0 if (end == *pstr) return STAT_BAD_NUMBER_FORMAT; - *pstr = end; + *pstr = end; // pointer points to next character after the word - return STAT_OK; // pointer points to next character after the word + return STAT_OK; } /// Isolate the decimal point value as an integer static uint8_t _point(float value) { - // isolate the decimal point as an int - return (uint8_t)(value * 10 - trunc(value) * 10); + return value * 10 - trunc(value) * 10; } @@ -219,46 +180,193 @@ static stat_t _validate_gcode_block() { // activity of the group 1 G-code is suspended for that line. The // axis word-using G-codes from group 0 are G10, G28, G30, and G92" - // if (gp.modals[MODAL_GROUP_G0] && gp.modals[MODAL_GROUP_G1]) + // if (modals[MODAL_GROUP_G0] && modals[MODAL_GROUP_G1]) // return STAT_MODAL_GROUP_VIOLATION; // look for commands that require an axis word to be present - // if (gp.modals[MODAL_GROUP_G0] || gp.modals[MODAL_GROUP_G1]) + // if (modals[MODAL_GROUP_G0] || modals[MODAL_GROUP_G1]) // if (!_axis_changed()) return STAT_GCODE_AXIS_IS_MISSING; return STAT_OK; } -/* - * Parses one line of 0 terminated G-Code. +/* Execute parsed block + * + * Conditionally (based on whether a flag is set in gf) call the canonical + * machining functions in order of execution as per RS274NGC_3 table 8 + * (below, with modifications): + * + * 0. record the line number + * 1. comment (includes message) [handled during block normalization] + * 2. set feed rate mode (G93, G94 - inverse time or per minute) + * 3. set feed rate (F) + * 3a. set feed override rate (M50.1) + * 3a. set traverse override rate (M50.2) + * 4. set spindle speed (S) + * 4a. set spindle override rate (M51.1) + * 5. select tool (T) + * 6. change tool (M6) + * 7. spindle on or off (M3, M4, M5) + * 8. coolant on or off (M7, M8, M9) + * 9. enable or disable overrides (M48, M49, M50, M51) + * 10. dwell (G4) + * 11. set active plane (G17, G18, G19) + * 12. set length units (G20, G21) + * 13. cutter radius compensation on or off (G40, G41, G42) + * 14. cutter length compensation on or off (G43, G49) + * 15. coordinate system selection (G54, G55, G56, G57, G58, G59) + * 16. set path control mode (G61, G61.1, G64) + * 17. set distance mode (G90, G91) + * 18. set retract mode (G98, G99) + * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) + * 19b. update system data (G10) + * 19c. set axis offsets (G92, G92.1, G92.2, G92.3) + * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 + * 21. stop and end (M0, M1, M2, M30, M60) + * + * Values in gn are in original units and should not be unit converted prior + * to calling the canonical functions (which do the unit conversions) + */ +static stat_t _execute_gcode_block() { + stat_t status = STAT_OK; + + cm_set_model_linenum(cm.gn.linenum); + 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); + EXEC_FUNC(cm_traverse_override_factor, traverse_override_factor); + EXEC_FUNC(cm_set_spindle_speed, spindle_speed); + EXEC_FUNC(cm_spindle_override_factor, spindle_override_factor); + EXEC_FUNC(cm_select_tool, tool_select); + EXEC_FUNC(cm_change_tool, tool_change); + EXEC_FUNC(cm_spindle_control, spindle_mode); + EXEC_FUNC(cm_mist_coolant_control, mist_coolant); + EXEC_FUNC(cm_flood_coolant_control, flood_coolant); + EXEC_FUNC(cm_feed_rate_override_enable, feed_rate_override_enable); + EXEC_FUNC(cm_traverse_override_enable, traverse_override_enable); + EXEC_FUNC(cm_spindle_override_enable, spindle_override_enable); + EXEC_FUNC(cm_override_enables, override_enables); + + if (cm.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell + ritorno(cm_dwell(cm.gn.parameter)); + + EXEC_FUNC(cm_set_plane, select_plane); + EXEC_FUNC(cm_set_units_mode, units_mode); + //--> cutter radius compensation goes here + //--> cutter length compensation goes here + EXEC_FUNC(cm_set_coord_system, coord_system); + EXEC_FUNC(cm_set_path_control, path_control); + EXEC_FUNC(cm_set_distance_mode, distance_mode); + //--> set retract mode goes here + + switch (cm.gn.next_action) { + case NEXT_ACTION_SET_G28_POSITION: // G28.1 + cm_set_g28_position(); + break; + case NEXT_ACTION_GOTO_G28_POSITION: // G28 + status = cm_goto_g28_position(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_SET_G30_POSITION: // G30.1 + cm_set_g30_position(); + break; + case NEXT_ACTION_GOTO_G30_POSITION: // G30 + status = cm_goto_g30_position(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_SEARCH_HOME: // G28.2 + status = cm_homing_cycle_start(); + break; + case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3 + cm_set_absolute_origin(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_HOMING_NO_SET: // G28.4 + status = cm_homing_cycle_start_no_set(); + break; + case NEXT_ACTION_STRAIGHT_PROBE: // G38.2 + status = cm_straight_probe(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_SET_COORD_DATA: + cm_set_coord_offsets(cm.gn.parameter, cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_SET_ORIGIN_OFFSETS: + cm_set_origin_offsets(cm.gn.target, cm.gf.target); + break; + case NEXT_ACTION_RESET_ORIGIN_OFFSETS: + cm_reset_origin_offsets(); + break; + case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: + cm_suspend_origin_offsets(); + break; + case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: + cm_resume_origin_offsets(); + break; + case NEXT_ACTION_DWELL: break; // Handled above + + case NEXT_ACTION_DEFAULT: + // apply override setting to gm struct + cm_set_absolute_override(cm.gn.absolute_override); + + switch (cm.gn.motion_mode) { + case MOTION_MODE_CANCEL_MOTION_MODE: + cm.gm.motion_mode = cm.gn.motion_mode; + break; + case MOTION_MODE_STRAIGHT_TRAVERSE: + status = cm_straight_traverse(cm.gn.target, cm.gf.target); + break; + case MOTION_MODE_STRAIGHT_FEED: + status = cm_straight_feed(cm.gn.target, cm.gf.target); + break; + case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: + // gf.radius sets radius mode if radius was collected in gn + status = cm_arc_feed(cm.gn.target, cm.gf.target, cm.gn.arc_offset[0], + cm.gn.arc_offset[1], cm.gn.arc_offset[2], + cm.gn.arc_radius, cm.gn.motion_mode); + break; + default: break; // Should not get here + } + } + // un-set absolute override once the move is planned + cm_set_absolute_override(false); + + // do the program stops and ends : M0, M1, M2, M30, M60 + if (cm.gf.program_flow) { + if (cm.gn.program_flow == PROGRAM_STOP) cm_program_stop(); + else cm_program_end(); + } + + return status; +} + + +/* Parses one line of 0 terminated G-Code. * - * All the parser does is load the state values in gn (next model - * state) and set flags in gf (model state flags). The execute - * routine applies them. The buffer is assumed to contain only - * uppercase characters and signed floats (no whitespace). + * All the parser does is load the state values in gn (next model + * state) and set flags in gf (model state flags). The execute + * routine applies them. The buffer is assumed to contain only + * uppercase characters and signed floats (no whitespace). * - * A number of implicit things happen when the gn struct is zeroed: - * - inverse feed rate mode is canceled - set back to units_per_minute mode + * A number of implicit things happen when the gn struct is zeroed: + * - inverse feed rate mode is canceled - set back to units_per_minute mode */ static stat_t _parse_gcode_block(char *buf) { - char *pstr = (char *)buf; // persistent pointer for parsing words + char *pstr = buf; // persistent pointer for parsing words char letter; // parsed letter, eg.g. G or X or Y float value = 0; // value parsed from letter (e.g. 2 for G2) stat_t status = STAT_OK; // set initial state for new move - memset(&gp, 0, sizeof(gp)); // clear all parser values + memset(modals, 0, sizeof(modals)); // clear all parser values memset(&cm.gf, 0, sizeof(GCodeState_t)); // clear all next-state flags memset(&cm.gn, 0, sizeof(GCodeState_t)); // clear all next-state values + // get motion mode from previous block cm.gn.motion_mode = cm_get_motion_mode(); // extract commands and parameters while ((status = _get_next_gcode_word(&pstr, &letter, &value)) == STAT_OK) { - switch(letter) { + switch (letter) { case 'G': - switch((uint8_t)value) { + switch ((uint8_t)value) { case 0: SET_MODAL(MODAL_GROUP_G1, motion_mode, MOTION_MODE_STRAIGHT_TRAVERSE); case 1: @@ -415,151 +523,26 @@ static stat_t _parse_gcode_block(char *buf) { } -/* Execute parsed block - * - * Conditionally (based on whether a flag is set in gf) call the canonical - * machining functions in order of execution as per RS274NGC_3 table 8 - * (below, with modifications): - * - * 0. record the line number - * 1. comment (includes message) [handled during block normalization] - * 2. set feed rate mode (G93, G94 - inverse time or per minute) - * 3. set feed rate (F) - * 3a. set feed override rate (M50.1) - * 3a. set traverse override rate (M50.2) - * 4. set spindle speed (S) - * 4a. set spindle override rate (M51.1) - * 5. select tool (T) - * 6. change tool (M6) - * 7. spindle on or off (M3, M4, M5) - * 8. coolant on or off (M7, M8, M9) - * 9. enable or disable overrides (M48, M49, M50, M51) - * 10. dwell (G4) - * 11. set active plane (G17, G18, G19) - * 12. set length units (G20, G21) - * 13. cutter radius compensation on or off (G40, G41, G42) - * 14. cutter length compensation on or off (G43, G49) - * 15. coordinate system selection (G54, G55, G56, G57, G58, G59) - * 16. set path control mode (G61, G61.1, G64) - * 17. set distance mode (G90, G91) - * 18. set retract mode (G98, G99) - * 19a. homing functions (G28.2, G28.3, G28.1, G28, G30) - * 19b. update system data (G10) - * 19c. set axis offsets (G92, G92.1, G92.2, G92.3) - * 20. perform motion (G0 to G3, G80-G89) as modified (possibly) by G53 - * 21. stop and end (M0, M1, M2, M30, M60) - * - * Values in gn are in original units and should not be unit converted prior - * to calling the canonical functions (which do the unit conversions) - */ -static stat_t _execute_gcode_block() { - stat_t status = STAT_OK; - - cm_set_model_linenum(cm.gn.linenum); - 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); - EXEC_FUNC(cm_traverse_override_factor, traverse_override_factor); - EXEC_FUNC(cm_set_spindle_speed, spindle_speed); - EXEC_FUNC(cm_spindle_override_factor, spindle_override_factor); - // tool_select is where it's written - EXEC_FUNC(cm_select_tool, tool_select); - EXEC_FUNC(cm_change_tool, tool_change); - EXEC_FUNC(cm_spindle_control, spindle_mode); // spindle on or off - EXEC_FUNC(cm_mist_coolant_control, mist_coolant); - // also disables mist coolant if OFF - EXEC_FUNC(cm_flood_coolant_control, flood_coolant); - EXEC_FUNC(cm_feed_rate_override_enable, feed_rate_override_enable); - EXEC_FUNC(cm_traverse_override_enable, traverse_override_enable); - EXEC_FUNC(cm_spindle_override_enable, spindle_override_enable); - EXEC_FUNC(cm_override_enables, override_enables); - - if (cm.gn.next_action == NEXT_ACTION_DWELL) // G4 - dwell - // return if error, otherwise complete the block - ritorno(cm_dwell(cm.gn.parameter)); - - EXEC_FUNC(cm_set_plane, select_plane); - EXEC_FUNC(cm_set_units_mode, units_mode); - //--> cutter radius compensation goes here - //--> cutter length compensation goes here - EXEC_FUNC(cm_set_coord_system, coord_system); - EXEC_FUNC(cm_set_path_control, path_control); - EXEC_FUNC(cm_set_distance_mode, distance_mode); - //--> set retract mode goes here +/// Parse a block (line) of gcode +/// Top level of gcode parser. Normalizes block and looks for special cases +stat_t gc_gcode_parser(char *block) { + char *str = block; // gcode command or 0 string + char none = 0; + char *com = &none; // gcode comment or 0 string + char *msg = &none; // gcode message or 0 string + uint8_t block_delete_flag; - switch (cm.gn.next_action) { - case NEXT_ACTION_SET_G28_POSITION: // G28.1 - cm_set_g28_position(); - break; - case NEXT_ACTION_GOTO_G28_POSITION: // G28 - status = cm_goto_g28_position(cm.gn.target, cm.gf.target); - break; - case NEXT_ACTION_SET_G30_POSITION: // G30.1 - cm_set_g30_position(); - break; - case NEXT_ACTION_GOTO_G30_POSITION: // G30 - status = cm_goto_g30_position(cm.gn.target, cm.gf.target); - break; - case NEXT_ACTION_SEARCH_HOME: // G28.2 - status = cm_homing_cycle_start(); - break; - case NEXT_ACTION_SET_ABSOLUTE_ORIGIN: // G28.3 - cm_set_absolute_origin(cm.gn.target, cm.gf.target); - break; - case NEXT_ACTION_HOMING_NO_SET: // G28.4 - status = cm_homing_cycle_start_no_set(); - break; - case NEXT_ACTION_STRAIGHT_PROBE: // G38.2 - status = cm_straight_probe(cm.gn.target, cm.gf.target); - break; - case NEXT_ACTION_SET_COORD_DATA: - cm_set_coord_offsets(cm.gn.parameter, cm.gn.target, cm.gf.target); - break; - case NEXT_ACTION_SET_ORIGIN_OFFSETS: - cm_set_origin_offsets(cm.gn.target, cm.gf.target); - break; - case NEXT_ACTION_RESET_ORIGIN_OFFSETS: - cm_reset_origin_offsets(); - break; - case NEXT_ACTION_SUSPEND_ORIGIN_OFFSETS: - cm_suspend_origin_offsets(); - break; - case NEXT_ACTION_RESUME_ORIGIN_OFFSETS: - cm_resume_origin_offsets(); - break; - case NEXT_ACTION_DWELL: break; // Handled above + // don't process Gcode blocks if in alarmed state + if (cm.machine_state == MACHINE_ALARM) return STAT_MACHINE_ALARMED; - case NEXT_ACTION_DEFAULT: - // apply override setting to gm struct - cm_set_absolute_override(cm.gn.absolute_override); + _normalize_gcode_block(str, &com, &msg, &block_delete_flag); - switch (cm.gn.motion_mode) { - case MOTION_MODE_CANCEL_MOTION_MODE: - cm.gm.motion_mode = cm.gn.motion_mode; - break; - case MOTION_MODE_STRAIGHT_TRAVERSE: - status = cm_straight_traverse(cm.gn.target, cm.gf.target); - break; - case MOTION_MODE_STRAIGHT_FEED: - status = cm_straight_feed(cm.gn.target, cm.gf.target); - break; - case MOTION_MODE_CW_ARC: case MOTION_MODE_CCW_ARC: - // gf.radius sets radius mode if radius was collected in gn - status = cm_arc_feed(cm.gn.target, cm.gf.target, cm.gn.arc_offset[0], - cm.gn.arc_offset[1], cm.gn.arc_offset[2], - cm.gn.arc_radius, cm.gn.motion_mode); - break; - default: break; // Should not get here - } - } - // un-set absolute override once the move is planned - cm_set_absolute_override(false); + // Block delete omits the line if a / char is present in the first space + // For now this is unconditional and will always delete + if (block_delete_flag) return STAT_NOOP; - // do the program stops and ends : M0, M1, M2, M30, M60 - if (cm.gf.program_flow) { - if (cm.gn.program_flow == PROGRAM_STOP) cm_program_stop(); - else cm_program_end(); - } + // queue a "(MSG" response + if (*msg) cm_message(msg); // queue the message - return status; + return _parse_gcode_block(block); } diff --git a/src/motor.c b/src/motor.c index 4a45be7..9ce699f 100644 --- a/src/motor.c +++ b/src/motor.c @@ -35,6 +35,7 @@ #include "tmc2660.h" #include "plan/planner.h" +#include "plan/calibrate.h" #include #include @@ -232,8 +233,20 @@ void motor_set_encoder(int motor, float encoder) { /// returns true if motor is in an error state -static bool _error(int motor) { - return motors[motor].flags & MOTOR_FLAG_ERROR_bm; +bool motor_error(int motor) { + uint8_t flags = motors[motor].flags; + if (calibrate_busy()) flags &= ~MOTOR_FLAG_STALLED_bm; + return flags & MOTOR_FLAG_ERROR_bm; +} + + +bool motor_stall(int motor) { + return motors[motor].flags & MOTOR_FLAG_STALLED_bm; +} + + +void motor_reset(int motor) { + motors[motor].flags = 0; } @@ -248,7 +261,7 @@ static void _deenergize(int motor) { /// Apply power to a motor static void _energize(int motor) { - if (motors[motor].power_state == MOTOR_IDLE && !_error(motor)) { + if (motors[motor].power_state == MOTOR_IDLE && !motor_error(motor)) { motors[motor].power_state = MOTOR_ENERGIZING; tmc2660_enable(motor); } @@ -284,7 +297,7 @@ void motor_driver_callback(int motor) { stat_t motor_power_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 || _error(motor) || + if (motors[motor].power_mode == MOTOR_DISABLED || motor_error(motor) || motors[motor].timeout < rtc_get_time()) _deenergize(motor); @@ -298,7 +311,7 @@ void motor_error_callback(int motor, cmMotorFlags_t errors) { motors[motor].flags |= errors; report_request(); - if (_error(motor)) { + if (motor_error(motor)) { _deenergize(motor); // Stop and flush motion @@ -543,11 +556,11 @@ uint8_t get_status_strings(int motor) { void command_mreset(int argc, char *argv[]) { if (argc == 1) for (int motor = 0; motor < MOTORS; motor++) - motors[motor].flags = 0; + motor_reset(motor); else { int motor = atoi(argv[1]); - if (motor < MOTORS) motors[motor].flags = 0; + if (motor < MOTORS) motor_reset(motor); } report_request(); diff --git a/src/motor.h b/src/motor.h index ab431f7..81cef29 100644 --- a/src/motor.h +++ b/src/motor.h @@ -69,6 +69,9 @@ int motor_get_axis(int motor); float motor_get_steps_per_unit(int motor); int32_t motor_get_encoder(int motor); void motor_set_encoder(int motor, float encoder); +bool motor_error(int motor); +bool motor_stall(int motor); +void motor_reset(int motor); bool motor_energizing(); diff --git a/src/plan/buffer.h b/src/plan/buffer.h index 0e321f5..cc5e481 100644 --- a/src/plan/buffer.h +++ b/src/plan/buffer.h @@ -47,7 +47,6 @@ typedef enum { // bf->move_type values MOVE_TYPE_ALINE, // acceleration planned line MOVE_TYPE_DWELL, // delay with no movement MOVE_TYPE_COMMAND, // general command - MOVE_TYPE_JOG, // interactive jogging } moveType_t; typedef enum { diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c new file mode 100644 index 0000000..9d17e96 --- /dev/null +++ b/src/plan/calibrate.c @@ -0,0 +1,207 @@ +/******************************************************************************\ + + 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" + +\******************************************************************************/ + + +#include "calibrate.h" + +#include "buffer.h" +#include "motor.h" +#include "canonical_machine.h" +#include "planner.h" +#include "stepper.h" +#include "rtc.h" +#include "tmc2660.h" +#include "config.h" + +#include +#include +#include + + +#define CAL_THRESHOLDS 32 +#define CAL_VELOCITIES 16 +#define CAL_MIN_THRESH -10 +#define CAL_MAX_THRESH 63 +#define CAL_WAIT_TIME 3 // ms + + +enum { + CAL_START, + CAL_ACCEL, + CAL_MEASURE, + CAL_DECEL, +}; + + +typedef struct { + bool busy; + + uint32_t wait; + int state; + int motor; + int axis; + int tstep; + int vstep; + + float current_velocity; + + uint16_t stallguard[CAL_VELOCITIES][CAL_THRESHOLDS]; + uint16_t velocities[CAL_VELOCITIES]; + int8_t thresholds[CAL_THRESHOLDS]; +} calibrate_t; + +static calibrate_t cal = {}; + + +static stat_t _exec_calibrate(mpBuf_t *bf) { + if (bf->move_state == MOVE_NEW) bf->move_state = MOVE_RUN; + + const float time = MIN_SEGMENT_TIME; // In minutes + const float maxDeltaV = JOG_ACCELERATION * time; + + if (cal.wait <= rtc_get_time()) + switch (cal.state) { + case CAL_START: { + cal.axis = motor_get_axis(cal.motor); + cal.state = CAL_ACCEL; + + cal.current_velocity = 0; + float max_velocity = cm.a[cal.axis].velocity_max / 2; + for (int i = 0; i < CAL_VELOCITIES; i++) + cal.velocities[i] = (1 + i) * max_velocity / CAL_VELOCITIES; + + memset(cal.stallguard, 0, sizeof(cal.stallguard)); + + tmc2660_set_stallguard_threshold(cal.motor, CAL_MAX_THRESH); + cal.wait = rtc_get_time() + CAL_WAIT_TIME; + + printf("%d", cal.motor); + for (int i = 0; i < CAL_THRESHOLDS; i++) + printf(",%d", cal.thresholds[i]); + putchar('\n'); + + break; + } + + case CAL_ACCEL: + cal.current_velocity += maxDeltaV; + + // Check if we are at the target velocity + if (cal.velocities[cal.vstep] <= cal.current_velocity) { + cal.current_velocity = cal.velocities[cal.vstep]; + cal.tstep = 0; + cal.state = CAL_MEASURE; + + tmc2660_set_stallguard_threshold(cal.motor, cal.thresholds[0]); + cal.wait = rtc_get_time() + CAL_WAIT_TIME; + } + break; + + case CAL_MEASURE: + if (++cal.tstep == CAL_THRESHOLDS) { + if (++cal.vstep == CAL_VELOCITIES) cal.state = CAL_DECEL; + else { + cal.state = CAL_ACCEL; + + printf("%d", cal.velocities[cal.vstep - 1]); + for (int i = 0; i < CAL_THRESHOLDS; i++) + printf(",%d", cal.stallguard[cal.vstep - 1][i]); + putchar('\n'); + } + + } else { + tmc2660_set_stallguard_threshold(cal.motor, cal.thresholds[cal.tstep]); + cal.wait = rtc_get_time() + CAL_WAIT_TIME; + } + break; + + case CAL_DECEL: + cal.current_velocity -= maxDeltaV; + if (cal.current_velocity < 0) cal.current_velocity = 0; + + if (!cal.current_velocity) { + // Print results + + mp_free_run_buffer(); // Release buffer + cal.busy = false; + + putchar('\n'); + return STAT_OK; + } + break; + } + + if (!cal.current_velocity) return STAT_OK; + + // Compute travel + float travel[AXES] = {}; // In mm + travel[cal.axis] = time * cal.current_velocity; + + // Convert to steps + float steps[MOTORS] = {0}; + mp_kinematics(travel, steps); + + // Queue segment + float error[MOTORS] = {0}; + st_prep_line(steps, error, time); + + return STAT_OK; +} + + +bool calibrate_busy() {return cal.busy;} + + +void calibrate_set_stallguard(int motor, uint16_t sg) { + if (cal.vstep < CAL_VELOCITIES && cal.tstep < CAL_THRESHOLDS && + cal.motor == motor) + cal.stallguard[cal.vstep][cal.tstep] = sg; +} + + +uint8_t command_calibrate(int argc, char *argv[]) { + if (cal.busy) return 0; + + mpBuf_t *bf = mp_get_write_buffer(); + if (!bf) { + cm_hard_alarm(STAT_BUFFER_FULL_FATAL); + return 0; + } + + // Start + memset(&cal, 0, sizeof(cal)); + cal.busy = true; + + for (int i = 0; i < CAL_THRESHOLDS; i++) + cal.thresholds[i] = CAL_MIN_THRESH + i * + (CAL_MAX_THRESH - CAL_MIN_THRESH) / (CAL_THRESHOLDS - 1); + + bf->bf_func = _exec_calibrate; // register callback + mp_commit_write_buffer(MOVE_TYPE_COMMAND); + + return 0; +} diff --git a/src/plan/calibrate.h b/src/plan/calibrate.h new file mode 100644 index 0000000..4d97c60 --- /dev/null +++ b/src/plan/calibrate.h @@ -0,0 +1,35 @@ +/******************************************************************************\ + + 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 +#include + + +bool calibrate_busy(); +void calibrate_set_stallguard(int motor, uint16_t sg); diff --git a/src/plan/exec.c b/src/plan/exec.c index f360ef7..634efd6 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -33,364 +33,93 @@ #include "motor.h" #include "util.h" #include "report.h" +#include "config.h" #include #include #include -// Execute routines. Called from the LO interrupt -static stat_t _exec_aline_head(); -static stat_t _exec_aline_body(); -static stat_t _exec_aline_tail(); -static stat_t _exec_aline_segment(); - -/// Dequeues buffer and executes move continuations. Manages run buffers and -/// other details. -stat_t mp_exec_move() { - mpBuf_t *bf = mp_get_run_buffer(); - - if (!bf) { // null if nothing's running - st_prep_null(); - return STAT_NOOP; - } - - // Manage cycle and motion state transitions - // Cycle auto-start for lines only - if (bf->move_type == MOVE_TYPE_ALINE && cm.motion_state == MOTION_STOP) - cm_set_motion_state(MOTION_RUN); - - if (!bf->bf_func) - return cm_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here - - return bf->bf_func(bf); // run the move callback in the planner buffer -} - - -/* Aline execution routines - * - * Everything here fires from interrupts and must be interrupt safe - * - * Returns: - * STAT_OK move is done - * STAT_EAGAIN move is not finished - has more segments to run - * STAT_NOOP cause no operation from the steppers - do not load the - * move - * STAT_xxxxx fatal error. Ends the move and frees the bf buffer - * - * This routine is called from the (LO) interrupt level. The interrupt - * sequencing relies on the behaviors of the routines being exactly correct. - * Each call to _exec_aline() must execute and prep *one and only one* - * segment. If the segment is the not the last segment in the bf buffer the - * _aline() must return STAT_EAGAIN. If it's the last segment it must return - * STAT_OK. If it encounters a fatal error that would terminate the move it - * should return a valid error code. Failure to obey this will introduce - * subtle and very difficult to diagnose bugs (trust me on this). - * - * Note 1 Returning STAT_OK ends the move and frees the bf buffer. - * Returning STAT_OK at this point does NOT advance position meaning - * any position error will be compensated by the next move. - * - * Note 2 Solves a potential race condition where the current move ends but - * the new move has not started because the previous move is still - * being run by the steppers. Planning can overwrite the new move. - * - * Operation: - * Aline generates jerk-controlled S-curves as per Ed Red's course notes: - * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf - * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations - * - * A full trapezoid is divided into 5 periods Periods 1 and 2 are the - * first and second halves of the acceleration ramp (the concave and convex - * parts of the S curve in the "head"). Periods 3 and 4 are the first - * and second parts of the deceleration ramp (the tail). There is also - * a period for the constant-velocity plateau of the trapezoid (the body). - * There are various degraded trapezoids possible, including 2 section - * combinations (head and tail; head and body; body and tail), and single - * sections - any one of the three. - * - * The equations that govern the acceleration and deceleration ramps are: - * - * Period 1 V = Vi + Jm*(T^2)/2 - * Period 2 V = Vh + As*T - Jm*(T^2)/2 - * Period 3 V = Vi - Jm*(T^2)/2 - * Period 4 V = Vh + As*T + Jm*(T^2)/2 - * - * These routines play some games with the acceleration and move timing - * to make sure this actually all works out. move_time is the actual time of - * the move, accel_time is the time valaue needed to compute the velocity - - * which takes the initial velocity into account (move_time does not need - * to). +/* Segment runner helper * - * --- State transitions - hierarchical state machine --- + * Notes on step error correction: * - * bf->move_state transitions: - * from _NEW to _RUN on first call (sub_state set to _OFF) - * from _RUN to _OFF on final call - * or just remains _OFF + * The commanded_steps are the target_steps delayed by one more + * segment. This lines them up in time with the encoder readings so a + * following error can be generated * - * mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY, - * _TAIL. Within each section state may be: + * The following_error term is positive if the encoder reading is + * greater than (ahead of) the commanded steps, and negative (behind) + * if the encoder reading is less than the commanded steps. The + * following error is not affected by the direction of movement - it's + * purely a statement of relative position. Examples: * - * _NEW - trigger initialization - * _RUN1 - run the first part - * _RUN2 - run the second part + * Encoder Commanded Following Err + * 100 90 +10 encoder is 10 steps ahead of commanded steps + * -90 -100 +10 encoder is 10 steps ahead of commanded steps + * 90 100 -10 encoder is 10 steps behind commanded steps + * -100 -90 -10 encoder is 10 steps behind commanded steps */ -stat_t mp_exec_aline(mpBuf_t *bf) { - if (bf->move_state == MOVE_OFF) return STAT_NOOP; - - // start a new move by setting up local context (singleton) - if (mr.move_state == MOVE_OFF) { - if (cm.hold_state == FEEDHOLD_HOLD) - return STAT_NOOP; // stops here if holding - - // initialization to process the new incoming bf buffer (Gcode block) - // copy in the gcode model state - memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t)); - bf->replannable = false; - - // short lines have already been removed, look for an actual zero - if (fp_ZERO(bf->length)) { - mr.move_state = MOVE_OFF; // reset mr buffer - mr.section_state = SECTION_OFF; - // prevent overplanning (Note 2) - bf->nx->replannable = false; - // call this to keep the loader happy - st_prep_null(); - // free buffer & end cycle if planner is empty - if (mp_free_run_buffer()) cm_cycle_end(); - - return STAT_NOOP; - } - - bf->move_state = MOVE_RUN; - mr.move_state = MOVE_RUN; - mr.section = SECTION_HEAD; - mr.section_state = SECTION_NEW; - mr.jerk = bf->jerk; -#ifdef __JERK_EXEC - mr.jerk_div2 = bf->jerk / 2; // only needed by __JERK_EXEC -#endif - mr.head_length = bf->head_length; - mr.body_length = bf->body_length; - mr.tail_length = bf->tail_length; - - mr.entry_velocity = bf->entry_velocity; - mr.cruise_velocity = bf->cruise_velocity; - mr.exit_velocity = bf->exit_velocity; - - copy_vector(mr.unit, bf->unit); - copy_vector(mr.final_target, bf->ms.target); // save move final target - - // generate the waypoints for position correction at section ends - for (uint8_t axis = 0; axis < AXES; axis++) { - mr.waypoint[SECTION_HEAD][axis] = - mr.position[axis] + mr.unit[axis] * mr.head_length; - mr.waypoint[SECTION_BODY][axis] = - mr.position[axis] + mr.unit[axis] * - (mr.head_length + mr.body_length); - mr.waypoint[SECTION_TAIL][axis] = - mr.position[axis] + mr.unit[axis] * - (mr.head_length + mr.body_length + mr.tail_length); - } - } - - // main dispatcher to process segments - // from this point on the contents of the bf buffer do not affect execution - stat_t status = STAT_OK; - - 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_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here - - // Feedhold processing. Refer to canonical_machine.h for state machine - // Catch the feedhold request and start the planning the hold - if (cm.hold_state == FEEDHOLD_SYNC) cm.hold_state = FEEDHOLD_PLAN; +static stat_t _exec_aline_segment() { + float travel_steps[MOTORS]; - // Look for the end of the decel to go into HOLD state - if (cm.hold_state == FEEDHOLD_DECEL && status == STAT_OK) { - cm.hold_state = FEEDHOLD_HOLD; - cm_set_motion_state(MOTION_HOLD); - report_request(); - } + // Set target position for the segment + // If the segment ends on a section waypoint, synchronize to the + // head, body or tail end. Otherwise, if not at a section waypoint + // compute target from segment time and velocity Don't do waypoint + // correction if you are going into a hold. + if (--mr.segment_count == 0 && mr.section_state == SECTION_2nd_HALF && + cm.motion_state == MOTION_RUN && cm.cycle_state == CYCLE_MACHINING) + copy_vector(mr.ms.target, mr.waypoint[mr.section]); - // There are 3 things that can happen here depending on return conditions: - // status bf->move_state Description - // ----------- -------------- ------------------------------------ - // STAT_EAGAIN mr buffer has more segments to run - // STAT_OK MOVE_RUN mr and bf buffers are done - // STAT_OK MOVE_NEW mr done; bf must be run again - // (it's been reused) - if (status == STAT_EAGAIN) report_request(); else { - mr.move_state = MOVE_OFF; // reset mr buffer - mr.section_state = SECTION_OFF; - bf->nx->replannable = false; // prevent overplanning (Note 2) - - if (bf->move_state == MOVE_RUN && mp_free_run_buffer()) - cm_cycle_end(); // free buffer & end cycle if planner is empty - } - - return status; -} - -/// Helper for cruise section -/// The body is broken into little segments even though it is a -/// straight line so that feedholds can happen in the middle of a line -/// with a minimum of latency -static stat_t _exec_aline_body() { - if (mr.section_state == SECTION_NEW) { - if (fp_ZERO(mr.body_length)) { - mr.section = SECTION_TAIL; - return _exec_aline_tail(); // skip ahead to tail periods - } - - mr.ms.move_time = mr.body_length / mr.cruise_velocity; - mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); - mr.segment_time = mr.ms.move_time / mr.segments; - mr.segment_velocity = mr.cruise_velocity; - mr.segment_count = (uint32_t)mr.segments; - - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position - - mr.section = SECTION_BODY; - // uses PERIOD_2 so last segment detection works - mr.section_state = SECTION_2nd_HALF; - } - - if (mr.section_state == SECTION_2nd_HALF) // straight part (period 3) - if (_exec_aline_segment() == STAT_OK) { // OK means this section is done - if (fp_ZERO(mr.tail_length)) return STAT_OK; // ends the move - mr.section = SECTION_TAIL; - mr.section_state = SECTION_NEW; - } - - return STAT_EAGAIN; -} - - -#ifdef __JERK_EXEC -/// Helper for acceleration section -static stat_t _exec_aline_head() { - if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) - if (fp_ZERO(mr.head_length)) { - mr.section = SECTION_BODY; - return _exec_aline_body(); // skip ahead to the body generator - } - - mr.midpoint_velocity = (mr.entry_velocity + mr.cruise_velocity) / 2; - // time for entire accel region - mr.ms.move_time = mr.head_length / mr.midpoint_velocity; - // # of segments in *each half* - mr.segments = ceil(uSec(mr.ms.move_time) / (2 * NOM_SEGMENT_USEC)); - mr.segment_time = mr.ms.move_time / (2 * mr.segments); - mr.accel_time = - 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk); - mr.midpoint_acceleration = - 2 * (mr.cruise_velocity - mr.entry_velocity) / mr.accel_time; - // time to advance for each segment - mr.segment_accel_time = mr.accel_time / (2 * mr.segments); - // elapsed time starting point (offset) - mr.elapsed_accel_time = mr.segment_accel_time / 2; - mr.segment_count = (uint32_t)mr.segments; - - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position - - mr.section = SECTION_HEAD; - mr.section_state = SECTION_1st_HALF; - } - - // First half (concave part of accel curve) - if (mr.section_state == SECTION_1st_HALF) { - mr.segment_velocity = - mr.entry_velocity + (square(mr.elapsed_accel_time) * mr.jerk_div2); - - if (_exec_aline_segment() == STAT_OK) { // set up for second half - mr.segment_count = (uint32_t)mr.segments; - mr.section_state = SECTION_2nd_HALF; - // start time from midpoint of segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; - } - - return STAT_EAGAIN; - } - - // Second half (convex part of accel curve) - if (mr.section_state == SECTION_2nd_HALF) { - mr.segment_velocity = mr.midpoint_velocity + - (mr.elapsed_accel_time * mr.midpoint_acceleration) - - (square(mr.elapsed_accel_time) * mr.jerk_div2); - - if (_exec_aline_segment() == STAT_OK) { // OK means this section is done - if (fp_ZERO(mr.body_length) && fp_ZERO(mr.tail_length)) - return STAT_OK; // ends the move + float segment_length = mr.segment_velocity * mr.segment_time; - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; - } + for (int i = 0; i < AXES; i++) + mr.ms.target[i] = mr.position[i] + mr.unit[i] * segment_length; } - return STAT_EAGAIN; -} - - -/// helper for deceleration section -static stat_t _exec_aline_tail() { - if (mr.section_state == SECTION_NEW) { // INITIALIZATION - if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move - - mr.midpoint_velocity = (mr.cruise_velocity + mr.exit_velocity) / 2; - mr.ms.move_time = mr.tail_length / mr.midpoint_velocity; - // # of segments in *each half* - mr.segments = ceil(uSec(mr.ms.move_time) / (2 * NOM_SEGMENT_USEC)); - // time to advance for each segment - mr.segment_time = mr.ms.move_time / (2 * mr.segments); - mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.exit_velocity) / mr.jerk); - mr.midpoint_acceleration = - 2 * (mr.cruise_velocity - mr.exit_velocity) / mr.accel_time; - // time to advance for each segment - mr.segment_accel_time = mr.accel_time / (2 * mr.segments); - // compute time from midpoint of segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; - mr.segment_count = (uint32_t)mr.segments; - if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position - mr.section = SECTION_TAIL; - mr.section_state = SECTION_1st_HALF; + // Convert target position to steps. Bucket-brigade the old target + // down the chain before getting the new target from kinematics + // + // The direct manipulation of steps to compute travel_steps only + // works for Cartesian kinematics. Other kinematics may require + // transforming travel distance as opposed to simply subtracting + // steps. + for (int i = 0; i < MOTORS; i++) { + // previous segment's position, delayed by 1 segment + mr.commanded_steps[i] = mr.position_steps[i]; + // previous segment's target becomes position + mr.position_steps[i] = mr.target_steps[i]; + // current encoder position (aligns to commanded_steps) + mr.encoder_steps[i] = motor_get_encoder(i); + mr.following_error[i] = mr.encoder_steps[i] - mr.commanded_steps[i]; } - // FIRST HALF - convex part (period 4) - if (mr.section_state == SECTION_1st_HALF) { - mr.segment_velocity = - mr.cruise_velocity - (square(mr.elapsed_accel_time) * mr.jerk_div2); + // now determine the target steps + mp_kinematics(mr.ms.target, mr.target_steps); - if (_exec_aline_segment() == STAT_OK) { // set up for second half - mr.segment_count = (uint32_t)mr.segments; - mr.section_state = SECTION_2nd_HALF; - // start time from midpoint of segment - mr.elapsed_accel_time = mr.segment_accel_time / 2; - } + // and compute the distances to be traveled + for (int i = 0; i < MOTORS; i++) + travel_steps[i] = mr.target_steps[i] - mr.position_steps[i]; - return STAT_EAGAIN; - } + // Call the stepper prep function + ritorno(st_prep_line(travel_steps, mr.following_error, mr.segment_time)); - // SECOND HALF - concave part (period 5) - if (mr.section_state == SECTION_2nd_HALF) { - mr.segment_velocity = mr.midpoint_velocity - - (mr.elapsed_accel_time * mr.midpoint_acceleration) + - (square(mr.elapsed_accel_time) * mr.jerk_div2); - return _exec_aline_segment(); // ends the move or continues EAGAIN - } + // update position from target + copy_vector(mr.position, mr.ms.target); - return STAT_EAGAIN; // should never get here +#ifdef __JERK_EXEC + // needed by jerk-based exec (ignored if running body) + mr.elapsed_accel_time += mr.segment_accel_time; +#endif + + if (!mr.segment_count) return STAT_OK; // this section has run all segments + return STAT_EAGAIN; // this section still has more segments to run } -#else // __JERK_EXEC +#ifndef __JERK_EXEC /* Forward difference math explained: * * We are using a quintic (fifth-degree) Bezier polynomial for the @@ -544,48 +273,108 @@ static void _init_forward_diffs(float Vi, float Vt) { float half_Ah_5 = C * half_h * half_h * half_h * half_h * half_h; mr.segment_velocity = half_Ah_5 + half_Bh_4 + half_Ch_3 + Vi; } +#endif // !__JERK_EXEC -/// Helper for acceleration section -static stat_t _exec_aline_head() { - if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) - if (fp_ZERO(mr.head_length)) { - mr.section = SECTION_BODY; - return _exec_aline_body(); // skip ahead to the body generator +#ifdef __JERK_EXEC +/// helper for deceleration section +static stat_t _exec_aline_tail() { + if (mr.section_state == SECTION_NEW) { // Initialization + if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move + + mr.midpoint_velocity = (mr.cruise_velocity + mr.exit_velocity) / 2; + mr.ms.move_time = mr.tail_length / mr.midpoint_velocity; + + // segments in each half + mr.segments = ceil(uSec(mr.ms.move_time) / (2 * NOM_SEGMENT_USEC)); + + // time to advance for each segment + mr.segment_time = mr.ms.move_time / (2 * mr.segments); + mr.accel_time = 2 * sqrt((mr.cruise_velocity - mr.exit_velocity) / mr.jerk); + mr.midpoint_acceleration = + 2 * (mr.cruise_velocity - mr.exit_velocity) / mr.accel_time; + + // time to advance for each segment + mr.segment_accel_time = mr.accel_time / (2 * mr.segments); + + // compute time from midpoint of segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; + mr.segment_count = mr.segments; + + if (mr.segment_time < MIN_SEGMENT_TIME) + return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position + + mr.section = SECTION_TAIL; + mr.section_state = SECTION_1st_HALF; + } + + // FIRST HALF - convex part (period 4) + if (mr.section_state == SECTION_1st_HALF) { + mr.segment_velocity = + mr.cruise_velocity - (square(mr.elapsed_accel_time) * mr.jerk_div2); + + if (_exec_aline_segment() == STAT_OK) { // set up for second half + mr.segment_count = mr.segments; + mr.section_state = SECTION_2nd_HALF; + // start time from midpoint of segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; } - // Time for entire accel region + return STAT_EAGAIN; + } + + // SECOND HALF - concave part (period 5) + if (mr.section_state == SECTION_2nd_HALF) { + mr.segment_velocity = mr.midpoint_velocity - + (mr.elapsed_accel_time * mr.midpoint_acceleration) + + (square(mr.elapsed_accel_time) * mr.jerk_div2); + + return _exec_aline_segment(); // ends the move or continues EAGAIN + } + + return STAT_EAGAIN; // should never get here +} + + +#else // __JERK_EXEC +/// helper for deceleration section +static stat_t _exec_aline_tail() { + if (mr.section_state == SECTION_NEW) { // Initialization + if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move + + // len/avg. velocity mr.ms.move_time = - 2 * mr.head_length / (mr.entry_velocity + mr.cruise_velocity); + 2 * mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); // # of segments for the section mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); + // time to advance for each segment mr.segment_time = mr.ms.move_time / mr.segments; - _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); + _init_forward_diffs(mr.cruise_velocity, mr.exit_velocity); mr.segment_count = (uint32_t)mr.segments; if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position - mr.section = SECTION_HEAD; - // Note: Set to SECTION_1st_HALF for one segment + mr.section = SECTION_TAIL; mr.section_state = SECTION_1st_HALF; } - // For forward differencing we should have one segment in - // SECTION_1st_HALF However, if it returns from that as STAT_OK, - // then there was only one segment in this section. - // First half (concave part of accel curve) + // First half - convex part (period 4) if (mr.section_state == SECTION_1st_HALF) { - if (_exec_aline_segment() == STAT_OK) { // set up for second half - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; + if (_exec_aline_segment() == STAT_OK) { + // For forward differencing we should have one segment in + // SECTION_1st_HALF. However, if it returns from that as STAT_OK, then + // there was only one segment in this section. Show that we did complete + // section 2 ... effectively. + mr.section_state = SECTION_2nd_HALF; + return STAT_OK; } else mr.section_state = SECTION_2nd_HALF; return STAT_EAGAIN; } - // Second half (convex part of accel curve) + // Second half - concave part (period 5) if (mr.section_state == SECTION_2nd_HALF) { #ifndef __KAHAN mr.segment_velocity += mr.forward_diff[4]; @@ -597,14 +386,8 @@ static stat_t _exec_aline_head() { mr.segment_velocity = v; #endif - if (_exec_aline_segment() == STAT_OK) { // set up for body - if (fp_ZERO(mr.body_length) && fp_ZERO(mr.tail_length)) - return STAT_OK; // ends the move - - mr.section = SECTION_BODY; - mr.section_state = SECTION_NEW; - - } else { + if (_exec_aline_segment() == STAT_OK) return STAT_OK; // set up for body + else { #ifndef __KAHAN mr.forward_diff[4] += mr.forward_diff[3]; mr.forward_diff[3] += mr.forward_diff[2]; @@ -636,46 +419,160 @@ static stat_t _exec_aline_head() { return STAT_EAGAIN; } +#endif // !__JERK_EXEC -/// helper for deceleration section -static stat_t _exec_aline_tail() { - if (mr.section_state == SECTION_NEW) { // Initialization - if (fp_ZERO(mr.tail_length)) return STAT_OK; // end the move +/// Helper for cruise section +/// The body is broken into little segments even though it is a +/// straight line so that feedholds can happen in the middle of a line +/// with a minimum of latency +static stat_t _exec_aline_body() { + if (mr.section_state == SECTION_NEW) { + if (fp_ZERO(mr.body_length)) { + mr.section = SECTION_TAIL; + + return _exec_aline_tail(); // skip ahead to tail periods + } + + mr.ms.move_time = mr.body_length / mr.cruise_velocity; + mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); + mr.segment_time = mr.ms.move_time / mr.segments; + mr.segment_velocity = mr.cruise_velocity; + mr.segment_count = mr.segments; + + if (mr.segment_time < MIN_SEGMENT_TIME) + return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + + mr.section = SECTION_BODY; + + // uses PERIOD_2 so last segment detection works + mr.section_state = SECTION_2nd_HALF; + } + + if (mr.section_state == SECTION_2nd_HALF) // straight part (period 3) + if (_exec_aline_segment() == STAT_OK) { // OK means this section is done + if (fp_ZERO(mr.tail_length)) return STAT_OK; // ends the move + + mr.section = SECTION_TAIL; + mr.section_state = SECTION_NEW; + } + + return STAT_EAGAIN; +} + + + +#ifdef __JERK_EXEC +/// Helper for acceleration section +static stat_t _exec_aline_head() { + if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) + if (fp_ZERO(mr.head_length)) { + mr.section = SECTION_BODY; + + return _exec_aline_body(); // skip ahead to the body generator + } + + mr.midpoint_velocity = (mr.entry_velocity + mr.cruise_velocity) / 2; + + // time for entire accel region + mr.ms.move_time = mr.head_length / mr.midpoint_velocity; + + // segments in each half + mr.segments = ceil(uSec(mr.ms.move_time) / (2 * NOM_SEGMENT_USEC)); + mr.segment_time = mr.ms.move_time / (2 * mr.segments); + mr.accel_time = + 2 * sqrt((mr.cruise_velocity - mr.entry_velocity) / mr.jerk); + mr.midpoint_acceleration = + 2 * (mr.cruise_velocity - mr.entry_velocity) / mr.accel_time; + + // time to advance for each segment + mr.segment_accel_time = mr.accel_time / (2 * mr.segments); + + // elapsed time starting point (offset) + mr.elapsed_accel_time = mr.segment_accel_time / 2; + mr.segment_count = mr.segments; + + if (mr.segment_time < MIN_SEGMENT_TIME) + return STAT_MINIMUM_TIME_MOVE; // exit without advancing position + + mr.section = SECTION_HEAD; + mr.section_state = SECTION_1st_HALF; + } + + // First half (concave part of accel curve) + if (mr.section_state == SECTION_1st_HALF) { + mr.segment_velocity = + mr.entry_velocity + (square(mr.elapsed_accel_time) * mr.jerk_div2); + + if (_exec_aline_segment() == STAT_OK) { // set up for second half + mr.segment_count = mr.segments; + mr.section_state = SECTION_2nd_HALF; + // start time from midpoint of segment + mr.elapsed_accel_time = mr.segment_accel_time / 2; + } + + return STAT_EAGAIN; + } + + // Second half (convex part of accel curve) + if (mr.section_state == SECTION_2nd_HALF) { + mr.segment_velocity = mr.midpoint_velocity + + (mr.elapsed_accel_time * mr.midpoint_acceleration) - + (square(mr.elapsed_accel_time) * mr.jerk_div2); + + if (_exec_aline_segment() == STAT_OK) { // OK means this section is done + if (fp_ZERO(mr.body_length) && fp_ZERO(mr.tail_length)) + return STAT_OK; // ends the move + + mr.section = SECTION_BODY; + mr.section_state = SECTION_NEW; + } + } + + return STAT_EAGAIN; +} + +#else // __JERK_EXEC +/// Helper for acceleration section +static stat_t _exec_aline_head() { + if (mr.section_state == SECTION_NEW) { // initialize the move singleton (mr) + if (fp_ZERO(mr.head_length)) { + mr.section = SECTION_BODY; + return _exec_aline_body(); // skip ahead to the body generator + } - // len/avg. velocity + // Time for entire accel region mr.ms.move_time = - 2 * mr.tail_length / (mr.cruise_velocity + mr.exit_velocity); + 2 * mr.head_length / (mr.entry_velocity + mr.cruise_velocity); // # of segments for the section mr.segments = ceil(uSec(mr.ms.move_time) / NOM_SEGMENT_USEC); - // time to advance for each segment mr.segment_time = mr.ms.move_time / mr.segments; - _init_forward_diffs(mr.cruise_velocity, mr.exit_velocity); + _init_forward_diffs(mr.entry_velocity, mr.cruise_velocity); mr.segment_count = (uint32_t)mr.segments; if (mr.segment_time < MIN_SEGMENT_TIME) - return STAT_MINIMUM_TIME_MOVE; // exit /wo advancing position + return STAT_MINIMUM_TIME_MOVE; // exit without advancing position - mr.section = SECTION_TAIL; + mr.section = SECTION_HEAD; + // Note: Set to SECTION_1st_HALF for one segment mr.section_state = SECTION_1st_HALF; } - // First half - convex part (period 4) + // For forward differencing we should have one segment in + // SECTION_1st_HALF However, if it returns from that as STAT_OK, + // then there was only one segment in this section. + // First half (concave part of accel curve) if (mr.section_state == SECTION_1st_HALF) { - if (_exec_aline_segment() == STAT_OK) { - // For forward differencing we should have one segment in - // SECTION_1st_HALF. However, if it returns from that as STAT_OK, then - // there was only one segment in this section. Show that we did complete - // section 2 ... effectively. - mr.section_state = SECTION_2nd_HALF; - return STAT_OK; + if (_exec_aline_segment() == STAT_OK) { // set up for second half + mr.section = SECTION_BODY; + mr.section_state = SECTION_NEW; } else mr.section_state = SECTION_2nd_HALF; return STAT_EAGAIN; } - // Second half - concave part (period 5) + // Second half (convex part of accel curve) if (mr.section_state == SECTION_2nd_HALF) { #ifndef __KAHAN mr.segment_velocity += mr.forward_diff[4]; @@ -687,8 +584,14 @@ static stat_t _exec_aline_tail() { mr.segment_velocity = v; #endif - if (_exec_aline_segment() == STAT_OK) return STAT_OK; // set up for body - else { + if (_exec_aline_segment() == STAT_OK) { // set up for body + if (fp_ZERO(mr.body_length) && fp_ZERO(mr.tail_length)) + return STAT_OK; // ends the move + + mr.section = SECTION_BODY; + mr.section_state = SECTION_NEW; + + } else { #ifndef __KAHAN mr.forward_diff[4] += mr.forward_diff[3]; mr.forward_diff[3] += mr.forward_diff[2]; @@ -723,79 +626,189 @@ static stat_t _exec_aline_tail() { #endif // !__JERK_EXEC -/* Segment runner helper +/* Aline execution routines * - * Notes on step error correction: + * Everything here fires from interrupts and must be interrupt safe * - * The commanded_steps are the target_steps delayed by one more - * segment. This lines them up in time with the encoder readings so a - * following error can be generated + * Returns: + * STAT_OK move is done + * STAT_EAGAIN move is not finished - has more segments to run + * STAT_NOOP cause no operation from the steppers - do not load the + * move + * STAT_xxxxx fatal error. Ends the move and frees the bf buffer * - * The following_error term is positive if the encoder reading is - * greater than (ahead of) the commanded steps, and negative (behind) - * if the encoder reading is less than the commanded steps. The - * following error is not affected by the direction of movement - it's - * purely a statement of relative position. Examples: + * This routine is called from the (LO) interrupt level. The interrupt + * sequencing relies on the behaviors of the routines being exactly correct. + * Each call to _exec_aline() must execute and prep *one and only one* + * segment. If the segment is the not the last segment in the bf buffer the + * _aline() must return STAT_EAGAIN. If it's the last segment it must return + * STAT_OK. If it encounters a fatal error that would terminate the move it + * should return a valid error code. Failure to obey this will introduce + * subtle and very difficult to diagnose bugs (trust me on this). * - * Encoder Commanded Following Err - * 100 90 +10 encoder is 10 steps ahead of commanded steps - * -90 -100 +10 encoder is 10 steps ahead of commanded steps - * 90 100 -10 encoder is 10 steps behind commanded steps - * -100 -90 -10 encoder is 10 steps behind commanded steps + * Note 1 Returning STAT_OK ends the move and frees the bf buffer. + * Returning STAT_OK at this point does NOT advance position meaning + * any position error will be compensated by the next move. + * + * Note 2 Solves a potential race condition where the current move ends but + * the new move has not started because the previous move is still + * being run by the steppers. Planning can overwrite the new move. + * + * Operation: + * Aline generates jerk-controlled S-curves as per Ed Red's course notes: + * http://www.et.byu.edu/~ered/ME537/Notes/Ch5.pdf + * http://www.scribd.com/doc/63521608/Ed-Red-Ch5-537-Jerk-Equations + * + * A full trapezoid is divided into 5 periods Periods 1 and 2 are the + * first and second halves of the acceleration ramp (the concave and convex + * parts of the S curve in the "head"). Periods 3 and 4 are the first + * and second parts of the deceleration ramp (the tail). There is also + * a period for the constant-velocity plateau of the trapezoid (the body). + * There are various degraded trapezoids possible, including 2 section + * combinations (head and tail; head and body; body and tail), and single + * sections - any one of the three. + * + * The equations that govern the acceleration and deceleration ramps are: + * + * Period 1 V = Vi + Jm*(T^2)/2 + * Period 2 V = Vh + As*T - Jm*(T^2)/2 + * Period 3 V = Vi - Jm*(T^2)/2 + * Period 4 V = Vh + As*T + Jm*(T^2)/2 + * + * These routines play some games with the acceleration and move timing + * to make sure this actually all works out. move_time is the actual time of + * the move, accel_time is the time valaue needed to compute the velocity - + * which takes the initial velocity into account (move_time does not need + * to). + * + * --- State transitions - hierarchical state machine --- + * + * bf->move_state transitions: + * from _NEW to _RUN on first call (sub_state set to _OFF) + * from _RUN to _OFF on final call + * or just remains _OFF + * + * mr.move_state transitions on first call from _OFF to one of _HEAD, _BODY, + * _TAIL. Within each section state may be: + * + * _NEW - trigger initialization + * _RUN1 - run the first part + * _RUN2 - run the second part */ -static stat_t _exec_aline_segment() { - uint8_t i; - float travel_steps[MOTORS]; +stat_t mp_exec_aline(mpBuf_t *bf) { + if (bf->move_state == MOVE_OFF) return STAT_NOOP; - // Set target position for the segment - // If the segment ends on a section waypoint synchronize to the - // head, body or tail end Otherwise if not at a section waypoint - // compute target from segment time and velocity Don't do waypoint - // correction if you are going into a hold. - if (--mr.segment_count == 0 && mr.section_state == SECTION_2nd_HALF && - cm.motion_state == MOTION_RUN && cm.cycle_state == CYCLE_MACHINING) - copy_vector(mr.ms.target, mr.waypoint[mr.section]); + // start a new move by setting up local context (singleton) + if (mr.move_state == MOVE_OFF) { + if (cm.hold_state == FEEDHOLD_HOLD) + return STAT_NOOP; // stops here if holding - else { - float segment_length = mr.segment_velocity * mr.segment_time; + // initialization to process the new incoming bf buffer (Gcode block) + // copy in the gcode model state + memcpy(&mr.ms, &bf->ms, sizeof(MoveState_t)); + bf->replannable = false; + + // short lines have already been removed, look for an actual zero + if (fp_ZERO(bf->length)) { + mr.move_state = MOVE_OFF; // reset mr buffer + mr.section_state = SECTION_OFF; + // prevent overplanning (Note 2) + bf->nx->replannable = false; + // free buffer & end cycle if planner is empty + if (mp_free_run_buffer()) cm_cycle_end(); + + return STAT_NOOP; + } + + bf->move_state = MOVE_RUN; + mr.move_state = MOVE_RUN; + mr.section = SECTION_HEAD; + mr.section_state = SECTION_NEW; + mr.jerk = bf->jerk; +#ifdef __JERK_EXEC + mr.jerk_div2 = bf->jerk / 2; // only needed by __JERK_EXEC +#endif + mr.head_length = bf->head_length; + mr.body_length = bf->body_length; + mr.tail_length = bf->tail_length; - for (i = 0; i < AXES; i++) - mr.ms.target[i] = mr.position[i] + (mr.unit[i] * segment_length); + mr.entry_velocity = bf->entry_velocity; + mr.cruise_velocity = bf->cruise_velocity; + mr.exit_velocity = bf->exit_velocity; + + copy_vector(mr.unit, bf->unit); + copy_vector(mr.final_target, bf->ms.target); // save move final target + + // generate the waypoints for position correction at section ends + for (uint8_t axis = 0; axis < AXES; axis++) { + mr.waypoint[SECTION_HEAD][axis] = + mr.position[axis] + mr.unit[axis] * mr.head_length; + + mr.waypoint[SECTION_BODY][axis] = + mr.position[axis] + mr.unit[axis] * + (mr.head_length + mr.body_length); + + mr.waypoint[SECTION_TAIL][axis] = + mr.position[axis] + mr.unit[axis] * + (mr.head_length + mr.body_length + mr.tail_length); + } } - // Convert target position to steps. Bucket-brigade the old target - // down the chain before getting the new target from kinematics - // - // The direct manipulation of steps to compute travel_steps only - // works for Cartesian kinematics. Other kinematics may require - // transforming travel distance as opposed to simply subtracting - // steps. - for (i = 0; i < MOTORS; i++) { - // previous segment's position, delayed by 1 segment - mr.commanded_steps[i] = mr.position_steps[i]; - // previous segment's target becomes position - mr.position_steps[i] = mr.target_steps[i]; - // current encoder position (aligns to commanded_steps) - mr.encoder_steps[i] = motor_get_encoder(i); - mr.following_error[i] = mr.encoder_steps[i] - mr.commanded_steps[i]; + // main dispatcher to process segments + // from this point on the contents of the bf buffer do not affect execution + stat_t status = STAT_OK; + + 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_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here + + // Feedhold processing. Refer to canonical_machine.h for state machine + // Catch the feedhold request and start the planning the hold + if (cm.hold_state == FEEDHOLD_SYNC) cm.hold_state = FEEDHOLD_PLAN; + + // Look for the end of the decel to go into HOLD state + if (cm.hold_state == FEEDHOLD_DECEL && status == STAT_OK) { + cm.hold_state = FEEDHOLD_HOLD; + cm_set_motion_state(MOTION_HOLD); + report_request(); } - // now determine the target steps - mp_kinematics(mr.ms.target, mr.target_steps); + // There are 3 things that can happen here depending on return conditions: + // status bf->move_state Description + // ----------- -------------- ------------------------------------ + // STAT_EAGAIN mr buffer has more segments to run + // STAT_OK MOVE_RUN mr and bf buffers are done + // STAT_OK MOVE_NEW mr done; bf must be run again + // (it's been reused) + if (status == STAT_EAGAIN) report_request(); + else { + mr.move_state = MOVE_OFF; // reset mr buffer + mr.section_state = SECTION_OFF; + bf->nx->replannable = false; // prevent overplanning (Note 2) - // and compute the distances to be traveled - for (i = 0; i < MOTORS; i++) - travel_steps[i] = mr.target_steps[i] - mr.position_steps[i]; + if (bf->move_state == MOVE_RUN && mp_free_run_buffer()) + cm_cycle_end(); // free buffer & end cycle if planner is empty + } - // Call the stepper prep function - ritorno(st_prep_line(travel_steps, mr.following_error, mr.segment_time)); - // update position from target - copy_vector(mr.position, mr.ms.target); -#ifdef __JERK_EXEC - // needed by jerk-based exec (ignored if running body) - mr.elapsed_accel_time += mr.segment_accel_time; -#endif + return status; +} - if (!mr.segment_count) return STAT_OK; // this section has run all segments - return STAT_EAGAIN; // this section still has more segments to run + +/// Dequeues buffer, executes move continuations and manages run buffers. +stat_t mp_exec_move() { + mpBuf_t *bf = mp_get_run_buffer(); + + if (!bf) return STAT_NOOP; // nothing's running + + // Manage cycle and motion state transitions + // Cycle auto-start for lines only + if (bf->move_type == MOVE_TYPE_ALINE && cm.motion_state == MOTION_STOP) + cm_set_motion_state(MOTION_RUN); + + if (!bf->bf_func) + return cm_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here + + return bf->bf_func(bf); // run the move callback on the planner buffer } diff --git a/src/plan/jog.c b/src/plan/jog.c index 37d6e88..e871da4 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -33,11 +33,13 @@ #include "motor.h" #include "canonical_machine.h" #include "motor.h" +#include "config.h" #include #include #include #include +#include typedef struct { @@ -63,8 +65,8 @@ static stat_t _exec_jog(mpBuf_t *bf) { // Compute next line segment float travel[AXES]; // In mm - float time = MIN_SEGMENT_TIME; // In minutes - float maxDeltaV = JOG_ACCELERATION * time; + const float time = MIN_SEGMENT_TIME; // In minutes + const float maxDeltaV = JOG_ACCELERATION * time; bool done = true; // Compute new velocities and travel @@ -91,7 +93,7 @@ static stat_t _exec_jog(mpBuf_t *bf) { cm_set_position(axis, steps / motor_get_steps_per_unit(motor)); } - // Release queue + // Release buffer mp_free_run_buffer(); jr.running = false; @@ -111,7 +113,16 @@ static stat_t _exec_jog(mpBuf_t *bf) { } -void mp_jog(float velocity[AXES]) { +bool mp_jog_busy() {return jr.running;} + + +uint8_t command_jog(int argc, char *argv[]) { + float velocity[AXES]; + + for (int axis = 0; axis < AXES; axis++) + if (axis < argc - 1) velocity[axis] = atof(argv[axis + 1]); + else velocity[axis] = 0; + // Reset if (!jr.running) memset(&jr, 0, sizeof(jr)); @@ -125,17 +136,14 @@ void mp_jog(float velocity[AXES]) { mpBuf_t *bf = mp_get_write_buffer(); if (!bf) { cm_hard_alarm(STAT_BUFFER_FULL_FATAL); - return; + return 0; } // Start jr.running = true; bf->bf_func = _exec_jog; // register callback - mp_commit_write_buffer(MOVE_TYPE_JOG); + mp_commit_write_buffer(MOVE_TYPE_COMMAND); } -} - -bool mp_jog_busy() { - return jr.running; + return 0; } diff --git a/src/plan/jog.h b/src/plan/jog.h index f71cee9..32554d8 100644 --- a/src/plan/jog.h +++ b/src/plan/jog.h @@ -27,9 +27,7 @@ #pragma once -#include "config.h" - #include -void mp_jog(float velocity[AXES]); + bool mp_jog_busy(); diff --git a/src/plan/planner.c b/src/plan/planner.c index 04575b6..9247b04 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -70,7 +70,7 @@ #include -mpMoveRuntimeSingleton_t mr = {}; // context for line runtime +mpMoveRuntimeSingleton_t mr = {}; // context for line runtime void planner_init() { diff --git a/src/plan/planner.h b/src/plan/planner.h index f7cb191..bf08ad3 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -69,8 +69,6 @@ typedef struct mpMoveRuntimeSingleton { // persistent runtime variables float final_target[AXES]; /// current move position float position[AXES]; - /// for Kahan summation in _exec_aline_segment() - float position_c[AXES]; /// head/body/tail endpoints for correction float waypoint[SECTIONS][AXES]; /// current MR target (absolute target as steps) diff --git a/src/report.c b/src/report.c index 7779630..5614f15 100644 --- a/src/report.c +++ b/src/report.c @@ -31,8 +31,6 @@ #include "rtc.h" #include "vars.h" -#include "plan/planner.h" - #include #include @@ -66,13 +64,3 @@ stat_t report_callback() { return STAT_OK; } - - -float get_position(int index) { - return mp_get_runtime_absolute_position(index); -} - - -float get_velocity() { - return mp_get_runtime_velocity(); -} diff --git a/src/stepper.c b/src/stepper.c index 409abff..b935bdb 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -39,6 +39,7 @@ #include "cpp_magic.h" #include +#include typedef struct { @@ -184,14 +185,6 @@ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { } -/// Keeps the loader happy. Otherwise performs no action -void st_prep_null() { - if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); - st.move_type = MOVE_TYPE_NULL; - st.move_ready = false; // signal prep buffer empty -} - - /// Stage command to execution void st_prep_command(mpBuf_t *bf) { if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); diff --git a/src/stepper.h b/src/stepper.h index bcd318b..a77ca07 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -40,6 +40,5 @@ void st_shutdown(); uint8_t st_runtime_isbusy(); stat_t st_prep_line(float travel_steps[], float following_error[], float segment_time); -void st_prep_null(); void st_prep_command(mpBuf_t *bf); void st_prep_dwell(float seconds); diff --git a/src/tmc2660.c b/src/tmc2660.c index 4f0ccac..729158c 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -31,6 +31,7 @@ #include "rtc.h" #include "cpp_magic.h" #include "canonical_machine.h" +#include "plan/calibrate.h" #include #include @@ -91,6 +92,7 @@ typedef struct { static spi_t spi = {}; + static void _report_error_flags(int driver) { tmc2660_driver_t *drv = &drivers[driver]; @@ -168,6 +170,8 @@ static bool _driver_read(int driver) { drv->sguard = (spi.in >> 14) & 0x3ff; drv->flags = spi.in >> 4; + calibrate_set_stallguard(driver, drv->sguard); + // Write driver 0 stallguard to DAC if (driver == 0 && (DACB.STATUS & DAC_CH0DRE_bm)) DACB.CH0DATA = drv->sguard << 2; @@ -191,7 +195,7 @@ static bool _driver_read(int driver) { motor_driver_callback(driver); drv->configured = true; - // Enable when first fully configured + // Enable motor when first fully configured drv->port->OUTCLR = MOTOR_ENABLE_BIT_bm; } @@ -384,6 +388,13 @@ void tmc2660_disable(int driver) { } +void tmc2660_set_stallguard_threshold(int driver, int8_t threshold) { + drivers[driver].regs[TMC2660_SGCSCONF] = + (drivers[driver].regs[TMC2660_SGCSCONF] & ~TMC2660_SGCSCONF_THRESH_bm) | + TMC2660_SGCSCONF_THRESH(threshold); +} + + float get_power_level(int driver) { return drivers[driver].drive_current; } diff --git a/src/tmc2660.h b/src/tmc2660.h index a8bcf2c..89c2796 100644 --- a/src/tmc2660.h +++ b/src/tmc2660.h @@ -42,6 +42,7 @@ 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); #define TMC2660_DRVCTRL 0 @@ -101,6 +102,7 @@ void tmc2660_disable(int driver); #define TMC2660_SGCSCONF 3 #define TMC2660_SGCSCONF_ADDR (6UL << 17) #define TMC2660_SGCSCONF_SFILT (1UL << 16) +#define TMC2660_SGCSCONF_THRESH_bm 0x7f00 #define TMC2660_SGCSCONF_THRESH(x) (((int32_t)x & 0x7f) << 8) #define TMC2660_SGCSCONF_CS(x) (((int32_t)x & 0x1f) << 0) #define TMC2660_SGCSCONF_CS_NONE (31UL << 0) diff --git a/src/usart.c b/src/usart.c index cff9bdc..966acfc 100644 --- a/src/usart.c +++ b/src/usart.c @@ -149,12 +149,17 @@ void usart_set_baud(int baud) { } -void usart_ctrl(int flag, bool enable) { +void usart_set(int flag, bool enable) { if (enable) usart_flags |= flag; else usart_flags &= ~flag; } +bool usart_is_set(int flags) { + return (usart_flags & flags) == flags; +} + + static void usart_sleep() { cli(); SLEEP.CTRL = SLEEP_SMODE_IDLE_gc | SLEEP_SEN_bm; @@ -231,7 +236,7 @@ int16_t usart_peek() { void usart_flush() { - usart_ctrl(USART_FLUSH, true); + usart_set(USART_FLUSH, true); while (!tx_buf_empty() || !(USARTC0.STATUS & USART_DREIF_bm) || !(USARTC0.STATUS & USART_TXCIF_bm)) diff --git a/src/usart.h b/src/usart.h index babe2d0..ccb3ac1 100644 --- a/src/usart.h +++ b/src/usart.h @@ -56,7 +56,8 @@ enum { void usart_init(); void usart_set_baud(int baud); -void usart_ctrl(int flag, bool enable); +void usart_set(int flag, bool enable); +bool usart_is_set(int flags); void usart_putc(char c); void usart_puts(const char *s); int8_t usart_getc(); diff --git a/src/varcb.c b/src/varcb.c new file mode 100644 index 0000000..289a783 --- /dev/null +++ b/src/varcb.c @@ -0,0 +1,50 @@ +/******************************************************************************\ + + 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 . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#include "usart.h" +#include "plan/planner.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 true; // Always true so that echo is always enabled after reboot +} + + +void set_echo(bool value) { + return usart_set(USART_ECHO, value); +} diff --git a/src/vars.c b/src/vars.c index f069355..4c12de8 100644 --- a/src/vars.c +++ b/src/vars.c @@ -59,18 +59,13 @@ static const char bool_name [] PROGMEM = ""; MAP(TYPE_NAME, SEMI, flags_t, string, float, int8_t, uint8_t, uint16_t); +// String static void var_print_string(const char *s) { printf_P(PSTR("\"%s\""), s); } -#if 0 -static void var_print_bool(bool x) { - printf_P(x ? PSTR("true") : PSTR("false")); -} -#endif - - +// Flags extern void print_status_flags(uint8_t x); static void var_print_flags_t(uint8_t x) { @@ -78,6 +73,7 @@ static void var_print_flags_t(uint8_t x) { } +// Float static void var_print_float(float x) { char buf[20]; @@ -98,49 +94,61 @@ static void var_print_float(float x) { } -static void var_print_int8_t(int8_t x) { - printf_P(PSTR("%"PRIi8), x); +static float var_parse_float(const char *value) { + return strtod(value, 0); } -static void var_print_uint8_t(uint8_t x) { - printf_P(PSTR("%"PRIu8), x); +// Bool +static void var_print_bool(bool x) { + printf_P(x ? PSTR("true") : PSTR("false")); } -static void var_print_uint16_t(uint16_t x) { - printf_P(PSTR("%"PRIu16), x); +static bool var_parse_bool(const char *value) { + return !strcasecmp(value, "true") || var_parse_float(value); } -static float var_parse_float(const char *value) { - return strtod(value, 0); +static uint8_t eeprom_read_bool(bool *addr) { + return eeprom_read_byte((uint8_t *)addr); } -#if 0 -static bool var_parse_bool(const char *value) { - return !strcasecmp(value, "true") || var_parse_float(value); +static void eeprom_update_bool(bool *addr, bool value) { + eeprom_update_byte((uint8_t *)addr, value); +} + + +// int8 +static void var_print_int8_t(int8_t x) { + printf_P(PSTR("%"PRIi8), x); } -#endif static int8_t var_parse_int8_t(const char *value) { return strtol(value, 0, 0); } -static uint8_t var_parse_uint8_t(const char *value) { - return strtol(value, 0, 0); + +static int8_t eeprom_read_int8_t(int8_t *addr) { + return eeprom_read_byte((uint8_t *)addr); } -static uint16_t var_parse_uint16_t(const char *value) { - return strtol(value, 0, 0); +static void eeprom_update_int8_t(int8_t *addr, int8_t value) { + eeprom_update_byte((uint8_t *)addr, value); } -static int8_t eeprom_read_int8_t(int8_t *addr) { - return eeprom_read_byte((uint8_t *)addr); +// uint8 +static void var_print_uint8_t(uint8_t x) { + printf_P(PSTR("%"PRIu8), x); +} + + +static uint8_t var_parse_uint8_t(const char *value) { + return strtol(value, 0, 0); } @@ -149,18 +157,24 @@ static uint8_t eeprom_read_uint8_t(uint8_t *addr) { } -static uint16_t eeprom_read_uint16_t(uint16_t *addr) { - return eeprom_read_word(addr); +static void eeprom_update_uint8_t(uint8_t *addr, uint8_t value) { + eeprom_update_byte(addr, value); } -static void eeprom_update_int8_t(int8_t *addr, int8_t value) { - eeprom_update_byte((uint8_t *)addr, value); +// unit16 +static void var_print_uint16_t(uint16_t x) { + printf_P(PSTR("%"PRIu16), x); } -static void eeprom_update_uint8_t(uint8_t *addr, uint8_t value) { - eeprom_update_byte(addr, value); +static uint16_t var_parse_uint16_t(const char *value) { + return strtol(value, 0, 0); +} + + +static uint16_t eeprom_read_uint16_t(uint16_t *addr) { + return eeprom_read_word(addr); } diff --git a/src/vars.def b/src/vars.def index ff002c5..e43971a 100644 --- a/src/vars.def +++ b/src/vars.def @@ -79,5 +79,6 @@ VAR(spin_down, "sd", float, 0, 1, 0, "Spin down velocity") VAR(switch_type, "sw", uint8_t, SWITCHES, 1, 0, "Normally open or closed") // System -VAR(velocity, "v", float, 0, 0, 0, "Current velocity") +VAR(velocity, "v", float, 0, 0, 0, "Current velocity") VAR(hw_id, "id", string, 0, 0, 0, "Hardware ID") +VAR(echo, "echo", bool, 0, 1, 1, "Enable or disable echo") -- 2.27.0