From 8f2c8eabdef5dbed8fa3d1d0f400db8171bba82a Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 28 Jun 2016 21:45:20 -0700 Subject: [PATCH] Allow unsaved vars, Work on switches and estop --- src/axes.c | 28 ------- src/command.c | 10 ++- src/cycle_homing.c | 39 ++++------ src/cycle_probing.c | 30 +------ src/estop.c | 27 ++++++- src/pins.h | 12 +-- src/switch.c | 186 +++++++++++++------------------------------- src/switch.h | 39 ++++------ src/varcb.c | 2 +- src/vars.c | 23 +++--- src/vars.def | 76 +++++++++--------- 11 files changed, 170 insertions(+), 302 deletions(-) diff --git a/src/axes.c b/src/axes.c index eec0822..93684d1 100644 --- a/src/axes.c +++ b/src/axes.c @@ -148,31 +148,3 @@ float get_zero_backoff(int axis) { void set_zero_backoff(int axis, float value) { cm.a[axis].zero_backoff = value; } - - - - - - - -// TODO fix these -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/command.c b/src/command.c index 64aba85..69e8e9a 100644 --- a/src/command.c +++ b/src/command.c @@ -179,7 +179,9 @@ stat_t command_hi() { default: return STAT_OK; // Continue processing in command_lo() } + report_request(); _cmd = 0; // Command complete + return status; } @@ -189,13 +191,15 @@ stat_t command_lo() { usart_tx_full()) return STAT_OK; // Wait + // Consume command + char *cmd = _cmd; + _cmd = 0; + if (estop_triggered()) return STAT_MACHINE_ALARMED; if (calibrate_busy()) return STAT_BUSY; if (mp_jog_busy()) return STAT_BUSY; - stat_t status = gc_gcode_parser(_cmd); - _cmd = 0; - return status; + return gc_gcode_parser(cmd); } diff --git a/src/cycle_homing.c b/src/cycle_homing.c index bb9234c..74ed909 100644 --- a/src/cycle_homing.c +++ b/src/cycle_homing.c @@ -47,8 +47,6 @@ typedef stat_t (*homing_func_t)(int8_t axis); struct hmHomingSingleton { // controls for homing cycle int8_t axis; // axis currently being homed - uint8_t min_mode; // mode for min switch for this axis - uint8_t max_mode; // mode for max switch for this axis /// homing switch for current axis (index into switch flag table) int8_t homing_switch; @@ -230,46 +228,37 @@ static stat_t _homing_axis_start(int8_t axis) { if (fp_ZERO(travel_distance)) return _homing_error_exit(axis, STAT_HOMING_ERROR_TRAVEL_MIN_MAX_IDENTICAL); - // determine the switch setup and that config is OK - hm.min_mode = switch_get_mode(MIN_SWITCH(axis)); - hm.max_mode = switch_get_mode(MAX_SWITCH(axis)); - - // one or the other must be homing - if (!((hm.min_mode & SW_HOMING_BIT) ^ (hm.max_mode & SW_HOMING_BIT))) - // axis cannot be homed - return _homing_error_exit(axis, STAT_HOMING_ERROR_SWITCH_MISCONFIGURATION); - hm.axis = axis; // persist the axis // search velocity is always positive hm.search_velocity = fabs(cm.a[axis].search_velocity); // latch velocity is always positive hm.latch_velocity = fabs(cm.a[axis].latch_velocity); - // setup parameters homing to the minimum switch - if (hm.min_mode & SW_HOMING_BIT) { + // determine which switch to use + bool min_enabled = switch_is_enabled(MIN_SWITCH(axis)); + bool max_enabled = switch_is_enabled(MAX_SWITCH(axis)); + + if (min_enabled) { + // setup parameters homing to the minimum switch hm.homing_switch = MIN_SWITCH(axis); // min is the homing switch hm.limit_switch = MAX_SWITCH(axis); // max would be limit switch hm.search_travel = -travel_distance; // in negative direction hm.latch_backoff = cm.a[axis].latch_backoff; // in positive direction hm.zero_backoff = cm.a[axis].zero_backoff; + } else if (max_enabled) { // setup parameters for positive travel (homing to the maximum switch) - } else { hm.homing_switch = MAX_SWITCH(axis); // max is homing switch hm.limit_switch = MIN_SWITCH(axis); // min would be limit switch hm.search_travel = travel_distance; // in positive direction hm.latch_backoff = -cm.a[axis].latch_backoff; // in negative direction hm.zero_backoff = -cm.a[axis].zero_backoff; - } - // if homing is disabled for the axis then skip to the next axis - uint8_t sw_mode = switch_get_mode(hm.homing_switch); - if (sw_mode != SW_MODE_HOMING && sw_mode != SW_MODE_HOMING_LIMIT) + } else { + // if homing is disabled for the axis then skip to the next axis + hm.limit_switch = -1; // disable the limit switch parameter return _set_homing_func(_homing_axis_start); - - // disable the limit switch parameter if there is no limit switch - if (switch_get_mode(hm.limit_switch) == SW_MODE_DISABLED) - hm.limit_switch = -1; + } hm.saved_jerk = cm_get_axis_jerk(axis); // save the max jerk value @@ -282,10 +271,10 @@ static stat_t _homing_axis_clear(int8_t axis) { // Handle an initial switch closure by backing off the closed switch // NOTE: Relies on independent switches per axis (not shared) - if (switch_get_active(hm.homing_switch)) + if (switch_is_active(hm.homing_switch)) _homing_axis_move(axis, hm.latch_backoff, hm.search_velocity); - else if (switch_get_active(hm.limit_switch)) + else if (switch_is_active(hm.limit_switch)) _homing_axis_move(axis, -hm.latch_backoff, hm.search_velocity); return _set_homing_func(_homing_axis_search); @@ -306,7 +295,7 @@ static stat_t _homing_axis_search(int8_t axis) { static stat_t _homing_axis_latch(int8_t axis) { // verify assumption that we arrived here because of homing switch closure // rather than user-initiated feedhold or other disruption - if (!switch_get_active(hm.homing_switch)) + if (!switch_is_active(hm.homing_switch)) return _set_homing_func(_homing_abort); _homing_axis_move(axis, hm.latch_backoff, hm.latch_velocity); diff --git a/src/cycle_probing.c b/src/cycle_probing.c index d48acca..c62fb17 100644 --- a/src/cycle_probing.c +++ b/src/cycle_probing.c @@ -47,11 +47,6 @@ struct pbProbingSingleton { // persistent probing runtime variables stat_t (*func)(); // binding for callback function state machine - // switch configuration - uint8_t probe_switch; // which switch should we check? - uint8_t saved_switch_type; // saved switch type NO/NC - uint8_t saved_switch_mode; // save the probe switch's original settings - // state saved from gcode model uint8_t saved_distance_mode; // G90,G91 global setting uint8_t saved_coord_system; // G54 - G59 setting @@ -164,23 +159,6 @@ static uint8_t _probing_init() { if (fp_NE(pb.start_position[axis], pb.target[axis])) _probing_error_exit(axis); - // initialize the probe switch - - // switch the switch type mode for the probe - // FIXME: we should be able to use the homing switch at this point too, - // Can't because switch mode is global and our probe is NO, not NC. - - pb.probe_switch = SW_MIN_Z; // FIXME: hardcoded... - pb.saved_switch_mode = switch_get_mode(pb.probe_switch); - - switch_set_mode(pb.probe_switch, SW_MODE_HOMING); - // save the switch type for recovery later. - pb.saved_switch_type = switch_get_type(pb.probe_switch); - // contact probes are NO switches... usually - switch_set_type(pb.probe_switch, SW_TYPE_NORMALLY_OPEN); - // re-init to pick up new switch settings - switch_init(); - // probe in absolute machine coords pb.saved_coord_system = cm_get_coord_system(&cm.gm); pb.saved_distance_mode = cm_get_distance_mode(&cm.gm); @@ -196,7 +174,7 @@ static uint8_t _probing_init() { static stat_t _probing_start() { // initial probe state, don't probe if we're already contacted! - bool closed = switch_get_active(pb.probe_switch); + bool closed = switch_is_active(SW_PROBE); if (!closed) RITORNO(cm_straight_feed(pb.target, pb.flags)); @@ -205,7 +183,7 @@ static stat_t _probing_start() { static stat_t _probing_finish() { - bool closed = switch_get_active(pb.probe_switch); + bool closed = switch_is_active(SW_PROBE); cm.probe_state = closed ? PROBE_SUCCEEDED : PROBE_FAILED; for (uint8_t axis = 0; axis < AXES; axis++) { @@ -224,10 +202,6 @@ static void _probe_restore_settings() { // we should be stopped now, but in case of switch closure mp_flush_planner(); - switch_set_type(pb.probe_switch, pb.saved_switch_type); - switch_set_mode(pb.probe_switch, pb.saved_switch_mode); - switch_init(); // re-init to pick up changes - // restore axis jerk for (uint8_t axis = 0; axis < AXES; axis++ ) cm_set_axis_jerk(axis, pb.saved_jerk[axis]); diff --git a/src/estop.c b/src/estop.c index 966a402..45074e5 100644 --- a/src/estop.c +++ b/src/estop.c @@ -29,6 +29,8 @@ #include "motor.h" #include "stepper.h" #include "spindle.h" +#include "switch.h" +#include "report.h" #include "config.h" #include "plan/planner.h" @@ -42,12 +44,24 @@ typedef struct { static estop_t estop = {0}; +static void _switch_callback(switch_id_t id, bool active) { + if (active) estop_trigger(); + else estop_clear(); + + report_request(); +} + + void estop_init() { + switch_set_callback(SW_ESTOP, _switch_callback); + + OUTCLR_PIN(FAULT_PIN); // Low + DIRSET_PIN(FAULT_PIN); // Output } bool estop_triggered() { - return estop.triggered; + return estop.triggered || switch_is_active(SW_ESTOP); } @@ -64,6 +78,9 @@ void estop_trigger() { // Set alarm state cm_set_machine_state(MACHINE_ALARM); + + // Assert fault signal + OUTSET_PIN(FAULT_PIN); // High } @@ -76,6 +93,9 @@ void estop_clear() { // Clear alarm state cm_set_machine_state(MACHINE_READY); + + // Clear fault signal + OUTCLR_PIN(FAULT_PIN); // Low } @@ -85,7 +105,10 @@ bool get_estop() { void set_estop(bool value) { - if (estop.triggered != value) { + bool triggered = estop_triggered(); + estop.triggered = value; + + if (triggered != estop_triggered()) { if (value) estop_trigger(); else estop_clear(); } diff --git a/src/pins.h b/src/pins.h index 14d29db..75a4fc1 100644 --- a/src/pins.h +++ b/src/pins.h @@ -30,20 +30,10 @@ #include -// Ports -enum { - PORT_A, - PORT_B, - PORT_C, - PORT_D, - PORT_E, - PORT_F, -}; - +enum {PORT_A, PORT_B, PORT_C, PORT_D, PORT_E, PORT_F}; extern PORT_t *pin_ports[]; - #define PORT(PIN) pin_ports[PIN >> 3] #define BM(PIN) (1 << (PIN & 7)) diff --git a/src/switch.c b/src/switch.c index 8e369b8..a08277b 100644 --- a/src/switch.c +++ b/src/switch.c @@ -26,22 +26,12 @@ \******************************************************************************/ -/* Switch Modes - * - * The switches are considered to be homing switches when machine_state is - * MACHINE_HOMING. At all other times they are treated as limit switches: - * - * - Hitting a homing switch puts the current move into feedhold - * - * - Hitting a limit switch causes the machine to shut down and go into - * lockdown until reset - * - * The normally open switch modes (NO) trigger an interrupt on the +/* Normally open switches (NO) trigger an interrupt on the * falling edge and lockout subsequent interrupts for the defined * lockout period. This approach beats doing debouncing as an * integration as switches fire immediately. * - * The normally closed switch modes (NC) trigger an interrupt on the + * Normally closed switches (NC) trigger an interrupt on the * rising edge and lockout subsequent interrupts for the defined * lockout period. * @@ -68,100 +58,50 @@ typedef enum { SW_IDLE, SW_DEGLITCHING, SW_LOCKOUT -} swDebounce_t; +} switch_debounce_t; typedef struct { - swType_t type; - swMode_t mode; uint8_t pin; - bool min; + switch_type_t type; + switch_callback_t cb; bool state; - swDebounce_t debounce; - int8_t count; + switch_debounce_t debounce; + int16_t count; } switch_t; -typedef struct { - bool limit_thrown; - switch_t switches[SWITCHES]; -} swSingleton_t; - - -swSingleton_t sw = { - .switches = { - { // X min - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_MODE_HOMING, - .pin = MIN_X_PIN, - .min = true, - }, { // X max - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_MODE_DISABLED, - .pin = MAX_X_PIN, - .min = false, - }, { // Y min - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_MODE_HOMING, - .pin = MIN_Y_PIN, - .min = true, - }, { // Y max - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_MODE_DISABLED, - .pin = MAX_X_PIN, - .min = false, - }, { // Z min - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_MODE_DISABLED, - .pin = MIN_Z_PIN, - .min = true, - }, { // Z max - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_MODE_HOMING, - .pin = MAX_Z_PIN, - .min = false, - }, { // A min - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_MODE_DISABLED, // SW_MODE_HOMING, - .pin = MIN_A_PIN, - .min = true, - }, { // A max - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_MODE_DISABLED, - .pin = MAX_A_PIN, - .min = false, - }, { // EStop - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_ESTOP_BIT, - .pin = ESTOP_PIN, - }, { // Probe - .type = SW_TYPE_NORMALLY_OPEN, - .mode = SW_PROBE_BIT, - .pin = PROBE_PIN, - }, - } + +static switch_t switches[SWITCHES] = { + {.pin = MIN_X_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MIN_Y_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MAX_X_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MIN_Z_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MAX_Z_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MIN_A_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = MAX_A_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = ESTOP_PIN, .type = SW_NORMALLY_OPEN}, + {.pin = PROBE_PIN, .type = SW_NORMALLY_OPEN}, }; static bool _read_state(const switch_t *s) { // A normally open switch drives the pin low when thrown - return (s->type == SW_TYPE_NORMALLY_OPEN) ^ IN_PIN(s->pin); + return (s->type == SW_NORMALLY_OPEN) ^ IN_PIN(s->pin); } static void _switch_isr() { for (int i = 0; i < SWITCHES; i++) { - switch_t *s = &sw.switches[i]; - + switch_t *s = &switches[i]; bool state = _read_state(s); - if (state == s->state || s->mode == SW_MODE_DISABLED || - s->debounce == SW_LOCKOUT) continue; - // either transitions state from IDLE or overwrites it + if (state == s->state || s->type == SW_DISABLED) continue; + s->debounce = SW_DEGLITCHING; - // reset deglitch count regardless of entry state - s->count = -SW_DEGLITCH_TICKS; + s->count = -SW_DEGLITCH_TICKS; // reset deglitch count s->state = state; PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0 } @@ -179,29 +119,24 @@ ISR(PORTF_INT0_vect) {_switch_isr();} void _switch_enable(switch_t *s, bool enable) { if (enable) { - PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0 - - // Pull up and trigger on both edges - PINCTRL_PIN(s->pin) = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; - - // Initialize state - s->state = _read_state(s); + PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0 + s->state = _read_state(s); // Initialize state + s->debounce = SW_IDLE; - } else { - PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0 - PINCTRL_PIN(s->pin) = 0; - } + } else PORT(s->pin)->INT0MASK &= ~BM(s->pin); // Disable INT0 } void switch_init() { for (int i = 0; i < SWITCHES; i++) { - switch_t *s = &sw.switches[i]; + switch_t *s = &switches[i]; + // Pull up and trigger on both edges + PINCTRL_PIN(s->pin) = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; DIRCLR_PIN(s->pin); // Input PORT(s->pin)->INTCTRL |= SWITCH_INTLVL; // Set interrupt level - _switch_enable(s, s->mode != SW_MODE_DISABLED); + _switch_enable(s, s->type != SW_DISABLED); } } @@ -209,18 +144,19 @@ void switch_init() { /// Called from RTC on each tick void switch_rtc_callback() { for (int i = 0; i < SWITCHES; i++) { - switch_t *s = &sw.switches[i]; + switch_t *s = &switches[i]; - if (s->mode == SW_MODE_DISABLED || s->debounce == SW_IDLE) - continue; + if (s->type == SW_DISABLED || s->debounce == SW_IDLE) continue; // state is either lockout or deglitching if (++s->count == SW_LOCKOUT_TICKS) { + PORT(s->pin)->INT0MASK |= BM(s->pin); // Renable INT0 + bool state = _read_state(s); s->debounce = SW_IDLE; - PORT(s->pin)->INT0MASK |= BM(s->pin); // Enable INT0 // check if the state has changed while we were in lockout - if (s->state != _read_state(s)) { + if (s->state != state) { + s->state = state; s->debounce = SW_DEGLITCHING; s->count = -SW_DEGLITCH_TICKS; } @@ -230,58 +166,46 @@ void switch_rtc_callback() { if (!s->count) { // switch triggered s->debounce = SW_LOCKOUT; + if (s->cb) s->cb(i, s->state); + // TODO fix this if (cm.cycle_state == CYCLE_HOMING || cm.cycle_state == CYCLE_PROBE) cm_request_feedhold(); - - else if (s->mode & SW_LIMIT_BIT || s->mode & SW_ESTOP_BIT) - sw.limit_thrown = true; // triggers an emergency shutdown } } } -bool switch_get_active(int index) { - return sw.switches[index].state; +bool switch_is_active(int index) { + return switches[index].state; } -swType_t switch_get_type(int index) { - return sw.switches[index].type; +bool switch_is_enabled(int index) { + return switch_get_type(index) != SW_DISABLED; } -void switch_set_type(int index, swType_t type) { - sw.switches[index].type = type; +switch_type_t switch_get_type(int index) { + return switches[index].type; } -swMode_t switch_get_mode(int index) { - return sw.switches[index].mode; -} +void switch_set_type(int index, switch_type_t type) { + switch_t *s = &switches[index]; - -void switch_set_mode(int index, swMode_t mode) { - switch_t *s = &sw.switches[index]; - - if (s->mode != mode) { - s->mode = mode; - _switch_enable(s, s->mode != SW_MODE_DISABLED); + if (s->type != type) { + s->type = type; + _switch_enable(s, type != SW_DISABLED); } } -bool switch_get_limit_thrown() { - return sw.limit_thrown; +void switch_set_callback(int index, switch_callback_t cb) { + switches[index].cb = cb; } // Var callbacks -uint8_t get_switch_type(int index) { - return sw.switches[index].type; -} - - -void set_switch_type(int index, uint8_t value) { - sw.switches[index].type = value; -} +uint8_t get_switch_type(int index) {return switch_get_type(index);} +void set_switch_type(int index, uint8_t value) {switch_set_type(index, value);} diff --git a/src/switch.h b/src/switch.h index 4aa6c10..3d0db61 100644 --- a/src/switch.h +++ b/src/switch.h @@ -34,43 +34,36 @@ #include #include + // macros for finding the index into the switch table give the axis number #define MIN_SWITCH(axis) (axis * 2) #define MAX_SWITCH(axis) (axis * 2 + 1) -/// switch modes -typedef enum { - SW_MODE_DISABLED = 0, - SW_HOMING_BIT = 1 << 0, - SW_LIMIT_BIT = 1 << 1, - SW_ESTOP_BIT = 1 << 2, - SW_PROBE_BIT = 1 << 4, -} swMode_t; - -#define SW_MODE_HOMING SW_HOMING_BIT // enable switch for homing only -#define SW_MODE_LIMIT SW_LIMIT_BIT // enable switch for limits only -#define SW_MODE_HOMING_LIMIT (SW_HOMING_BIT | SW_LIMIT_BIT) // homing & limits typedef enum { - SW_TYPE_NORMALLY_OPEN, - SW_TYPE_NORMALLY_CLOSED -} swType_t; + SW_DISABLED, + SW_NORMALLY_OPEN, + SW_NORMALLY_CLOSED +} switch_type_t; -/// indices into switch arrays + +/// Switch IDs typedef enum { SW_MIN_X, SW_MAX_X, SW_MIN_Y, SW_MAX_Y, SW_MIN_Z, SW_MAX_Z, SW_MIN_A, SW_MAX_A, SW_ESTOP, SW_PROBE -} swNums_t; +} switch_id_t; + + +typedef void (*switch_callback_t)(switch_id_t sw, bool active); void switch_init(); void switch_rtc_callback(); -bool switch_get_active(int index); -swType_t switch_get_type(int index); -void switch_set_type(int index, swType_t type); -swMode_t switch_get_mode(int index); -void switch_set_mode(int index, swMode_t mode); -bool switch_get_limit_thrown(); +bool switch_is_active(int index); +bool switch_is_enabled(int index); +switch_type_t switch_get_type(int index); +void switch_set_type(int index, switch_type_t type); +void switch_set_callback(int index, switch_callback_t cb); diff --git a/src/varcb.c b/src/varcb.c index 289a783..7cd4a85 100644 --- a/src/varcb.c +++ b/src/varcb.c @@ -41,7 +41,7 @@ float get_velocity() { bool get_echo() { - return true; // Always true so that echo is always enabled after reboot + return usart_is_set(USART_ECHO); } diff --git a/src/vars.c b/src/vars.c index ba7dbcc..03e21c7 100644 --- a/src/vars.c +++ b/src/vars.c @@ -110,6 +110,7 @@ static bool var_parse_bool(const char *value) { } +#if 0 static uint8_t eeprom_read_bool(bool *addr) { return eeprom_read_byte((uint8_t *)addr); } @@ -118,6 +119,7 @@ static uint8_t eeprom_read_bool(bool *addr) { static void eeprom_update_bool(bool *addr, bool value) { eeprom_update_byte((uint8_t *)addr, value); } +#endif // int8 @@ -193,7 +195,7 @@ static void eeprom_update_uint16_t(uint16_t *addr, uint16_t value) { #undef VAR // Var names, count & help -#define VAR(NAME, CODE, TYPE, INDEX, SET, DEFAULT, HELP) \ +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \ static const char NAME##_name[] PROGMEM = #NAME; \ static const char NAME##_help[] PROGMEM = HELP; @@ -201,8 +203,8 @@ static void eeprom_update_uint16_t(uint16_t *addr, uint16_t value) { #undef VAR // EEPROM storage -#define VAR(NAME, CODE, TYPE, INDEX, SET, DEFAULT, HELP) \ - IF(SET) \ +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, HELP) \ + IF(SAVE) \ (static TYPE NAME##_eeprom IF(INDEX)([INDEX]) EEMEM;) #include "vars.def" @@ -223,7 +225,7 @@ void vars_init() { vars_restore(); // Initialize var state -#define VAR(NAME, CODE, TYPE, INDEX, SET, DEFAULT, ...) \ +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ IF(INDEX)(for (int i = 0; i < INDEX; i++)) \ (NAME##_state)IF(INDEX)([i]) = get_##NAME(IF(INDEX)(i)); @@ -336,7 +338,6 @@ bool vars_set(const char *name, const char *value) { \ TYPE x = var_parse_##TYPE(value); \ set_##NAME(IF(INDEX)(i,) x); \ - NAME##_state IF(INDEX)([i]) = x; \ \ return true; \ }) \ @@ -405,8 +406,8 @@ uint16_t vars_crc() { CRC.CTRL = CRC_RESET_RESET0_gc; CRC.CTRL = CRC_SOURCE_IO_gc; // Must be after reset -#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ - IF(SET) \ +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ + IF(SAVE) \ ({ \ for (int j = 0; ; j++) { \ char c = pgm_read_byte(&NAME##_name[j]); \ @@ -432,8 +433,8 @@ void vars_save() { // Save and disable watchdog uint8_t wd_state = hw_disable_watchdog(); -#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ - IF(SET) \ +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ + IF(SAVE) \ (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ TYPE value = get_##NAME(IF(INDEX)(i)); \ eeprom_update_##TYPE(&NAME##_eeprom IF(INDEX)([i]), value); \ @@ -462,8 +463,8 @@ stat_t vars_restore() { // Save and disable watchdog uint8_t wd_state = hw_disable_watchdog(); -#define VAR(NAME, CODE, TYPE, INDEX, SET, ...) \ - IF(SET) \ +#define VAR(NAME, CODE, TYPE, INDEX, SET, SAVE, ...) \ + IF(SAVE) \ (IF(INDEX)(for (int i = 0; i < (INDEX ? INDEX : 1); i++)) { \ TYPE value = eeprom_read_##TYPE(&NAME##_eeprom IF(INDEX)([i])); \ set_##NAME(IF(INDEX)(i,) value); \ diff --git a/src/vars.def b/src/vars.def index 80feee0..633b537 100644 --- a/src/vars.def +++ b/src/vars.def @@ -29,54 +29,52 @@ #define MOTORS_LABEL "0123" #define SWITCHES_LABEL "0123456789" -// VAR(name, code, type, index, settable, default, help) +// VAR(name, code, type, index, settable, save, help) // Motor -VAR(motor_map, "ma", uint8_t, MOTORS, 1, 0, "Motor to axis mapping") -VAR(step_angle, "sa", float, MOTORS, 1, 0, "In degrees per full step") -VAR(travel, "tr", float, MOTORS, 1, 0, "Travel in mm per revolution") -VAR(microstep, "mi", uint16_t, MOTORS, 1, 0, "Microsteps per full step") -VAR(polarity, "po", uint8_t, MOTORS, 1, 0, "Normal or reversed") - -VAR(power_mode, "pm", uint8_t, MOTORS, 1, 0, "Motor power mode") -VAR(power_level, "pl", float, MOTORS, 1, 0, "Motor drive current") -VAR(idle_level, "il", float, MOTORS, 1, 0, "Motor idle current") -VAR(current_level, "cl", float, MOTORS, 1, 0, "Motor current now") -VAR(stallguard, "th", int8_t, MOTORS, 1, 0, "StallGuard threshold") +VAR(motor_map, "ma", uint8_t, MOTORS, 1, 1, "Motor to axis mapping") +VAR(step_angle, "sa", float, MOTORS, 1, 1, "In degrees per full step") +VAR(travel, "tr", float, MOTORS, 1, 1, "Travel in mm per revolution") +VAR(microstep, "mi", uint16_t, MOTORS, 1, 1, "Microsteps per full step") +VAR(polarity, "po", uint8_t, MOTORS, 1, 1, "Normal or reversed") + +VAR(power_mode, "pm", uint8_t, MOTORS, 1, 1, "Motor power mode") +VAR(power_level, "pl", float, MOTORS, 1, 1, "Motor drive current") +VAR(idle_level, "il", float, MOTORS, 1, 1, "Motor idle current") +VAR(current_level, "cl", float, MOTORS, 1, 1, "Motor current now") +VAR(stallguard, "th", int8_t, MOTORS, 1, 1, "StallGuard threshold") VAR(sg_value, "sg", uint16_t, MOTORS, 0, 0, "StallGuard reading") VAR(status_flags, "mf", uint8_t, MOTORS, 0, 0, "Motor status flags") VAR(status_strings, "ms", flags_t, MOTORS, 0, 0, "Motor status strings") // Axis VAR(position, "p", float, AXES, 0, 0, "Current axis position") -VAR(axis_mode, "am", uint8_t, AXES, 1, 0, "Axis mode") -VAR(max_velocity, "vm", float, AXES, 1, 0, "Maxium velocity in mm/min") -VAR(max_feedrate, "fr", float, AXES, 1, 0, "Maxium feedrate in mm/min") -VAR(max_jerk, "jm", float, AXES, 1, 0, "Maxium jerk in mm/min^3") -VAR(junction_dev, "jd", float, AXES, 1, 0, "Junction deviation") - -VAR(travel_min, "tn", float, AXES, 1, 0, "Minimum soft limit") -VAR(travel_max, "tm", float, AXES, 1, 0, "Maximum soft limit") -VAR(min_switch, "sn", uint8_t, AXES, 1, 0, "Minimum switch's ID") -VAR(max_switch, "sx", uint8_t, AXES, 1, 0, "Maximum switch's ID") - -VAR(jerk_homing, "jh", float, AXES, 1, 0, "Maxium homing jerk") -VAR(search_vel, "sv", float, AXES, 1, 0, "Homing search velocity") -VAR(latch_vel, "lv", float, AXES, 1, 0, "Homing latch velocity") -VAR(latch_backoff, "lb", float, AXES, 1, 0, "Homing latch backof") -VAR(zero_backoff, "zb", float, AXES, 1, 0, "Homing zero backof") +VAR(axis_mode, "am", uint8_t, AXES, 1, 1, "Axis mode") +VAR(max_velocity, "vm", float, AXES, 1, 1, "Maxium velocity in mm/min") +VAR(max_feedrate, "fr", float, AXES, 1, 1, "Maxium feedrate in mm/min") +VAR(max_jerk, "jm", float, AXES, 1, 1, "Maxium jerk in mm/min^3") +VAR(junction_dev, "jd", float, AXES, 1, 1, "Junction deviation") + +VAR(travel_min, "tn", float, AXES, 1, 1, "Minimum soft limit") +VAR(travel_max, "tm", float, AXES, 1, 1, "Maximum soft limit") + +VAR(jerk_homing, "jh", float, AXES, 1, 1, "Maxium homing jerk") +VAR(search_vel, "sv", float, AXES, 1, 1, "Homing search velocity") +VAR(latch_vel, "lv", float, AXES, 1, 1, "Homing latch velocity") +VAR(latch_backoff, "lb", float, AXES, 1, 1, "Homing latch backof") +VAR(zero_backoff, "zb", float, AXES, 1, 1, "Homing zero backof") // Spindle -VAR(max_spin, "ss", float, 0, 1, 0, "Maximum spindle speed") -VAR(spindle_type, "st", uint8_t, 0, 1, 0, "PWM=0 or HUANYANG=1") -VAR(spin_min_pulse, "np", float, 0, 1, 0, "Minimum pulse width") -VAR(spin_max_pulse, "mp", float, 0, 1, 0, "Maximum pulse width") -VAR(spin_polarity, "sp", uint8_t, 0, 1, 0, "Normal or reversed") -VAR(spin_up, "su", float, 0, 1, 0, "Spin up velocity") -VAR(spin_down, "sd", float, 0, 1, 0, "Spin down velocity") +VAR(max_spin, "ss", float, 0, 1, 1, "Maximum spindle speed") +VAR(spindle_type, "st", uint8_t, 0, 1, 1, "PWM=0 or HUANYANG=1") +VAR(spin_min_pulse, "np", float, 0, 1, 1, "Minimum pulse width") +VAR(spin_max_pulse, "mp", float, 0, 1, 1, "Maximum pulse width") +VAR(spin_polarity, "sp", uint8_t, 0, 1, 1, "Normal or reversed") +VAR(spin_up, "su", float, 0, 1, 1, "Spin up velocity") +VAR(spin_down, "sd", float, 0, 1, 1, "Spin down velocity") // Huanyang spindle -VAR(huanyang_id, "hi", uint8_t, 0, 1, 0, "Huanyang ID") +VAR(huanyang_id, "hi", uint8_t, 0, 1, 1, "Huanyang ID") VAR(huanyang_freq, "hz", float, 0, 0, 0, "Huanyang actual freq") VAR(huanyang_current, "hc", float, 0, 0, 0, "Huanyang actual current") VAR(huanyang_rpm, "hr", uint16_t, 0, 0, 0, "Huanyang actual RPM") @@ -91,10 +89,10 @@ VAR(huanyang_debug, "hb", bool, 0, 1, 0, "Huanyang debugging") VAR(huanyang_connected, "he", bool, 0, 0, 0, "Huanyang connected") // Switches -VAR(switch_type, "sw", uint8_t, SWITCHES, 1, 0, "Normally open or closed") +VAR(switch_type, "sw", uint8_t, SWITCHES, 1, 1, "Normally open or closed") // System VAR(velocity, "v", float, 0, 0, 0, "Current velocity") VAR(hw_id, "id", string, 0, 0, 0, "Hardware ID") -VAR(echo, "ec", bool, 0, 1, 1, "Enable or disable echo") -VAR(estop, "es", bool, 0, 1, 1, "Emergency stop") +VAR(echo, "ec", bool, 0, 1, 0, "Enable or disable echo") +VAR(estop, "es", bool, 0, 1, 0, "Emergency stop") -- 2.27.0