From: Joseph Coffland Date: Wed, 23 Mar 2016 06:12:22 +0000 (-0700) Subject: Adaptive motor power, fixed SPI glitch, other fixes and optimizations, runs pretty... X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=4476d6323345fd2a1d3847f9b538134db54244a0;p=bbctrl-firmware Adaptive motor power, fixed SPI glitch, other fixes and optimizations, runs pretty well. --- diff --git a/src/axes.c b/src/axes.c new file mode 100644 index 0000000..d208e2e --- /dev/null +++ b/src/axes.c @@ -0,0 +1,178 @@ +/******************************************************************************\ + + 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 "canonical_machine.h" + + +uint8_t get_axis_mode(int axis) { + return cm.a[axis].axis_mode; +} + + +void set_axis_mode(int axis, uint8_t value) { + if (value < AXIS_MODE_MAX) + cm.a[axis].axis_mode = value; +} + + +float get_max_velocity(int axis) { + return cm.a[axis].velocity_max; +} + + +void set_max_velocity(int axis, float value) { + cm.a[axis].velocity_max = value; +} + + +float get_max_feedrate(int axis) { + return cm.a[axis].feedrate_max; +} + + +void set_max_feedrate(int axis, float value) { + cm.a[axis].feedrate_max = value; +} + + +float get_max_jerk(int axis) { + return cm.a[axis].jerk_max; +} + + +void set_max_jerk(int axis, float value) { + cm.a[axis].jerk_max = value; +} + + +float get_junction_dev(int axis) { + return cm.a[axis].junction_dev; +} + + +void set_junction_dev(int axis, float value) { + cm.a[axis].junction_dev = value; +} + + +float get_travel_min(int axis) { + return cm.a[axis].travel_min; +} + + +void set_travel_min(int axis, float value) { + cm.a[axis].travel_min = value; +} + + +float get_travel_max(int axis) { + return cm.a[axis].travel_max; +} + + +void set_travel_max(int axis, float value) { + cm.a[axis].travel_max = value; +} + + +float get_jerk_homing(int axis) { + return cm.a[axis].jerk_homing; +} + + +void set_jerk_homing(int axis, float value) { + cm.a[axis].jerk_homing = value; +} + + +float get_search_vel(int axis) { + return cm.a[axis].search_velocity; +} + + +void set_search_vel(int axis, float value) { + cm.a[axis].search_velocity = value; +} + + +float get_latch_vel(int axis) { + return cm.a[axis].latch_velocity; +} + + +void set_latch_vel(int axis, float value) { + cm.a[axis].latch_velocity = value; +} + + +float get_latch_backoff(int axis) { + return cm.a[axis].latch_backoff; +} + + +void set_latch_backoff(int axis, float value) { + cm.a[axis].latch_backoff = value; +} + + +float get_zero_backoff(int axis) { + return cm.a[axis].zero_backoff; +} + + +void set_zero_backoff(int axis, float value) { + cm.a[axis].zero_backoff = value; +} + + + + + + + + +uint8_t get_min_switch(int axis) { + //return cm.a[axis].min_switch; + return 0; +} + + +void set_min_switch(int axis, uint8_t value) { + //cm.a[axis].min_switch = value; +} + + +uint8_t get_max_switch(int axis) { + //return cm.a[axis].max_switch; + return 0; +} + + +void set_max_switch(int axis, uint8_t value) { + //cm.a[axis].max_switch = value; +} diff --git a/src/canonical_machine.c b/src/canonical_machine.c index ef2a7bd..1ee6a59 100644 --- a/src/canonical_machine.c +++ b/src/canonical_machine.c @@ -1238,14 +1238,14 @@ stat_t cm_feedhold_sequencing_callback() { if (cm.queue_flush_requested) { if ((cm.motion_state == MOTION_STOP || - (cm.motion_state == MOTION_HOLD && cm.hold_state == FEEDHOLD_HOLD)) && + (cm.motion_state == MOTION_HOLD && cm.hold_state == FEEDHOLD_HOLD)) && !cm_get_runtime_busy()) { cm.queue_flush_requested = false; cm_queue_flush(); } } - bool feedhold_processing = // added feedhold processing lockout from omco fork + bool feedhold_processing = cm.hold_state == FEEDHOLD_SYNC || cm.hold_state == FEEDHOLD_PLAN || cm.hold_state == FEEDHOLD_DECEL; diff --git a/src/canonical_machine.h b/src/canonical_machine.h index 217f130..2120d7d 100644 --- a/src/canonical_machine.h +++ b/src/canonical_machine.h @@ -350,7 +350,7 @@ typedef enum { } cmMotionState_t; -typedef enum { // feedhold_state machine +typedef enum { // feedhold_state machine FEEDHOLD_OFF, // no feedhold in effect FEEDHOLD_SYNC, // start hold - sync to latest aline segment FEEDHOLD_PLAN, // replan blocks for feedhold @@ -360,14 +360,14 @@ typedef enum { // feedhold_state machine } cmFeedholdState_t; -typedef enum { // applies to cm.homing_state +typedef enum { // applies to cm.homing_state HOMING_NOT_HOMED, // machine is not homed (0=false) HOMING_HOMED, // machine is homed (1=true) HOMING_WAITING // machine waiting to be homed } cmHomingState_t; -typedef enum { // applies to cm.probe_state +typedef enum { // applies to cm.probe_state PROBE_FAILED, // probe reached endpoint without triggering PROBE_SUCCEEDED, // probe was triggered, cm.probe_results has position PROBE_WAITING // probe is waiting to be started @@ -725,5 +725,3 @@ stat_t cm_homing_callback(); // Probe cycles stat_t cm_straight_probe(float target[], float flags[]); stat_t cm_probe_callback(); - - diff --git a/src/command.c b/src/command.c index d92256f..0b357df 100644 --- a/src/command.c +++ b/src/command.c @@ -44,9 +44,6 @@ #include -static char input[INPUT_BUFFER_LEN]; - - // Command forward declarations // (Don't be afraid, X-Macros rock!) #define CMD(name, func, minArgs, maxArgs, help) \ @@ -76,7 +73,8 @@ static const command_t commands[] PROGMEM = { stat_t command_dispatch() { // Read input line or return if incomplete, usart_gets() is non-blocking - ritorno(usart_gets(input, sizeof(input))); + char *input = usart_readline(); + if (!input) return STAT_EAGAIN; return command_eval(input); } @@ -97,31 +95,43 @@ int command_find(const char *match) { int command_exec(int argc, char *argv[]) { stat_t status = STAT_INVALID_OR_MALFORMED_COMMAND; + putchar('\n'); + int i = command_find(argv[0]); if (i != -1) { - putchar('\n'); - uint8_t minArgs = pgm_read_byte(&commands[i].minArgs); uint8_t maxArgs = pgm_read_byte(&commands[i].maxArgs); - if (argc <= minArgs) printf_P(PSTR("Too few arguments\n")); - else if (maxArgs < argc - 1) printf_P(PSTR("Too many arguments\n")); - else { + if (argc <= minArgs) { + printf_P(PSTR("Too few arguments\n")); + return status; + + } else if (maxArgs < argc - 1) { + printf_P(PSTR("Too many arguments\n")); + return status; + + } else { command_cb_t cb = pgm_read_word(&commands[i].cb); return cb(argc, argv); } - } else if (argc == 1) { - char *p = strchr(argv[0], '='); - if (p) { - *p++ = 0; - if (vars_set(argv[0], p)) return STAT_OK; - } + } else if (argc != 1) { + printf_P(PSTR("Unknown command '%s' or invalid arguments\n"), argv[0]); + return status; + } - } else if (argc == 2 && vars_set(argv[0], argv[1])) + // Get or set variable + char *value = strchr(argv[0], '='); + if (value) { + *value++ = 0; + if (vars_set(argv[0], value)) return STAT_OK; + + } else if (vars_print(argv[0])) { + putchar('\n'); return STAT_OK; + } - printf_P(PSTR("Unknown command or unsetable variable '%s'\n"), argv[0]); + printf_P(PSTR("Unknown command or variable '%s'\n"), argv[0]); return status; } diff --git a/src/command.def b/src/command.def index d7dd746..d431f5f 100644 --- a/src/command.def +++ b/src/command.def @@ -28,4 +28,4 @@ CMD(help, command_help, 0, 1, "Print this help screen") CMD(reboot, command_reboot, 0, 0, "Reboot the controller") CMD(jog, command_jog, 1, 4, "Jog") -CMD(mreset, command_mreset, 1, 1, "Reset motor") +CMD(mreset, command_mreset, 0, 1, "Reset motor") diff --git a/src/command.h b/src/command.h index e23c7f3..4641f12 100644 --- a/src/command.h +++ b/src/command.h @@ -47,5 +47,3 @@ stat_t command_dispatch(); int command_find(const char *name); int command_exec(int argc, char *argv[]); int command_eval(char *cmd); - - diff --git a/src/config.h b/src/config.h index 248c2b5..66c4196 100644 --- a/src/config.h +++ b/src/config.h @@ -46,9 +46,10 @@ // Motor settings #define MOTOR_CURRENT 0.8 // 1.0 is full power +#define MOTOR_IDLE_CURRENT 0.1 // 1.0 is full power #define MOTOR_MICROSTEPS 16 -#define MOTOR_POWER_MODE MOTOR_POWERED_IN_CYCLE // See stepper.c -#define MOTOR_IDLE_TIMEOUT 2.00 // secs, motor off after this time +#define MOTOR_POWER_MODE MOTOR_POWERED_ONLY_WHEN_MOVING // See stepper.c +#define MOTOR_IDLE_TIMEOUT 2 // secs, motor off after this time #define M1_MOTOR_MAP AXIS_X #define M1_STEP_ANGLE 1.8 @@ -108,15 +109,15 @@ // Machine settings -#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing -#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on -#define JERK_MAX 40 // yes, that's km/min^3 -#define JUNCTION_DEVIATION 0.05 // default value, in mm -#define JUNCTION_ACCELERATION 100000 // centripetal corner acceleration -#define JOG_ACCELERATION 500000 // mm/min^2 +#define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arc drawing +#define SOFT_LIMIT_ENABLE 0 // 0 = off, 1 = on +#define JERK_MAX 50 // yes, that's km/min^3 +#define JUNCTION_DEVIATION 0.05 // default value, in mm +#define JUNCTION_ACCELERATION 100000 // centripetal corner acceleration +#define JOG_ACCELERATION 500000 // mm/min^2 // Axis settings -#define VELOCITY_MAX 16000 +#define VELOCITY_MAX 15000 // mm/min #define FEEDRATE_MAX VELOCITY_MAX // See canonical_machine.h cmAxisMode for valid values @@ -170,51 +171,51 @@ #define A_JERK_HOMING (A_JERK_MAX * 2) #define A_JUNCTION_DEVIATION JUNCTION_DEVIATION #define A_RADIUS (M1_TRAVEL_PER_REV / 2 / M_PI) -#define A_SEARCH_VELOCITY 600 -#define A_LATCH_VELOCITY 100 -#define A_LATCH_BACKOFF 5 -#define A_ZERO_BACKOFF 2 - -#define B_AXIS_MODE AXIS_DISABLED -#define B_VELOCITY_MAX 3600 -#define B_FEEDRATE_MAX B_VELOCITY_MAX -#define B_TRAVEL_MIN -1 -#define B_TRAVEL_MAX -1 -#define B_JERK_MAX JERK_MAX -#define B_JERK_HOMING (B_JERK_MAX * 2) -#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define B_RADIUS 1 -#define B_SEARCH_VELOCITY 600 -#define B_LATCH_VELOCITY 100 -#define B_LATCH_BACKOFF 5 -#define B_ZERO_BACKOFF 2 - -#define C_AXIS_MODE AXIS_DISABLED -#define C_VELOCITY_MAX 3600 -#define C_FEEDRATE_MAX C_VELOCITY_MAX -#define C_TRAVEL_MIN -1 -#define C_TRAVEL_MAX -1 -#define C_JERK_MAX JERK_MAX -#define C_JERK_HOMING (C_JERK_MAX * 2) -#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define C_RADIUS 1 -#define C_SEARCH_VELOCITY 600 -#define C_LATCH_VELOCITY 100 -#define C_LATCH_BACKOFF 5 -#define C_ZERO_BACKOFF 2 +#define A_SEARCH_VELOCITY 600 +#define A_LATCH_VELOCITY 100 +#define A_LATCH_BACKOFF 5 +#define A_ZERO_BACKOFF 2 + +#define B_AXIS_MODE AXIS_DISABLED +#define B_VELOCITY_MAX 3600 +#define B_FEEDRATE_MAX B_VELOCITY_MAX +#define B_TRAVEL_MIN -1 +#define B_TRAVEL_MAX -1 +#define B_JERK_MAX JERK_MAX +#define B_JERK_HOMING (B_JERK_MAX * 2) +#define B_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define B_RADIUS 1 +#define B_SEARCH_VELOCITY 600 +#define B_LATCH_VELOCITY 100 +#define B_LATCH_BACKOFF 5 +#define B_ZERO_BACKOFF 2 + +#define C_AXIS_MODE AXIS_DISABLED +#define C_VELOCITY_MAX 3600 +#define C_FEEDRATE_MAX C_VELOCITY_MAX +#define C_TRAVEL_MIN -1 +#define C_TRAVEL_MAX -1 +#define C_JERK_MAX JERK_MAX +#define C_JERK_HOMING (C_JERK_MAX * 2) +#define C_JUNCTION_DEVIATION JUNCTION_DEVIATION +#define C_RADIUS 1 +#define C_SEARCH_VELOCITY 600 +#define C_LATCH_VELOCITY 100 +#define C_LATCH_BACKOFF 5 +#define C_ZERO_BACKOFF 2 // PWM settings -#define P1_PWM_FREQUENCY 100 // in Hz -#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) -#define P1_CW_SPEED_HI 2000 -#define P1_CW_PHASE_LO 0.125 // phase [0..1] -#define P1_CW_PHASE_HI 0.2 -#define P1_CCW_SPEED_LO 1000 -#define P1_CCW_SPEED_HI 2000 -#define P1_CCW_PHASE_LO 0.125 -#define P1_CCW_PHASE_HI 0.2 -#define P1_PWM_PHASE_OFF 0.1 +#define P1_PWM_FREQUENCY 100 // in Hz +#define P1_CW_SPEED_LO 1000 // in RPM (arbitrary units) +#define P1_CW_SPEED_HI 2000 +#define P1_CW_PHASE_LO 0.125 // phase [0..1] +#define P1_CW_PHASE_HI 0.2 +#define P1_CCW_SPEED_LO 1000 +#define P1_CCW_SPEED_HI 2000 +#define P1_CCW_PHASE_LO 0.125 +#define P1_CCW_PHASE_HI 0.2 +#define P1_PWM_PHASE_OFF 0.1 // Gcode defaults @@ -267,24 +268,15 @@ #define MOTOR_PORT_DIR_gm 0x2f // pin dir settings -/// motor control port bit positions -#define STEP_BIT_bp 0 -#define DIRECTION_BIT_bp 1 -#define MOTOR_ENABLE_BIT_bp 2 -#define CHIP_SELECT_BIT_bp 3 -#define FAULT_BIT_bp 4 -#define GPIO1_OUT_BIT_bp 5 -#define SW_MIN_BIT_bp 6 //4 homing/limit switches -#define SW_MAX_BIT_bp 7 // homing/limit switches - -#define STEP_BIT_bm (1 << STEP_BIT_bp) -#define DIRECTION_BIT_bm (1 << DIRECTION_BIT_bp) -#define MOTOR_ENABLE_BIT_bm (1 << MOTOR_ENABLE_BIT_bp) -#define CHIP_SELECT_BIT_bm (1 << CHIP_SELECT_BIT_bp) -#define FAULT_BIT_bm (1 << FAULT_BIT_bp) -#define GPIO1_OUT_BIT_bm (1 << GPIO1_OUT_BIT_bp) // spindle and coolant -#define SW_MIN_BIT_bm (1 << SW_MIN_BIT_bp) // minimum switch inputs -#define SW_MAX_BIT_bm (1 << SW_MAX_BIT_bp) // maximum switch inputs +// Motor control port +#define STEP_BIT_bm (1 << 0) +#define DIRECTION_BIT_bm (1 << 1) +#define MOTOR_ENABLE_BIT_bm (1 << 2) +#define CHIP_SELECT_BIT_bm (1 << 3) +#define FAULT_BIT_bm (1 << 4) +#define GPIO1_OUT_BIT_bm (1 << 5) // spindle and coolant +#define SW_MIN_BIT_bm (1 << 6) // minimum switch inputs +#define SW_MAX_BIT_bm (1 << 7) // maximum switch inputs // Bit assignments for GPIO1_OUTs for spindle, PWM and coolant #define SPINDLE_BIT 8 // spindle on/off @@ -335,6 +327,41 @@ #define STEP_TIMER_ISR TCC0_OVF_vect #define STEP_TIMER_INTLVL TC_OVFINTLVL_HI_gc +/* Step correction settings + * + * Step correction settings determine how the encoder error is fed + * back to correct position errors. Since the following_error is + * running 2 segments behind the current segment you have to be + * careful not to overcompensate. The threshold determines if a + * correction should be applied, and the factor is how much. The + * holdoff is how many segments before applying another correction. If + * threshold is too small and/or amount too large and/or holdoff is + * too small you may get a runaway correction and error will grow + * instead of shrink (or oscillate). + */ +/// magnitude of forwarding error (in steps) +#define STEP_CORRECTION_THRESHOLD (float)2.00 +/// apply to step correction for a single segment +#define STEP_CORRECTION_FACTOR (float)0.25 +/// max step correction allowed in a single segment +#define STEP_CORRECTION_MAX (float)0.60 +/// minimum wait between error correction +#define STEP_CORRECTION_HOLDOFF 5 +#define STEP_INITIAL_DIRECTION DIRECTION_CW + + +// TMC2660 driver settings +#define TMC2660_SPI_PORT PORTC +#define TMC2660_SPI_SS_PIN 4 +#define TMC2660_SPI_SCK_PIN 5 +#define TMC2660_SPI_MISO_PIN 6 +#define TMC2660_SPI_MOSI_PIN 7 + +#define TMC2660_TIMER TCC1 + +#define TMC2660_POLL_RATE 0.01 // sec. Must be in (0, 1] +#define TMC2660_STABILIZE_TIME 0.01 // sec. Must be at least 1ms + // PWM settings #define PWM1_CTRLB (3 | TC1_CCBEN_bm) // single slope PWM channel B diff --git a/src/controller.c b/src/controller.c index f53b907..52d00d0 100644 --- a/src/controller.c +++ b/src/controller.c @@ -31,6 +31,8 @@ #include "canonical_machine.h" #include "stepper.h" +#include "motor.h" +#include "tmc2660.h" #include "gpio.h" #include "switch.h" #include "encoder.h" @@ -133,8 +135,10 @@ void controller_run() { DISPATCH(cm_feedhold_sequencing_callback()); // 5a. feedhold state machine DISPATCH(mp_plan_hold_callback()); // 5b. plan a feedhold + DISPATCH(tmc2660_sync()); // synchronize driver config + DISPATCH(motor_power_callback()); // stepper motor power sequencing + // Planner hierarchy for gcode and cycles - DISPATCH(st_motor_power_callback()); // stepper motor power sequencing DISPATCH(cm_arc_callback()); // arc generation runs DISPATCH(cm_homing_callback()); // G28.2 continuation DISPATCH(cm_probe_callback()); // G38.2 continuation diff --git a/src/main.c b/src/main.c index 05abdb6..7d59634 100644 --- a/src/main.c +++ b/src/main.c @@ -31,6 +31,7 @@ #include "controller.h" #include "canonical_machine.h" #include "stepper.h" +#include "motor.h" #include "encoder.h" #include "switch.h" #include "pwm.h" @@ -58,6 +59,7 @@ static void init() { // do these next tmc2660_init(); // motor drivers stepper_init(); // stepper subsystem + motor_init(); encoder_init(); // virtual encoders switch_init(); // switches pwm_init(); // pulse width modulation drivers diff --git a/src/motor.c b/src/motor.c new file mode 100644 index 0000000..7cc429e --- /dev/null +++ b/src/motor.c @@ -0,0 +1,516 @@ +/******************************************************************************\ + + 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 "motor.h" +#include "config.h" +#include "hardware.h" +#include "cpp_magic.h" +#include "rtc.h" +#include "report.h" +#include "stepper.h" +#include "encoder.h" +#include "tmc2660.h" + +#include "plan/planner.h" + +#include +#include + +#include +#include +#include +#include + + +#define TIMER_CC_BM(x) CAT3(TC1_, x, EN_bm) + + +typedef enum { + MOTOR_IDLE, // motor stopped and may be partially energized + MOTOR_ENERGIZING, + MOTOR_ACTIVE +} motorPowerState_t; + + +typedef enum { + MOTOR_DISABLED, // motor enable is deactivated + MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON + MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, + // de-powered out of cycle + MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after stopped, even in cycle + MOTOR_POWER_MODE_MAX_VALUE // for input range checking +} cmMotorPowerMode_t; + + +typedef enum { + MOTOR_POLARITY_NORMAL, + MOTOR_POLARITY_REVERSED +} cmMotorPolarity_t; + + +typedef struct { + // Config + uint8_t motor_map; // map motor to axis + uint16_t microsteps; // microsteps to apply for each axis (ex: 8) + cmMotorPolarity_t polarity; + cmMotorPowerMode_t power_mode; + float step_angle; // degrees per whole step (ex: 1.8) + float travel_rev; // mm or deg of travel per motor revolution + TC0_t *timer; + + // Runtime state + motorPowerState_t power_state; // state machine for managing motor power + uint32_t timeout; + cmMotorFlags_t flags; + + // Move prep + uint8_t timer_clock; // clock divisor setting or zero for off + uint16_t timer_period; // clock period counter + uint32_t steps; // expected steps + + // direction and direction change + cmDirection_t direction; // travel direction corrected for polarity + cmDirection_t prev_direction; // travel direction from previous segment run + int8_t step_sign; // set to +1 or -1 for encoders + + // step error correction + int32_t correction_holdoff; // count down segments between corrections + float corrected_steps; // accumulated for cycle (diagnostic) +} motor_t; + +static motor_t motors[MOTORS]; + + +/// Special interrupt for X-axis +ISR(TCE1_CCA_vect) { + PORT_MOTOR_1.OUTTGL = STEP_BIT_bm; +} + + +void motor_init() { + // Reset steppers to known state + memset(&motors, 0, sizeof(motors)); + + for (int motor = 0; motor < MOTORS; motor++) + motors[motor].prev_direction = STEP_INITIAL_DIRECTION; + + // Defaults + motors[0].motor_map = M1_MOTOR_MAP; + motors[0].step_angle = M1_STEP_ANGLE; + motors[0].travel_rev = M1_TRAVEL_PER_REV; + motors[0].microsteps = M1_MICROSTEPS; + motors[0].polarity = M1_POLARITY; + motors[0].power_mode = M1_POWER_MODE; + motors[0].timer = (TC0_t *)&M1_TIMER; + + motors[1].motor_map = M2_MOTOR_MAP; + motors[1].step_angle = M2_STEP_ANGLE; + motors[1].travel_rev = M2_TRAVEL_PER_REV; + motors[1].microsteps = M2_MICROSTEPS; + motors[1].polarity = M2_POLARITY; + motors[1].power_mode = M2_POWER_MODE; + motors[1].timer = &M2_TIMER; + + motors[2].motor_map = M3_MOTOR_MAP; + motors[2].step_angle = M3_STEP_ANGLE; + motors[2].travel_rev = M3_TRAVEL_PER_REV; + motors[2].microsteps = M3_MICROSTEPS; + motors[2].polarity = M3_POLARITY; + motors[2].power_mode = M3_POWER_MODE; + motors[2].timer = &M3_TIMER; + + motors[3].motor_map = M4_MOTOR_MAP; + motors[3].step_angle = M4_STEP_ANGLE; + motors[3].travel_rev = M4_TRAVEL_PER_REV; + motors[3].microsteps = M4_MICROSTEPS; + motors[3].polarity = M4_POLARITY; + motors[3].power_mode = M4_POWER_MODE; + motors[3].timer = &M4_TIMER; + + // Reset position + mp_set_steps_to_runtime_position(); + + // Setup motor timers + M1_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M1_TIMER_CC); + M2_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M2_TIMER_CC); + M3_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M3_TIMER_CC); + M4_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M4_TIMER_CC); + + // Setup special interrupt for X-axis mapping + M1_TIMER.INTCTRLB = TC_CCAINTLVL_HI_gc; +} + + +int motor_get_axis(int motor) { + return motors[motor].motor_map; +} + + +int motor_get_steps_per_unit(int motor) { + return (360 * motors[motor].microsteps) / + (motors[motor].travel_rev * motors[motor].step_angle); +} + + +/// returns true if motor is in an error state +static bool _error(int motor) { + return motors[motor].flags & MOTOR_FLAG_ERROR_bm; +} + + +/// Remove power from a motor +static void _deenergize(int motor) { + if (motors[motor].power_state == MOTOR_ACTIVE) { + motors[motor].power_state = MOTOR_IDLE; + tmc2660_disable(motor); + } +} + + +/// Apply power to a motor +static void _energize(int motor) { + if (motors[motor].power_state == MOTOR_IDLE && !_error(motor)) { + motors[motor].power_state = MOTOR_ENERGIZING; + tmc2660_enable(motor); + } + + // Reset timeout, regardless + motors[motor].timeout = rtc_get_time() + MOTOR_IDLE_TIMEOUT * 1000; +} + + +bool motor_energizing() { + for (int motor = 0; motor < MOTORS; motor++) + if (motors[motor].power_state == MOTOR_ENERGIZING) + return true; + + return false; +} + + +void motor_driver_callback(int motor) { + motor_t *m = &motors[motor]; + + if (m->power_state == MOTOR_IDLE) m->flags &= ~MOTOR_FLAG_ENABLED_bm; + else { + m->power_state = MOTOR_ACTIVE; + m->flags |= MOTOR_FLAG_ENABLED_bm; + } + + st_request_load_move(); + report_request(); +} + + +/// Callback to manage motor power sequencing and power-down timing. +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) || + (cm_get_combined_state() != COMBINED_HOLD && + motors[motor].timeout < rtc_get_time())) + _deenergize(motor); + + return STAT_OK; +} + + +void motor_error_callback(int motor, cmMotorFlags_t errors) { + if (motors[motor].power_state != MOTOR_ACTIVE) return; + + motors[motor].flags |= errors; + report_request(); + + if (_error(motor)) { + _deenergize(motor); + + // Stop and flush motion + cm_request_feedhold(); + cm_request_queue_flush(); + } +} + + +void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, + float error) { + motor_t *m = &motors[motor]; + + if (fp_ZERO(travel_steps)) { + m->timer_clock = 0; // Motor clock off + return; + } + + // Setup the direction, compensating for polarity. + // Set the step_sign which is used by the stepper ISR to accumulate step + // position + if (0 <= travel_steps) { // positive direction + m->direction = DIRECTION_CW ^ m->polarity; + m->step_sign = 1; + + } else { + m->direction = DIRECTION_CCW ^ m->polarity; + m->step_sign = -1; + } + +#ifdef __STEP_CORRECTION + float correction; + + // 'Nudge' correction strategy. Inject a single, scaled correction value + // then hold off + if (--m->correction_holdoff < 0 && + STEP_CORRECTION_THRESHOLD < fabs(error)) { + + m->correction_holdoff = STEP_CORRECTION_HOLDOFF; + correction = error * STEP_CORRECTION_FACTOR; + + if (0 < correction) + correction = min3(correction, fabs(travel_steps), STEP_CORRECTION_MAX); + else correction = + max3(correction, -fabs(travel_steps), -STEP_CORRECTION_MAX); + + m->corrected_steps += correction; + travel_steps -= correction; + } +#endif + + // Compute motor timer clock and period. Rounding is performed to eliminate + // a negative bias in the uint32_t conversion that results in long-term + // negative drift. + uint16_t steps = round(fabs(travel_steps)); + uint32_t ticks_per_step = seg_clocks / (steps + 0.5); + + // Find the right clock rate + if (ticks_per_step & 0xffff0000UL) { + ticks_per_step /= 2; + seg_clocks /= 2; + + if (ticks_per_step & 0xffff0000UL) { + ticks_per_step /= 2; + seg_clocks /= 2; + + if (ticks_per_step & 0xffff0000UL) { + ticks_per_step /= 2; + seg_clocks /= 2; + + if (ticks_per_step & 0xffff0000UL) m->timer_clock = 0; // Off + else m->timer_clock = TC_CLKSEL_DIV8_gc; + } else m->timer_clock = TC_CLKSEL_DIV4_gc; + } else m->timer_clock = TC_CLKSEL_DIV2_gc; + } else m->timer_clock = TC_CLKSEL_DIV1_gc; + + m->timer_period = ticks_per_step * 2; // TODO why do we need *2 here? + m->steps = seg_clocks / ticks_per_step; +} + + +void motor_begin_move(int motor) { + motor_t *m = &motors[motor]; + + // Energize motor + switch (m->power_mode) { + case MOTOR_DISABLED: return; + + case MOTOR_POWERED_ONLY_WHEN_MOVING: + if (!m->timer_clock) return; // Not moving + // Fall through + + case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: + _energize(motor); + break; + + case MOTOR_POWER_MODE_MAX_VALUE: break; // Shouldn't get here + } +} + + +void motor_load_move(int motor) { + motor_t *m = &motors[motor]; + + // Set or zero runtime clock and period + m->timer->CTRLFCLR = TC0_DIR_bm; // Count up + m->timer->CNT = 0; // Start at zero + m->timer->CCA = m->timer_period; // Set frequency + m->timer->CTRLA = m->timer_clock; // Start or stop + + // If motor has 0 steps the following is all skipped. This ensures that + // state comparisons always operate on the last segment actually run by + // this motor, regardless of how many segments it may have been inactive + // in between. + if (m->timer_clock) { + // Detect direction change and set the direction bit in hardware. + if (m->direction != m->prev_direction) { + m->prev_direction = m->direction; + + if (m->direction == DIRECTION_CW) + hw.st_port[motor]->OUTCLR = DIRECTION_BIT_bm; + else hw.st_port[motor]->OUTSET = DIRECTION_BIT_bm; + } + + // Accumulate encoder + en[motor].encoder_steps += m->steps * m->step_sign; + m->steps = 0; + } +} + + +void motor_end_move(int motor) { + // Disable motor clock + motors[motor].timer->CTRLA = 0; +} + + +// Var callbacks +float get_step_angle(int motor) { + return motors[motor].step_angle; +} + + +void set_step_angle(int motor, float value) { + motors[motor].step_angle = value; +} + + +float get_travel(int motor) { + return motors[motor].travel_rev; +} + + +void set_travel(int motor, float value) { + motors[motor].travel_rev = value; +} + + +uint16_t get_microstep(int motor) { + return motors[motor].microsteps; +} + + +void set_microstep(int motor, uint16_t value) { + switch (value) { + case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256: + break; + default: return; + } + + motors[motor].microsteps = value; +} + + +uint8_t get_polarity(int motor) { + if (motor < 0 || MOTORS <= motor) return 0; + return motors[motor].polarity; +} + + +void set_polarity(int motor, uint8_t value) { + motors[motor].polarity = value; +} + + +uint8_t get_motor_map(int motor) { + return motors[motor].motor_map; +} + + +void set_motor_map(int motor, uint16_t value) { + if (value < AXES) motors[motor].motor_map = value; +} + + +uint8_t get_power_mode(int motor) { + return motors[motor].power_mode; +} + + +void set_power_mode(int motor, uint16_t value) { + if (value < MOTOR_POWER_MODE_MAX_VALUE) + motors[motor].power_mode = value; +} + + + +uint8_t get_status_flags(int motor) { + return motors[motor].flags; +} + + +void print_status_flags(uint8_t flags) { + bool first = true; + + putchar('"'); + + if (MOTOR_FLAG_ENABLED_bm & flags) { + printf_P(PSTR("enable")); + first = false; + } + + if (MOTOR_FLAG_STALLED_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("stall")); + first = false; + } + + if (MOTOR_FLAG_OVERTEMP_WARN_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("temp warn")); + first = false; + } + + if (MOTOR_FLAG_OVERTEMP_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("over temp")); + first = false; + } + + if (MOTOR_FLAG_SHORTED_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("short")); + first = false; + } + + putchar('"'); +} + + +uint8_t get_status_strings(int motor) { + return get_status_flags(motor); +} + + +// Command callback +void command_mreset(int argc, char *argv[]) { + if (argc == 1) + for (int motor = 0; motor < MOTORS; motor++) + motors[motor].flags = 0; + + else { + int motor = atoi(argv[1]); + if (motor < MOTORS) motors[motor].flags = 0; + } + + report_request(); +} diff --git a/src/motor.h b/src/motor.h new file mode 100644 index 0000000..8b408a9 --- /dev/null +++ b/src/motor.h @@ -0,0 +1,64 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2016 Buildbotics LLC + All rights reserved. + + This file ("the software") is free software: you can redistribute it + and/or modify it under the terms of the GNU General Public License, + version 2 as published by the Free Software Foundation. You should + have received a copy of the GNU General Public License, version 2 + along with the software. If not, see . + + The software is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with the software. If not, see + . + + For information regarding this software email: + "Joseph Coffland" + +\******************************************************************************/ + +#pragma once + +#include "status.h" + +#include +#include + + +typedef enum { + MOTOR_FLAG_ENABLED_bm = 1 << 0, + MOTOR_FLAG_STALLED_bm = 1 << 1, + 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_OVERTEMP_WARN_bm | + MOTOR_FLAG_OVERTEMP_bm | + MOTOR_FLAG_SHORTED_bm) +} cmMotorFlags_t; + + +void motor_init(); + +int motor_get_axis(int motor); +int motor_get_steps_per_unit(int motor); + +bool motor_energizing(); + +void motor_driver_callback(int motor); +stat_t motor_power_callback(); +void motor_error_callback(int motor, cmMotorFlags_t errors); + +void motor_prep_move(int motor, uint32_t seg_clocks, float travel_steps, + float error); +void motor_begin_move(int motor); +void motor_load_move(int motor); +void motor_end_move(int motor); diff --git a/src/plan/exec.c b/src/plan/exec.c index df94390..014a0c4 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -44,17 +44,13 @@ static stat_t _exec_aline_body(); static stat_t _exec_aline_tail(); static stat_t _exec_aline_segment(); -#ifndef __JERK_EXEC -static void _init_forward_diffs(float Vi, float Vt); -#endif - /// Dequeues buffer and executes move continuations. Manages run buffers and /// other details. stat_t mp_exec_move() { mpBuf_t *bf; - if (!(bf = mp_get_run_buffer())) { // 0 means nothing's running + if (!(bf = mp_get_run_buffer())) { // null if nothing's running st_prep_null(); return STAT_NOOP; } diff --git a/src/plan/feedhold.c b/src/plan/feedhold.c index d95d8d6..1c98492 100644 --- a/src/plan/feedhold.c +++ b/src/plan/feedhold.c @@ -137,13 +137,13 @@ stat_t mp_plan_hold_callback() { braking_length = mp_get_target_length(braking_velocity, 0, bp); // Hack to prevent Case 2 moves for perfect-fit decels. Happens in - // homing situations The real fix: The braking velocity cannot + // homing situations. The real fix: The braking velocity cannot // simply be the mr.segment_velocity as this is the velocity of the // last segment, not the one that's going to be executed next. The // braking_velocity needs to be the velocity of the next segment // that has not yet been computed. In the mean time, this hack will // work. - if (braking_length > mr_available_length && fp_ZERO(bp->exit_velocity)) + if (mr_available_length < braking_length && fp_ZERO(bp->exit_velocity)) braking_length = mr_available_length; // Case 1: deceleration fits entirely into the length remaining in mr buffer @@ -170,7 +170,6 @@ stat_t mp_plan_hold_callback() { // Case 2: deceleration exceeds length remaining in mr buffer // First, replan mr to minimum (but non-zero) exit velocity - mr.section = SECTION_TAIL; mr.section_state = SECTION_NEW; mr.tail_length = mr_available_length; diff --git a/src/plan/jog.c b/src/plan/jog.c index 91d6b7c..6e5cf36 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -28,6 +28,7 @@ #include "jog.h" #include "planner.h" #include "stepper.h" +#include "motor.h" #include "canonical_machine.h" #include "kinematics.h" #include "encoder.h" @@ -84,9 +85,9 @@ static stat_t _exec_jog(mpBuf_t *bf) { if (done) { // Update machine position for (int motor = 0; motor < MOTORS; motor++) { - int axis = st_cfg.mot[motor].motor_map; + int axis = motor_get_axis(motor); float steps = en_read_encoder(axis); - cm_set_position(axis, steps / st_cfg.mot[motor].steps_per_unit); + cm_set_position(axis, steps / motor_get_steps_per_unit(motor)); } // Release queue diff --git a/src/plan/kinematics.c b/src/plan/kinematics.c index 040a7d4..55ae593 100644 --- a/src/plan/kinematics.c +++ b/src/plan/kinematics.c @@ -29,7 +29,7 @@ #include "kinematics.h" #include "canonical_machine.h" -#include "stepper.h" +#include "motor.h" #include @@ -56,8 +56,8 @@ void ik_kinematics(const float travel[], float steps[]) { // steps_per_unit() which takes axis travel, step angle and microsteps into // account. for (int motor = 0; motor < MOTORS; motor++) { - int axis = st_cfg.mot[motor].motor_map; + int axis = motor_get_axis(motor); if (cm.a[axis].axis_mode == AXIS_INHIBITED) steps[motor] = 0; - else steps[motor] = travel[axis] * st_cfg.mot[motor].steps_per_unit; + else steps[motor] = travel[axis] * motor_get_steps_per_unit(motor); } } diff --git a/src/plan/planner.c b/src/plan/planner.c index 8423833..cb90406 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -146,9 +146,8 @@ void mp_set_steps_to_runtime_position() { // write steps to encoder register en_set_encoder_steps(motor, step_position[motor]); - // These must be zero: + // must be zero mr.following_error[motor] = 0; - st_pre.mot[motor].corrected_steps = 0; } } diff --git a/src/plan/planner.h b/src/plan/planner.h index 69122b7..a7b3de4 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -35,7 +35,7 @@ #include "config.h" typedef enum { // bf->move_type values - MOVE_TYPE_0, // null move - does a no-op + MOVE_TYPE_NULL, // null move - does a no-op MOVE_TYPE_ALINE, // acceleration planned line MOVE_TYPE_DWELL, // delay with no movement MOVE_TYPE_COMMAND, // general command diff --git a/src/plan/zoid.c b/src/plan/zoid.c index 99ff142..4df3314 100644 --- a/src/plan/zoid.c +++ b/src/plan/zoid.c @@ -399,7 +399,7 @@ void mp_calculate_trapezoid(mpBuf_t *bf) { /// Derive accel/decel length from delta V and jerk float mp_get_target_length(const float Vi, const float Vf, const mpBuf_t *bf) { - return fabs(Vi - Vf * sqrt(fabs(Vi - Vf) * bf->recip_jerk)); + return fabs(Vi - Vf) * sqrt(fabs(Vi - Vf) * bf->recip_jerk); } diff --git a/src/report.c b/src/report.c index e4e7d7e..ab28db8 100644 --- a/src/report.c +++ b/src/report.c @@ -56,12 +56,14 @@ void report_request_full() { stat_t report_callback() { - uint32_t now = rtc_get_time(); - if (now - last_report < 100) return STAT_OK; - last_report = now; - - if (report_requested && usart_tx_empty()) vars_report(report_full); - report_requested = report_full = false; + if (report_requested && usart_tx_empty()) { + uint32_t now = rtc_get_time(); + if (now - last_report < 100) return STAT_OK; + last_report = now; + + vars_report(report_full); + report_requested = report_full = false; + } return STAT_OK; } diff --git a/src/rtc.c b/src/rtc.c index 03626f3..8fa9b9a 100644 --- a/src/rtc.c +++ b/src/rtc.c @@ -33,54 +33,35 @@ #include #include +#include + + +static uint32_t ticks; + + +ISR(RTC_OVF_vect) { + ticks++; + switch_rtc_callback(); // switch debouncing +} -rtClock_t rtc; // allocate clock control struct /// Initialize and start the clock /// This routine follows the code in app note AVR1314. void rtc_init() { - OSC.CTRL |= OSC_RC32KEN_bm; // enable internal 32kHz. - while (!(OSC.STATUS & OSC_RC32KRDY_bm)); // 32kHz osc stabilize - while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy + ticks = 0; - CLK.RTCCTRL = CLK_RTCSRC_RCOSC_gc | CLK_RTCEN_bm; // 32kHz clock as RTC src - while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy - - // the following must be in this order or it doesn't work - RTC.PER = RTC_MILLISECONDS - 1; // overflow period ~10ms - RTC.CNT = 0; - RTC.COMP = RTC_MILLISECONDS - 1; - RTC.CTRL = RTC_PRESCALER_DIV1_gc; // no prescale (1x) - RTC.INTCTRL = RTC_COMPINTLVL; // interrupt on compare - rtc.rtc_ticks = 0; // reset tick counter - rtc.sys_ticks = 0; // reset tick counter -} + OSC.CTRL |= OSC_RC32KEN_bm; // enable internal 32kHz. + while (!(OSC.STATUS & OSC_RC32KRDY_bm)); // 32kHz osc stabilize + while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy + CLK.RTCCTRL = CLK_RTCSRC_RCOSC32_gc | CLK_RTCEN_bm; // 32kHz clock as RTC src + while (RTC.STATUS & RTC_SYNCBUSY_bm); // wait RTC not busy -/* rtc ISR - * - * It is the responsibility of callback code to ensure atomicity and volatiles - * are observed correctly as the callback will be run at the interrupt level. - * - * Here's the code in case the main loop (non-interrupt) function needs to - * create a critical region for variables set or used by the callback: - * - * #include "gpio.h" - * #include "rtc.h" - * - * RTC.INTCTRL = RTC_OVFINTLVL_OFF_gc; // disable interrupt - * blah blah blah critical region - * RTC.INTCTRL = RTC_OVFINTLVL_LO_gc; // enable interrupt - */ -ISR(RTC_COMP_vect) { - rtc.sys_ticks = ++rtc.rtc_ticks * 10; // advance both tick counters - - // callbacks to whatever you need to happen on each RTC tick go here: - switch_rtc_callback(); // switch debouncing + // the following must be in this order or it doesn't work + RTC.PER = 33; // overflow period ~1ms + RTC.INTCTRL = RTC_OVFINTLVL_LO_gc; // overflow LO interrupt + RTC.CTRL = RTC_PRESCALER_DIV1_gc; // no prescale } -/// This is a hack to get around some compatibility problems -uint32_t rtc_get_time() { - return rtc.sys_ticks; -} +uint32_t rtc_get_time() {return ticks;} diff --git a/src/rtc.h b/src/rtc.h index 8e41f79..e47a6c6 100644 --- a/src/rtc.h +++ b/src/rtc.h @@ -33,17 +33,5 @@ #define RTC_MILLISECONDS 10 // interrupt on every 10 RTC ticks (~10 ms) -// Interrupt level -#define RTC_COMPINTLVL RTC_COMPINTLVL_LO_gc; - -// Note: sys_ticks is in ms but is only accurate to 10 ms as it's derived from -// rtc_ticks -typedef struct rtClock { - uint32_t rtc_ticks; // RTC tick counter, 10 uSec each - uint32_t sys_ticks; // system tick counter, 1 ms each -} rtClock_t; - -extern rtClock_t rtc; - void rtc_init(); // initialize and start general timer uint32_t rtc_get_time(); diff --git a/src/stepper.c b/src/stepper.c index faad639..16e7ca8 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -32,278 +32,57 @@ #include "stepper.h" +#include "motor.h" #include "config.h" -#include "encoder.h" #include "hardware.h" #include "util.h" -#include "rtc.h" -#include "report.h" #include "cpp_magic.h" -#include "usart.h" - -#include "plan/planner.h" #include -#include -#include -#include - - -#define TIMER_CC_BM(x) CAT3(TC1_, x, EN_bm) -enum {MOTOR_1, MOTOR_2, MOTOR_3, MOTOR_4}; +typedef struct { + // Runtime + bool busy; + moveType_t run_move; + uint16_t run_dwell; + // Move prep + bool move_ready; // preped move ready for loader + moveType_t prep_move; + uint32_t prep_dwell; + struct mpBuffer *bf; // used for command moves + uint16_t seg_period; +} stepper_t; -stConfig_t st_cfg; -stPrepSingleton_t st_pre; -static stRunSingleton_t st_run; - -static void _load_move(); -static void _request_load_move(); -static void _update_steps_per_unit(int motor); +static stepper_t st; void stepper_init() { /// clear all values, pointers and status - memset(&st_run, 0, sizeof(st_run)); - memset(&st_pre, 0, sizeof(st_pre)); - - // Setup ports - for (int motor = 0; motor < MOTORS; motor++) { - hw.st_port[motor]->OUTSET = MOTOR_ENABLE_BIT_bm; // disable motor - hw.st_port[motor]->DIR = MOTOR_PORT_DIR_gm; // pin directions - } + memset(&st, 0, sizeof(st)); // Setup step timer TIMER_STEP.CTRLA = STEP_TIMER_DISABLE; // turn timer off TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode - - st_pre.owner = PREP_BUFFER_OWNER_EXEC; - - // Reset steppers to known state - for (int motor = 0; motor < MOTORS; motor++) - st_pre.mot[motor].prev_direction = STEP_INITIAL_DIRECTION; - - // Motor configuration defaults - st_cfg.motor_power_timeout = MOTOR_IDLE_TIMEOUT; - - st_cfg.mot[MOTOR_1].motor_map = M1_MOTOR_MAP; - st_cfg.mot[MOTOR_1].step_angle = M1_STEP_ANGLE; - st_cfg.mot[MOTOR_1].travel_rev = M1_TRAVEL_PER_REV; - st_cfg.mot[MOTOR_1].microsteps = M1_MICROSTEPS; - st_cfg.mot[MOTOR_1].polarity = M1_POLARITY; - st_cfg.mot[MOTOR_1].power_mode = M1_POWER_MODE; - st_cfg.mot[MOTOR_1].timer = (TC0_t *)&M1_TIMER; - - st_cfg.mot[MOTOR_2].motor_map = M2_MOTOR_MAP; - st_cfg.mot[MOTOR_2].step_angle = M2_STEP_ANGLE; - st_cfg.mot[MOTOR_2].travel_rev = M2_TRAVEL_PER_REV; - st_cfg.mot[MOTOR_2].microsteps = M2_MICROSTEPS; - st_cfg.mot[MOTOR_2].polarity = M2_POLARITY; - st_cfg.mot[MOTOR_2].power_mode = M2_POWER_MODE; - st_cfg.mot[MOTOR_2].timer = &M2_TIMER; - - st_cfg.mot[MOTOR_3].motor_map = M3_MOTOR_MAP; - st_cfg.mot[MOTOR_3].step_angle = M3_STEP_ANGLE; - st_cfg.mot[MOTOR_3].travel_rev = M3_TRAVEL_PER_REV; - st_cfg.mot[MOTOR_3].microsteps = M3_MICROSTEPS; - st_cfg.mot[MOTOR_3].polarity = M3_POLARITY; - st_cfg.mot[MOTOR_3].power_mode = M3_POWER_MODE; - st_cfg.mot[MOTOR_3].timer = &M3_TIMER; - - st_cfg.mot[MOTOR_4].motor_map = M4_MOTOR_MAP; - st_cfg.mot[MOTOR_4].step_angle = M4_STEP_ANGLE; - st_cfg.mot[MOTOR_4].travel_rev = M4_TRAVEL_PER_REV; - st_cfg.mot[MOTOR_4].microsteps = M4_MICROSTEPS; - st_cfg.mot[MOTOR_4].polarity = M4_POLARITY; - st_cfg.mot[MOTOR_4].power_mode = M4_POWER_MODE; - st_cfg.mot[MOTOR_4].timer = &M4_TIMER; - - // Setup motor timers - M1_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M1_TIMER_CC); - M2_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M2_TIMER_CC); - M3_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M3_TIMER_CC); - M4_TIMER.CTRLB = TC_WGMODE_FRQ_gc | TIMER_CC_BM(M4_TIMER_CC); - - // Setup special interrupt for X-axis mapping - M1_TIMER.INTCTRLB = TC_CCAINTLVL_HI_gc; - - // Init steps per unit - for (int motor = 0; motor < MOTORS; motor++) - _update_steps_per_unit(motor); - - // Reset position, must be after update steps per unit - mp_set_steps_to_runtime_position(); } /// Return true if motors or dwell are running -uint8_t st_runtime_isbusy() {return st_run.busy;} - - -/// returns true if motor is enabled -static bool _motor_is_enabled(uint8_t motor) { - return st_run.mot[motor].flags & MOTOR_FLAG_ENABLED_bm; -} - -/// returns true if motor is in an error state -static bool _motor_error(uint8_t motor) { - return st_run.mot[motor].flags & MOTOR_FLAG_ERROR_bm; -} - - -/// Remove power from a motor -static void _deenergize_motor(uint8_t motor) { - if (_motor_is_enabled(motor)) { - hw.st_port[motor]->OUTSET = MOTOR_ENABLE_BIT_bm; - st_run.mot[motor].power_state = MOTOR_OFF; - st_run.mot[motor].flags &= MOTOR_FLAG_ENABLED_bm; - report_request(); // request a status report when motors change state - } -} - - -/// Apply power to a motor -static void _energize_motor(uint8_t motor) { - if (!_motor_is_enabled(motor) && !_motor_error(motor)) { - hw.st_port[motor]->OUTCLR = MOTOR_ENABLE_BIT_bm; - st_run.mot[motor].power_state = MOTOR_POWER_TIMEOUT_START; - st_run.mot[motor].flags |= MOTOR_FLAG_ENABLED_bm; - report_request(); // request a status report when motors change state - } -} - - -/// Callback to manage motor power sequencing -/// Handles motor power-down timing, low-power idle, and adaptive motor power -stat_t st_motor_power_callback() { // called by controller - // Manage power for each motor individually - for (int motor = 0; motor < MOTORS; motor++) - switch (st_cfg.mot[motor].power_mode) { - case MOTOR_ALWAYS_POWERED: _energize_motor(motor); break; - - case MOTOR_POWERED_IN_CYCLE: case MOTOR_POWERED_ONLY_WHEN_MOVING: - if (st_run.mot[motor].power_state == MOTOR_POWER_TIMEOUT_START) { - // Start a countdown - st_run.mot[motor].power_state = MOTOR_POWER_TIMEOUT_COUNTDOWN; - st_run.mot[motor].power_systick = rtc_get_time() + - (st_cfg.motor_power_timeout * 1000); - } - - // Run the countdown if not in feedhold and in a countdown - if (cm_get_combined_state() != COMBINED_HOLD && - st_run.mot[motor].power_state == MOTOR_POWER_TIMEOUT_COUNTDOWN && - rtc_get_time() > st_run.mot[motor].power_systick) - _deenergize_motor(motor); - break; - - default: _deenergize_motor(motor); break; // Motor disabled - } - - return STAT_OK; -} - - -void st_motor_error_callback(uint8_t motor, cmMotorFlags_t errors) { - st_run.mot[motor].flags |= MOTOR_FLAG_ERROR_bm & errors; - if (_motor_error(motor)) _deenergize_motor(motor); -} - - -/// Special interrupt for X-axis -ISR(TCE1_CCA_vect) { - PORT_MOTOR_1.OUTTGL = STEP_BIT_bm; -} - - -/// Step timer interrupt routine -ISR(STEP_TIMER_ISR) { - if (st_run.move_type == MOVE_TYPE_DWELL && --st_run.dwell) return; - st_run.busy = false; - _load_move(); -} +uint8_t st_runtime_isbusy() {return st.busy;} /// "Software" interrupt to request move execution void st_request_exec_move() { - if (st_pre.owner == PREP_BUFFER_OWNER_EXEC) { + if (!st.move_ready) { ADCB_CH0_INTCTRL = ADC_CH_INTLVL_LO_gc; // trigger LO interrupt ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH0START_bm; } } -/// Interrupt handler for calling move exec function. -/// Uses ADC channel 0 as software interrupt. -ISR(ADCB_CH0_vect) { - if (st_pre.owner == PREP_BUFFER_OWNER_EXEC && - mp_exec_move() != STAT_NOOP) { - st_pre.owner = PREP_BUFFER_OWNER_LOADER; // flip it back - _request_load_move(); - } -} - - -/// Fires a "software" interrupt to request to load a move -static void _request_load_move() { - if (st_runtime_isbusy()) return; // don't request a load if runtime is busy - - if (st_pre.owner == PREP_BUFFER_OWNER_LOADER) { - ADCB_CH1_INTCTRL = ADC_CH_INTLVL_HI_gc; // trigger HI interrupt - ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH1START_bm; - } -} - - -/// Interrupt handler for running the loader -ISR(ADCB_CH1_vect) { - // Use ADC channel 1 as software interrupt. - // _load_move() can only be called be called from an ISR at the same or - // higher level as the step timer ISR. A software interrupt is used to allow a - // non-ISR to request a load. See st_request_load_move() - _load_move(); -} - - -static inline void _load_motor_move(int motor) { - stPrepMotor_t *pre_mot = &st_pre.mot[motor]; - const cfgMotor_t *cfg_mot = &st_cfg.mot[motor]; - - // Set or zero runtime clock and period - cfg_mot->timer->CTRLFCLR = TC0_DIR_bm; // Count up - cfg_mot->timer->CNT = 0; // Start at zero - cfg_mot->timer->CCA = pre_mot->timer_period; // Set frequency - cfg_mot->timer->CTRLA = pre_mot->timer_clock; // Start or stop - - // If motor has 0 steps the following is all skipped. This ensures that - // state comparisons always operate on the last segment actually run by - // this motor, regardless of how many segments it may have been inactive - // in between. - if (pre_mot->timer_clock) { - // Detect direction change and set the direction bit in hardware. - if (pre_mot->direction != pre_mot->prev_direction) { - pre_mot->prev_direction = pre_mot->direction; - - if (pre_mot->direction == DIRECTION_CW) - hw.st_port[motor]->OUTCLR = DIRECTION_BIT_bm; - else hw.st_port[motor]->OUTSET = DIRECTION_BIT_bm; - } - - // Accumulate encoder - en[motor].encoder_steps += pre_mot->steps * pre_mot->step_sign; - pre_mot->steps = 0; - } - - // Energize motor and start power management - if ((pre_mot->timer_clock && cfg_mot->power_mode != MOTOR_DISABLED) || - cfg_mot->power_mode == MOTOR_POWERED_IN_CYCLE) - _energize_motor(motor); -} - - /* Dequeue move and load into stepper struct * * This routine can only be called from an ISR at the same or @@ -319,32 +98,34 @@ static inline void _load_motor_move(int motor) { static void _load_move() { if (st_runtime_isbusy()) return; - if (st_pre.owner != PREP_BUFFER_OWNER_LOADER) { - // There are no more moves, disable motor clocks - for (int motor = 0; motor < MOTORS; motor++) - st_cfg.mot[motor].timer->CTRLA = 0; - return; - } + // If there are no more moves + if (!st.move_ready) return; - st_run.move_type = st_pre.move_type; + for (int motor = 0; motor < MOTORS; motor++) + motor_begin_move(motor); - switch (st_pre.move_type) { + // Wait until motors have energized + if (motor_energizing()) return; + + st.run_move = st.prep_move; + + switch (st.run_move) { case MOVE_TYPE_DWELL: - st_run.dwell = st_pre.dwell; + st.run_dwell = st.prep_dwell; // Fall through case MOVE_TYPE_ALINE: for (int motor = 0; motor < MOTORS; motor++) - if (st_pre.move_type == MOVE_TYPE_ALINE) _load_motor_move(motor); - else st_pre.mot[motor].timer_clock = 0; // Off + if (st.run_move == MOVE_TYPE_ALINE) + motor_load_move(motor); - st_run.busy = true; - TIMER_STEP.PER = st_pre.seg_period; + st.busy = true; + TIMER_STEP.PER = st.seg_period; TIMER_STEP.CTRLA = STEP_TIMER_ENABLE; // enable step timer, if not enabled break; case MOVE_TYPE_COMMAND: // handle synchronous commands - mp_runtime_command(st_pre.bf); + mp_runtime_command(st.bf); // Fall through default: @@ -353,83 +134,49 @@ static void _load_move() { } // we are done with the prep buffer - flip the flag back - st_pre.move_type = MOVE_TYPE_0; - st_pre.owner = PREP_BUFFER_OWNER_EXEC; + st.prep_move = MOVE_TYPE_NULL; + st.move_ready = false; st_request_exec_move(); // exec and prep next move } -static void _prep_motor_line(int motor, float travel_steps, float error) { - stPrepMotor_t *pre_mot = &st_pre.mot[motor]; - cfgMotor_t *cfg_mot = &st_cfg.mot[motor]; +/// Step timer interrupt routine +ISR(STEP_TIMER_ISR) { + if (st.run_move == MOVE_TYPE_DWELL && --st.run_dwell) return; - // Disable this motor's clock if there are no new steps - if (fp_ZERO(travel_steps)) { - pre_mot->timer_clock = 0; // Off - return; - } + for (int motor = 0; motor < MOTORS; motor++) + motor_end_move(motor); - // Setup the direction, compensating for polarity. - // Set the step_sign which is used by the stepper ISR to accumulate step - // position - if (0 <= travel_steps) { // positive direction - pre_mot->direction = DIRECTION_CW ^ cfg_mot->polarity; - pre_mot->step_sign = 1; + st.busy = false; + _load_move(); +} - } else { - pre_mot->direction = DIRECTION_CCW ^ cfg_mot->polarity; - pre_mot->step_sign = -1; - } -#ifdef __STEP_CORRECTION - float correction; +/// Interrupt handler for running the loader. +/// ADC channel 1 triggered by st_request_load_move() as a "software" interrupt. +ISR(ADCB_CH1_vect) { + _load_move(); +} + - // 'Nudge' correction strategy. Inject a single, scaled correction value - // then hold off - if (--pre_mot->correction_holdoff < 0 && - STEP_CORRECTION_THRESHOLD < fabs(error)) { +/// Fires a "software" interrupt to request a move load. +void st_request_load_move() { + if (st_runtime_isbusy()) return; // don't request a load if runtime is busy - pre_mot->correction_holdoff = STEP_CORRECTION_HOLDOFF; - correction = error * STEP_CORRECTION_FACTOR; + if (st.move_ready) { + ADCB_CH1_INTCTRL = ADC_CH_INTLVL_HI_gc; // trigger HI interrupt + ADCB_CTRLA = ADC_ENABLE_bm | ADC_CH1START_bm; + } +} - if (0 < correction) - correction = min3(correction, fabs(travel_steps), STEP_CORRECTION_MAX); - else correction = - max3(correction, -fabs(travel_steps), -STEP_CORRECTION_MAX); - pre_mot->corrected_steps += correction; - travel_steps -= correction; +/// Interrupt handler for calling move exec function. +/// ADC channel 0 triggered by st_request_exec_move() as a "software" interrupt. +ISR(ADCB_CH0_vect) { + if (!st.move_ready && mp_exec_move() != STAT_NOOP) { + st.move_ready = true; // flip it back - TODO is this really needed? + st_request_load_move(); } -#endif - - // Compute motor timer clock and period. Rounding is performed to eliminate - // a negative bias in the uint32_t conversion that results in long-term - // negative drift. - uint16_t steps = round(fabs(travel_steps)); - uint32_t seg_clocks = (uint32_t)st_pre.seg_period * STEP_TIMER_DIV; - uint32_t ticks_per_step = seg_clocks / (steps + 0.5); - - // Find the right clock rate - if (ticks_per_step & 0xffff0000UL) { - ticks_per_step /= 2; - seg_clocks /= 2; - - if (ticks_per_step & 0xffff0000UL) { - ticks_per_step /= 2; - seg_clocks /= 2; - - if (ticks_per_step & 0xffff0000UL) { - ticks_per_step /= 2; - seg_clocks /= 2; - - if (ticks_per_step & 0xffff0000UL) pre_mot->timer_clock = 0; // Off - else pre_mot->timer_clock = TC_CLKSEL_DIV8_gc; - } else pre_mot->timer_clock = TC_CLKSEL_DIV4_gc; - } else pre_mot->timer_clock = TC_CLKSEL_DIV2_gc; - } else pre_mot->timer_clock = TC_CLKSEL_DIV1_gc; - - pre_mot->timer_period = ticks_per_step * 2; // TODO why do we need *2 here? - pre_mot->steps = seg_clocks / ticks_per_step; } @@ -450,30 +197,28 @@ static void _prep_motor_line(int motor, float travel_steps, float error) { * - error[] is a vector of measured errors to the step count. Used for * correction. * - * - segment_time - how many minutes the segment should run. If timing is not + * - seg_time - how many minutes the segment should run. If timing is not * 100% accurate this will affect the move velocity, but not the distance * traveled. */ -stat_t st_prep_line(float travel_steps[], float error[], float segment_time) { +stat_t st_prep_line(float travel_steps[], float error[], float seg_time) { // Trap conditions that would prevent queueing the line - if (st_pre.owner != PREP_BUFFER_OWNER_EXEC) - return cm_hard_alarm(STAT_INTERNAL_ERROR); - if (isinf(segment_time)) + if (st.move_ready) return cm_hard_alarm(STAT_INTERNAL_ERROR); + if (isinf(seg_time)) return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_INFINITE); - if (isnan(segment_time)) - return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_NAN); - if (segment_time < EPSILON) return STAT_MINIMUM_TIME_MOVE; + if (isnan(seg_time)) return cm_hard_alarm(STAT_PREP_LINE_MOVE_TIME_IS_NAN); + if (seg_time < EPSILON) return STAT_MINIMUM_TIME_MOVE; - // Setup segment parameters (need st_pre.seg_period for motor calcs) - st_pre.seg_period = segment_time * 60 * F_CPU / STEP_TIMER_DIV; - st_pre.move_type = MOVE_TYPE_ALINE; + // Setup segment parameters + st.prep_move = MOVE_TYPE_ALINE; + st.seg_period = seg_time * 60 * F_CPU / STEP_TIMER_DIV; // Must fit 16-bit + uint32_t seg_clocks = (uint32_t)st.seg_period * STEP_TIMER_DIV; - // Setup motor parameters + // Prepare motor moves for (uint8_t motor = 0; motor < MOTORS; motor++) - _prep_motor_line(motor, travel_steps[motor], error[motor]); + motor_prep_move(motor, seg_clocks, travel_steps[motor], error[motor]); - // Signal prep buffer ready (do this last) - st_pre.owner = PREP_BUFFER_OWNER_LOADER; + st.move_ready = true; // signal prep buffer ready(do this last) return STAT_OK; } @@ -481,115 +226,26 @@ stat_t st_prep_line(float travel_steps[], float error[], float segment_time) { /// Keeps the loader happy. Otherwise performs no action void st_prep_null() { - if (st_pre.owner != PREP_BUFFER_OWNER_EXEC) - cm_hard_alarm(STAT_INTERNAL_ERROR); - - st_pre.move_type = MOVE_TYPE_0; - st_pre.owner = PREP_BUFFER_OWNER_EXEC; // signal prep buffer empty + if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); + st.prep_move = MOVE_TYPE_NULL; + st.move_ready = false; // signal prep buffer empty } /// Stage command to execution void st_prep_command(void *bf) { - if (st_pre.owner != PREP_BUFFER_OWNER_EXEC) - cm_hard_alarm(STAT_INTERNAL_ERROR); - - st_pre.move_type = MOVE_TYPE_COMMAND; - st_pre.bf = (mpBuf_t *)bf; - st_pre.owner = PREP_BUFFER_OWNER_LOADER; // signal prep buffer ready + if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); + st.prep_move = MOVE_TYPE_COMMAND; + st.bf = (mpBuf_t *)bf; + st.move_ready = true; // signal prep buffer ready } /// Add a dwell to the move buffer void st_prep_dwell(float seconds) { - if (st_pre.owner != PREP_BUFFER_OWNER_EXEC) - cm_hard_alarm(STAT_INTERNAL_ERROR); - - st_pre.move_type = MOVE_TYPE_DWELL; - st_pre.seg_period = F_CPU / STEP_TIMER_DIV / 1000; // 1 ms - st_pre.dwell = seconds * 1000; // convert to ms - st_pre.owner = PREP_BUFFER_OWNER_LOADER; // signal prep buffer ready -} - - -static void _update_steps_per_unit(int motor) { - st_cfg.mot[motor].steps_per_unit = - (360 * st_cfg.mot[motor].microsteps) / - (st_cfg.mot[motor].travel_rev * st_cfg.mot[motor].step_angle); -} - - -float get_step_angle(int index) { - return st_cfg.mot[index].step_angle; -} - - -void set_step_angle(int index, float value) { - st_cfg.mot[index].step_angle = value; - _update_steps_per_unit(index); -} - - -float get_travel(int index) { - return st_cfg.mot[index].travel_rev; -} - - -void set_travel(int index, float value) { - st_cfg.mot[index].travel_rev = value; - _update_steps_per_unit(index); -} - - -uint16_t get_microstep(int index) { - return st_cfg.mot[index].microsteps; -} - - -void set_microstep(int index, uint16_t value) { - switch (value) { - case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256: - break; - default: return; - } - - st_cfg.mot[index].microsteps = value; - _update_steps_per_unit(index); -} - - -uint8_t get_polarity(int index) { - if (index < 0 || MOTORS <= index) return 0; - return st_cfg.mot[index].polarity; -} - - -void set_polarity(int index, uint8_t value) { - st_cfg.mot[index].polarity = value; -} - - -uint8_t get_motor_map(int index) { - return st_cfg.mot[index].motor_map; -} - - -void set_motor_map(int index, uint16_t value) { - if (value < AXES) st_cfg.mot[index].motor_map = value; -} - - -uint8_t get_power_mode(int index) { - return st_cfg.mot[index].power_mode; -} - - -void set_power_mode(int index, uint16_t value) { - if (value < MOTOR_POWER_MODE_MAX_VALUE) - st_cfg.mot[index].power_mode = value; -} - - -void command_mreset(int motor) { - if (motor < MOTORS) st_run.mot[motor].flags &= 0; + if (st.move_ready) cm_hard_alarm(STAT_INTERNAL_ERROR); + st.prep_move = MOVE_TYPE_DWELL; + st.seg_period = F_CPU / STEP_TIMER_DIV / 1000; // 1 ms + st.prep_dwell = seconds * 1000; // convert to ms + st.move_ready = true; // signal prep buffer ready } diff --git a/src/stepper.h b/src/stepper.h index 69229c5..4d1b3b8 100644 --- a/src/stepper.h +++ b/src/stepper.h @@ -221,86 +221,6 @@ #include -typedef enum { - PREP_BUFFER_OWNER_LOADER, // staging buffer is ready for load - PREP_BUFFER_OWNER_EXEC // staging buffer is being loaded -} prepBufferOwner_t; - - -// Currently there is no distinction between IDLE and OFF (DEENERGIZED) -// In the future IDLE will be powered at a low, torque-maintaining current -// Used w/start and stop flags to sequence motor power -typedef enum { - MOTOR_OFF, // motor stopped and deenergized - MOTOR_IDLE, // motor stopped and may be partially energized - MOTOR_RUNNING, // motor is running (and fully energized) - MOTOR_POWER_TIMEOUT_START, // transition state to start power-down timeout - MOTOR_POWER_TIMEOUT_COUNTDOWN // count down the time to de-energizing motors -} motorPowerState_t; - - -typedef enum { - MOTOR_DISABLED, // motor enable is deactivated - MOTOR_ALWAYS_POWERED, // motor is always powered while machine is ON - MOTOR_POWERED_IN_CYCLE, // motor fully powered during cycles, - // de-powered out of cycle - MOTOR_POWERED_ONLY_WHEN_MOVING, // idles shortly after stopped, even in cycle - MOTOR_POWER_MODE_MAX_VALUE // for input range checking -} cmMotorPowerMode_t; - - -typedef enum { - MOTOR_POLARITY_NORMAL, - MOTOR_POLARITY_REVERSED -} cmMotorPolarity_t; - - -typedef enum { - MOTOR_FLAG_ENABLED_bm = 1 << 0, - MOTOR_FLAG_STALLED_bm = 1 << 1, - 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_OVERTEMP_WARN_bm | - MOTOR_FLAG_OVERTEMP_bm | - MOTOR_FLAG_SHORTED_bm) -} cmMotorFlags_t; - - -/// Min/Max timeouts allowed for motor disable. Allow for inertial stop. -/// Must be non-zero -#define MOTOR_TIMEOUT_SECONDS_MIN (float)0.1 -/// For conversion to uint32_t (4294967295 / 1000) -#define MOTOR_TIMEOUT_SECONDS_MAX (float)4294967 -/// seconds in DISABLE_AXIS_WHEN_IDLE mode -#define MOTOR_TIMEOUT_SECONDS (float)0.1 -/// timeout for a motor in _ONLY_WHEN_MOVING mode -#define MOTOR_TIMEOUT_WHEN_MOVING (float)0.25 - - -/* Step correction settings - * - * Step correction settings determine how the encoder error is fed - * back to correct position errors. Since the following_error is - * running 2 segments behind the current segment you have to be - * careful not to overcompensate. The threshold determines if a - * correction should be applied, and the factor is how much. The - * holdoff is how many segments before applying another correction. If - * threshold is too small and/or amount too large and/or holdoff is - * too small you may get a runaway correction and error will grow - * instead of shrink (or oscillate). - */ -/// magnitude of forwarding error (in steps) -#define STEP_CORRECTION_THRESHOLD (float)2.00 -/// apply to step correction for a single segment -#define STEP_CORRECTION_FACTOR (float)0.25 -/// max step correction allowed in a single segment -#define STEP_CORRECTION_MAX (float)0.60 -/// minimum wait between error correction -#define STEP_CORRECTION_HOLDOFF 5 -#define STEP_INITIAL_DIRECTION DIRECTION_CW - /* Stepper control structures * @@ -319,84 +239,12 @@ typedef enum { * better. */ -// Per motor config structure -typedef struct { - uint8_t motor_map; // map motor to axis - uint16_t microsteps; // microsteps to apply for each axis (ex: 8) - cmMotorPolarity_t polarity; - cmMotorPowerMode_t power_mode; - float step_angle; // degrees per whole step (ex: 1.8) - float travel_rev; // mm or deg of travel per motor revolution - float steps_per_unit; // microsteps per mm (or degree) of travel - TC0_t *timer; -} cfgMotor_t; - - -/// stepper configs -typedef struct { - float motor_power_timeout; // seconds before idle current - cfgMotor_t mot[MOTORS]; // settings for motors 1-N -} stConfig_t; - - -/// Motor runtime structure. Used by step generation ISR (HI) -typedef struct { // one per controlled motor - motorPowerState_t power_state; // state machine for managing motor power - uint32_t power_systick; // for next motor power state transition - cmMotorFlags_t flags; -} stRunMotor_t; - - -/// Stepper static values and axis parameters -typedef struct { - moveType_t move_type; - bool busy; - uint16_t dwell; - stRunMotor_t mot[MOTORS]; // runtime motor structures -} stRunSingleton_t; - - -/// Motor prep structure. Used by exec/prep ISR (LO) and read-only during load -/// Must be careful about volatiles in this one -typedef struct { - uint8_t timer_clock; // clock divisor setting or zero for off - uint16_t timer_period; // clock period counter - uint32_t steps; // expected steps - - // direction and direction change - cmDirection_t direction; // travel direction corrected for polarity - cmDirection_t prev_direction; // travel direction from previous segment run - int8_t step_sign; // set to +1 or -1 for encoders - - // step error correction - int32_t correction_holdoff; // count down segments between corrections - float corrected_steps; // accumulated for cycle (diagnostic) -} stPrepMotor_t; - - -typedef struct { - volatile prepBufferOwner_t owner; // owned by exec or loader - moveType_t move_type; - struct mpBuffer *bf; // used for command moves - uint16_t seg_period; - uint32_t dwell; - stPrepMotor_t mot[MOTORS]; // prep time motor structs -} stPrepSingleton_t; - - -extern stConfig_t st_cfg; // used by kienmatics.c -extern stPrepSingleton_t st_pre; // used by planner.c - - void stepper_init(); uint8_t st_runtime_isbusy(); - -stat_t st_motor_power_callback(); -void st_motor_error_callback(uint8_t motor, cmMotorFlags_t errors); - void st_request_exec_move(); +void st_request_load_move(); +stat_t st_prep_line(float travel_steps[], float following_error[], + float segment_time); void st_prep_null(); void st_prep_command(void *bf); // void * since mpBuf_t is not visible here void st_prep_dwell(float seconds); -stat_t st_prep_line(float travel_steps[], float following_error[], - float segment_time); diff --git a/src/switch.c b/src/switch.c index ef1fc97..d129dd0 100644 --- a/src/switch.c +++ b/src/switch.c @@ -81,7 +81,7 @@ typedef struct { /* Switch control structures * Note 1: The term "thrown" is used because switches could be normally-open - * or normally-closed. "Thrown" means activated or hit. + * or normally-closed. "Thrown" means activated. */ typedef struct { bool limit_thrown; @@ -174,7 +174,7 @@ void switch_init() { } -/// Called from RTC for each RTC tick +/// Called from RTC on each tick void switch_rtc_callback() { for (int i = 0; i < SWITCHES; i++) { switch_t *s = &sw.switches[i]; @@ -211,8 +211,14 @@ void switch_rtc_callback() { } -bool switch_get_closed(uint8_t n) {return sw.switches[n].state;} -swType_t switch_get_type(uint8_t n) {return sw.switches[n].type;} +bool switch_get_closed(uint8_t n) { + return sw.switches[n].state; +} + + +swType_t switch_get_type(uint8_t n) { + return sw.switches[n].type; +} void switch_set_type(uint8_t n, swType_t type) { @@ -220,7 +226,9 @@ void switch_set_type(uint8_t n, swType_t type) { } -swMode_t switch_get_mode(uint8_t n) {return sw.switches[n].mode;} +swMode_t switch_get_mode(uint8_t n) { + return sw.switches[n].mode; +} void switch_set_mode(uint8_t n, swMode_t mode) { @@ -228,7 +236,9 @@ void switch_set_mode(uint8_t n, swMode_t mode) { } -bool switch_get_limit_thrown() {return sw.limit_thrown;} +bool switch_get_limit_thrown() { + return sw.limit_thrown; +} uint8_t get_switch_type(int index) { diff --git a/src/tmc2660.c b/src/tmc2660.c index b60326e..90d9baf 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -27,8 +27,9 @@ #include "tmc2660.h" #include "status.h" -#include "stepper.h" +#include "motor.h" #include "hardware.h" +#include "rtc.h" #include "cpp_magic.h" #include @@ -46,7 +47,7 @@ void set_power_level(int driver, float value); typedef enum { TMC2660_STATE_CONFIG, TMC2660_STATE_MONITOR, - TMC2660_STATE_RESET, + TMC2660_STATE_RECONFIGURE, } tmc2660_state_t; typedef enum { @@ -58,8 +59,10 @@ typedef enum { typedef struct { tmc2660_state_t state; - uint8_t reset; + bool reconfigure; uint8_t reg; + uint32_t stabilizing; + bool callback; uint16_t sguard; uint8_t flags; @@ -90,17 +93,24 @@ static spi_t spi; static void _report_error_flags(int driver) { + if (drivers[driver].stabilizing < rtc_get_time()) return; + uint8_t dflags = drivers[driver].flags; uint8_t mflags = 0; if ((TMC2660_DRVSTATUS_SHORT_TO_GND_A | TMC2660_DRVSTATUS_SHORT_TO_GND_B) & dflags) mflags |= MOTOR_FLAG_SHORTED_bm; + if (TMC2660_DRVSTATUS_OVERTEMP_WARN & dflags) mflags |= MOTOR_FLAG_OVERTEMP_WARN_bm; + if (TMC2660_DRVSTATUS_OVERTEMP & dflags) mflags |= MOTOR_FLAG_OVERTEMP_bm; - st_motor_error_callback(driver, mflags); + if (hw.st_port[driver]->IN & FAULT_BIT_bm) + mflags |= MOTOR_FLAG_STALLED_bm; + + if (mflags) motor_error_callback(driver, mflags); } @@ -128,7 +138,7 @@ static void spi_send() { void spi_next() { tmc2660_driver_t *drv = &drivers[spi.driver]; - uint16_t spi_delay = 1; + uint16_t spi_delay = 4; switch (spi.state) { case SPI_STATE_SELECT: @@ -136,7 +146,7 @@ void spi_next() { spi_cs(spi.driver, 1); // Next state - spi_delay = 2; + spi_delay = 4; spi.state = SPI_STATE_WRITE; break; @@ -147,19 +157,18 @@ void spi_next() { break; case TMC2660_STATE_MONITOR: - spi.out = reg_addrs[TMC2660_DRVCTRL] | drv->regs[TMC2660_DRVCTRL]; + spi.out = TMC2660_DRVCTRL_ADDR | drv->regs[TMC2660_DRVCTRL]; break; - case TMC2660_STATE_RESET: - spi.out = - reg_addrs[TMC2660_CHOPCONF] | (drv->regs[TMC2660_CHOPCONF] & 0xffff0); + case TMC2660_STATE_RECONFIGURE: + spi.out = TMC2660_CHOPCONF_ADDR | (drv->regs[TMC2660_CHOPCONF] & 0xffff0); break; } spi_send(); // Next state - spi_delay = 4; + spi_delay = 8; spi.state = SPI_STATE_READ; break; @@ -173,28 +182,41 @@ void spi_next() { if (++drv->reg == 5) { drv->state = TMC2660_STATE_MONITOR; drv->reg = 0; + drv->stabilizing = rtc_get_time() + TMC2660_STABILIZE_TIME * 1000; + drv->callback = true; + hw.st_port[spi.driver]->OUTCLR = MOTOR_ENABLE_BIT_bm; // Enable } break; case TMC2660_STATE_MONITOR: - // Read response - drv->sguard = (uint16_t)((spi.in >> 14) & 0x1ff); + // Read response (in bits [23, 4]) + drv->sguard = (uint16_t)((spi.in >> 14) & 0x3ff); drv->flags = spi.in >> 4; + if (spi.driver == 0) { + DACB.STATUS = DAC_CH0DRE_bm; + DACB.CH0DATA = drv->sguard << 2; + } + + if (drv->stabilizing < rtc_get_time() && drv->callback) { + motor_driver_callback(spi.driver); + drv->callback = false; + } + _report_error_flags(spi.driver); - if (drv->reset) { - drv->state = TMC2660_STATE_RESET; - drv->reset = 0; + if (drv->reconfigure) { + drv->state = TMC2660_STATE_RECONFIGURE; + drv->reconfigure = false; } else if (++spi.driver == MOTORS) { spi.driver = 0; - spi_delay = 500; + spi_delay = F_CPU / 1024 * TMC2660_POLL_RATE; break; } break; - case TMC2660_STATE_RESET: + case TMC2660_STATE_RECONFIGURE: drv->state = TMC2660_STATE_CONFIG; break; } @@ -220,7 +242,8 @@ ISR(TCC1_OVF_vect) { void _fault_isr(int motor) { - st_motor_error_callback(motor, MOTOR_FLAG_STALLED_bm); + if (drivers[motor].stabilizing < rtc_get_time()) + motor_error_callback(motor, MOTOR_FLAG_STALLED_bm); } @@ -232,8 +255,7 @@ ISR(PORT_4_FAULT_ISR_vect) {_fault_isr(3);} void tmc2660_init() { // Reset state - spi.state = SPI_STATE_SELECT; - spi.driver = spi.byte = 0; + memset(&spi, 0, sizeof(spi)); memset(drivers, 0, sizeof(drivers)); // Configure motors @@ -257,20 +279,21 @@ void tmc2660_init() { drivers[i].regs[TMC2660_DRVCTRL] = TMC2660_DRVCTRL_DEDGE | mstep | (MOTOR_MICROSTEPS == 16 ? TMC2660_DRVCTRL_INTPOL : 0); - drivers[i].regs[TMC2660_CHOPCONF] = TMC2660_CHOPCONF_TBL_36 | + drivers[i].regs[TMC2660_CHOPCONF] = TMC2660_CHOPCONF_TBL_16 | TMC2660_CHOPCONF_HEND(3) | TMC2660_CHOPCONF_HSTART(7) | TMC2660_CHOPCONF_TOFF(4); //drivers[i].regs[TMC2660_CHOPCONF] = TMC2660_CHOPCONF_TBL_36 | // TMC2660_CHOPCONF_CHM | TMC2660_CHOPCONF_HEND(7) | // TMC2660_CHOPCONF_FASTD(6) | TMC2660_CHOPCONF_TOFF(7); drivers[i].regs[TMC2660_SMARTEN] = TMC2660_SMARTEN_SEIMIN | - TMC2660_SMARTEN_MAX(2) | TMC2660_SMARTEN_MIN(2); + TMC2660_SMARTEN_SE(350, 450); + drivers[i].regs[TMC2660_SMARTEN] = 0; // Disable CoolStep drivers[i].regs[TMC2660_SGCSCONF] = TMC2660_SGCSCONF_SFILT | TMC2660_SGCSCONF_THRESH(63); drivers[i].regs[TMC2660_DRVCONF] = TMC2660_DRVCONF_RDSEL_SG; - set_power_level(i, MOTOR_CURRENT); - drivers[i].reset = 0; // No need to reset + set_power_level(i, MOTOR_IDLE_CURRENT); + drivers[i].reconfigure = false; // No need to reconfigure } // Setup pins @@ -286,9 +309,10 @@ void tmc2660_init() { TMC2660_SPI_PORT.DIRSET = 1 << TMC2660_SPI_MOSI_PIN; // Output for (int motor = 0; motor < MOTORS; motor++) { - hw.st_port[motor]->OUTSET = CHIP_SELECT_BIT_bm; // High - hw.st_port[motor]->DIRSET = CHIP_SELECT_BIT_bm; // Output - hw.st_port[motor]->DIRCLR = FAULT_BIT_bm; // Input + hw.st_port[motor]->OUTSET = CHIP_SELECT_BIT_bm; // High + hw.st_port[motor]->OUTSET = MOTOR_ENABLE_BIT_bm; // High (disabled) + hw.st_port[motor]->DIR = MOTOR_PORT_DIR_gm; // Pin directions + hw.st_port[motor]->PIN4CTRL = PORT_ISC_RISING_gc; hw.st_port[motor]->INT1MASK = FAULT_BIT_bm; // INT1 hw.st_port[motor]->INTCTRL |= PORT_INT1LVL_HI_gc; @@ -299,13 +323,18 @@ void tmc2660_init() { SPIC.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_3_gc | SPI_PRESCALER_DIV128_gc; // enable, big endian, master, mode 3, clock/128 PORTC.REMAP = PORT_SPI_bm; // Swap SCK and MOSI - SPIC.INTCTRL = SPI_INTLVL_MED_gc; // interupt level + SPIC.INTCTRL = SPI_INTLVL_LO_gc; // interupt level // Configure timer PR.PRPC &= ~PR_TC1_bm; // Disable power reduction - TMC2660_TIMER.PER = F_CPU / 1024 / 10; // Set timer period - TMC2660_TIMER.INTCTRLA = TC_OVFINTLVL_MED_gc; // overflow interupt level - TMC2660_TIMER.CTRLA = TC_CLKSEL_DIV1024_gc; // enable, clock/1024 + TMC2660_TIMER.PER = 1; // initial period not important + TMC2660_TIMER.INTCTRLA = TC_OVFINTLVL_LO_gc; // overflow interupt level + TMC2660_TIMER.CTRLA = TC_CLKSEL_DIV1024_gc; // enable, clock/1024 + + // Configure DAC channel 0 for output + DACB.CTRLB = DAC_CHSEL_SINGLE_gc; + DACB.CTRLC = DAC_REFSEL_AVCC_gc; + DACB.CTRLA = DAC_CH0EN_bm | DAC_ENABLE_bm; } @@ -314,27 +343,32 @@ uint8_t tmc2660_flags(int motor) { } -void tmc2660_reset(int motor) { - if (motor < MOTORS) drivers[motor].reset = 1; +void tmc2660_reconfigure(int motor) { + if (motor < MOTORS) drivers[motor].reconfigure = true; } -int tmc2660_ready(int motor) { - return - drivers[motor].state == TMC2660_STATE_MONITOR && !drivers[motor].reset; +bool tmc2660_ready(int motor) { + return drivers[motor].state == TMC2660_STATE_MONITOR && + !drivers[motor].reconfigure; } -int tmc2660_all_ready() { +stat_t tmc2660_sync() { for (int i = 0; i < MOTORS; i++) - if (!tmc2660_ready(i)) return 0; + if (!tmc2660_ready(i)) return STAT_EAGAIN; + + return STAT_OK; +} + - return 1; +void tmc2660_enable(int driver) { + set_power_level(driver, MOTOR_CURRENT); } -uint8_t get_status_flags(int motor) { - return drivers[motor].flags; +void tmc2660_disable(int driver) { + set_power_level(driver, MOTOR_IDLE_CURRENT); } @@ -353,7 +387,7 @@ void set_power_level(int motor, float value) { tmc2660_driver_t *d = &drivers[motor]; d->regs[TMC2660_SGCSCONF] = (d->regs[TMC2660_SGCSCONF] & ~31) | x; - tmc2660_reset(motor); + tmc2660_reconfigure(motor); } @@ -363,7 +397,7 @@ uint16_t get_sg_value(int motor) { int8_t get_stallguard(int motor) { - uint8_t x = (drivers[motor].regs[TMC2660_SGCSCONF] & 0x7f) >> 8; + uint8_t x = (drivers[motor].regs[TMC2660_SGCSCONF] & 0x7f00) >> 8; return (x & (1 << 6)) ? (x & 0xc0) : x; } @@ -372,8 +406,8 @@ void set_stallguard(int motor, int8_t value) { if (value < -64 || 63 < value) return; tmc2660_driver_t *d = &drivers[motor]; - d->regs[TMC2660_SGCSCONF] = (d->regs[TMC2660_SGCSCONF] & ~31) | + d->regs[TMC2660_SGCSCONF] = (d->regs[TMC2660_SGCSCONF] & ~0x7f00) | TMC2660_SGCSCONF_THRESH(value); - tmc2660_reset(motor); + tmc2660_reconfigure(motor); } diff --git a/src/tmc2660.h b/src/tmc2660.h index d0b7b8a..a8bcf2c 100644 --- a/src/tmc2660.h +++ b/src/tmc2660.h @@ -29,22 +29,20 @@ #include "config.h" +#include "status.h" #include +#include -#define TMC2660_SPI_PORT PORTC -#define TMC2660_SPI_SS_PIN 4 -#define TMC2660_SPI_SCK_PIN 5 -#define TMC2660_SPI_MISO_PIN 6 -#define TMC2660_SPI_MOSI_PIN 7 - -#define TMC2660_TIMER TCC1 void tmc2660_init(); uint8_t tmc2660_status(int driver); void tmc2660_reset(int driver); -int tmc2660_ready(int driver); -int tmc2660_all_ready(); +bool tmc2660_ready(int driver); +stat_t tmc2660_sync(); +void tmc2660_enable(int driver); +void tmc2660_disable(int driver); + #define TMC2660_DRVCTRL 0 #define TMC2660_DRVCTRL_ADDR (0UL << 18) @@ -92,12 +90,13 @@ int tmc2660_all_ready(); #define TMC2660_SMARTEN_SEDN_8 (1UL << 13) #define TMC2660_SMARTEN_SEDN_2 (2UL << 13) #define TMC2660_SMARTEN_SEDN_1 (3UL << 13) -#define TMC2660_SMARTEN_MAX(x) ((x & 0xf) << 8) #define TMC2660_SMARTEN_SEUP_1 (0UL << 5) #define TMC2660_SMARTEN_SEUP_2 (1UL << 5) #define TMC2660_SMARTEN_SEUP_4 (2UL << 5) #define TMC2660_SMARTEN_SEUP_8 (3UL << 5) -#define TMC2660_SMARTEN_MIN(x) (((int32_t)x & 0xf) << 0) +#define TMC2660_SMARTEN_SE(MIN, MAX) \ + (((uint32_t)(MIN / 32) & 0xf) | \ + (((uint32_t)(MAX / 32 - MIN / 32 - 1) & 0xf) << 8)) #define TMC2660_SGCSCONF 3 #define TMC2660_SGCSCONF_ADDR (6UL << 17) diff --git a/src/usart.c b/src/usart.c index 9633d5a..5e44046 100644 --- a/src/usart.c +++ b/src/usart.c @@ -26,12 +26,14 @@ \******************************************************************************/ #include "usart.h" +#include "config.h" #include #include #include #include +#include // Ring buffers #define RING_BUF_NAME tx_buf @@ -189,26 +191,37 @@ int8_t usart_getc() { } -int usart_gets(char *buf, int size) { - int fill = rx_buf_fill(); +char *usart_readline() { + static char line[INPUT_BUFFER_LEN]; + static int i = 0; + bool eol = false; - for (int i = 0; i < fill; i++) { - uint8_t data = rx_buf_get(i); + while (!rx_buf_empty()) { + char data = rx_buf_peek(); + rx_buf_pop(); - if (data == '\r' || data == '\n' || i == size) { - for (int j = 0; j < i; j++) { - buf[j] = rx_buf_peek(); - rx_buf_pop(); - } + switch (data) { + case '\r': case '\n': eol = true; break; - buf[i] = 0; - if (i != size) rx_buf_pop(); - set_rxc_interrupt(1); // Enable interrupt - return 0; // OK + case '\b': + printf(" \b"); + if (i) i--; + break; + + default: + line[i++] = data; + if (i == INPUT_BUFFER_LEN - 1) eol = true; + break; + } + + if (eol) { + line[i] = 0; + i = 0; + return line; } } - return 2; // EAGAIN + return 0; } diff --git a/src/usart.h b/src/usart.h index bbe8bc8..acd6842 100644 --- a/src/usart.h +++ b/src/usart.h @@ -58,7 +58,7 @@ void usart_ctrl(int flag, int enable); void usart_putc(char c); void usart_puts(const char *s); int8_t usart_getc(); -int usart_gets(char *buf, int size); +char *usart_readline(); int16_t usart_peek(); void usart_rx_flush(); diff --git a/src/vars.c b/src/vars.c index f084aff..18c9a25 100644 --- a/src/vars.c +++ b/src/vars.c @@ -42,10 +42,21 @@ #include #include + +typedef uint8_t flags_t; + + // Type names static const char bool_name [] PROGMEM = ""; #define TYPE_NAME(TYPE) static const char TYPE##_name [] PROGMEM = "<" #TYPE ">" -MAP(TYPE_NAME, SEMI, string, float, int8_t, uint8_t, uint16_t); +MAP(TYPE_NAME, SEMI, flags_t, float, int8_t, uint8_t, uint16_t); + + +extern void print_status_flags(uint8_t x); + +static void var_print_flags_t(uint8_t x) { + print_status_flags(x); +} static void var_print_float(float x) { @@ -103,9 +114,9 @@ static uint16_t var_parse_uint16_t(const char *value) { // Var forward declarations -#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ - TYPE get_##NAME(IF(INDEX)(int index)); \ - IF(SET) \ +#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ + TYPE get_##NAME(IF(INDEX)(int index)); \ + IF(SET) \ (void set_##NAME(IF(INDEX)(int index,) TYPE value);) #include "vars.def" @@ -150,8 +161,9 @@ void vars_report(bool full) { #define VAR(NAME, CODE, TYPE, INDEX, ...) \ IF(INDEX)(for (int i = 0; i < NAME##_count; i++)) { \ TYPE value = get_##NAME(IF(INDEX)(i)); \ + TYPE last = (NAME##_last)IF(INDEX)([i]); \ \ - if (value != (NAME##_last)IF(INDEX)([i]) || full) { \ + if (value != last || full) { \ (NAME##_last)IF(INDEX)([i]) = value; \ \ if (!reported) { \ @@ -175,10 +187,56 @@ void vars_report(bool full) { } -bool vars_set(const char *name, const char *value) { +int vars_find(const char *name) { + uint8_t i = 0; + uint8_t n = 0; + unsigned len = strlen(name); + + if (!len) return -1; + +#define VAR(NAME, CODE, TYPE, INDEX, ...) \ + if (!strcmp(IF_ELSE(INDEX)(name + 1, name), CODE)) { \ + IF(INDEX) \ + (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ + if (INDEX <= i) return -1); \ + return n; \ + } \ + n++; + +#include "vars.def" +#undef VAR + + return -1; +} + + +bool vars_print(const char *name) { uint8_t i; unsigned len = strlen(name); + if (!len) return false; + +#define VAR(NAME, CODE, TYPE, INDEX, ...) \ + if (!strcmp(IF_ELSE(INDEX)(name + 1, name), CODE)) { \ + IF(INDEX) \ + (i = strchr(INDEX##_LABEL, name[0]) - INDEX##_LABEL; \ + if (INDEX <= i) return false); \ + \ + var_print_##TYPE(get_##NAME(IF(INDEX)(i))); \ + \ + return true; \ + } + +#include "vars.def" +#undef VAR + + return false; +} + + +bool vars_set(const char *name, const char *value) { + uint8_t i; + unsigned len = strlen(name); if (!len) return false; diff --git a/src/vars.def b/src/vars.def index bf23eeb..2403167 100644 --- a/src/vars.def +++ b/src/vars.def @@ -29,7 +29,7 @@ #define MOTORS_LABEL "1234" #define SWITCHES_LABEL "12345678" -// VAR(name, code, type, index, settable, default, help) +// VAR(name, code, type, index, settable, default, help) // Motor VAR(motor_map, "ma", uint8_t, MOTORS, 1, 0, "Motor to axis mapping") @@ -42,7 +42,8 @@ VAR(power_mode, "pm", uint8_t, MOTORS, 1, 0, "Motor power mode") VAR(power_level, "pl", float, MOTORS, 1, 0, "Motor current") VAR(stallguard, "sg", int8_t, MOTORS, 1, 0, "StallGuard threshold") VAR(sg_value, "sgv", uint16_t, MOTORS, 0, 0, "StallGuard reading") -VAR(status_flags, "sf", uint8_t, MOTORS, 0, 0, "Motor status flags") +VAR(status_flags, "mf", uint8_t, MOTORS, 0, 0, "Motor status flags") +VAR(status_strings,"mfs", flags_t, MOTORS, 0, 0, "Motor status strings") // Axis VAR(position, "p", float, AXES, 0, 0, "Current axis position") @@ -76,4 +77,4 @@ VAR(spin_down, "sd", float, 0, 1, 0, "Spin down velocity") VAR(switch_type, "sw", uint8_t, SWITCHES, 1, 0, "Normally open or closed") // System -VAR(velocity, "v", float, 0, 0, 0, "Current velocity") +VAR(velocity, "v", float, 0, 0, 0, "Current velocity") diff --git a/src/vars.h b/src/vars.h index a10d0cb..4e02216 100644 --- a/src/vars.h +++ b/src/vars.h @@ -31,6 +31,8 @@ void vars_init(); void vars_report(bool full); +int vars_find(const char *name); +bool vars_print(const char *name); bool vars_set(const char *name, const char *value); int vars_parser(char *vars); void vars_print_help();