From 8db23c8104767bd8a9ea196bf10bf426d2cb3984 Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Sat, 24 Dec 2016 01:16:11 -0800 Subject: [PATCH] Updates for v6 board, basic stall homing working --- src/axes.c | 53 ++++++++++---- src/axes.h | 35 +++++++-- src/command.def | 1 + src/config.h | 67 ++++++++--------- src/drv8711.c | 134 ++++++++++++++++++++++++---------- src/drv8711.h | 3 + src/gcode_parser.c | 1 + src/home.c | 176 +++++++++++++++++++++++++++++++++++++++++++++ src/home.h | 31 ++++++++ src/machine.c | 114 +++++------------------------ src/machine.h | 8 ++- src/main.c | 6 +- src/motor.c | 93 +++++++++++++++++------- src/motor.h | 12 +++- src/plan/arc.c | 11 ++- src/plan/exec.c | 14 +++- src/plan/exec.h | 1 + src/plan/line.c | 116 ++++++++++++++++++++++++++---- src/plan/line.h | 6 +- src/plan/planner.c | 18 +---- src/plan/planner.h | 2 +- src/probing.c | 2 +- src/stepper.c | 10 +-- src/util.c | 5 -- src/util.h | 1 - src/vars.def | 3 +- 26 files changed, 648 insertions(+), 275 deletions(-) create mode 100644 src/home.c create mode 100644 src/home.h diff --git a/src/axes.c b/src/axes.c index f11efef..3d45dff 100644 --- a/src/axes.c +++ b/src/axes.c @@ -27,15 +27,15 @@ #include "axes.h" - +#include "motor.h" #include "plan/planner.h" #include -axis_config_t axes[AXES] = { +axis_t axes[AXES] = { { - .axis_mode = X_AXIS_MODE, + .mode = X_AXIS_MODE, .velocity_max = X_VELOCITY_MAX, .feedrate_max = X_FEEDRATE_MAX, .travel_min = X_TRAVEL_MIN, @@ -47,8 +47,9 @@ axis_config_t axes[AXES] = { .latch_velocity = X_LATCH_VELOCITY, .latch_backoff = X_LATCH_BACKOFF, .zero_backoff = X_ZERO_BACKOFF, + .homing_mode = X_HOMING_MODE, }, { - .axis_mode = Y_AXIS_MODE, + .mode = Y_AXIS_MODE, .velocity_max = Y_VELOCITY_MAX, .feedrate_max = Y_FEEDRATE_MAX, .travel_min = Y_TRAVEL_MIN, @@ -60,8 +61,9 @@ axis_config_t axes[AXES] = { .latch_velocity = Y_LATCH_VELOCITY, .latch_backoff = Y_LATCH_BACKOFF, .zero_backoff = Y_ZERO_BACKOFF, + .homing_mode = Y_HOMING_MODE, }, { - .axis_mode = Z_AXIS_MODE, + .mode = Z_AXIS_MODE, .velocity_max = Z_VELOCITY_MAX, .feedrate_max = Z_FEEDRATE_MAX, .travel_min = Z_TRAVEL_MIN, @@ -73,8 +75,9 @@ axis_config_t axes[AXES] = { .latch_velocity = Z_LATCH_VELOCITY, .latch_backoff = Z_LATCH_BACKOFF, .zero_backoff = Z_ZERO_BACKOFF, + .homing_mode = Z_HOMING_MODE, }, { - .axis_mode = A_AXIS_MODE, + .mode = A_AXIS_MODE, .velocity_max = A_VELOCITY_MAX, .feedrate_max = A_FEEDRATE_MAX, .travel_min = A_TRAVEL_MIN, @@ -87,8 +90,9 @@ axis_config_t axes[AXES] = { .latch_velocity = A_LATCH_VELOCITY, .latch_backoff = A_LATCH_BACKOFF, .zero_backoff = A_ZERO_BACKOFF, + .homing_mode = A_HOMING_MODE, }, { - .axis_mode = B_AXIS_MODE, + .mode = B_AXIS_MODE, .velocity_max = B_VELOCITY_MAX, .feedrate_max = B_FEEDRATE_MAX, .travel_min = B_TRAVEL_MIN, @@ -97,7 +101,7 @@ axis_config_t axes[AXES] = { .junction_dev = B_JUNCTION_DEVIATION, .radius = B_RADIUS, }, { - .axis_mode = C_AXIS_MODE, + .mode = C_AXIS_MODE, .velocity_max = C_VELOCITY_MAX, .feedrate_max = C_FEEDRATE_MAX, .travel_min = C_TRAVEL_MIN, @@ -109,6 +113,11 @@ axis_config_t axes[AXES] = { }; +char axis_get_char(int axis) { + return (axis < 0 || AXES <= axis) ? '?' : "XYZABCUVW"[axis]; +} + + /* Jerk functions * * Jerk values can be rather large. Jerk values are stored in the system in @@ -118,21 +127,39 @@ axis_config_t axes[AXES] = { */ /// Returns axis jerk -float axes_get_jerk(uint8_t axis) {return axes[axis].jerk_max;} +float axes_get_jerk(int axis) {return axes[axis].jerk_max;} /// Sets jerk and its reciprocal for axis -void axes_set_jerk(uint8_t axis, float jerk) { +void axes_set_jerk(int axis, float jerk) { axes[axis].jerk_max = jerk; axes[axis].recip_jerk = 1 / (jerk * JERK_MULTIPLIER); } -uint8_t get_axis_mode(int axis) {return axes[axis].axis_mode;} +int axes_get_motor(int axis) { + for (int motor = 0; motor < MOTORS; motor++) + if (motor_get_axis(motor) == axis) return motor; + return -1; +} + + +float axes_get_vector_length(const float a[], const float b[]) { + return sqrt(square(a[AXIS_X] - b[AXIS_X]) + square(a[AXIS_Y] - b[AXIS_Y]) + + square(a[AXIS_Z] - b[AXIS_Z]) + square(a[AXIS_A] - b[AXIS_A]) + + square(a[AXIS_B] - b[AXIS_B]) + square(a[AXIS_C] - b[AXIS_C])); +} + + +bool axes_get_homed(int axis) {return axes[axis].homed;} +void axes_set_homed(int axis, bool homed) {axes[axis].homed = homed;} + + +int get_axis_mode(int axis) {return axes[axis].mode;} -void set_axis_mode(int axis, uint8_t value) { - if (value <= AXIS_RADIUS) axes[axis].axis_mode = value; +void set_axis_mode(int axis, int value) { + if (value <= AXIS_RADIUS) axes[axis].mode = value; } diff --git a/src/axes.h b/src/axes.h index 2f3d728..cdbb130 100644 --- a/src/axes.h +++ b/src/axes.h @@ -30,6 +30,15 @@ #include "config.h" +#include + + +enum { + AXIS_X, AXIS_Y, AXIS_Z, + AXIS_A, AXIS_B, AXIS_C, + AXIS_U, AXIS_V, AXIS_W // reserved +}; + typedef enum { AXIS_DISABLED, // disabled axis @@ -38,8 +47,17 @@ typedef enum { } axis_mode_t; +typedef enum { + HOMING_DISABLED, + HOMING_STALL_MIN, + HOMING_STALL_MAX, + HOMING_SWITCH_MIN, + HOMING_SWITCH_MAX, +} homing_mode_t; + + typedef struct { - axis_mode_t axis_mode; + axis_mode_t mode; float feedrate_max; // max velocity in mm/min or deg/min float velocity_max; // max velocity in mm/min or deg/min float travel_max; // max work envelope for soft limits @@ -53,10 +71,17 @@ typedef struct { float latch_velocity; // homing latch velocity float latch_backoff; // backoff from switches prior to homing latch movement float zero_backoff; // backoff from switches for machine zero -} axis_config_t; + homing_mode_t homing_mode; + bool homed; +} axis_t; -extern axis_config_t axes[AXES]; +extern axis_t axes[AXES]; -float axes_get_jerk(uint8_t axis); -void axes_set_jerk(uint8_t axis, float jerk); +char axis_get_char(int axis); +float axes_get_jerk(int axis); +void axes_set_jerk(int axis, float jerk); +int axes_get_motor(int axis); +float axes_get_vector_length(const float a[], const float b[]); +bool axes_get_homed(int axis); +void axes_set_homed(int axis, bool homed); diff --git a/src/command.def b/src/command.def index 2198f44..559e45d 100644 --- a/src/command.def +++ b/src/command.def @@ -38,3 +38,4 @@ CMD(mreset, 0, 1, "Reset motor") CMD(calibrate, 0, 0, "Calibrate motors") CMD(messages, 0, 0, "Dump all possible status messages") CMD(resume, 0, 0, "Resume processing after a flush") +CMD(home, 0, 0, "Home all axes") diff --git a/src/config.h b/src/config.h index 868d506..a1feda3 100644 --- a/src/config.h +++ b/src/config.h @@ -37,10 +37,10 @@ // Pins enum { - RESERVED_0_PIN = PORT_A << 3, - ENABLE_Y_PIN, - ENABLE_Z_PIN, - ENABLE_A_PIN, + STALL_X_PIN = PORT_A << 3, + STALL_Y_PIN, + STALL_Z_PIN, + STALL_A_PIN, SPIN_DIR_PIN, SPIN_ENABLE_PIN, ANALOG_PIN, @@ -65,7 +65,7 @@ enum { SPI_MOSI_PIN, STEP_X_PIN = PORT_D << 3, - RESERVED_1_PIN, + SPI_CS_X_PIN, SPI_CS_A_PIN, SPI_CS_Z_PIN, SPIN_PWM_PIN, @@ -76,20 +76,20 @@ enum { STEP_Y_PIN = PORT_E << 3, SPI_CS_Y_PIN, DIR_X_PIN, - SPI_CS_X_PIN, + DIR_Y_PIN, STEP_A_PIN, SWITCH_2_PIN, - ENABLE_X_PIN, - FAULT_X_PIN, + DIR_Z_PIN, + DIR_A_PIN, STEP_Z_PIN = PORT_F << 3, RS485_RW_PIN, FAULT_PIN, ESTOP_PIN, - RESERVED_2_PIN, - FAULT_Y_PIN, - FAULT_Z_PIN, - FAULT_A_PIN, + MOTOR_FAULT_PIN, + MOTOR_ENABLE_PIN, + NC_0_PIN, + NC_1_PIN, }; #define SPI_SS_PIN SERIAL_CTS_PIN // Needed for SPI configuration @@ -107,19 +107,12 @@ enum { #define PWMS 2 // number of supported PWM channels -// Axes -typedef enum { - AXIS_X, AXIS_Y, AXIS_Z, - AXIS_A, AXIS_B, AXIS_C, - AXIS_U, AXIS_V, AXIS_W // reserved -} axis_t; - - // Motor settings. See motor.c -#define MOTOR_CURRENT 0.3 // 1.0 is full power -#define MOTOR_IDLE_CURRENT 0.01 // 1.0 is full power -#define MOTOR_STALL_THRESHOLD 0 // 0 -> 1 is least -> most sensitive -#define MOTOR_MICROSTEPS 16 +#define MOTOR_MAX_CURRENT 1.0 // 1.0 is full power +#define MOTOR_MIN_CURRENT 0.15 // 1.0 is full power +#define MOTOR_IDLE_CURRENT 0.05 // 1.0 is full power +#define MOTOR_STALL_THRESHOLD 0 // 0 -> 1 is least -> most sensitive +#define MOTOR_MICROSTEPS 32 #define MOTOR_POWER_MODE MOTOR_POWERED_ONLY_WHEN_MOVING #define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time @@ -159,7 +152,7 @@ typedef enum { // Machine settings -#define STEP_CORRECTION // Enable step correction +//#define STEP_CORRECTION // Enable step correction #define MAX_STEP_CORRECTION 4 // In steps per segment #define CHORDAL_TOLERANCE 0.01 // chordal accuracy for arcs #define JERK_MAX 50 // yes, that's km/min^3 @@ -170,34 +163,36 @@ typedef enum { #define CAL_ACCELERATION 500000 // mm/min^2 // Axis settings -#define VELOCITY_MAX 13000 // mm/min +#define VELOCITY_MAX 10000 // mm/min #define FEEDRATE_MAX VELOCITY_MAX #define X_AXIS_MODE AXIS_STANDARD // See machine.h #define X_VELOCITY_MAX VELOCITY_MAX // G0 max velocity in mm/min #define X_FEEDRATE_MAX FEEDRATE_MAX // G1 max feed rate in mm/min #define X_TRAVEL_MIN 0 // minimum travel for soft limits -#define X_TRAVEL_MAX 150 // between switches or crashes +#define X_TRAVEL_MAX 300 // between switches or crashes #define X_JERK_MAX JERK_MAX #define X_JERK_HOMING (X_JERK_MAX * 2) #define X_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define X_SEARCH_VELOCITY 500 // move in negative direction +#define X_SEARCH_VELOCITY 3000 // move in negative direction #define X_LATCH_VELOCITY 100 // mm/min #define X_LATCH_BACKOFF 5 // mm #define X_ZERO_BACKOFF 1 // mm +#define X_HOMING_MODE HOMING_STALL_MIN #define Y_AXIS_MODE AXIS_STANDARD #define Y_VELOCITY_MAX VELOCITY_MAX #define Y_FEEDRATE_MAX FEEDRATE_MAX #define Y_TRAVEL_MIN 0 -#define Y_TRAVEL_MAX 150 +#define Y_TRAVEL_MAX 300 #define Y_JERK_MAX JERK_MAX #define Y_JERK_HOMING (Y_JERK_MAX * 2) #define Y_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Y_SEARCH_VELOCITY 500 +#define Y_SEARCH_VELOCITY 3000 #define Y_LATCH_VELOCITY 100 #define Y_LATCH_BACKOFF 5 #define Y_ZERO_BACKOFF 1 +#define Y_HOMING_MODE HOMING_STALL_MIN #define Z_AXIS_MODE AXIS_STANDARD #define Z_VELOCITY_MAX 2000 // VELOCITY_MAX @@ -207,10 +202,11 @@ typedef enum { #define Z_JERK_MAX JERK_MAX #define Z_JERK_HOMING (Z_JERK_MAX * 2) #define Z_JUNCTION_DEVIATION JUNCTION_DEVIATION -#define Z_SEARCH_VELOCITY 400 +#define Z_SEARCH_VELOCITY 800 #define Z_LATCH_VELOCITY 100 #define Z_LATCH_BACKOFF 5 #define Z_ZERO_BACKOFF 1 +#define Z_HOMING_MODE HOMING_DISABLED //STALL_MAX // A values are chosen to make the A motor react the same as X for testing // set to the same speed as X axis @@ -227,6 +223,7 @@ typedef enum { #define A_LATCH_VELOCITY 100 #define A_LATCH_BACKOFF 5 #define A_ZERO_BACKOFF 2 +#define A_HOMING_MODE HOMING_DISABLED #define B_AXIS_MODE AXIS_DISABLED #define B_VELOCITY_MAX 3600 @@ -276,11 +273,9 @@ typedef enum { #define GCODE_DEFAULT_ARC_DISTANCE_MODE INCREMENTAL_MODE -// 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 +// Motor ISRs +#define STALL_ISR_vect PORTA_INT1_vect +#define FAULT_ISR_vect PORTF_INT1_vect /* Interrupt usage: diff --git a/src/drv8711.c b/src/drv8711.c index 30fea04..f4b8235 100644 --- a/src/drv8711.c +++ b/src/drv8711.c @@ -37,14 +37,17 @@ #include -#define DRIVERS 1 -#define COMMANDS 4 +#define DRIVERS MOTORS +#define COMMANDS 10 #define DRV8711_WORD_BYTE_PTR(WORD, LOW) \ (((uint8_t *)&(WORD)) + ((LOW) ? 0 : 1)) +bool motor_fault = false; + + typedef struct { uint8_t status; @@ -52,19 +55,21 @@ typedef struct { float idle_current; float drive_current; float stall_threshold; + float power; uint8_t mode; // microstepping mode + stall_callback_t stall_cb; uint8_t cs_pin; - uint8_t fault_pin; + uint8_t stall_pin; } drv8711_driver_t; -static drv8711_driver_t drivers[MOTORS] = { - {.cs_pin = SPI_CS_X_PIN, .fault_pin = FAULT_X_PIN}, - {.cs_pin = SPI_CS_Y_PIN, .fault_pin = FAULT_Y_PIN}, - {.cs_pin = SPI_CS_Z_PIN, .fault_pin = FAULT_Z_PIN}, - {.cs_pin = SPI_CS_A_PIN, .fault_pin = FAULT_A_PIN}, +static drv8711_driver_t drivers[DRIVERS] = { + {.cs_pin = SPI_CS_X_PIN, .stall_pin = STALL_X_PIN}, + {.cs_pin = SPI_CS_Y_PIN, .stall_pin = STALL_Y_PIN}, + {.cs_pin = SPI_CS_Z_PIN, .stall_pin = STALL_Z_PIN}, + {.cs_pin = SPI_CS_A_PIN, .stall_pin = STALL_A_PIN}, }; @@ -98,13 +103,21 @@ static void _driver_check_status(int driver) { 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); + //if (mflags) motor_error_callback(driver, mflags); TODO } static float _driver_get_current(int driver) { drv8711_driver_t *drv = &drivers[driver]; + +#if 1 + if (!drv->active) return drv->idle_current; + return + MOTOR_MIN_CURRENT + (drv->drive_current - MOTOR_MIN_CURRENT) * drv->power; + +#else return drv->active ? drv->drive_current : drv->idle_current; +#endif } @@ -115,12 +128,8 @@ static uint8_t _spi_next_command(uint8_t cmd) { if (DRV8711_CMD_IS_READ(command) && DRV8711_CMD_ADDR(command) == DRV8711_STATUS_REG) { - uint8_t status = spi.responses[driver]; - - if (status != drivers[driver].status) { - drivers[driver].status = status; - _driver_check_status(driver); - } + drivers[driver].status = spi.responses[driver]; + _driver_check_status(driver); } } @@ -137,6 +146,11 @@ static uint8_t _spi_next_command(uint8_t cmd) { uint16_t *command = &spi.commands[driver][cmd]; switch (DRV8711_CMD_ADDR(*command)) { + case DRV8711_STATUS_REG: + if (!DRV8711_CMD_IS_READ(*command)) + *command = (*command & 0xf000) | (0x0fff & ~(drivers[driver].status)); + break; + case DRV8711_TORQUE_REG: // Update motor current setting *command = (*command & 0xff00) | (uint8_t)round(0xff * _driver_get_current(driver)); @@ -207,19 +221,41 @@ static void _init_spi_commands() { uint16_t *commands = spi.commands[driver]; spi.ncmds = 0; - // Enable motor + // 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_CTRL_REG, DRV8711_CTRL_ENBL_bm | - DRV8711_CTRL_EXSTALL_bm); + DRV8711_WRITE(DRV8711_DECAY_REG, DRV8711_DECAY_DECMOD_MIXED | 6); + + // Set STALL + commands[spi.ncmds++] = + DRV8711_WRITE(DRV8711_STALL_REG, + DRV8711_STALL_SDCNT_2 | DRV8711_STALL_VDIV_16 | 133); + + // 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 current + // Set CTRL enable motor & set ISENSE gain commands[spi.ncmds++] = - DRV8711_WRITE(DRV8711_TORQUE_REG, DRV8711_TORQUE_SMPLTH_100); + DRV8711_WRITE(DRV8711_CTRL_REG, DRV8711_CTRL_ENBL_bm | + DRV8711_CTRL_ISGAIN_5 | DRV8711_CTRL_DTIME_850); - // Read status + // Read STATUS commands[spi.ncmds++] = DRV8711_READ(DRV8711_STATUS_REG); - // Clear status + // Clear STATUS commands[spi.ncmds++] = DRV8711_WRITE(DRV8711_STATUS_REG, 0); } @@ -232,25 +268,25 @@ static void _init_spi_commands() { } -ISR(SPIC_INT_vect) { - _spi_send(); -} +ISR(SPIC_INT_vect) {_spi_send();} -void _fault_isr(int motor) {} +ISR(STALL_ISR_vect) { + for (int i = 0; i < DRIVERS; i++) { + drv8711_driver_t *driver = &drivers[i]; + if (driver->stall_cb) driver->stall_cb(i); + } +} -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);} +ISR(FAULT_ISR_vect) {motor_fault = !IN_PIN(MOTOR_FAULT_PIN);} // TODO void drv8711_init() { - // Configure motors - for (int i = 0; i < MOTORS; i++) { + // Configure drivers + for (int i = 0; i < DRIVERS; i++) { drivers[i].idle_current = MOTOR_IDLE_CURRENT; - drivers[i].drive_current = MOTOR_CURRENT; + drivers[i].drive_current = MOTOR_MAX_CURRENT; drivers[i].stall_threshold = MOTOR_STALL_THRESHOLD; drv8711_disable(i); @@ -268,18 +304,24 @@ void drv8711_init() { for (int i = 0; i < DRIVERS; i++) { uint8_t cs_pin = drivers[i].cs_pin; - uint8_t fault_pin = drivers[i].fault_pin; + uint8_t stall_pin = drivers[i].stall_pin; OUTSET_PIN(cs_pin); // High DIRSET_PIN(cs_pin); // Output - OUTCLR_PIN(fault_pin); // Input + DIRCLR_PIN(stall_pin); // Input - // Fault interrupt - //PINCTRL_PIN(fault_pin) = PORT_ISC_RISING_gc; - //PORT(fault_pin)->INT1MASK = BM(fault_pin); // INT1 - //PORT(fault_pin)->INTCTRL |= PORT_INT1LVL_HI_gc; + // Stall interrupt + PINCTRL_PIN(stall_pin) = PORT_ISC_FALLING_gc; + PORT(stall_pin)->INT1MASK |= BM(stall_pin); + PORT(stall_pin)->INTCTRL |= PORT_INT1LVL_HI_gc; } + // Fault interrupt + DIRCLR_PIN(MOTOR_FAULT_PIN); + PINCTRL_PIN(MOTOR_FAULT_PIN) = PORT_ISC_RISING_gc; + PORT(MOTOR_FAULT_PIN)->INT1MASK |= BM(MOTOR_FAULT_PIN); + PORT(MOTOR_FAULT_PIN)->INTCTRL |= PORT_INT1LVL_HI_gc; + // Configure SPI PR.PRPC &= ~PR_SPI_bm; // Disable power reduction SPIC.CTRL = SPI_ENABLE_bm | SPI_MASTER_bm | SPI_MODE_0_gc | @@ -303,6 +345,12 @@ void drv8711_disable(int driver) { } +void drv8711_set_power(int driver, float power) { + if (driver < 0 || DRIVERS <= driver) return; + drivers[driver].power = power < 0 ? 0 : (1 < power ? 1 : power); +} + + void drv8711_set_microsteps(int driver, uint16_t msteps) { if (driver < 0 || DRIVERS <= driver) return; switch (msteps) { @@ -315,6 +363,11 @@ void drv8711_set_microsteps(int driver, uint16_t msteps) { } +void drv8711_set_stall_callback(int driver, stall_callback_t cb) { + drivers[driver].stall_cb = cb; +} + + float get_drive_power(int driver) { if (driver < 0 || DRIVERS <= driver) return 0; return drivers[driver].drive_current; @@ -343,3 +396,6 @@ float get_current_power(int driver) { if (driver < 0 || DRIVERS <= driver) return 0; return _driver_get_current(driver); } + + +bool get_motor_fault() {return motor_fault;} diff --git a/src/drv8711.h b/src/drv8711.h index 4e06cab..187c88a 100644 --- a/src/drv8711.h +++ b/src/drv8711.h @@ -30,6 +30,7 @@ #include "config.h" #include "status.h" +#include "motor.h" #include #include @@ -163,4 +164,6 @@ enum { void drv8711_init(); void drv8711_enable(int driver); void drv8711_disable(int driver); +void drv8711_set_power(int driver, float power); void drv8711_set_microsteps(int driver, uint16_t msteps); +void drv8711_set_stall_callback(int driver, stall_callback_t cb); diff --git a/src/gcode_parser.c b/src/gcode_parser.c index 1bc0a6a..e6095ea 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -32,6 +32,7 @@ #include "plan/arc.h" #include "probing.h" #include "homing.h" +#include "axes.h" #include "util.h" #include diff --git a/src/home.c b/src/home.c new file mode 100644 index 0000000..99fed3f --- /dev/null +++ b/src/home.c @@ -0,0 +1,176 @@ +/******************************************************************************\ + + 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 "home.h" + +#include "plan/runtime.h" +#include "plan/line.h" +#include "plan/state.h" +#include "plan/exec.h" +#include "axes.h" +#include "motor.h" +#include "util.h" +#include "config.h" + +#include +#include + + +typedef enum { + STATE_INIT, + STATE_HOMING, + STATE_STALLED, + STATE_BACKOFF, + STATE_DONE, +} home_state_t; + + +typedef struct { + bool homed[AXES]; + unsigned axis; + home_state_t state; + float velocity; + uint16_t microsteps; +} home_t; + +static home_t home; + + +void _stall_callback(int motor) { + if (home.velocity == mp_runtime_get_velocity()) { + mp_exec_abort(); + home.state = STATE_STALLED; + } +} + + +void _move_axis(float travel) { + float target[AXES]; + float *position = mp_runtime_get_position(); + copy_vector(target, position); + target[home.axis] += travel; + mp_aline(target, false, false, false, home.velocity, 1, -1); +} + + +void home_callback() { + if (mp_get_cycle() != CYCLE_HOMING || !mp_is_quiescent() || + !mp_queue_is_empty()) return; + + while (true) { + axis_t *axis = &axes[home.axis]; + int motor = axes_get_motor(home.axis); + int direction = (axis->homing_mode == HOMING_STALL_MIN || + axis->homing_mode == HOMING_SWITCH_MIN) ? -1 : 1; + + switch (home.state) { + case STATE_INIT: { + if (motor == -1 || axis->mode == AXIS_DISABLED || + axis->homing_mode == HOMING_DISABLED || !motor_is_enabled(motor)) { + home.state = STATE_DONE; + break; + } + + STATUS_INFO("Homing %c axis", axis_get_char(home.axis)); + + // Set axis not homed + home.homed[home.axis] = false; + + // Determine homing type: switch or stall + + // Configure driver, set stall callback and compute homing velocity + home.microsteps = motor_get_microsteps(motor); + motor_set_microsteps(motor, 8); + motor_set_stall_callback(motor, _stall_callback); + //home.velocity = motor_get_stall_homing_velocity(motor); + home.velocity = axis->search_velocity; + + // Move in home direction + float travel = axis->travel_max - axis->travel_min; + _move_axis(travel * direction); + + home.state = STATE_HOMING; + return; + } + + case STATE_HOMING: + case STATE_STALLED: + // Restore motor driver config + motor_set_microsteps(motor, home.microsteps); + motor_set_stall_callback(motor, 0); + + if (home.state == STATE_HOMING) { + STATUS_ERROR(STAT_HOMING_CYCLE_FAILED, "Failed to find %c axis home", + axis_get_char(home.axis)); + mp_set_cycle(CYCLE_MACHINING); + + } else { + STATUS_INFO("Backing off %c axis", axis_get_char(home.axis)); + mach_set_axis_position(home.axis, direction < 0 ? axis->travel_min : + axis->travel_max); + _move_axis(axis->zero_backoff * direction * -1); + home.state = STATE_BACKOFF; + } + return; + + case STATE_BACKOFF: + STATUS_INFO("Homed %c axis", axis_get_char(home.axis)); + + // Set axis position & homed + mach_set_axis_position(home.axis, axis->travel_min); + home.homed[home.axis] = true; + home.state = STATE_DONE; + break; + + case STATE_DONE: + if (home.axis == AXIS_X) { + // Done + mp_set_cycle(CYCLE_MACHINING); + return; + } + + // Next axis + home.axis--; + home.state = STATE_INIT; + break; + } + } +} + + +uint8_t command_home(int argc, char *argv[]) { + if (mp_get_cycle() != CYCLE_MACHINING || mp_get_state() != STATE_READY) + return 0; + + // Init + memset(&home, 0, sizeof(home)); + home.axis = AXIS_Z; + + mp_set_cycle(CYCLE_HOMING); + + return 0; +} diff --git a/src/home.h b/src/home.h new file mode 100644 index 0000000..dd6f286 --- /dev/null +++ b/src/home.h @@ -0,0 +1,31 @@ +/******************************************************************************\ + + 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 + +void home_init(); +void home_callback(); diff --git a/src/machine.c b/src/machine.c index e295827..75e0d75 100644 --- a/src/machine.c +++ b/src/machine.c @@ -115,6 +115,11 @@ float mach_get_feed_rate() {return mach.gm.feed_rate;} feed_mode_t mach_get_feed_mode() {return mach.gm.feed_mode;} +bool mach_is_inverse_time_mode() { + return mach.gm.feed_mode == INVERSE_TIME_MODE; +} + + float mach_get_feed_override() { return mach.gm.feed_override_enable ? mach.gm.feed_override : 1; } @@ -126,11 +131,13 @@ float mach_get_spindle_override() { motion_mode_t mach_get_motion_mode() {return mach.gm.motion_mode;} +bool mach_is_rapid() {return mach.gm.motion_mode == MOTION_MODE_RAPID;} plane_t mach_get_plane() {return mach.gm.plane;} units_t mach_get_units() {return mach.gm.units;} coord_system_t mach_get_coord_system() {return mach.gm.coord_system;} bool mach_get_absolute_mode() {return mach.gm.absolute_mode;} path_mode_t mach_get_path_mode() {return mach.gm.path_mode;} +bool mach_is_exact_stop() {return mach.gm.path_mode == PATH_EXACT_STOP;} distance_mode_t mach_get_distance_mode() {return mach.gm.distance_mode;} distance_mode_t mach_get_arc_distance_mode() {return mach.gm.arc_distance_mode;} @@ -280,102 +287,6 @@ float mach_get_axis_position(uint8_t axis) {return mach.position[axis];} * These functions are not part of the NIST defined functions */ -/* Compute optimal and minimum move times into the gcode_state - * - * "Minimum time" is the fastest the move can be performed given the velocity - * constraints on each participating axis - regardless of the feed rate - * requested. The minimum time is the time limited by the rate-limiting - * axis. The minimum time is needed to compute the optimal time and is recorded - * for possible feed override computation. - * - * "Optimal time" is either the time resulting from the requested feed rate or - * the minimum time if the requested feed rate is not achievable. Optimal times - * for rapids are always the minimum time. - * - * The gcode state must have targets set prior by having mach_set_target(). Axis - * modes are taken into account by this. - * - * The following times are compared and the longest is returned: - * - G93 inverse time (if G93 is active) - * - time for coordinated move at requested feed rate - * - time that the slowest axis would require for the move - * - * Sets the following variables in the gcode_state struct - move_time is set to - * optimal time - * - * NIST RS274NGC_v3 Guidance - * - * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for - * moves that combine both linear and rotational movement, the feed rate should - * apply to the XYZ movement, with the rotational axis (or axes) timed to start - * and end at the same time the linear move is performed. It is possible under - * this case for the rotational move to rate-limit the linear move. - * - * 2.1.2.5 Feed Rate - * - * The rate at which the controlled point or the axes move is nominally a steady - * rate which may be set by the user. In the Interpreter, the interpretation of - * the feed rate is as follows unless inverse time feed rate mode is being used - * in the RS274/NGC view (see Section 3.5.19). The machining functions view of - * feed rate, as described in Section 4.3.5.1, has conditions under which the - * set feed rate is applied differently, but none of these is used in the - * Interpreter. - * - * A. For motion involving one or more of the X, Y, and Z axes (with or without - * simultaneous rotational axis motion), the feed rate means length units - * per minute along the programmed XYZ path, as if the rotational axes were - * not moving. - * - * B. For motion of one rotational axis with X, Y, and Z axes not moving, the - * feed rate means degrees per minute rotation of the rotational axis. - * - * C. For motion of two or three rotational axes with X, Y, and Z axes not - * moving, the rate is applied as follows. Let dA, dB, and dC be the angles - * in degrees through which the A, B, and C axes, respectively, must move. - * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total - * angular motion, using the usual Euclidean metric. Let T be the amount of - * time required to move through D degrees at the current feed rate in - * degrees per minute. The rotational axes should be moved in coordinated - * linear motion so that the elapsed time from the start to the end of the - * motion is T plus any time required for acceleration or deceleration. - */ -float mach_calc_move_time(const float axis_length[], - const float axis_square[]) { - float max_time = 0; - - // Compute times for feed motion - if (mach.gm.motion_mode != MOTION_MODE_RAPID) { - if (mach.gm.feed_mode == INVERSE_TIME_MODE) - // Feed rate was un-inverted to minutes by mach_set_feed_rate() - max_time = mach.gm.feed_rate; - - else { - // Compute length of linear move in millimeters. Feed rate in mm/min. - max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + - axis_square[AXIS_Z]) / mach.gm.feed_rate; - - // If no linear axes, compute length of multi-axis rotary move in degrees. - // Feed rate is provided as degrees/min - if (fp_ZERO(max_time)) - max_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + - axis_square[AXIS_C]) / mach.gm.feed_rate; - } - } - - // Apply feed override - max_time /= mach_get_feed_override(); - - // Compute time required for rate-limiting axis - for (int axis = 0; axis < AXES; axis++) { - float time = fabs(axis_length[axis]) / - (mach.gm.motion_mode == MOTION_MODE_RAPID ? axes[axis].velocity_max : - axes[axis].feedrate_max); - - if (max_time < time) max_time = time; - } - - return max_time < MIN_SEGMENT_TIME ? MIN_SEGMENT_TIME : max_time; -} /*** Calculate target vector @@ -404,7 +315,7 @@ void mach_calc_target(float target[], const float values[], const float offset = mach.gm.distance_mode == ABSOLUTE_MODE ? mach_get_active_coord_offset(axis) : mach.position[axis]; - switch (axes[axis].axis_mode) { + switch (axes[axis].mode) { case AXIS_DISABLED: break; case AXIS_STANDARD: // For ABC axes no mm conversion - it's already in degrees @@ -654,6 +565,13 @@ void mach_resume_origin_offsets() { } +stat_t mach_plan_line(float target[]) { + return mp_aline(target, mach_is_rapid(), mach_is_inverse_time_mode(), + mach_is_exact_stop(), mach.gm.feed_rate, + mach_get_feed_override(), mach_get_line()); +} + + // Free Space Motion (4.3.4) static stat_t _feed(float values[], bool flags[]) { float target[AXES]; @@ -665,7 +583,7 @@ static stat_t _feed(float values[], bool flags[]) { // prep and plan the move mach_update_work_offsets(); // update resolved offsets - status = mp_aline(target, mach_get_line()); // send move to planner + mach_plan_line(target); copy_vector(mach.position, target); // update model position return status; diff --git a/src/machine.h b/src/machine.h index 2151ddb..c6f5497 100644 --- a/src/machine.h +++ b/src/machine.h @@ -39,15 +39,18 @@ // Model state getters and setters uint32_t mach_get_line(); float mach_get_feed_rate(); +bool mach_is_inverse_time_mode(); feed_mode_t mach_get_feed_mode(); float mach_get_feed_override(); float mach_get_spindle_override(); motion_mode_t mach_get_motion_mode(); +bool mach_is_rapid(); plane_t mach_get_plane(); units_t mach_get_units(); coord_system_t mach_get_coord_system(); bool mach_get_absolute_mode(); path_mode_t mach_get_path_mode(); +bool mach_is_exact_stop(); distance_mode_t mach_get_distance_mode(); distance_mode_t mach_get_arc_distance_mode(); @@ -65,9 +68,7 @@ void mach_set_position(const float position[]); float mach_get_axis_position(uint8_t axis); // Critical helpers -float mach_calc_move_time(const float axis_length[], const float axis_square[]); -void mach_calc_target(float target[], const float values[], - const bool flags[]); +void mach_calc_target(float target[], const float values[], const bool flags[]); stat_t mach_test_soft_limits(float target[]); // machining functions defined by NIST [organized by NIST Gcode doc] @@ -96,6 +97,7 @@ void mach_suspend_origin_offsets(); void mach_resume_origin_offsets(); // Free Space Motion (4.3.4) +stat_t mach_plan_line(float target[]); stat_t mach_rapid(float target[], bool flags[]); void mach_set_g28_position(); stat_t mach_goto_g28_position(float target[], bool flags[]); diff --git a/src/main.c b/src/main.c index 4b73e13..973ebbd 100644 --- a/src/main.c +++ b/src/main.c @@ -41,6 +41,7 @@ #include "estop.h" #include "probing.h" #include "homing.h" +#include "home.h" #include "i2c.h" #include "plan/planner.h" @@ -55,7 +56,7 @@ int main() { - //wdt_enable(WDTO_250MS); + //wdt_enable(WDTO_250MS); TODO // Init cli(); // disable interrupts @@ -88,7 +89,8 @@ int main() { if (!estop_triggered()) { mp_state_callback(); mach_arc_callback(); // arc generation runs - mach_homing_callback(); // G28.2 continuation + home_callback(); + //mach_homing_callback(); // G28.2 continuation mach_probe_callback(); // G38.2 continuation } command_callback(); // process next command diff --git a/src/motor.c b/src/motor.c index bbbace8..73f76f6 100644 --- a/src/motor.c +++ b/src/motor.c @@ -35,6 +35,7 @@ #include "drv8711.h" #include "estop.h" #include "gcode_state.h" +#include "axes.h" #include "util.h" #include "plan/runtime.h" @@ -66,7 +67,6 @@ typedef struct { float travel_rev; // mm or deg of travel per motor revolution uint8_t step_pin; uint8_t dir_pin; - uint8_t enable_pin; TC0_t *timer; DMA_CH_t *dma; uint8_t dma_trigger; @@ -91,6 +91,7 @@ typedef struct { direction_t direction; // travel direction corrected for polarity int32_t position; bool round_up; // toggle rounding direction + float power; } motor_t; @@ -104,7 +105,6 @@ static motor_t motors[MOTORS] = { .power_mode = M1_POWER_MODE, .step_pin = STEP_X_PIN, .dir_pin = DIR_X_PIN, - .enable_pin = ENABLE_X_PIN, .timer = &M1_TIMER, .dma = &M1_DMA_CH, .dma_trigger = M1_DMA_TRIGGER, @@ -116,8 +116,7 @@ static motor_t motors[MOTORS] = { .polarity = M2_POLARITY, .power_mode = M2_POWER_MODE, .step_pin = STEP_Y_PIN, - .dir_pin = RESERVED_2_PIN, // TODO - .enable_pin = ENABLE_Y_PIN, + .dir_pin = DIR_Y_PIN, .timer = &M2_TIMER, .dma = &M2_DMA_CH, .dma_trigger = M2_DMA_TRIGGER, @@ -129,8 +128,7 @@ static motor_t motors[MOTORS] = { .polarity = M3_POLARITY, .power_mode = M3_POWER_MODE, .step_pin = STEP_Z_PIN, - .dir_pin = RESERVED_2_PIN, // TODO - .enable_pin = ENABLE_Z_PIN, + .dir_pin = DIR_Z_PIN, .timer = &M3_TIMER, .dma = &M3_DMA_CH, .dma_trigger = M3_DMA_TRIGGER, @@ -142,8 +140,7 @@ static motor_t motors[MOTORS] = { .polarity = M4_POLARITY, .power_mode = M4_POWER_MODE, .step_pin = STEP_A_PIN, - .dir_pin = RESERVED_2_PIN, // TODO - .enable_pin = ENABLE_A_PIN, + .dir_pin = DIR_A_PIN, .timer = (TC0_t *)&M4_TIMER, .dma = &M4_DMA_CH, .dma_trigger = M4_DMA_TRIGGER, @@ -160,14 +157,16 @@ void motor_init() { 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 + for (int motor = 0; motor < MOTORS; motor++) { motor_t *m = &motors[motor]; // IO pins DIRSET_PIN(m->step_pin); // Output DIRSET_PIN(m->dir_pin); // Output - OUTCLR_PIN(m->enable_pin); // Low (disabled) - DIRSET_PIN(m->enable_pin); // Output // Setup motor timer m->timer->CTRLB = TC_WGMODE_FRQ_gc | TC1_CCAEN_bm; @@ -197,17 +196,38 @@ void motor_init() { void motor_enable(int motor, bool enable) { - if (enable) OUTSET_PIN(motors[motor].enable_pin); // Active high + if (enable) OUTSET_PIN(MOTOR_ENABLE_PIN); // Active high else { - OUTCLR_PIN(motors[motor].enable_pin); + OUTCLR_PIN(MOTOR_ENABLE_PIN); motors[motor].power_state = MOTOR_IDLE; } } +bool motor_is_enabled(int motor) { + return motors[motor].power_mode != MOTOR_DISABLED; +} + + int motor_get_axis(int motor) {return motors[motor].axis;} +void motor_set_stall_callback(int motor, stall_callback_t cb) { + drv8711_set_stall_callback(motor, cb); +} + + +/// @return computed homing velocity +float motor_get_stall_homing_velocity(int motor) { + // Compute velocity: + // velocity = travel_rev * step_angle * 60 / (SMPLTH * mstep * 360 * 2) + // SMPLTH = 50us = 0.00005s + // mstep = 8 + return motors[motor].travel_rev * motors[motor].step_angle * 1667 / + motors[motor].microsteps; +} + + float motor_get_steps_per_unit(int motor) { return 360 * motors[motor].microsteps / motors[motor].travel_rev / motors[motor].step_angle; @@ -220,7 +240,27 @@ float motor_get_units_per_step(int motor) { } -int motor_get_microsteps(int motor) {return motors[motor].microsteps;} +float _get_max_velocity(int motor) { + return + axes[motors[motor].axis].velocity_max * motor_get_steps_per_unit(motor); +} + + +uint16_t motor_get_microsteps(int motor) {return motors[motor].microsteps;} + + +void motor_set_microsteps(int motor, uint16_t microsteps) { + switch (microsteps) { + case 1: case 2: case 4: case 8: case 16: case 32: case 64: case 128: case 256: + break; + default: return; + } + + motors[motor].microsteps = microsteps; + drv8711_set_microsteps(motor, microsteps); +} + + int32_t motor_get_encoder(int motor) {return motors[motor].encoder;} @@ -279,6 +319,8 @@ static void _energize(int motor) { if (motors[motor].power_state == MOTOR_IDLE && !motor_error(motor)) { motors[motor].power_state = MOTOR_ENERGIZING; drv8711_enable(motor); + + motor_driver_callback(motor); // TODO Shouldn't call this directly } // Reset timeout, regardless @@ -325,7 +367,7 @@ void motor_error_callback(int motor, motor_flags_t errors) { if (m->power_state != MOTOR_ACTIVE) return; - m->flags |= errors; + m->flags |= errors & MOTOR_FLAG_ERROR_bm; report_request(); if (motor_error(motor)) { @@ -381,6 +423,9 @@ void motor_load_move(int motor) { // Compute error if (!m->reading) m->error = m->commanded - m->encoder; m->commanded = m->position; + + // Set power + drv8711_set_power(motor, m->power); } @@ -392,7 +437,8 @@ void motor_end_move(int motor) { } -stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error) { +stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error, + float time) { motor_t *m = &motors[motor]; // Validate input @@ -454,6 +500,9 @@ stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error) { default: break; // Shouldn't get here } + // Compute power from axis velocity + m->power = travel / (_get_max_velocity(motor) * time); + return STAT_OK; } @@ -467,14 +516,8 @@ 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; - drv8711_set_microsteps(motor, value); + if (motor < 0 || MOTORS <= motor) return; + motor_set_microsteps(motor, value); } @@ -485,10 +528,10 @@ uint8_t get_polarity(int motor) { void set_polarity(int motor, uint8_t value) {motors[motor].polarity = value;} -uint8_t get_motor_map(int motor) {return motors[motor].axis;} +uint8_t get_motor_axis(int motor) {return motors[motor].axis;} -void set_motor_map(int motor, uint16_t value) { +void set_motor_axis(int motor, uint16_t value) { if (value < AXES) motors[motor].axis = value; } diff --git a/src/motor.h b/src/motor.h index d5f69cd..2d2b2de 100644 --- a/src/motor.h +++ b/src/motor.h @@ -64,13 +64,20 @@ typedef enum { } motor_polarity_t; +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); +void motor_set_stall_callback(int motor, stall_callback_t cb); +float motor_get_stall_homing_velocity(int motor); float motor_get_steps_per_unit(int motor); float motor_get_units_per_step(int motor); -int motor_get_microsteps(int motor); +uint16_t motor_get_microsteps(int motor); +void motor_set_microsteps(int motor, uint16_t microsteps); int32_t motor_get_encoder(int motor); void motor_set_encoder(int motor, float encoder); int32_t motor_get_error(int motor); @@ -87,4 +94,5 @@ void motor_error_callback(int motor, motor_flags_t errors); void motor_load_move(int motor); void motor_end_move(int motor); -stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error); +stat_t motor_prep_move(int motor, int32_t clocks, float target, int32_t error, + float time); diff --git a/src/plan/arc.c b/src/plan/arc.c index 6aeab3d..bc149ff 100644 --- a/src/plan/arc.c +++ b/src/plan/arc.c @@ -47,7 +47,6 @@ typedef struct { bool running; - int32_t line; // gcode block line number float target[AXES]; // XYZABC where the move should go float position[AXES]; // end point of the current segment @@ -82,7 +81,7 @@ static float _estimate_arc_time(float length, float linear_travel, float planar_travel) { // Determine move time at requested feed rate // Inverse feed rate is normalized to minutes - float time = mach_get_feed_mode() == INVERSE_TIME_MODE ? + float time = mach_is_inverse_time_mode() ? mach_get_feed_rate() : length / mach_get_feed_rate(); @@ -370,7 +369,7 @@ stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints // trap missing feed rate if (fp_ZERO(mach_get_feed_rate()) || - (mach_get_feed_mode() == INVERSE_TIME_MODE && !parser.gf.feed_rate)) + (mach_is_inverse_time_mode() && !parser.gf.feed_rate)) return STAT_GCODE_FEEDRATE_NOT_SPECIFIED; // radius must be positive and > minimum @@ -445,8 +444,7 @@ stat_t mach_arc_feed(float values[], bool values_f[], // arc endpoints // now get down to the rest of the work setting up the arc for execution mach_set_motion_mode(motion_mode); mach_update_work_offsets(); // Update resolved offsets - arc.line = mach_get_line(); // copy line number - arc.radius = TO_MM(radius); // set arc radius or zero + arc.radius = TO_MM(radius); // set arc radius or zero float offset[3]; for (int i = 0; i < 3; i++) offset[i] = TO_MM(offsets[i]); @@ -493,7 +491,8 @@ void mach_arc_callback() { arc.position[arc.linear_axis] += arc.segment_linear_travel; } - mp_aline(arc.position, arc.line); // run the line + // run the line + mach_plan_line(arc.position); if (!--arc.segments) arc.running = false; } diff --git a/src/plan/exec.c b/src/plan/exec.c index a4c75ce..d9f006e 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -29,7 +29,7 @@ #include "planner.h" #include "buffer.h" -#include "util.h" +#include "axes.h" #include "runtime.h" #include "state.h" #include "forward_dif.h" @@ -65,6 +65,7 @@ typedef struct { bool hold_planned; // true when a feedhold has been planned move_section_t section; // current move section bool section_new; // true if it's a new section + bool abort; } mp_exec_t; @@ -209,7 +210,7 @@ static void _plan_hold() { // Examine and process current buffer and compute length left for decel float available_length = - get_axis_vector_length(ex.final_target, mp_runtime_get_position()); + axes_get_vector_length(ex.final_target, mp_runtime_get_position()); // Compute next_segment velocity, velocity left to shed to brake to zero float braking_velocity = _compute_next_segment_velocity(); // Distance to brake to zero from braking_velocity, bf is OK to use here @@ -288,6 +289,9 @@ static stat_t _exec_aline_init(mp_buffer_t *bf) { } +void mp_exec_abort() {ex.abort = true;} + + /// Aline execution routines /// /// Everything here fires from interrupts and must be interrupt safe @@ -342,6 +346,12 @@ static stat_t _exec_aline_init(mp_buffer_t *bf) { stat_t mp_exec_aline(mp_buffer_t *bf) { stat_t status = STAT_OK; + if (ex.abort) { + ex.abort = false; + mp_runtime_set_velocity(0); // Assume a hard stop + return STAT_NOOP; + } + // Start a new move if (bf->state == BUFFER_INIT) { bf->state = BUFFER_ACTIVE; diff --git a/src/plan/exec.h b/src/plan/exec.h index 00972b2..9020e96 100644 --- a/src/plan/exec.h +++ b/src/plan/exec.h @@ -31,4 +31,5 @@ stat_t mp_exec_move(); +void mp_exec_abort(); stat_t mp_exec_aline(mp_buffer_t *bf); diff --git a/src/plan/line.c b/src/plan/line.c index ec64561..06527e1 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -32,15 +32,7 @@ #include "axes.h" #include "planner.h" #include "exec.h" -#include "runtime.h" #include "buffer.h" -#include "machine.h" -#include "stepper.h" -#include "util.h" - -#include -#include -#include /* Sonny's algorithm - simple @@ -244,7 +236,8 @@ static void _calc_and_cache_jerk_values(mp_buffer_t *bf) { } -static void _calc_max_velocities(mp_buffer_t *bf, float move_time) { +static void _calc_max_velocities(mp_buffer_t *bf, float move_time, + bool exact_stop) { float junction_velocity = _get_junction_vmax(mp_buffer_prev(bf)->unit, bf->unit); @@ -255,12 +248,101 @@ static void _calc_max_velocities(mp_buffer_t *bf, float move_time) { bf->braking_velocity = bf->delta_vmax; // Zero out exact stop cases - if (mach_get_path_mode() == PATH_EXACT_STOP) - bf->entry_vmax = bf->exit_vmax = 0; + if (exact_stop) bf->entry_vmax = bf->exit_vmax = 0; else bf->replannable = true; } +/* Compute optimal and minimum move times + * + * "Minimum time" is the fastest the move can be performed given the velocity + * constraints on each participating axis - regardless of the feed rate + * requested. The minimum time is the time limited by the rate-limiting + * axis. The minimum time is needed to compute the optimal time and is recorded + * for possible feed override computation. + * + * "Optimal time" is either the time resulting from the requested feed rate or + * the minimum time if the requested feed rate is not achievable. Optimal times + * for rapids are always the minimum time. + * + * The following times are compared and the longest is returned: + * - G93 inverse time (if G93 is active) + * - time for coordinated move at requested feed rate + * - time that the slowest axis would require for the move + * + * NIST RS274NGC_v3 Guidance + * + * The following is verbatim text from NIST RS274NGC_v3. As I interpret A for + * moves that combine both linear and rotational movement, the feed rate should + * apply to the XYZ movement, with the rotational axis (or axes) timed to start + * and end at the same time the linear move is performed. It is possible under + * this case for the rotational move to rate-limit the linear move. + * + * 2.1.2.5 Feed Rate + * + * The rate at which the controlled point or the axes move is nominally a steady + * rate which may be set by the user. In the Interpreter, the interpretation of + * the feed rate is as follows unless inverse time feed rate mode is being used + * in the RS274/NGC view (see Section 3.5.19). The machining functions view of + * feed rate, as described in Section 4.3.5.1, has conditions under which the + * set feed rate is applied differently, but none of these is used in the + * Interpreter. + * + * A. For motion involving one or more of the X, Y, and Z axes (with or without + * simultaneous rotational axis motion), the feed rate means length units + * per minute along the programmed XYZ path, as if the rotational axes were + * not moving. + * + * B. For motion of one rotational axis with X, Y, and Z axes not moving, the + * feed rate means degrees per minute rotation of the rotational axis. + * + * C. For motion of two or three rotational axes with X, Y, and Z axes not + * moving, the rate is applied as follows. Let dA, dB, and dC be the angles + * in degrees through which the A, B, and C axes, respectively, must move. + * Let D = sqrt(dA^2 + dB^2 + dC^2). Conceptually, D is a measure of total + * angular motion, using the usual Euclidean metric. Let T be the amount of + * time required to move through D degrees at the current feed rate in + * degrees per minute. The rotational axes should be moved in coordinated + * linear motion so that the elapsed time from the start to the end of the + * motion is T plus any time required for acceleration or deceleration. + */ +static float _calc_move_time(const float axis_length[], + const float axis_square[], bool rapid, + bool inverse_time, float feed_rate, + float feed_override) { + float max_time = 0; + + // Compute times for feed motion + if (!rapid) { + if (inverse_time) max_time = feed_rate; + else { + // Compute length of linear move in millimeters. Feed rate in mm/min. + max_time = sqrt(axis_square[AXIS_X] + axis_square[AXIS_Y] + + axis_square[AXIS_Z]) / feed_rate; + + // If no linear axes, compute length of multi-axis rotary move in degrees. + // Feed rate is provided as degrees/min + if (fp_ZERO(max_time)) + max_time = sqrt(axis_square[AXIS_A] + axis_square[AXIS_B] + + axis_square[AXIS_C]) / feed_rate; + } + } + + // Apply feed override + max_time /= feed_override; + + // Compute time required for rate-limiting axis + for (int axis = 0; axis < AXES; axis++) { + float time = fabs(axis_length[axis]) / + (rapid ? axes[axis].velocity_max : axes[axis].feedrate_max); + + if (max_time < time) max_time = time; + } + + return max_time < MIN_SEGMENT_TIME ? MIN_SEGMENT_TIME : max_time; +} + + /*** Plan line acceleration / deceleration * * This function uses constant jerk motion equations to plan acceleration and @@ -276,8 +358,13 @@ static void _calc_max_velocities(mp_buffer_t *bf, float move_time) { * [2] Returning a status that is not STAT_OK means the endpoint is NOT * advanced. So lines that are too short to move will accumulate and get * executed once the accumulated error exceeds the minimums. + * + * @param reed_rate is in minutes when @param inverse_time is true. + * See mach_set_feed_rate() */ -stat_t mp_aline(const float target[], int32_t line) { +stat_t mp_aline(const float target[], bool rapid, bool inverse_time, + bool exact_stop, float feed_rate, float feed_override, + int32_t line) { // Compute axis and move lengths float axis_length[AXES]; float axis_square[AXES]; @@ -308,8 +395,9 @@ stat_t mp_aline(const float target[], int32_t line) { _calc_and_cache_jerk_values(bf); // Compute move time and velocities - float time = mach_calc_move_time(axis_length, axis_square); - _calc_max_velocities(bf, time); + float time = _calc_move_time(axis_length, axis_square, rapid, inverse_time, + feed_rate, feed_override); + _calc_max_velocities(bf, time, exact_stop); // Note, the following lines must remain in order. bf->line = line; // Planner needs then when planning steps diff --git a/src/plan/line.h b/src/plan/line.h index 44930ed..9b109e7 100644 --- a/src/plan/line.h +++ b/src/plan/line.h @@ -29,6 +29,10 @@ #include "status.h" +#include -stat_t mp_aline(const float target[], int32_t line); + +stat_t mp_aline(const float target[], bool rapid, bool inverse_time, + bool exact_stop, float feed_rate, float feed_override, + int32_t line); int mp_find_jerk_axis(const float axis_square[]); diff --git a/src/plan/planner.c b/src/plan/planner.c index e83c63e..3e0ff1e 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -80,25 +80,13 @@ typedef struct { static planner_t mp = {{0}}; -void mp_init() { - mp_queue_init(); -} +void mp_init() {mp_queue_init();} /// Set planner position for a single axis -void mp_set_axis_position(int axis, float position) { - mp.position[axis] = position; -} - - +void mp_set_axis_position(int axis, float p) {mp.position[axis] = p;} float mp_get_axis_position(int axis) {return mp.position[axis];} - - -void mp_set_position(const float position[]) { - copy_vector(mp.position, position); -} - - +void mp_set_position(const float p[]) {copy_vector(mp.position, p);} void mp_set_plan_steps(bool plan_steps) {mp.plan_steps = plan_steps;} diff --git a/src/plan/planner.h b/src/plan/planner.h index 0872f5c..04c5a6d 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -74,7 +74,7 @@ void mp_init(); void mp_set_axis_position(int axis, float position); float mp_get_axis_position(int axis); -void mp_set_position(const float position[]); +void mp_set_position(const float p[]); void mp_set_plan_steps(bool plan_steps); void mp_flush_planner(); diff --git a/src/probing.c b/src/probing.c index 75a2135..1bf26f2 100644 --- a/src/probing.c +++ b/src/probing.c @@ -165,7 +165,7 @@ static void _probing_init() { } // error if the probe target is too close to the current position - if (get_axis_vector_length(pb.start_position, pb.target) < + if (axes_get_vector_length(pb.start_position, pb.target) < MINIMUM_PROBE_TRAVEL) _probing_error_exit(STAT_PROBE_INVALID_DEST); diff --git a/src/stepper.c b/src/stepper.c index aad14a5..c929d25 100644 --- a/src/stepper.c +++ b/src/stepper.c @@ -97,10 +97,10 @@ ISR(ADCB_CH0_vect) { stat_t status = mp_exec_move(); switch (status) { - case STAT_NOOP: break; // No command executed - case STAT_EAGAIN: continue; // No command executed, try again + case STAT_NOOP: st.busy = false; break; // No command executed + case STAT_EAGAIN: continue; // No command executed, try again - case STAT_OK: // Move executed + case STAT_OK: // Move executed if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued st.move_queued = false; st.move_ready = true; @@ -134,7 +134,6 @@ ISR(STEP_TIMER_ISR) { if (st.dwell && --st.dwell) return; // End last move - st.busy = false; TIMER_STEP.PER = STEP_TIMER_POLL; DMA.INTFLAGS = 0xff; // clear all interrups @@ -209,7 +208,8 @@ stat_t st_prep_line(float time, const float target[], const int32_t error[]) { // Prepare motor moves for (int motor = 0; motor < MOTORS; motor++) - RITORNO(motor_prep_move(motor, seg_clocks, target[motor], error[motor])); + RITORNO + (motor_prep_move(motor, seg_clocks, target[motor], error[motor], time)); st.move_queued = true; // signal prep buffer ready (do this last) diff --git a/src/util.c b/src/util.c index f30feb6..74ea051 100644 --- a/src/util.c +++ b/src/util.c @@ -29,11 +29,6 @@ #include "util.h" -float get_axis_vector_length(const float a[], const float b[]) { - return sqrt(square(a[AXIS_X] - b[AXIS_X]) + square(a[AXIS_Y] - b[AXIS_Y]) + - square(a[AXIS_Z] - b[AXIS_Z]) + square(a[AXIS_A] - b[AXIS_A]) + - square(a[AXIS_B] - b[AXIS_B]) + square(a[AXIS_C] - b[AXIS_C])); -} /// Fast inverse square root originally from Quake III Arena code. Original diff --git a/src/util.h b/src/util.h index 6367c2a..581bbb3 100644 --- a/src/util.h +++ b/src/util.h @@ -40,7 +40,6 @@ // Vector utilities #define clear_vector(a) memset(a, 0, sizeof(a)) #define copy_vector(d, s) memcpy(d, s, sizeof(d)) -float get_axis_vector_length(const float a[], const float b[]); // Math utilities inline float min(float a, float b) {return a < b ? a : b;} diff --git a/src/vars.def b/src/vars.def index 76d28a8..e2c8d60 100644 --- a/src/vars.def +++ b/src/vars.def @@ -32,7 +32,7 @@ // VAR(name, code, type, index, settable, save, help) // Motor -VAR(motor_map, "ma", uint8_t, MOTORS, 1, 1, "Motor to axis mapping") +VAR(motor_axis, "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") @@ -44,6 +44,7 @@ VAR(idle_power, "ip", float, MOTORS, 1, 1, "Motor idle power") VAR(current_power, "cp", float, MOTORS, 0, 0, "Motor power now") VAR(status_flags, "mf", uint8_t, MOTORS, 0, 0, "Motor status flags") VAR(status_strings, "ms", flags_t, MOTORS, 0, 0, "Motor status strings") +VAR(motor_fault, "mf", bool, 0, 0, 0, "Motor fault status") // Axis VAR(position, "p", float, AXES, 0, 0, "Current axis position") -- 2.27.0