#include "axes.h"
-
+#include "motor.h"
#include "plan/planner.h"
#include <math.h>
-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,
.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,
.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,
.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,
.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,
.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,
};
+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
*/
/// 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;
}
#include "config.h"
+#include <stdbool.h>
+
+
+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
} 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
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);
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")
// 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,
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,
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
#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
// 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
#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
#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
#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
#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:
#include <stdio.h>
-#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;
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},
};
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
}
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);
}
}
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));
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);
}
}
-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);
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 |
}
+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) {
}
+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;
if (driver < 0 || DRIVERS <= driver) return 0;
return _driver_get_current(driver);
}
+
+
+bool get_motor_fault() {return motor_fault;}
#include "config.h"
#include "status.h"
+#include "motor.h"
#include <stdint.h>
#include <stdbool.h>
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);
#include "plan/arc.h"
#include "probing.h"
#include "homing.h"
+#include "axes.h"
#include "util.h"
#include <stdbool.h>
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#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 <stdint.h>
+#include <string.h>
+
+
+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;
+}
--- /dev/null
+/******************************************************************************\
+
+ 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 <http://www.gnu.org/licenses/>.
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+ For information regarding this software email:
+ "Joseph Coffland" <joseph@buildbotics.com>
+
+\******************************************************************************/
+
+#pragma once
+
+void home_init();
+void home_callback();
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;
}
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;}
* 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
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
}
+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];
// 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;
// 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();
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]
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[]);
#include "estop.h"
#include "probing.h"
#include "homing.h"
+#include "home.h"
#include "i2c.h"
#include "plan/planner.h"
int main() {
- //wdt_enable(WDTO_250MS);
+ //wdt_enable(WDTO_250MS); TODO
// Init
cli(); // disable interrupts
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
#include "drv8711.h"
#include "estop.h"
#include "gcode_state.h"
+#include "axes.h"
#include "util.h"
#include "plan/runtime.h"
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;
direction_t direction; // travel direction corrected for polarity
int32_t position;
bool round_up; // toggle rounding direction
+ float power;
} motor_t;
.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,
.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,
.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,
.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,
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;
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;
}
-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;}
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
if (m->power_state != MOTOR_ACTIVE) return;
- m->flags |= errors;
+ m->flags |= errors & MOTOR_FLAG_ERROR_bm;
report_request();
if (motor_error(motor)) {
// Compute error
if (!m->reading) m->error = m->commanded - m->encoder;
m->commanded = m->position;
+
+ // Set power
+ drv8711_set_power(motor, m->power);
}
}
-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
default: break; // Shouldn't get here
}
+ // Compute power from axis velocity
+ m->power = travel / (_get_max_velocity(motor) * time);
+
return STAT_OK;
}
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);
}
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;
}
} 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);
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);
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
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();
// 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
// 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]);
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;
}
#include "planner.h"
#include "buffer.h"
-#include "util.h"
+#include "axes.h"
#include "runtime.h"
#include "state.h"
#include "forward_dif.h"
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;
// 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
}
+void mp_exec_abort() {ex.abort = true;}
+
+
/// Aline execution routines
///
/// Everything here fires from interrupts and must be interrupt safe
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;
stat_t mp_exec_move();
+void mp_exec_abort();
stat_t mp_exec_aline(mp_buffer_t *bf);
#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 <string.h>
-#include <stdbool.h>
-#include <math.h>
/* Sonny's algorithm - simple
}
-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);
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
* [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];
_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
#include "status.h"
+#include <stdbool.h>
-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[]);
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;}
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();
}
// 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);
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;
if (st.dwell && --st.dwell) return;
// End last move
- st.busy = false;
TIMER_STEP.PER = STEP_TIMER_POLL;
DMA.INTFLAGS = 0xff; // clear all interrups
// 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)
#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
// 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;}
// 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")
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")
projects / bbctrl-firmware / commitdiff