#include "report.h"
#include "vars.h"
#include "plan/jog.h"
+#include "plan/calibrate.h"
#include "config.h"
#include <avr/pgmspace.h>
#include <avr/wdt.h>
#include <stdio.h>
-#include <stdlib.h>
#include <string.h>
#include <ctype.h>
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);
}
}
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;
-}
CMD(clear, 0, 0, "Clear saved settings")
CMD(jog, 1, 4, "Jog")
CMD(mreset, 0, 1, "Reset motor")
+CMD(calibrate, 0, 0, "Calibrate motors")
#include "status.h"
+#include <stdint.h>
+
+
#define MAX_ARGS 16
typedef uint8_t (*command_cb_t)(int argc, char *argv[]);
#include <math.h>
-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
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;
*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;
}
// 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:
}
-/* 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);
}
#include "tmc2660.h"
#include "plan/planner.h"
+#include "plan/calibrate.h"
#include <avr/interrupt.h>
#include <avr/pgmspace.h>
/// 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;
}
/// 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);
}
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);
motors[motor].flags |= errors;
report_request();
- if (_error(motor)) {
+ if (motor_error(motor)) {
_deenergize(motor);
// Stop and flush motion
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();
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();
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 {
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ This file ("the software") is free software: you can redistribute it
+ and/or modify it under the terms of the GNU General Public License,
+ version 2 as published by the Free Software Foundation. You should
+ have received a copy of the GNU General Public License, version 2
+ along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+
+#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 <stdint.h>
+#include <stdio.h>
+#include <string.h>
+
+
+#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;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ All rights reserved.
+
+ This file ("the software") is free software: you can redistribute it
+ and/or modify it under the terms of the GNU General Public License,
+ version 2 as published by the Free Software Foundation. You should
+ have received a copy of the GNU General Public License, version 2
+ along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+bool calibrate_busy();
+void calibrate_set_stallguard(int motor, uint16_t sg);
#include "motor.h"
#include "util.h"
#include "report.h"
+#include "config.h"
#include <string.h>
#include <stdbool.h>
#include <math.h>
-// 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 <don't care> 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
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];
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];
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];
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];
#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 <don't care> 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
}
#include "motor.h"
#include "canonical_machine.h"
#include "motor.h"
+#include "config.h"
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include <stdio.h>
+#include <stdlib.h>
typedef struct {
// 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
cm_set_position(axis, steps / motor_get_steps_per_unit(motor));
}
- // Release queue
+ // Release buffer
mp_free_run_buffer();
jr.running = false;
}
-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));
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;
}
#pragma once
-#include "config.h"
-
#include <stdbool.h>
-void mp_jog(float velocity[AXES]);
+
bool mp_jog_busy();
#include <stdio.h>
-mpMoveRuntimeSingleton_t mr = {}; // context for line runtime
+mpMoveRuntimeSingleton_t mr = {}; // context for line runtime
void planner_init() {
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)
#include "rtc.h"
#include "vars.h"
-#include "plan/planner.h"
-
#include <avr/pgmspace.h>
#include <stdio.h>
return STAT_OK;
}
-
-
-float get_position(int index) {
- return mp_get_runtime_absolute_position(index);
-}
-
-
-float get_velocity() {
- return mp_get_runtime_velocity();
-}
#include "cpp_magic.h"
#include <string.h>
+#include <stdio.h>
typedef struct {
}
-/// 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);
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);
#include "rtc.h"
#include "cpp_magic.h"
#include "canonical_machine.h"
+#include "plan/calibrate.h"
#include <avr/io.h>
#include <avr/interrupt.h>
static spi_t spi = {};
+
static void _report_error_flags(int driver) {
tmc2660_driver_t *drv = &drivers[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;
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;
}
}
+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;
}
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
#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)
}
-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;
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))
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();
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2016 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ All rights reserved.
+
+ This file ("the software") is free software: you can redistribute it
+ and/or modify it under the terms of the GNU General Public License,
+ version 2 as published by the Free Software Foundation. You should
+ have received a copy of the GNU General Public License, version 2
+ along with the software. If not, see <http://www.gnu.org/licenses/>.
+
+ The software is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with the software. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#include "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);
+}
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) {
}
+// Float
static void var_print_float(float x) {
char buf[20];
}
-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);
}
}
-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);
}
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")
projects / bbctrl-firmware / commitdiff