From: Joseph Coffland Date: Wed, 12 Apr 2017 01:31:38 +0000 (-0700) Subject: Cleaned up motor power state code, Check for DRV8711 comm error, fixed PWM spindle... X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=3e249838d16bf3316da985e0d29a2c56971e9090;p=bbctrl-firmware Cleaned up motor power state code, Check for DRV8711 comm error, fixed PWM spindle code --- diff --git a/avr/src/config.h b/avr/src/config.h index 5dc6094..0bde1f4 100644 --- a/avr/src/config.h +++ b/avr/src/config.h @@ -164,6 +164,20 @@ enum { #define SEGMENT_CLOCKS ((uint24_t)(F_CPU * SEGMENT_SEC)) #define SEGMENT_PERIOD ((uint16_t)(STEP_TIMER_FREQ * SEGMENT_SEC)) + +// DRV8711 settings +#define DRV8711_OFF 12 +#define DRV8711_BLANK 0x80 +#define DRV8711_DECAY (DRV8711_DECAY_DECMOD_AUTO_OPT | 6) +#define DRV8711_STALL (DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_8 | 200) +#define DRV8711_DRIVE (DRV8711_DRIVE_IDRIVEP_50 | \ + DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_500 | \ + DRV8711_DRIVE_TDRIVEN_500 | DRV8711_DRIVE_OCPDEG_2 | \ + DRV8711_DRIVE_OCPTH_500) +#define DRV8711_TORQUE DRV8711_TORQUE_SMPLTH_50 +#define DRV8711_CTRL (DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_850) + + // Huanyang settings #define HUANYANG_PORT USARTD1 #define HUANYANG_DRE_vect USARTD1_DRE_vect diff --git a/avr/src/coolant.c b/avr/src/coolant.c index e083044..46ec802 100644 --- a/avr/src/coolant.c +++ b/avr/src/coolant.c @@ -39,17 +39,7 @@ void coolant_init() { } -void coolant_set_mist(bool x) { - if (x) OUTCLR_PIN(SWITCH_1_PIN); - else OUTSET_PIN(SWITCH_1_PIN); -} - - -void coolant_set_flood(bool x) { - if (x) OUTCLR_PIN(SWITCH_2_PIN); - else OUTSET_PIN(SWITCH_2_PIN); -} - - +void coolant_set_mist(bool x) {SET_PIN(SWITCH_1_PIN, !x);} +void coolant_set_flood(bool x) {SET_PIN(SWITCH_2_PIN, !x);} bool coolant_get_mist() {return OUT_PIN(SWITCH_1_PIN);} bool coolant_get_flood() {return OUT_PIN(SWITCH_2_PIN);} diff --git a/avr/src/drv8711.c b/avr/src/drv8711.c index 71b840a..6f6e7b5 100644 --- a/avr/src/drv8711.c +++ b/avr/src/drv8711.c @@ -27,7 +27,8 @@ #include "drv8711.h" #include "status.h" -#include "motor.h" +#include "stepper.h" +#include "report.h" #include #include @@ -41,8 +42,7 @@ #define COMMANDS 10 -#define DRV8711_WORD_BYTE_PTR(WORD, LOW) \ - (((uint8_t *)&(WORD)) + ((LOW) ? 0 : 1)) +#define DRV8711_WORD_BYTE_PTR(WORD, LOW) (((uint8_t *)&(WORD)) + !(LOW)) bool motor_fault = false; @@ -50,6 +50,7 @@ bool motor_fault = false; typedef struct { uint8_t status; + uint16_t flags; drv8711_state_t state; float idle_current; @@ -91,23 +92,6 @@ typedef struct { static spi_t spi = {0}; -static void _driver_check_status(int driver) { - uint8_t status = drivers[driver].status; - uint8_t mflags = 0; - - if (status & DRV8711_STATUS_OTS_bm) mflags |= MOTOR_FLAG_OVER_TEMP_bm; - if (status & DRV8711_STATUS_AOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm; - if (status & DRV8711_STATUS_BOCP_bm) mflags |= MOTOR_FLAG_OVER_CURRENT_bm; - if (status & DRV8711_STATUS_APDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm; - if (status & DRV8711_STATUS_BPDF_bm) mflags |= MOTOR_FLAG_DRIVER_FAULT_bm; - if (status & DRV8711_STATUS_UVLO_bm) mflags |= MOTOR_FLAG_UNDER_VOLTAGE_bm; - if (status & DRV8711_STATUS_STD_bm) mflags |= MOTOR_FLAG_STALLED_bm; - if (status & DRV8711_STATUS_STDLAT_bm) mflags |= MOTOR_FLAG_STALLED_bm; - - //if (mflags) motor_error_callback(driver, mflags); TODO -} - - static float _driver_get_current(int driver) { drv8711_driver_t *drv = &drivers[driver]; @@ -118,29 +102,40 @@ static float _driver_get_current(int driver) { return drv->min_current + (drv->max_current - drv->min_current) * drv->power; - default: return 0; + default: return 0; // Off } } static uint8_t _spi_next_command(uint8_t cmd) { - // Process status responses + // Process command responses for (int driver = 0; driver < DRIVERS; driver++) { + drv8711_driver_t *drv = &drivers[driver]; uint16_t command = spi.commands[driver][cmd]; - if (DRV8711_CMD_IS_READ(command) && - DRV8711_CMD_ADDR(command) == DRV8711_STATUS_REG) { - drivers[driver].status = spi.responses[driver]; - _driver_check_status(driver); - } + if (DRV8711_CMD_IS_READ(command)) + switch (DRV8711_CMD_ADDR(command)) { + case DRV8711_STATUS_REG: + drv->status = spi.responses[driver]; + + if ((drv->status & drv->flags) != drv->status) { + drv->flags |= drv->status; + report_request(); + } + break; + + case DRV8711_OFF_REG: + // We read back the OFF register to test for communication failure. + if ((spi.responses[driver] & 0x1ff) != DRV8711_OFF) + drv->flags |= DRV8711_COMM_ERROR_bm; + break; + } } // Next command if (++cmd == spi.ncmds) { cmd = 0; // Wrap around - - for (int driver = 0; driver < DRIVERS; driver++) - motor_driver_callback(driver); + st_enable(); // Enable motors } // Prep next command @@ -151,6 +146,7 @@ static uint8_t _spi_next_command(uint8_t cmd) { switch (DRV8711_CMD_ADDR(*command)) { case DRV8711_STATUS_REG: if (!DRV8711_CMD_IS_READ(*command)) + // Clear STATUS flags *command = (*command & 0xf000) | (0x0fff & ~(drv->status)); break; @@ -225,41 +221,15 @@ static void _init_spi_commands() { uint16_t *commands = spi.commands[driver]; spi.ncmds = 0; - // Set OFF - commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_OFF_REG, 12); - - // Set BLANK - commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_BLANK_REG, 0x80); - - // Set DECAY - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY_DECMOD_AUTO_OPT | 6); - - // Set STALL - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_STALL_REG, - DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_8 | 200); - - // Set DRIVE - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_DRIVE_REG, DRV8711_DRIVE_IDRIVEP_50 | - DRV8711_DRIVE_IDRIVEN_100 | DRV8711_DRIVE_TDRIVEP_500 | - DRV8711_DRIVE_TDRIVEN_500 | DRV8711_DRIVE_OCPDEG_2 | - DRV8711_DRIVE_OCPTH_500); - - // Set TORQUE - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE_SMPLTH_50); - - // Set CTRL set ISENSE gain - commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_CTRL_REG, - DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_850); - - // Read STATUS + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_OFF_REG, DRV8711_OFF); + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_BLANK_REG, DRV8711_BLANK); + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY); + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_STALL_REG, DRV8711_STALL); + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_DRIVE_REG, DRV8711_DRIVE); + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE); + commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_CTRL_REG, DRV8711_CTRL); + commands[spi.ncmds++] = DRV8711_READ(DRV8711_OFF_REG); commands[spi.ncmds++] = DRV8711_READ(DRV8711_STATUS_REG); - - // Clear STATUS commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_STATUS_REG, 0); } @@ -406,3 +376,81 @@ float get_active_current(int driver) { bool get_motor_fault() {return motor_fault;} + + +uint16_t get_driver_flags(int driver) {return drivers[driver].flags;} + + +void print_status_flags(uint16_t flags) { + bool first = true; + + putchar('"'); + + if (DRV8711_STATUS_OTS_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("temp")); + first = false; + } + + if (DRV8711_STATUS_AOCP_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("current a")); + first = false; + } + + if (DRV8711_STATUS_BOCP_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("current b")); + first = false; + } + + if (DRV8711_STATUS_APDF_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("fault a")); + first = false; + } + + if (DRV8711_STATUS_BPDF_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("fault b")); + first = false; + } + + if (DRV8711_STATUS_UVLO_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("uvlo")); + first = false; + } + + if ((DRV8711_STATUS_STD_bm | DRV8711_STATUS_STDLAT_bm) & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("stall")); + first = false; + } + + if (DRV8711_COMM_ERROR_bm & flags) { + if (!first) printf_P(PSTR(", ")); + printf_P(PSTR("comm")); + first = false; + } + + putchar('"'); +} + + +uint16_t get_status_strings(int driver) {return get_driver_flags(driver);} + + +// Command callback +void command_mreset(int argc, char *argv[]) { + if (argc == 1) + for (int driver = 0; driver < DRIVERS; driver++) + drivers[driver].flags = 0; + + else { + int driver = atoi(argv[1]); + if (driver < DRIVERS) drivers[driver].flags = 0; + } + + report_request(); +} diff --git a/avr/src/drv8711.h b/avr/src/drv8711.h index e405677..e559ac9 100644 --- a/avr/src/drv8711.h +++ b/avr/src/drv8711.h @@ -152,6 +152,7 @@ enum { DRV8711_STATUS_UVLO_bm = 1 << 5, DRV8711_STATUS_STD_bm = 1 << 6, DRV8711_STATUS_STDLAT_bm = 1 << 7, + DRV8711_COMM_ERROR_bm = 1 << 8, }; diff --git a/avr/src/estop.c b/avr/src/estop.c index 116cdfb..d8887af 100644 --- a/avr/src/estop.c +++ b/avr/src/estop.c @@ -77,8 +77,7 @@ void estop_init() { if (estop.triggered) mp_state_estop(); // Fault signal - if (estop.triggered) OUTSET_PIN(FAULT_PIN); // High - else OUTCLR_PIN(FAULT_PIN); // Low + SET_PIN(FAULT_PIN, estop.triggered); DIRSET_PIN(FAULT_PIN); // Output } diff --git a/avr/src/huanyang.c b/avr/src/huanyang.c index 7b0d09a..0defd37 100644 --- a/avr/src/huanyang.c +++ b/avr/src/huanyang.c @@ -151,10 +151,7 @@ static void _set_baud(uint16_t bsel, uint8_t bscale) { } -static void _set_write(bool x) { - if (x) OUTSET_PIN(RS485_RW_PIN); // High - else OUTCLR_PIN(RS485_RW_PIN); // Low -} +static void _set_write(bool x) {SET_PIN(RS485_RW_PIN, x);} static void _set_dre_interrupt(bool enable) { diff --git a/avr/src/motor.c b/avr/src/motor.c index 818be43..3a08b7a 100644 --- a/avr/src/motor.c +++ b/avr/src/motor.c @@ -50,13 +50,6 @@ #include -typedef enum { - MOTOR_OFF, - MOTOR_IDLE, // motor stopped and may be partially energized - MOTOR_ACTIVE -} motor_power_state_t; - - typedef struct { // Config uint8_t axis; // map motor to axis @@ -75,9 +68,7 @@ typedef struct { float steps_per_unit; // Runtime state - motor_power_state_t power_state; // state machine for managing motor power uint32_t power_timeout; - motor_flags_t flags; int32_t commanded; int32_t encoder; int16_t error; @@ -140,11 +131,7 @@ void motor_init() { // Enable DMA DMA.CTRL = DMA_RESET_bm; DMA.CTRL = DMA_ENABLE_bm; - DMA.INTFLAGS = 0xff; // clear all interrupts - - // Motor enable - OUTSET_PIN(MOTOR_ENABLE_PIN); // Low (disabled) - DIRSET_PIN(MOTOR_ENABLE_PIN); // Output + DMA.INTFLAGS = 0xff; // clear all pending interrupts for (int motor = 0; motor < MOTORS; motor++) { motor_t *m = &motors[motor]; @@ -182,15 +169,6 @@ void motor_init() { } -void motor_enable(int motor, bool enable) { - if (enable) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high - else { - OUTCLR_PIN(MOTOR_ENABLE_PIN); - motors[motor].power_state = MOTOR_DISABLED; - } -} - - bool motor_is_enabled(int motor) { return motors[motor].power_mode != MOTOR_DISABLED; } @@ -199,6 +177,14 @@ bool motor_is_enabled(int motor) { int motor_get_axis(int motor) {return motors[motor].axis;} +void motor_set_axis(int motor, uint8_t axis) { + if (MOTORS <= motor || AXES <= axis || axis == motors[motor].axis) return; + axis_set_motor(motors[motor].axis, -1); + motors[motor].axis = axis; + axis_set_motor(axis, motor); +} + + void motor_set_stall_callback(int motor, stall_callback_t cb) { drv8711_set_stall_callback(motor, cb); } @@ -247,24 +233,6 @@ int32_t motor_get_position(int motor) { } -/// returns true if motor is in an error state -bool motor_error(int m) { - uint8_t flags = motors[m].flags; - if (calibrate_busy()) flags &= ~MOTOR_FLAG_STALLED_bm; - return flags & MOTOR_FLAG_ERROR_bm; -} - - -bool motor_stalled(int m) {return motors[m].flags & MOTOR_FLAG_STALLED_bm;} -void motor_reset(int m) {motors[m].flags = 0;} - - -static void _set_power_state(int motor, motor_power_state_t state) { - motors[motor].power_state = state; - report_request(); -} - - static void _update_power(int motor) { motor_t *m = &motors[motor]; @@ -272,43 +240,19 @@ static void _update_power(int motor) { case MOTOR_POWERED_ONLY_WHEN_MOVING: case MOTOR_POWERED_IN_CYCLE: if (rtc_expired(m->power_timeout)) { - if (m->power_state == MOTOR_ACTIVE) { - _set_power_state(motor, MOTOR_IDLE); - drv8711_set_state(motor, DRV8711_IDLE); - } + drv8711_set_state(motor, DRV8711_IDLE); break; } // Fall through case MOTOR_ALWAYS_POWERED: - if (m->power_state != MOTOR_ACTIVE && !motor_error(motor)) { - // TODO is ~5ms enough time to enable the motor? - drv8711_set_state(motor, DRV8711_ACTIVE); - m->power_state = MOTOR_ACTIVE; - motor_driver_callback(motor); // TODO Shouldn't call this directly - } + // TODO is ~5ms enough time to enable the motor? + drv8711_set_state(motor, DRV8711_ACTIVE); break; default: // Disabled - if (m->power_state != MOTOR_OFF) { - _set_power_state(motor, MOTOR_OFF); - drv8711_set_state(motor, DRV8711_DISABLED); - } - } -} - - -void motor_driver_callback(int motor) { - motor_t *m = &motors[motor]; - - if (m->power_state == MOTOR_IDLE) m->flags &= ~MOTOR_FLAG_ENABLED_bm; - else if (!estop_triggered()) { - m->power_state = MOTOR_ACTIVE; - m->flags |= MOTOR_FLAG_ENABLED_bm; - motor_enable(motor, true); + drv8711_set_state(motor, DRV8711_DISABLED); } - - report_request(); } @@ -321,24 +265,6 @@ stat_t motor_rtc_callback() { // called by controller } -void motor_error_callback(int motor, motor_flags_t errors) { - motor_t *m = &motors[motor]; - - if (m->power_state != MOTOR_ACTIVE) return; - - m->flags |= errors & MOTOR_FLAG_ERROR_bm; - report_request(); - - if (motor_error(motor)) { - if (m->flags & MOTOR_FLAG_STALLED_bm) ALARM(STAT_MOTOR_STALLED); - if (m->flags & MOTOR_FLAG_OVER_TEMP_bm) ALARM(STAT_MOTOR_OVER_TEMP); - if (m->flags & MOTOR_FLAG_OVER_CURRENT_bm) ALARM(STAT_MOTOR_OVER_CURRENT); - if (m->flags & MOTOR_FLAG_DRIVER_FAULT_bm) ALARM(STAT_MOTOR_DRIVER_FAULT); - if (m->flags & MOTOR_FLAG_UNDER_VOLTAGE_bm) ALARM(STAT_MOTOR_UNDER_VOLTAGE); - } -} - - void motor_end_move(int motor) { motor_t *m = &motors[motor]; @@ -366,9 +292,8 @@ void motor_load_move(int motor) { motor_end_move(motor); // Set direction, compensating for polarity - const bool clockwise = !(m->negative ^ m->reverse); - if (clockwise) OUTCLR_PIN(m->dir_pin); - else OUTSET_PIN(m->dir_pin); + const bool counterclockwise = m->negative ^ m->reverse; + SET_PIN(m->dir_pin, counterclockwise); // Adjust clock count if (m->last_clock) { @@ -511,98 +436,19 @@ bool get_reverse(int motor) { void set_reverse(int motor, bool value) {motors[motor].reverse = value;} -uint8_t get_motor_axis(int motor) {return motors[motor].axis;} - - -void set_motor_axis(int motor, uint8_t value) { - if (MOTORS <= motor || AXES <= value || value == motors[motor].axis) return; - axis_set_motor(motors[motor].axis, -1); - motors[motor].axis = value; - _update_config(motor); - axis_set_motor(value, motor); -} - - -char get_axis_name(int motor) {return axis_get_char(motors[motor].axis);} - - -void set_axis_name(int motor, char axis) { - int id = axis_get_id(axis); - if (id != -1) set_motor_axis(motor, id); -} +char get_motor_axis(int motor) {return motors[motor].axis;} +void set_motor_axis(int motor, uint8_t axis) {motor_set_axis(motor, axis);} uint8_t get_power_mode(int motor) {return motors[motor].power_mode;} void set_power_mode(int motor, uint8_t value) { - if (value < MOTOR_POWER_MODE_MAX_VALUE) + if (value <= MOTOR_POWERED_ONLY_WHEN_MOVING) motors[motor].power_mode = value; + else motors[motor].power_mode = MOTOR_DISABLED; } -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_OVER_TEMP_bm & flags) { - if (!first) printf_P(PSTR(", ")); - printf_P(PSTR("over temp")); - first = false; - } - - if (MOTOR_FLAG_OVER_CURRENT_bm & flags) { - if (!first) printf_P(PSTR(", ")); - printf_P(PSTR("over current")); - first = false; - } - - if (MOTOR_FLAG_DRIVER_FAULT_bm & flags) { - if (!first) printf_P(PSTR(", ")); - printf_P(PSTR("fault")); - first = false; - } - - if (MOTOR_FLAG_UNDER_VOLTAGE_bm & flags) { - if (!first) printf_P(PSTR(", ")); - printf_P(PSTR("uvlo")); - first = false; - } - - putchar('"'); -} - - -uint8_t get_status_strings(int m) {return get_status_flags(m);} int32_t get_encoder(int m) {return motors[m].encoder;} int32_t get_error(int m) {return motors[m].error;} - - -// Command callback -void command_mreset(int argc, char *argv[]) { - if (argc == 1) - for (int m = 0; m < MOTORS; m++) - motor_reset(m); - - else { - int m = atoi(argv[1]); - if (m < MOTORS) motor_reset(m); - } - - report_request(); -} diff --git a/avr/src/motor.h b/avr/src/motor.h index 5f33c91..a4f5bda 100644 --- a/avr/src/motor.h +++ b/avr/src/motor.h @@ -33,28 +33,12 @@ #include -typedef enum { - MOTOR_FLAG_ENABLED_bm = 1 << 0, - MOTOR_FLAG_STALLED_bm = 1 << 1, - MOTOR_FLAG_OVER_TEMP_bm = 1 << 2, - MOTOR_FLAG_OVER_CURRENT_bm = 1 << 3, - MOTOR_FLAG_DRIVER_FAULT_bm = 1 << 4, - MOTOR_FLAG_UNDER_VOLTAGE_bm = 1 << 5, - MOTOR_FLAG_ERROR_bm = (//MOTOR_FLAG_STALLED_bm | - MOTOR_FLAG_OVER_TEMP_bm | - MOTOR_FLAG_OVER_CURRENT_bm | - MOTOR_FLAG_DRIVER_FAULT_bm | - MOTOR_FLAG_UNDER_VOLTAGE_bm) -} motor_flags_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 } motor_power_mode_t; @@ -62,7 +46,6 @@ typedef void (*stall_callback_t)(int motor); void motor_init(); -void motor_enable(int motor, bool enable); bool motor_is_enabled(int motor); int motor_get_axis(int motor); @@ -74,13 +57,7 @@ void motor_set_microsteps(int motor, uint16_t microsteps); void motor_set_position(int motor, int32_t position); int32_t motor_get_position(int motor); -bool motor_error(int motor); -bool motor_stalled(int motor); -void motor_reset(int motor); - -void motor_driver_callback(int motor); stat_t motor_rtc_callback(); -void motor_error_callback(int motor, motor_flags_t errors); void motor_end_move(int motor); void motor_load_move(int motor); diff --git a/avr/src/pins.h b/avr/src/pins.h index 3426af5..d122e1b 100644 --- a/avr/src/pins.h +++ b/avr/src/pins.h @@ -46,4 +46,7 @@ extern PORT_t *pin_ports[]; #define IN_PIN(PIN) (!!(PORT(PIN)->IN & BM(PIN))) #define PINCTRL_PIN(PIN) ((&PORT(PIN)->PIN0CTRL)[PIN & 7]) +#define SET_PIN(PIN, X) \ + do {if (X) OUTSET_PIN(PIN); else OUTCLR_PIN(PIN);} while (0); + #endif // __AVR__ diff --git a/avr/src/pwm_spindle.c b/avr/src/pwm_spindle.c index 10cf8c1..98e25e5 100644 --- a/avr/src/pwm_spindle.c +++ b/avr/src/pwm_spindle.c @@ -29,6 +29,7 @@ #include "pwm_spindle.h" #include "config.h" +#include "estop.h" typedef struct { @@ -39,7 +40,7 @@ typedef struct { float max_duty; bool reverse; bool enable_invert; - bool estop; + bool pwm_invert; } pwm_spindle_t; @@ -47,16 +48,28 @@ static pwm_spindle_t spindle = {0}; static void _spindle_set_pwm(spindle_mode_t mode, float speed) { - if (mode == SPINDLE_OFF || speed < spindle.min_rpm || spindle.estop) { + if (mode == SPINDLE_OFF || speed < spindle.min_rpm || estop_triggered()) { TIMER_PWM.CTRLA = 0; return; } + // Invert PWM + if (spindle.pwm_invert) PINCTRL_PIN(SPIN_PWM_PIN) |= PORT_INVEN_bm; + else PINCTRL_PIN(SPIN_PWM_PIN) &= ~PORT_INVEN_bm; + + // 100% duty + if (spindle.max_rpm <= speed && spindle.max_duty == 1) { + TIMER_PWM.CTRLB = 0; + OUTSET_PIN(SPIN_PWM_PIN); + return; + } + // Clamp speed if (spindle.max_rpm < speed) speed = spindle.max_rpm; + if (speed < spindle.min_rpm) speed = 0; // Set clock period and optimal prescaler value - float prescale = F_CPU / 65536.0 / spindle.freq; + float prescale = (float)(F_CPU >> 16) / spindle.freq; if (prescale <= 1) { TIMER_PWM.PER = F_CPU / spindle.freq; TIMER_PWM.CTRLA = TC_CLKSEL_DIV1_gc; @@ -83,33 +96,30 @@ static void _spindle_set_pwm(spindle_mode_t mode, float speed) { float duty = (speed - spindle.min_rpm) / (spindle.max_rpm - spindle.min_rpm) * (spindle.max_duty - spindle.min_duty) + spindle.min_duty; - TIMER_PWM.CCB = TIMER_PWM.PER * duty; + // Configure clock + TIMER_PWM.CTRLB = TC1_CCAEN_bm | TC_WGMODE_SINGLESLOPE_gc; + TIMER_PWM.CCA = TIMER_PWM.PER * duty; } static void _spindle_set_enable(bool enable) { - if (enable ^ spindle.enable_invert) - PORT(SPIN_ENABLE_PIN)->OUTSET = BM(SPIN_ENABLE_PIN); - else PORT(SPIN_ENABLE_PIN)->OUTCLR = BM(SPIN_ENABLE_PIN); + SET_PIN(SPIN_ENABLE_PIN, enable ^ spindle.enable_invert); } static void _spindle_set_dir(bool forward) { - if (forward ^ spindle.reverse) PORT(SPIN_DIR_PIN)->OUTCLR = BM(SPIN_DIR_PIN); - else PORT(SPIN_DIR_PIN)->OUTSET = BM(SPIN_DIR_PIN); + SET_PIN(SPIN_DIR_PIN, !(forward ^ spindle.reverse)); } void pwm_spindle_init() { // Configure IO - PORT(SPIN_PWM_PIN)->DIRSET = BM(SPIN_PWM_PIN); // PWM Output _spindle_set_dir(true); - PORT(SPIN_DIR_PIN)->DIRSET = BM(SPIN_DIR_PIN); // Dir Output _spindle_set_enable(false); - PORT(SPIN_ENABLE_PIN)->DIRSET = BM(SPIN_ENABLE_PIN); // Enable output - // Configure clock - TIMER_PWM.CTRLB = TC1_CCBEN_bm | TC_WGMODE_SINGLESLOPE_gc; + DIRSET_PIN(SPIN_PWM_PIN); // Output + DIRSET_PIN(SPIN_DIR_PIN); // Output + DIRSET_PIN(SPIN_ENABLE_PIN); // Output } @@ -120,10 +130,7 @@ void pwm_spindle_set(spindle_mode_t mode, float speed) { } -void pwm_spindle_estop() { - spindle.estop = true; - _spindle_set_pwm(SPINDLE_OFF, 0); -} +void pwm_spindle_estop() {_spindle_set_pwm(SPINDLE_OFF, 0);} // TODO these need more effort and should work with the huanyang spindle too @@ -131,10 +138,10 @@ float get_max_spin() {return spindle.max_rpm;} void set_max_spin(float value) {spindle.max_rpm = value;} float get_min_spin() {return spindle.min_rpm;} void set_min_spin(float value) {spindle.min_rpm = value;} -float get_spin_min_pulse() {return spindle.min_duty;} -void set_spin_min_pulse(float value) {spindle.min_duty = value;} -float get_spin_max_pulse() {return spindle.max_duty;} -void set_spin_max_pulse(float value) {spindle.max_duty = value;} +float get_spin_min_duty() {return spindle.min_duty * 100;} +void set_spin_min_duty(float value) {spindle.min_duty = value / 100;} +float get_spin_max_duty() {return spindle.max_duty * 100;} +void set_spin_max_duty(float value) {spindle.max_duty = value / 100;} uint8_t get_spin_reverse() {return spindle.reverse;} void set_spin_reverse(uint8_t value) {spindle.reverse = value;} float get_spin_up() {return 0;} // TODO @@ -143,3 +150,7 @@ float get_spin_down() {return 0;} // TODO void set_spin_down(float value) {} // TODO uint16_t get_spin_freq() {return spindle.freq;} void set_spin_freq(uint16_t value) {spindle.freq = value;} +bool get_enable_invert() {return spindle.enable_invert;} +void set_enable_invert(bool value) {spindle.enable_invert = value;} +bool get_pwm_invert() {return spindle.pwm_invert;} +void set_pwm_invert(bool value) {spindle.pwm_invert = value;} diff --git a/avr/src/spindle.c b/avr/src/spindle.c index 76d8a45..d39e51b 100644 --- a/avr/src/spindle.c +++ b/avr/src/spindle.c @@ -77,6 +77,7 @@ void spindle_set_mode(spindle_mode_t mode) { void spindle_set_speed(float speed) { + if (speed < 0) speed = 0; spindle.speed = speed; switch (spindle.type) { diff --git a/avr/src/stepper.c b/avr/src/stepper.c index 2c226a4..2396f07 100644 --- a/avr/src/stepper.c +++ b/avr/src/stepper.c @@ -38,6 +38,7 @@ #include "estop.h" #include "util.h" #include "cpp_magic.h" +#include "report.h" #include #include @@ -69,6 +70,10 @@ static stepper_t st = {0}; void stepper_init() { + // Motor enable + OUTSET_PIN(MOTOR_ENABLE_PIN); // Low (disabled) + DIRSET_PIN(MOTOR_ENABLE_PIN); // Output + // Setup step timer TIMER_STEP.CTRLB = STEP_TIMER_WGMODE; // waveform mode TIMER_STEP.INTCTRLA = STEP_TIMER_INTLVL; // interrupt mode @@ -80,14 +85,18 @@ void stepper_init() { void st_shutdown() { - for (int motor = 0; motor < MOTORS; motor++) - motor_enable(motor, false); - + OUTCLR_PIN(MOTOR_ENABLE_PIN); st.dwell = 0; st.move_type = MOVE_TYPE_NULL; } +void st_enable() { + if (!estop_triggered()) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high + report_request(); +} + + /// Return true if motors or dwell are running bool st_is_busy() {return st.busy;} diff --git a/avr/src/stepper.h b/avr/src/stepper.h index ab5023c..44f2d45 100644 --- a/avr/src/stepper.h +++ b/avr/src/stepper.h @@ -37,6 +37,7 @@ void stepper_init(); void st_shutdown(); +void st_enable(); bool st_is_busy(); stat_t st_prep_line(float time, const float target[]); void st_prep_dwell(float seconds); diff --git a/avr/src/vars.c b/avr/src/vars.c index f942937..93325ac 100644 --- a/avr/src/vars.c +++ b/avr/src/vars.c @@ -43,7 +43,7 @@ #include -typedef uint8_t flags_t; +typedef uint16_t flags_t; typedef const char *string; typedef PGM_P pstring; @@ -75,8 +75,8 @@ static void var_print_pstring(pstring s) {printf_P(PSTR("\"%"PRPSTR"\""), s);} // Flags -static void var_print_flags_t(uint8_t x) { - extern void print_status_flags(uint8_t x); +static void var_print_flags_t(uint16_t x) { + extern void print_status_flags(uint16_t x); print_status_flags(x); } @@ -128,8 +128,10 @@ static bool var_parse_bool(const char *value) { // Char +#if 0 static void var_print_char(char x) {putchar('"'); putchar(x); putchar('"');} -static char var_parse_char(const char *value) {return value[0];} +static char var_parse_char(const char *value) {return value[1];} +#endif // int8 diff --git a/avr/src/vars.def b/avr/src/vars.def index 9d63e3b..bbdf3e7 100644 --- a/avr/src/vars.def +++ b/avr/src/vars.def @@ -31,7 +31,7 @@ // VAR(name, code, type, index, settable, help) // Motor -VAR(axis_name, an, char, MOTORS, 1, "Maps motor to axis name") +VAR(motor_axis, an, uint8_t, MOTORS, 1, "Maps motor to axis") VAR(step_angle, sa, float, MOTORS, 1, "In degrees per full step") VAR(travel, tr, float, MOTORS, 1, "Travel in mm per revolution") VAR(microstep, mi, uint16_t, MOTORS, 1, "Microsteps per full step") @@ -42,8 +42,8 @@ VAR(max_current, xc, float, MOTORS, 1, "Max motor drive current") VAR(min_current, lc, float, MOTORS, 1, "Min motor drive current") VAR(idle_current, ic, float, MOTORS, 1, "Motor idle current") VAR(active_current, ac, float, MOTORS, 0, "Motor current now") -VAR(status_flags, mf, uint8_t, MOTORS, 0, "Motor status flags") -VAR(status_strings, ms, flags_t, MOTORS, 0, "Motor status strings") +VAR(driver_flags, df, uint16_t, MOTORS, 0, "Motor driver status flags") +VAR(status_strings, ds, flags_t, MOTORS, 0, "Motor driver status strings") VAR(encoder, en, int32_t, MOTORS, 0, "Motor encoder") VAR(error, ee, int32_t, MOTORS, 0, "Motor position error") @@ -76,12 +76,16 @@ VAR(spindle_type, st, uint8_t, 0, 1, "PWM=0 or HUANYANG=1") VAR(spin_reverse, sr, bool, 0, 1, "Reverse spin") VAR(max_spin, sx, float, 0, 1, "Maximum spindle speed") VAR(min_spin, sm, float, 0, 1, "Minimum spindle speed") -VAR(spin_min_pulse, np, float, 0, 1, "Minimum pulse width") -VAR(spin_max_pulse, mp, float, 0, 1, "Maximum pulse width") +VAR(spin_min_duty, nd, float, 0, 1, "Minimum PWM duty cycle") +VAR(spin_max_duty, md, float, 0, 1, "Maximum PWM duty cycle") VAR(spin_up, su, float, 0, 1, "Spin up velocity") VAR(spin_down, sd, float, 0, 1, "Spin down velocity") VAR(spin_freq, sf, uint16_t, 0, 1, "Spindle PWM frequency") +// PWM spindle +VAR(pwm_invert, pi, bool, 0, 1, "Inverted spindle PWM") +VAR(enable_invert, ei, bool, 0, 1, "Inverted spindle enable") + // Huanyang spindle VAR(huanyang_id, hi, uint8_t, 0, 1, "Huanyang ID") VAR(huanyang_freq, hz, float, 0, 0, "Huanyang actual freq") diff --git a/avr/test/test.gc b/avr/test/test.gc index 7cf8315..20695cc 100644 --- a/avr/test/test.gc +++ b/avr/test/test.gc @@ -2,10 +2,15 @@ $resume $0vm=10000 $1vm=10000 +$2vm=10000 $0jm=50 $1jm=50 +$2jm=50 G0 X500Y500 G0 X0Y0 G0 X-500Y0 G0 X-500Y-500 + +G0 Z-100 +G0 Z0 diff --git a/src/resources/config-template.json b/src/resources/config-template.json index 7a78304..bceb4c0 100644 --- a/src/resources/config-template.json +++ b/src/resources/config-template.json @@ -173,17 +173,21 @@ "default": 0, "code": "sm" }, - "spin-min-pulse": { + "spin-min-duty": { "type": "float", - "unit": "ms", - "default": 20, - "code": "np" + "unit": "%", + "min": 0, + "max": 100, + "default": 1, + "code": "nd" }, - "spin-max-pulse": { + "spin-max-duty": { "type": "float", - "unit": "ms", - "default": 100, - "code": "mp" + "unit": "%", + "min": 0, + "max": 100, + "default": 99.99, + "code": "md" }, "spin-up-velocity": { "type": "float", @@ -204,8 +208,18 @@ "unit": "Hz", "min": 0, "max": 65535, - "default": 100, + "default": 1000, "code": "sf" + }, + "pwm-inverted": { + "type": "bool", + "default": "false", + "code": "pi" + }, + "enable-inverted": { + "type": "bool", + "default": "false", + "code": "ei" } },