#include "hardware.h"
#include "util.h"
#include "usart.h" // for serial queue flush
+#include "estop.h"
#include "plan/planner.h"
#include "plan/buffer.h"
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;
}
/// 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;
}
}
-/// 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)
// 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
// 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
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);
// 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);
#include "hardware.h"
#include "report.h"
#include "vars.h"
+#include "estop.h"
#include "plan/jog.h"
#include "plan/calibrate.h"
#include "config.h"
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);
#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
*
* (*) 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
--- /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 "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();
+ }
+}
--- /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>
+
+
+void estop_init();
+bool estop_triggered();
+void estop_trigger();
+void estop_clear();
#include "rtc.h"
#include "report.h"
#include "command.h"
+#include "estop.h"
#include "plan/planner.h"
#include "plan/buffer.h"
#include <stdbool.h>
-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;
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;
}
*
* 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
}
planner_init(); // motion planning
canonical_machine_init(); // gcode machine
vars_init(); // configuration variables
+ estop_init(); // emergency stop handler
sei(); // enable interrupts
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")
#include "report.h"
#include "stepper.h"
#include "tmc2660.h"
+#include "estop.h"
#include "plan/planner.h"
#include "plan/calibrate.h"
}
-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;
+ }
}
void motor_reset(int motor) {
motors[motor].flags = 0;
+ tmc2660_reset(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);
}
+void print_status_flags(uint8_t flags);
+
+
void motor_error_callback(int motor, cmMotorFlags_t errors) {
if (motors[motor].power_state != MOTOR_ACTIVE) return;
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);
}
}
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)
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);
mpBuf_t *bf = mp_get_write_buffer();
if (!bf) {
- cm_hard_alarm(STAT_BUFFER_FULL_FATAL);
+ CM_ALARM(STAT_BUFFER_FULL_FATAL);
return 0;
}
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.
}
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
#include "motor.h"
#include "util.h"
#include "report.h"
+#include "estop.h"
#include "config.h"
#include <string.h>
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
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
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
}
// 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;
}
// 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;
void cm_spindle_init() {
+ return;
pwm_spindle_init();
huanyang_init();
}
#include "status.h"
-#include <avr/pgmspace.h>
#include <stdio.h>
stat_t status_code; // allocate a variable for the ritorno macro
};
+const char *status_to_pgmstr(stat_t status) {
+ return pgm_read_ptr(&stat_msg[status]);
+}
+
+
/// 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);
}
#pragma once
+#include <avr/pgmspace.h>
+
// 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
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,
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,
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
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)
void st_shutdown() {
for (int motor = 0; motor < MOTORS; motor++)
- motor_shutdown(motor);
+ motor_enable(motor, false);
}
*/
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
/// 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
/// 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
void switch_init() {
+ return; // TODO
+
for (int i = 0; i < SWITCHES; i++) {
switch_t *s = &sw.switches[i];
typedef struct {
bool wrote_data;
bool configured;
- bool monitor;
- uint8_t reg;
+ bool reset;
+ uint32_t next_cmd;
uint32_t stabilizing;
uint16_t sguard;
TMC2660_DRVCTRL_ADDR,
TMC2660_CHOPCONF_ADDR,
TMC2660_SMARTEN_ADDR,
- TMC2660_SGCSCONF_ADDR,
TMC2660_DRVCONF_ADDR,
+ TMC2660_SGCSCONF_ADDR,
};
+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;
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;
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))
_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;
}
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);
}
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;
}
}
+void tmc2660_reset(int driver) {
+ drivers[driver].reset = true;
+}
+
+
bool tmc2660_ready(int motor) {
return drivers[motor].configured;
}
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);
}
(((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
#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)
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")
--- /dev/null
+#!/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)
projects / bbctrl-firmware / commitdiff