From: Joseph Coffland Date: Mon, 21 Mar 2016 03:21:53 +0000 (-0700) Subject: Interrupt on fault lines X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=43fc41c5d2b8a999950c373cce837a5d9261f933;p=bbctrl-firmware Interrupt on fault lines --- diff --git a/src/config.h b/src/config.h index efb2521..b8bd23a 100644 --- a/src/config.h +++ b/src/config.h @@ -235,6 +235,12 @@ #define PORT_MOTOR_3 PORTE #define PORT_MOTOR_4 PORTD +// Motor fault ISRs +#define PORT_1_FAULT_ISR_vect PORTA_INT1_vect +#define PORT_2_FAULT_ISR_vect PORTD_INT1_vect +#define PORT_3_FAULT_ISR_vect PORTE_INT1_vect +#define PORT_4_FAULT_ISR_vect PORTF_INT1_vect + // Switch axes mapped to ports #define PORT_SWITCH_X PORTA #define PORT_SWITCH_Y PORTF @@ -250,16 +256,14 @@ #define MOTOR_PORT_DIR_gm 0x2f // pin dir settings /// motor control port bit positions -enum cfgPortBits { - STEP_BIT_bp, // bit 0 - DIRECTION_BIT_bp, // bit 1 (low = clockwise) - MOTOR_ENABLE_BIT_bp, // bit 2 (low = enabled) - CHIP_SELECT_BIT_bp, // bit 3 - FAULT_BIT_bp, // bit 4 - GPIO1_OUT_BIT_bp, // bit 5 (gpio1 output bit; 1 from each axis) - SW_MIN_BIT_bp, // bit 6 (input bit for homing/limit switches) - SW_MAX_BIT_bp // bit 7 (input bit for homing/limit switches) -}; +#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) diff --git a/src/switch.c b/src/switch.c index 6fac71e..d158de7 100644 --- a/src/switch.c +++ b/src/switch.c @@ -60,6 +60,57 @@ swSingleton_t sw; +static bool _read_switch(uint8_t sw_num) { + switch (sw_num) { + case SW_MIN_X: return hw.sw_port[AXIS_X]->IN & SW_MIN_BIT_bm; + case SW_MAX_X: return hw.sw_port[AXIS_X]->IN & SW_MAX_BIT_bm; + case SW_MIN_Y: return hw.sw_port[AXIS_Y]->IN & SW_MIN_BIT_bm; + case SW_MAX_Y: return hw.sw_port[AXIS_Y]->IN & SW_MAX_BIT_bm; + case SW_MIN_Z: return hw.sw_port[AXIS_Z]->IN & SW_MIN_BIT_bm; + case SW_MAX_Z: return hw.sw_port[AXIS_Z]->IN & SW_MAX_BIT_bm; + case SW_MIN_A: return hw.sw_port[AXIS_A]->IN & SW_MIN_BIT_bm; + case SW_MAX_A: return hw.sw_port[AXIS_A]->IN & SW_MAX_BIT_bm; + default: return false; + } +} + + +/* These functions interact with each other to process switch closures + * and firing. Each switch has a counter which is initially set to + * negative SW_DEGLITCH_TICKS. When a switch closure is DETECTED the + * count increments for each RTC tick. When the count reaches zero + * the switch is tripped and action occurs. The counter continues to + * increment positive until the lockout is exceeded. + */ + +static void _switch_isr() { + for (int i = 0; i < SWITCHES; i++) { + switch_t *s = &sw.switches[i]; + + bool set = _read_switch(i); + if (set == s->last) continue; + + if (s->mode == SW_MODE_DISABLED) return; // never supposed to happen + if (s->debounce == SW_LOCKOUT) return; // switch is in lockout + + // either transitions state from IDLE or overwrites it + s->debounce = SW_DEGLITCHING; + // reset deglitch count regardless of entry state + s->count = -SW_DEGLITCH_TICKS; + + // A NO switch drives the pin LO when thrown + s->state = (s->type == SW_TYPE_NORMALLY_OPEN) ^ set; + } +} + + +// Switch interrupt handler vectors +ISR(X_ISR_vect) {_switch_isr();} +ISR(Y_ISR_vect) {_switch_isr();} +ISR(Z_ISR_vect) {_switch_isr();} +ISR(A_ISR_vect) {_switch_isr();} + + /* Initialize homing/limit switches * * This function assumes sys_init() and st_init() have been run previously to @@ -71,25 +122,20 @@ void switch_init() { if (sw.switches[MIN_SWITCH(i)].mode != SW_MODE_DISABLED) { hw.sw_port[i]->DIRCLR = SW_MIN_BIT_bm; // set min input - see 13.14.14 hw.sw_port[i]->PIN6CTRL = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; - hw.sw_port[i]->INT0MASK = SW_MIN_BIT_bm; // min on INT0 - - } else hw.sw_port[i]->INT0MASK = 0; // disable interrupt + hw.sw_port[i]->INT0MASK |= SW_MIN_BIT_bm; // min on INT0 + } if (sw.switches[MAX_SWITCH(i)].mode != SW_MODE_DISABLED) { hw.sw_port[i]->DIRCLR = SW_MAX_BIT_bm; // set max input - see 13.14.14 hw.sw_port[i]->PIN7CTRL = PORT_OPC_PULLUP_gc | PORT_ISC_BOTHEDGES_gc; - hw.sw_port[i]->INT1MASK = SW_MAX_BIT_bm; // max on INT1 - - } else hw.sw_port[i]->INT1MASK = 0; // disable interrupt + hw.sw_port[i]->INT0MASK |= SW_MAX_BIT_bm; // max on INT0 + } // set interrupt levels. Interrupts must be enabled in main() - hw.sw_port[i]->INTCTRL = SWITCH_INTLVL; + hw.sw_port[i]->INTCTRL |= SWITCH_INTLVL; } // Defaults - for (int i = 0; i < SWITCHES; i++) - sw.switches[i].type = SWITCH_TYPE; - sw.switches[0].mode = X_SWITCH_MODE_MIN; sw.switches[1].mode = X_SWITCH_MODE_MAX; sw.switches[2].mode = Y_SWITCH_MODE_MIN; @@ -99,45 +145,17 @@ void switch_init() { sw.switches[6].mode = A_SWITCH_MODE_MIN; sw.switches[7].mode = A_SWITCH_MODE_MAX; - reset_switches(); -} - - -/* These functions interact with each other to process switch closures - * and firing. Each switch has a counter which is initially set to - * negative SW_DEGLITCH_TICKS. When a switch closure is DETECTED the - * count increments for each RTC tick. When the count reaches zero - * the switch is tripped and action occurs. The counter continues to - * increment positive until the lockout is exceeded. - */ - -static void _switch_isr(uint8_t sw_num) { - switch_t *s = &sw.switches[sw_num]; - - if (s->mode == SW_MODE_DISABLED) return; // never supposed to happen - if (s->debounce == SW_LOCKOUT) return; // switch is in lockout - - // either transitions state from IDLE or overwrites it - s->debounce = SW_DEGLITCHING; - // reset deglitch count regardless of entry state - s->count = -SW_DEGLITCH_TICKS; + for (int i = 0; i < SWITCHES; i++) { + switch_t *s = &sw.switches[i]; + s->type = SWITCH_TYPE; + s->debounce = SW_IDLE; + s->state = (s->type == SW_TYPE_NORMALLY_OPEN) ^ _read_switch(i); + } - // sets the state value in the struct - read_switch(sw_num); + sw.limit_thrown = false; } -// Switch interrupt handler vectors -ISR(X_MIN_ISR_vect) {_switch_isr(SW_MIN_X);} -ISR(Y_MIN_ISR_vect) {_switch_isr(SW_MIN_Y);} -ISR(Z_MIN_ISR_vect) {_switch_isr(SW_MIN_Z);} -ISR(A_MIN_ISR_vect) {_switch_isr(SW_MIN_A);} -ISR(X_MAX_ISR_vect) {_switch_isr(SW_MAX_X);} -ISR(Y_MAX_ISR_vect) {_switch_isr(SW_MAX_Y);} -ISR(Z_MAX_ISR_vect) {_switch_isr(SW_MAX_Z);} -ISR(A_MAX_ISR_vect) {_switch_isr(SW_MAX_A);} - - /// Called from RTC for each RTC tick void switch_rtc_callback() { for (int i = 0; i < SWITCHES; i++) { @@ -151,8 +169,8 @@ void switch_rtc_callback() { s->debounce = SW_IDLE; // check if the state has changed while we were in lockout... - uint8_t old_state = s->state; - if (old_state != read_switch(i)) { + bool old_state = s->state; + if (old_state != _read_switch(i)) { s->debounce = SW_DEGLITCHING; s->count = -SW_DEGLITCH_TICKS; } @@ -161,7 +179,6 @@ void switch_rtc_callback() { } if (!s->count) { // trigger point - sw.switch_thrown = i; // record number of thrown switch s->debounce = SW_LOCKOUT; // regardless of switch type @@ -183,42 +200,6 @@ swMode_t get_switch_mode(uint8_t sw_num) {return sw.switches[sw_num].mode;} bool get_limit_switch_thrown() {return sw.limit_thrown;} -/// reset all switches and reset limit flag -void reset_switches() { - for (uint8_t i = 0; i < SWITCHES; i++) { - sw.switches[i].debounce = SW_IDLE; - read_switch(i); - } - - sw.limit_thrown = false; -} - - -/// read a switch directly with no interrupts or deglitching -uint8_t read_switch(uint8_t sw_num) { - if (sw_num < 0 || sw_num >= SWITCHES) return SW_DISABLED; - - bool hi = false; - switch (sw_num) { - case SW_MIN_X: hi = hw.sw_port[AXIS_X]->IN & SW_MIN_BIT_bm; break; - case SW_MAX_X: hi = hw.sw_port[AXIS_X]->IN & SW_MAX_BIT_bm; break; - case SW_MIN_Y: hi = hw.sw_port[AXIS_Y]->IN & SW_MIN_BIT_bm; break; - case SW_MAX_Y: hi = hw.sw_port[AXIS_Y]->IN & SW_MAX_BIT_bm; break; - case SW_MIN_Z: hi = hw.sw_port[AXIS_Z]->IN & SW_MIN_BIT_bm; break; - case SW_MAX_Z: hi = hw.sw_port[AXIS_Z]->IN & SW_MAX_BIT_bm; break; - case SW_MIN_A: hi = hw.sw_port[AXIS_A]->IN & SW_MIN_BIT_bm; break; - case SW_MAX_A: hi = hw.sw_port[AXIS_A]->IN & SW_MAX_BIT_bm; break; - } - - // A NO switch drives the pin LO when thrown - if (sw.switches[sw_num].type == SW_TYPE_NORMALLY_OPEN) - sw.switches[sw_num].state = hi ? SW_OPEN : SW_CLOSED; - else sw.switches[sw_num].state = hi ? SW_CLOSED : SW_OPEN; - - return sw.switches[sw_num].state; -} - - uint8_t get_switch_type(int index) { return sw.switches[index].type; } diff --git a/src/switch.h b/src/switch.h index 0301455..5a668a4 100644 --- a/src/switch.h +++ b/src/switch.h @@ -70,7 +70,6 @@ typedef enum { } swType_t; typedef enum { - SW_DISABLED = -1, SW_OPEN, SW_CLOSED } swState_t; @@ -100,21 +99,17 @@ typedef enum { // indexes into switch arrays /// Interrupt levels and vectors - The vectors are hard-wired to xmega ports /// If you change axis port assignments you need to change these, too. -#define SWITCH_INTLVL (PORT_INT0LVL_MED_gc | PORT_INT1LVL_MED_gc) +#define SWITCH_INTLVL PORT_INT0LVL_MED_gc // port assignments for vectors -#define X_MIN_ISR_vect PORTA_INT0_vect -#define Y_MIN_ISR_vect PORTD_INT0_vect -#define Z_MIN_ISR_vect PORTE_INT0_vect -#define A_MIN_ISR_vect PORTF_INT0_vect - -#define X_MAX_ISR_vect PORTA_INT1_vect -#define Y_MAX_ISR_vect PORTD_INT1_vect -#define Z_MAX_ISR_vect PORTE_INT1_vect -#define A_MAX_ISR_vect PORTF_INT1_vect +#define X_ISR_vect PORTA_INT0_vect +#define Y_ISR_vect PORTD_INT0_vect +#define Z_ISR_vect PORTE_INT0_vect +#define A_ISR_vect PORTF_INT0_vect typedef struct { - swState_t state; + bool last; + bool state; swType_t type; swMode_t mode; swDebounce_t debounce; // debounce state @@ -127,7 +122,6 @@ typedef struct { */ typedef struct { bool limit_thrown; - uint8_t switch_thrown; // number of switch that was just thrown switch_t switches[SWITCHES]; } swSingleton_t; @@ -135,7 +129,6 @@ extern swSingleton_t sw; void switch_init(); -uint8_t read_switch(uint8_t sw_num); swMode_t get_switch_mode(uint8_t sw_num); void switch_rtc_callback(); diff --git a/src/tmc2660.c b/src/tmc2660.c index e00e93b..42c95e2 100644 --- a/src/tmc2660.c +++ b/src/tmc2660.c @@ -29,6 +29,7 @@ #include "status.h" #include "stepper.h" #include "hardware.h" +#include "cpp_magic.h" #include #include @@ -214,6 +215,17 @@ ISR(TCC1_OVF_vect) { } +void _fault_isr(int motor) { + st_motor_error_callback(driver, MOTOR_FLAG_STALLED_bm); +} + + +ISR(PORT_1_FAULT_ISR_vect) {_fault_isr(0);} +ISR(PORT_2_FAULT_ISR_vect) {_fault_isr(1);} +ISR(PORT_3_FAULT_ISR_vect) {_fault_isr(2);} +ISR(PORT_4_FAULT_ISR_vect) {_fault_isr(3);} + + void tmc2660_init() { // Reset state spi_state = SPI_STATE_SELECT; @@ -270,9 +282,12 @@ 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]->DIRSET = CHIP_SELECT_BIT_bm; // Output + hw.st_port[motor]->DIRCLR = FAULT_BIT_bm; // Input + 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; } // Configure SPI