From: Joseph Coffland Date: Sun, 19 Jun 2016 19:14:06 +0000 (-0700) Subject: Added estop control, improved cm_alarm, Improved tmc2660 driver, temporarily disabled... X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=56fe31b84c51dd175d8e289c1dc577f0f2bb8819;p=bbctrl-firmware Added estop control, improved cm_alarm, Improved tmc2660 driver, temporarily disabled switch and spindle, improved status message printing and dispatch loop --- diff --git a/src/canonical_machine.c b/src/canonical_machine.c index fdad7be..b88db71 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -76,6 +76,7 @@ #include "hardware.h" #include "util.h" #include "usart.h" // for serial queue flush +#include "estop.h" #include "plan/planner.h" #include "plan/buffer.h" @@ -247,7 +248,11 @@ bool cm_get_runtime_busy() {return mp_get_runtime_busy();} float cm_get_feed_rate() {return cm.gm.feed_rate;} -/// Adjusts active model pointer as well +void cm_set_machine_state(cmMachineState_t machine_state) { + cm.machine_state = machine_state; +} + + void cm_set_motion_state(cmMotionState_t motion_state) { cm.motion_state = motion_state; } @@ -661,9 +666,9 @@ void canonical_machine_init() { /// Alarm state; send an exception report and stop processing input -stat_t cm_soft_alarm(stat_t status) { - print_status_message("Soft alarm", status); - cm.machine_state = MACHINE_ALARM; +stat_t cm_alarm(const char *location, stat_t status) { + status_error_P(location, PSTR("ALARM"), status); + estop_trigger(); return status; } @@ -678,18 +683,6 @@ stat_t cm_clear() { } -/// Alarm state; send an exception report and shut down machine -stat_t cm_hard_alarm(stat_t status) { - // Hard stop the motors and the spindle - st_shutdown(); - cm_spindle_control(SPINDLE_OFF); - print_status_message("HARD ALARM", status); - cm.machine_state = MACHINE_SHUTDOWN; - - return status; -} - - // Representation (4.3.3) // // Affect the Gcode model only (asynchronous) @@ -879,7 +872,7 @@ stat_t cm_straight_traverse(float target[], float flags[]) { // test soft limits stat_t status = cm_test_soft_limits(cm.ms.target); - if (status != STAT_OK) return cm_soft_alarm(status); + if (status != STAT_OK) return CM_ALARM(status); // prep and plan the move cm_set_work_offsets(&cm.gm); // capture fully resolved offsets to the state @@ -967,7 +960,7 @@ stat_t cm_straight_feed(float target[], float flags[]) { // test soft limits stat_t status = cm_test_soft_limits(cm.ms.target); - if (status != STAT_OK) return cm_soft_alarm(status); + if (status != STAT_OK) return CM_ALARM(status); // prep and plan the move cm_set_work_offsets(&cm.gm); // capture the fully resolved offsets to state diff --git a/src/canonical_machine.h b/src/canonical_machine.h index 3035a66..6d07f47 100644 --- a/src/canonical_machine.h +++ b/src/canonical_machine.h @@ -456,6 +456,7 @@ cmSpindleMode_t cm_get_spindle_mode(); bool cm_get_runtime_busy(); float cm_get_feed_rate(); +void cm_set_machine_state(cmMachineState_t machine_state); void cm_set_motion_state(cmMotionState_t motion_state); void cm_set_motion_mode(cmMotionMode_t motion_mode); void cm_set_spindle_mode(cmSpindleMode_t spindle_mode); @@ -486,12 +487,12 @@ stat_t cm_test_soft_limits(float target[]); // Initialization and termination (4.3.2) void canonical_machine_init(); -/// enter hard alarm state. returns same status code -stat_t cm_hard_alarm(stat_t status); -/// enter soft alarm state. returns same status code -stat_t cm_soft_alarm(stat_t status); +/// enter alarm state. returns same status code +stat_t cm_alarm(const char *location, stat_t status); stat_t cm_clear(); +#define CM_ALARM(CODE) cm_alarm(STATUS_LOCATION, CODE) + // Representation (4.3.3) void cm_set_plane(cmCanonicalPlane_t plane); void cm_set_units_mode(cmUnitsMode_t mode); diff --git a/src/command.c b/src/command.c index 4bd65da..6d4d2d3 100644 --- a/src/command.c +++ b/src/command.c @@ -32,6 +32,7 @@ #include "hardware.h" #include "report.h" #include "vars.h" +#include "estop.h" #include "plan/jog.h" #include "plan/calibrate.h" #include "config.h" @@ -170,6 +171,7 @@ int command_eval(char *cmd) { case '{': return vars_parser(cmd); case '$': return command_parser(cmd); default: + if (estop_triggered()) return STAT_MACHINE_ALARMED; if (calibrate_busy()) return STAT_OK; if (mp_jog_busy()) return STAT_OK; return gc_gcode_parser(cmd); diff --git a/src/config.h b/src/config.h index 2d5c3b1..d9345b6 100644 --- a/src/config.h +++ b/src/config.h @@ -73,16 +73,16 @@ typedef enum { #define M2_POLARITY MOTOR_POLARITY_NORMAL #define M2_POWER_MODE MOTOR_POWER_MODE -#define M3_MOTOR_MAP AXIS_A +#define M3_MOTOR_MAP AXIS_Z #define M3_STEP_ANGLE 1.8 -#define M3_TRAVEL_PER_REV 360 // degrees per motor rev +#define M3_TRAVEL_PER_REV (25.4 / 6.0) #define M3_MICROSTEPS MOTOR_MICROSTEPS #define M3_POLARITY MOTOR_POLARITY_NORMAL #define M3_POWER_MODE MOTOR_POWER_MODE -#define M4_MOTOR_MAP AXIS_Z +#define M4_MOTOR_MAP AXIS_A #define M4_STEP_ANGLE 1.8 -#define M4_TRAVEL_PER_REV (25.4 / 6.0) +#define M4_TRAVEL_PER_REV 360 // degrees per motor rev #define M4_MICROSTEPS MOTOR_MICROSTEPS #define M4_POLARITY MOTOR_POLARITY_NORMAL #define M4_POWER_MODE MOTOR_POWER_MODE @@ -270,8 +270,7 @@ typedef enum { * * (*) The TX cannot run at LO level or exception reports and other prints * called from a LO interrupt (as in prep_line()) will kill the system - * in a permanent sleep_mode() call in usart_putc() (usart.c) as no - * interrupt can release the sleep mode. + * in a permanent loop call in usart_putc() (usart.c). */ // Timer assignments - see specific modules for details diff --git a/src/estop.c b/src/estop.c new file mode 100644 index 0000000..966a402 --- /dev/null +++ b/src/estop.c @@ -0,0 +1,92 @@ +/******************************************************************************\ + + 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 "estop.h" +#include "motor.h" +#include "stepper.h" +#include "spindle.h" +#include "config.h" + +#include "plan/planner.h" + + +typedef struct { + bool triggered; +} estop_t; + + +static estop_t estop = {0}; + + +void estop_init() { +} + + +bool estop_triggered() { + return estop.triggered; +} + + +void estop_trigger() { + estop.triggered = true; + + // Hard stop the motors and the spindle + st_shutdown(); + cm_spindle_control(SPINDLE_OFF); + + // Stop and flush motion + cm_request_feedhold(); + cm_request_queue_flush(); + + // Set alarm state + cm_set_machine_state(MACHINE_ALARM); +} + + +void estop_clear() { + estop.triggered = false; + + // Clear motor errors + for (int motor = 0; motor < MOTORS; motor++) + motor_reset(motor); + + // Clear alarm state + cm_set_machine_state(MACHINE_READY); +} + + +bool get_estop() { + return estop.triggered; +} + + +void set_estop(bool value) { + if (estop.triggered != value) { + if (value) estop_trigger(); + else estop_clear(); + } +} diff --git a/src/estop.h b/src/estop.h new file mode 100644 index 0000000..189378a --- /dev/null +++ b/src/estop.h @@ -0,0 +1,36 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include + + +void estop_init(); +bool estop_triggered(); +void estop_trigger(); +void estop_clear(); diff --git a/src/main.c b/src/main.c index be3597d..046b14a 100644 --- a/src/main.c +++ b/src/main.c @@ -38,6 +38,7 @@ #include "rtc.h" #include "report.h" #include "command.h" +#include "estop.h" #include "plan/planner.h" #include "plan/buffer.h" @@ -52,14 +53,6 @@ #include -static stat_t _shutdown_idler() { - // TODO send some notification out the serial port - - // EAGAIN prevents any lower-priority actions from running - return cm_get_machine_state() == MACHINE_SHUTDOWN ? STAT_EAGAIN : STAT_OK; -} - - /// Return eagain if TX queue is backed up static stat_t _sync_to_tx_buffer() { return usart_tx_full() ? STAT_EAGAIN : STAT_OK; @@ -78,7 +71,20 @@ static stat_t _limit_switch_handler() { if (cm_get_machine_state() == MACHINE_ALARM) return STAT_NOOP; if (!switch_get_limit_thrown()) return STAT_NOOP; - return cm_hard_alarm(STAT_LIMIT_SWITCH_HIT); + return CM_ALARM(STAT_LIMIT_SWITCH_HIT); +} + + +static bool _dispatch(stat_t (*func)()) { + stat_t err = func(); + + switch (err) { + case STAT_EAGAIN: return true; + case STAT_OK: case STAT_NOOP: break; + default: printf_P(PSTR("%S\n"), status_to_pgmstr(err)); + } + + return false; } @@ -99,26 +105,25 @@ static stat_t _limit_switch_handler() { * * A routine that had no action (i.e. is OFF or idle) should return STAT_NOOP */ -void _run() { -#define DISPATCH(func) if (func == STAT_EAGAIN) return; - - DISPATCH(hw_reset_handler()); // handle hard reset requests - DISPATCH(_shutdown_idler()); // idle in shutdown state - DISPATCH(_limit_switch_handler()); // limit switch thrown - - DISPATCH(tmc2660_sync()); // synchronize driver config - DISPATCH(motor_power_callback()); // stepper motor power sequencing - - DISPATCH(cm_feedhold_sequencing_callback()); // feedhold state machine - DISPATCH(mp_plan_hold_callback()); // plan a feedhold - DISPATCH(cm_arc_callback()); // arc generation runs - DISPATCH(cm_homing_callback()); // G28.2 continuation - DISPATCH(cm_probe_callback()); // G38.2 continuation - - DISPATCH(_sync_to_planner()); // ensure a free planning buffer - DISPATCH(_sync_to_tx_buffer()); // sync with TX buffer - DISPATCH(report_callback()); // report changes - DISPATCH(command_dispatch()); // read and execute next command +static void _run() { +#define DISPATCH(func) if (_dispatch(func)) return; + + DISPATCH(hw_reset_handler); // handle hard reset requests + DISPATCH(_limit_switch_handler); // limit switch thrown + + DISPATCH(tmc2660_sync); // synchronize driver config + DISPATCH(motor_power_callback); // stepper motor power sequencing + + DISPATCH(cm_feedhold_sequencing_callback); // feedhold state machine + DISPATCH(mp_plan_hold_callback); // plan a feedhold + DISPATCH(cm_arc_callback); // arc generation runs + DISPATCH(cm_homing_callback); // G28.2 continuation + DISPATCH(cm_probe_callback); // G38.2 continuation + + DISPATCH(_sync_to_planner); // ensure a free planning buffer + DISPATCH(_sync_to_tx_buffer); // sync with TX buffer + DISPATCH(report_callback); // report changes + DISPATCH(command_dispatch); // read and execute next command } @@ -134,6 +139,7 @@ static void _init() { planner_init(); // motion planning canonical_machine_init(); // gcode machine vars_init(); // configuration variables + estop_init(); // emergency stop handler sei(); // enable interrupts diff --git a/src/messages.def b/src/messages.def index a7d5bd3..bb816ba 100644 --- a/src/messages.def +++ b/src/messages.def @@ -35,45 +35,46 @@ MSG(04, "No such device") MSG(05, "Buffer full") MSG(06, "Buffer full - fatal") MSG(07, "EEPROM data invalid") -MSG(08, "Internal error") - -MSG(09, "Move time is infinite") -MSG(10, "Move time is NAN") - -MSG(11, "Unrecognized command or config name") -MSG(12, "Invalid or malformed command") -MSG(13, "Bad number format") -MSG(14, "Parameter is read-only") -MSG(15, "Parameter cannot be read") -MSG(16, "Command not accepted at this time") -MSG(17, "Input exceeds max length") -MSG(18, "Input less than minimum value") -MSG(19, "Input exceeds maximum value") -MSG(20, "Input value range error") - -MSG(21, "Gcode command unsupported") -MSG(22, "M code unsupported") -MSG(23, "Axis word missing") -MSG(24, "Feedrate not specified") -MSG(25, "Arc specification error") -MSG(26, "Arc specification error - missing axis") +MSG(08, "Motor error") +MSG(09, "Internal error") + +MSG(10, "Move time is infinite") +MSG(11, "Move time is NAN") + +MSG(12, "Unrecognized command or config name") +MSG(13, "Invalid or malformed command") +MSG(14, "Bad number format") +MSG(15, "Parameter is read-only") +MSG(16, "Parameter cannot be read") +MSG(17, "Command not accepted at this time") +MSG(18, "Input exceeds max length") +MSG(19, "Input less than minimum value") +MSG(20, "Input exceeds maximum value") +MSG(21, "Input value range error") + +MSG(22, "Gcode command unsupported") +MSG(23, "M code unsupported") +MSG(24, "Axis word missing") +MSG(25, "Feedrate not specified") +MSG(26, "Arc specification error") +MSG(27, "Arc specification error - missing axis") // current longest message: 56 char -MSG(27, "Arc specification error - radius arc out of tolerance") -MSG(28, "Arc specification error - endpoint is starting point") +MSG(28, "Arc specification error - radius arc out of tolerance") +MSG(29, "Arc specification error - endpoint is starting point") -MSG(29, "Move less than minimum length") -MSG(30, "Move less than minimum time") -MSG(31, "Machine is alarmed - Command not processed") -MSG(32, "Limit switch hit - Shutdown occurred") +MSG(30, "Move less than minimum length") +MSG(31, "Move less than minimum time") +MSG(32, "Machine is alarmed - Command not processed") +MSG(33, "Limit switch hit - Shutdown occurred") -MSG(33, "Soft limit exceeded") +MSG(34, "Soft limit exceeded") -MSG(34, "Homing cycle failed") -MSG(35, "Homing Error - Bad or no axis specified") -MSG(36, "Homing Error - Search velocity is zero") -MSG(37, "Homing Error - Latch velocity is zero") -MSG(38, "Homing Error - Travel min & max are the same") -MSG(39, "Homing Error - Negative latch backoff") -MSG(40, "Homing Error - Homing switches misconfigured") +MSG(35, "Homing cycle failed") +MSG(36, "Homing Error - Bad or no axis specified") +MSG(37, "Homing Error - Search velocity is zero") +MSG(38, "Homing Error - Latch velocity is zero") +MSG(39, "Homing Error - Travel min & max are the same") +MSG(40, "Homing Error - Negative latch backoff") +MSG(41, "Homing Error - Homing switches misconfigured") -MSG(41, "Probe cycle failed") +MSG(42, "Probe cycle failed") diff --git a/src/motor.c b/src/motor.c index 965fcf5..6b48da6 100644 --- a/src/motor.c +++ b/src/motor.c @@ -33,6 +33,7 @@ #include "report.h" #include "stepper.h" #include "tmc2660.h" +#include "estop.h" #include "plan/planner.h" #include "plan/calibrate.h" @@ -193,8 +194,12 @@ void motor_init() { } -void motor_shutdown(int motor) { - motors[motor].port->OUTSET = MOTOR_ENABLE_BIT_bm; // Disable +void motor_enable(int motor, bool enable) { + if (enable) motors[motor].port->OUTCLR = MOTOR_ENABLE_BIT_bm; + else { + motors[motor].port->OUTSET = MOTOR_ENABLE_BIT_bm; + motors[motor].power_state = MOTOR_IDLE; + } } @@ -234,6 +239,7 @@ bool motor_stalled(int motor) { void motor_reset(int motor) { motors[motor].flags = 0; + tmc2660_reset(motor); } @@ -284,7 +290,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 || motor_error(motor) || + if (motors[motor].power_mode == MOTOR_DISABLED || estop_triggered() || rtc_expired(motors[motor].timeout)) _deenergize(motor); @@ -292,6 +298,9 @@ stat_t motor_power_callback() { // called by controller } +void print_status_flags(uint8_t flags); + + void motor_error_callback(int motor, cmMotorFlags_t errors) { if (motors[motor].power_state != MOTOR_ACTIVE) return; @@ -299,11 +308,9 @@ void motor_error_callback(int motor, cmMotorFlags_t errors) { report_request(); if (motor_error(motor)) { - _deenergize(motor); - - // Stop and flush motion - cm_request_feedhold(); - cm_request_queue_flush(); + printf_P(PSTR("\nmotor %d flags="), motor); + print_status_flags(motors[motor].flags); + CM_ALARM(STAT_MOTOR_ERROR); } } diff --git a/src/motor.h b/src/motor.h index 348a871..714bb60 100644 --- a/src/motor.h +++ b/src/motor.h @@ -39,7 +39,7 @@ typedef enum { MOTOR_FLAG_OVERTEMP_WARN_bm = 1 << 2, MOTOR_FLAG_OVERTEMP_bm = 1 << 3, MOTOR_FLAG_SHORTED_bm = 1 << 4, - MOTOR_FLAG_ERROR_bm = (MOTOR_FLAG_STALLED_bm | + MOTOR_FLAG_ERROR_bm = (//MOTOR_FLAG_STALLED_bm | TODO revisit this MOTOR_FLAG_OVERTEMP_WARN_bm | MOTOR_FLAG_OVERTEMP_bm | MOTOR_FLAG_SHORTED_bm) @@ -63,7 +63,7 @@ typedef enum { void motor_init(); -void motor_shutdown(int motor); +void motor_enable(int motor, bool enable); int motor_get_axis(int motor); float motor_get_steps_per_unit(int motor); diff --git a/src/plan/calibrate.c b/src/plan/calibrate.c index ae6909f..da06e33 100644 --- a/src/plan/calibrate.c +++ b/src/plan/calibrate.c @@ -168,7 +168,7 @@ uint8_t command_calibrate(int argc, char *argv[]) { mpBuf_t *bf = mp_get_write_buffer(); if (!bf) { - cm_hard_alarm(STAT_BUFFER_FULL_FATAL); + CM_ALARM(STAT_BUFFER_FULL_FATAL); return 0; } diff --git a/src/plan/command.c b/src/plan/command.c index 3d0f2bb..9a67ba2 100644 --- a/src/plan/command.c +++ b/src/plan/command.c @@ -63,7 +63,7 @@ void mp_queue_command(cm_exec_t cm_exec, float *value, float *flag) { mpBuf_t *bf = mp_get_write_buffer(); if (!bf) { - cm_hard_alarm(STAT_BUFFER_FULL_FATAL); + CM_ALARM(STAT_BUFFER_FULL_FATAL); return; // Shouldn't happen, buffer availability was checked upstream. } diff --git a/src/plan/dwell.c b/src/plan/dwell.c index 55953d4..281d283 100644 --- a/src/plan/dwell.c +++ b/src/plan/dwell.c @@ -53,7 +53,7 @@ stat_t mp_dwell(float seconds) { mpBuf_t *bf = mp_get_write_buffer(); // never supposed to fail - if (!bf) return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); + if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); bf->bf_func = _exec_dwell; // register callback to dwell start bf->ms.move_time = seconds; // in seconds, not minutes diff --git a/src/plan/exec.c b/src/plan/exec.c index 634efd6..ff041dd 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -33,6 +33,7 @@ #include "motor.h" #include "util.h" #include "report.h" +#include "estop.h" #include "config.h" #include @@ -762,7 +763,7 @@ stat_t mp_exec_aline(mpBuf_t *bf) { 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 + else return CM_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 @@ -801,6 +802,7 @@ stat_t mp_exec_move() { mpBuf_t *bf = mp_get_run_buffer(); if (!bf) return STAT_NOOP; // nothing's running + if (estop_triggered()) return STAT_MACHINE_ALARMED; // Manage cycle and motion state transitions // Cycle auto-start for lines only @@ -808,7 +810,7 @@ stat_t mp_exec_move() { cm_set_motion_state(MOTION_RUN); if (!bf->bf_func) - return cm_hard_alarm(STAT_INTERNAL_ERROR); // never supposed to get here + return CM_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 e871da4..f641abe 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -135,7 +135,7 @@ uint8_t command_jog(int argc, char *argv[]) { // Should always be at least one free buffer mpBuf_t *bf = mp_get_write_buffer(); if (!bf) { - cm_hard_alarm(STAT_BUFFER_FULL_FATAL); + CM_ALARM(STAT_BUFFER_FULL_FATAL); return 0; } diff --git a/src/plan/line.c b/src/plan/line.c index cd00979..3768553 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -240,7 +240,7 @@ stat_t mp_aline(MoveState_t *ms) { // Get a cleared buffer and setup move variables mpBuf_t *bf = mp_get_write_buffer(); // current move pointer - if (!bf) return cm_hard_alarm(STAT_BUFFER_FULL_FATAL); // never fails + if (!bf) return CM_ALARM(STAT_BUFFER_FULL_FATAL); // never fails // Register callback to exec function bf->bf_func = mp_exec_aline; diff --git a/src/spindle.c b/src/spindle.c index 7d1dcfd..2c81eda 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -68,6 +68,7 @@ static void _exec_spindle_speed(float *value, float *flag) { void cm_spindle_init() { + return; pwm_spindle_init(); huanyang_init(); } diff --git a/src/status.c b/src/status.c index 2eb1039..42afac6 100644 --- a/src/status.c +++ b/src/status.c @@ -27,7 +27,6 @@ #include "status.h" -#include #include stat_t status_code; // allocate a variable for the ritorno macro @@ -43,7 +42,13 @@ static const char *const stat_msg[] PROGMEM = { }; +const char *status_to_pgmstr(stat_t status) { + return pgm_read_ptr(&stat_msg[status]); +} + + /// Return the status message -void print_status_message(const char *msg, stat_t status) { - printf_P("%S: %S (%d)\n", pgm_read_word(&stat_msg[status])); +void status_error_P(const char *location, const char *msg, stat_t status) { + printf_P(PSTR("\nERROR: %S: %S: %S (%d)\n"), + msg, location, status_to_pgmstr(status), status); } diff --git a/src/status.h b/src/status.h index b837e25..bb9d89b 100644 --- a/src/status.h +++ b/src/status.h @@ -27,13 +27,15 @@ #pragma once +#include + // ritorno is a handy way to provide exception returns // It returns only if an error occurred. (ritorno is Italian for return) #define ritorno(a) if ((status_code = a) != STAT_OK) {return status_code;} typedef enum { -// OS, communications and low-level status + // OS, communications and low-level status STAT_OK, // function completed OK STAT_EAGAIN, // function would block (call again) STAT_NOOP, // function had no-operation @@ -42,12 +44,13 @@ typedef enum { STAT_BUFFER_FULL, STAT_BUFFER_FULL_FATAL, STAT_EEPROM_DATA_INVALID, + STAT_MOTOR_ERROR, STAT_INTERNAL_ERROR, // unrecoverable internal error STAT_PREP_LINE_MOVE_TIME_IS_INFINITE, STAT_PREP_LINE_MOVE_TIME_IS_NAN, -// Generic data input errors + // Generic data input errors STAT_UNRECOGNIZED_NAME, STAT_INVALID_OR_MALFORMED_COMMAND, STAT_BAD_NUMBER_FORMAT, @@ -59,8 +62,8 @@ typedef enum { STAT_INPUT_EXCEEDS_MAX_VALUE, STAT_INPUT_VALUE_RANGE_ERROR, -// Gcode errors and warnings (Most originate from NIST - by concept, not number) -// Fascinating: http://www.cncalarms.com/ + // Gcode errors & warnings (Most originate from NIST) + // Fascinating: http://www.cncalarms.com/ STAT_GCODE_COMMAND_UNSUPPORTED, STAT_MCODE_COMMAND_UNSUPPORTED, STAT_GCODE_AXIS_IS_MISSING, @@ -70,7 +73,7 @@ typedef enum { STAT_ARC_RADIUS_OUT_OF_TOLERANCE, STAT_ARC_ENDPOINT_IS_STARTING_POINT, -// Errors and warnings + // Errors and warnings STAT_MINIMUM_LENGTH_MOVE, // move is less than minimum length STAT_MINIMUM_TIME_MOVE, // move is less than minimum time STAT_MACHINE_ALARMED, // machine is alarmed @@ -93,4 +96,11 @@ typedef enum { extern stat_t status_code; -void print_status_message(const char *msg, stat_t status); +const char *status_to_pgmstr(stat_t status); +void status_error_P(const char *location, const char *msg, stat_t status); + +#define TO_STRING(x) _TO_STRING(x) +#define _TO_STRING(x) #x + +#define STATUS_LOCATION PSTR(__FILE__ ":" TO_STRING(__LINE__)) +#define STATUS_ERROR(MSG, CODE) status_error_P(STATUS_LOCATION, PSTR(MSG), CODE) diff --git a/src/stepper.c b/src/stepper.c index 44a8643..891c072 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -71,7 +71,7 @@ void stepper_init() { void st_shutdown() { for (int motor = 0; motor < MOTORS; motor++) - motor_shutdown(motor); + motor_enable(motor, false); } @@ -165,10 +165,10 @@ ISR(STEP_TIMER_ISR) { */ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { // Trap conditions that would prevent queueing the line - if (st.move_ready) return cm_hard_alarm(STAT_INTERNAL_ERROR); + if (st.move_ready) return CM_ALARM(STAT_INTERNAL_ERROR); if (isinf(seg_time)) - return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); - if (isnan(seg_time)) return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_NAN); + return CM_ALARM(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); + if (isnan(seg_time)) return CM_ALARM(STAT_PREP_LINE_MOVE_TIME_IS_NAN); if (seg_time < EPSILON) return STAT_MINIMUM_TIME_MOVE; // Setup segment parameters @@ -188,7 +188,7 @@ stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { /// Stage command to execution void st_prep_command(mpBuf_t *bf) { - if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); + if (st.move_ready) CM_ALARM(STAT_INTERNAL_ERROR); st.move_type = MOVE_TYPE_COMMAND; st.bf = bf; st.move_ready = true; // signal prep buffer ready @@ -197,7 +197,7 @@ void st_prep_command(mpBuf_t *bf) { /// Add a dwell to the move buffer void st_prep_dwell(float seconds) { - if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); + if (st.move_ready) CM_ALARM(STAT_INTERNAL_ERROR); st.move_type = MOVE_TYPE_DWELL; st.seg_period = STEP_TIMER_FREQ * 0.001; // 1 ms st.prep_dwell = seconds * 1000; // convert to ms diff --git a/src/switch.c b/src/switch.c index 3ceb00b..28cd0e8 100644 --- a/src/switch.c +++ b/src/switch.c @@ -203,6 +203,8 @@ void _switch_enable(switch_t *s, bool enable) { void switch_init() { + return; // TODO + for (int i = 0; i < SWITCHES; i++) { switch_t *s = &sw.switches[i]; diff --git a/src/tmc2660.c b/src/tmc2660.c index f6322a7..a697c04 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -48,8 +48,8 @@ typedef struct { bool wrote_data; bool configured; - bool monitor; - uint8_t reg; + bool reset; + uint32_t next_cmd; uint32_t stabilizing; uint16_t sguard; @@ -68,8 +68,8 @@ static const uint32_t reg_addrs[] = { TMC2660_DRVCTRL_ADDR, TMC2660_CHOPCONF_ADDR, TMC2660_SMARTEN_ADDR, - TMC2660_SGCSCONF_ADDR, TMC2660_DRVCONF_ADDR, + TMC2660_SGCSCONF_ADDR, }; @@ -93,10 +93,15 @@ static spi_t spi = {}; +static bool _driver_stabilized(tmc2660_driver_t *drv) { + return !drv->stabilizing || rtc_expired(drv->stabilizing); +} + + static void _report_error_flags(int driver) { tmc2660_driver_t *drv = &drivers[driver]; - if (drv->stabilizing < rtc_get_time()) return; + if (!_driver_stabilized(drv)) return; uint8_t dflags = drv->flags; uint8_t mflags = 0; @@ -147,22 +152,20 @@ static void _driver_write(int driver) { tmc2660_driver_t *drv = &drivers[driver]; _spi_cs(spi.driver, true); // Select driver - - if (drv->monitor) spi.out = TMC2660_DRVCTRL_ADDR | drv->regs[TMC2660_DRVCTRL]; - else spi.out = reg_addrs[drv->reg] | drv->regs[drv->reg]; - + spi.out = drv->next_cmd; drv->wrote_data = true; _spi_send(); // Start transfer } +// Returns true if the current driver has more data to send static bool _driver_read(int driver) { tmc2660_driver_t *drv = &drivers[driver]; _spi_cs(spi.driver, false); // Deselect driver // Read response - bool read_data = drv->wrote_data; + bool read_response = drv->wrote_data; if (drv->wrote_data) { drv->wrote_data = false; @@ -170,7 +173,7 @@ static bool _driver_read(int driver) { drv->sguard = (spi.in >> 14) & 0x3ff; drv->flags = spi.in >> 4; - calibrate_set_stallguard(driver, drv->sguard); + //calibrate_set_stallguard(driver, drv->sguard); // Write driver 0 stallguard to DAC //if (driver == 0 && (DACB.STATUS & DAC_CH0DRE_bm)) @@ -179,32 +182,38 @@ static bool _driver_read(int driver) { _report_error_flags(driver); } - // Check if regs have changed - for (int i = 0; i < REGS; i++) + // Handle reset + if (drv->reset) { + drv->reset = false; + for (int i = 0; i < REGS; i++) drv->last_regs[i] = -1; + } + + // Check if regs have changed (skipping DRVCTRL) + for (int i = 1; i < REGS; i++) if (drv->last_regs[i] != drv->regs[i]) { // Reg changed, update driver - drv->monitor = false; - drv->last_regs[drv->reg] = drv->regs[drv->reg]; + drv->last_regs[i] = drv->regs[i]; + drv->next_cmd = reg_addrs[i] | drv->regs[i]; drv->stabilizing = rtc_get_time() + TMC2660_STABILIZE_TIME * 1000; drv->configured = false; - return true; - } else if (++drv->reg == REGS) drv->reg = 0; + return true; + } - if (!drv->configured && drv->stabilizing < rtc_get_time()) { + // Update motor + if (!drv->configured && _driver_stabilized(drv)) { motor_driver_callback(driver); drv->configured = true; // Enable motor when first fully configured - drv->port->OUTCLR = MOTOR_ENABLE_BIT_bm; + motor_enable(driver, true); } - if (!drv->monitor || !read_data) { - drv->monitor = true; - return true; - } + // Switch back to monitoring + drv->next_cmd = TMC2660_DRVCTRL_ADDR | drv->regs[TMC2660_DRVCTRL]; - return false; + // Write command now if we didn't read a response above + return !read_response; } @@ -233,7 +242,7 @@ ISR(TCC1_OVF_vect) { void _fault_isr(int motor) { - if (drivers[motor].stabilizing < rtc_get_time()) + if (_driver_stabilized(&drivers[motor])) motor_error_callback(motor, MOTOR_FLAG_STALLED_bm); } @@ -334,10 +343,12 @@ void tmc2660_init() { static void _set_reg(int motor, int reg, uint32_t value) { - if (drivers[motor].regs[reg] == value) return; + tmc2660_driver_t *drv = &drivers[motor]; - drivers[motor].regs[reg] = value; - drivers[motor].configured = false; + if (drv->regs[reg] == value) return; + + drv->regs[reg] = value; + drv->configured = false; } @@ -365,6 +376,11 @@ uint8_t tmc2660_flags(int motor) { } +void tmc2660_reset(int driver) { + drivers[driver].reset = true; +} + + bool tmc2660_ready(int motor) { return drivers[motor].configured; } @@ -379,11 +395,14 @@ stat_t tmc2660_sync() { void tmc2660_enable(int driver) { + printf("Enable %d\n", driver); + tmc2660_reset(driver); _set_current(driver, drivers[driver].drive_current); } void tmc2660_disable(int driver) { + printf("Disable %d\n", driver); _set_current(driver, drivers[driver].idle_current); } diff --git a/src/tmc2660.h b/src/tmc2660.h index 89c2796..625308f 100644 --- a/src/tmc2660.h +++ b/src/tmc2660.h @@ -99,7 +99,7 @@ void tmc2660_set_stallguard_threshold(int driver, int8_t threshold); (((uint32_t)(MIN / 32) & 0xf) | \ (((uint32_t)(MAX / 32 - MIN / 32 - 1) & 0xf) << 8)) -#define TMC2660_SGCSCONF 3 +#define TMC2660_SGCSCONF 4 #define TMC2660_SGCSCONF_ADDR (6UL << 17) #define TMC2660_SGCSCONF_SFILT (1UL << 16) #define TMC2660_SGCSCONF_THRESH_bm 0x7f00 @@ -107,7 +107,7 @@ void tmc2660_set_stallguard_threshold(int driver, int8_t threshold); #define TMC2660_SGCSCONF_CS(x) (((int32_t)x & 0x1f) << 0) #define TMC2660_SGCSCONF_CS_NONE (31UL << 0) -#define TMC2660_DRVCONF 4 +#define TMC2660_DRVCONF 3 #define TMC2660_DRVCONF_ADDR (7UL << 17) #define TMC2660_DRVCONF_TST (1UL << 16) #define TMC2660_DRVCONF_SLPH_MIN (0UL << 14) diff --git a/src/vars.def b/src/vars.def index 4207380..072f1b0 100644 --- a/src/vars.def +++ b/src/vars.def @@ -97,3 +97,4 @@ VAR(switch_type, "sw", uint8_t, SWITCHES, 1, 0, "Normally open or closed") 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") +VAR(estop, "estop", bool, 0, 1, 1, "Emergency stop") diff --git a/tmc2660_decode.py b/tmc2660_decode.py new file mode 100755 index 0000000..4056d6b --- /dev/null +++ b/tmc2660_decode.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 + +import sys +import csv +import json + + +rdsel = 1 + + +def twos_comp(val, bits): + if val & (1 << (bits - 1)): return val - (1 << bits) + return val + + +def tmc2660_decode_response(x, rdsel = 1): + d = {'_hex': '0x%05x' % x} + + if rdsel == 0: + d['mstep'] = (x >> 10) & 0x3ff + + elif rdsel == 1: + d['sg'] = (x >> 10) & 0x3ff + + elif rdsel == 2: + d['sg'] = x >> 15 & 0x1f + d['se'] = (x >> 10) & 0x1f + + flags = [] + if x & (1 << 7): flags += ['Standstill'] + if x & (1 << 6): flags += ['Open B'] + if x & (1 << 5): flags += ['Open A'] + if x & (1 << 4): flags += ['Short B'] + if x & (1 << 3): flags += ['Short A'] + if x & (1 << 2): flags += ['Temp warn'] + if x & (1 << 1): flags += ['Overtemp'] + if x & (1 << 0): flags += ['Stall'] + + d['flags'] = flags + + return d + + +def tmc2660_decode_cmd(x, r): + global rdsel + + addr = x >> 17 + + d = {'_hex': '0x%05x' % x} + + if addr == 0 or addr == 1: + cmd = 'DRVCTRL' + d['mstep'] = (256, 128, 64, 32, 16, 8, 4, 2, 1)[x & 0xf] + d['dedge'] = bool(x & (1 << 8)) + d['intpol'] = bool(x & (1 << 9)) + + elif addr == 4: + chm = bool(x & (1 << 14)) + + cmd = 'CHOPCONF' + d['blank'] = (16, 24, 36, 54)[(x >> 15) & 3] + d['chm'] = 'standard' if chm else 'constant' + d['random toff'] = bool(x & (1 << 13)) + + if chm: d['fast decay mode'] = ('current', 'timer')[x >> 12] + else: d['hdec'] = (16, 32, 48, 64)[(x >> 11) & 3] + + if chm: d['SWO'] = ((x >> 7) & 0xf) - 3 + else: d['HEND'] = ((x >> 7) & 0xf) - 3 + + if chm: d['fast decay'] = (((x >> 4) & 7) + ((x >> 7) & 8)) * 32 + else: d['HSTART'] = ((x >> 4) & 7) + 1 + + # TODO this isn't quite right + toff = x & 0xf + if toff == 0: d['TOFF'] = 'disabled' + else: d['TOFF'] = 12 + (32 * toff) + + elif addr == 5: + cmd = 'SMARTEN' + d['SEIMIN'] = '1/2 CS' if x & (1 << 15) else '1/4 CS' + d['SEDN'] = (32, 8, 2, 1)[(x >> 13) & 3] + d['SEMAX'] = (x >> 8) & 0xf + d['SEUP'] = (1, 2, 4, 8)[(x >> 5) & 3] + d['SEMIN'] = x & 0xf + + elif addr == 6: + cmd = 'SGCSCONF' + d['SFILT'] = bool(x & (1 << 16)) + d['SGT'] = twos_comp((x >> 8) & 0x7f, 7) + d['CS'] = ((x & 0x1f) + 1) / 32.0 + + elif addr == 7: + cmd = 'DRVCONF' + d['TST'] = bool(x & (1 << 16)) + d['SLPH'] = ('min', 'min temp', 'med temp', 'max')[(x >> 14) & 3] + d['SLPL'] = ('min', 'min', 'med', 'max')[(x >> 12) & 3] + d['DISS2G'] = bool(x & (1 << 10)) + d['TS2G'] = (3.2, 1.6, 1.2, 0.8)[(x >> 8) & 3] + d['SDOFF'] = 'SPI' if x & (1 << 7) else 'step' + d['VSENSE'] = '165mV' if x & (1 << 6) else '305mV' + rdsel = (x >> 4) & 3 + d['RDSEL'] = ('mstep', 'SG', 'SG & CS', 'Invalid')[rdsel] + + else: cmd = 'INVALID' + + return {cmd: d, 'response': tmc2660_decode_response(r, rdsel)} + + +def tmc2660_decode(path): + with open(path, newline = '') as f: + reader = csv.reader(f) + + first = True + for time, packet, mosi, miso in reader: + if first: + first = False + continue + + mosi = int(mosi, 16) + miso = int(miso, 16) + + cmd = tmc2660_decode_cmd(mosi, miso) + + print(json.dumps(cmd, sort_keys = True)) + + +if __name__ == "__main__": + for path in sys.argv[1:]: + tmc2660_decode(path)