From: Joseph Coffland Date: Wed, 21 Sep 2016 05:13:41 +0000 (-0700) Subject: Accurate positioning but velocity wavers a bit X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=c8b27607d13fabede677e66fb2af0b3e3882bc51;p=bbctrl-firmware Accurate positioning but velocity wavers a bit --- diff --git a/src/command.c b/src/command.c index b14a392..c358004 100644 --- a/src/command.c +++ b/src/command.c @@ -71,17 +71,17 @@ static unsigned _parse_axis(uint8_t axis) { static void command_i2c_cb(i2c_cmd_t cmd, uint8_t *data, uint8_t length) { switch (cmd) { - case I2C_NULL: break; - case I2C_ESTOP: estop_trigger(ESTOP_USER); break; - case I2C_CLEAR: estop_clear(); break; - case I2C_PAUSE: mp_request_hold(); break; - case I2C_OPTIONAL_PAUSE: mp_request_optional_pause(); break; - case I2C_RUN: mp_request_start(); break; - case I2C_STEP: mp_request_step(); break; - case I2C_FLUSH: mp_request_flush(); break; - case I2C_REPORT: report_request_full(); break; - case I2C_HOME: break; // TODO - case I2C_REBOOT: _reboot(); break; + case I2C_NULL: break; + case I2C_ESTOP: estop_trigger(STAT_ESTOP_USER); break; + case I2C_CLEAR: estop_clear(); break; + case I2C_PAUSE: mp_request_hold(); break; + case I2C_OPTIONAL_PAUSE: mp_request_optional_pause(); break; + case I2C_RUN: mp_request_start(); break; + case I2C_STEP: mp_request_step(); break; + case I2C_FLUSH: mp_request_flush(); break; + case I2C_REPORT: report_request_full(); break; + case I2C_HOME: break; // TODO + case I2C_REBOOT: _reboot(); break; case I2C_ZERO: if (length == 0) mach_zero_all(); else if (length == 1) mach_zero_axis(_parse_axis(*data)); diff --git a/src/config.h b/src/config.h index 1e9b3ea..401e788 100644 --- a/src/config.h +++ b/src/config.h @@ -156,6 +156,7 @@ typedef enum { // Machine settings +#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 JUNCTION_DEVIATION 0.05 // default value, in mm diff --git a/src/estop.c b/src/estop.c index 6b2aea7..5f533ae 100644 --- a/src/estop.c +++ b/src/estop.c @@ -51,26 +51,26 @@ static estop_t estop = {0}; static uint16_t estop_reason_eeprom EEMEM; -static void _set_reason(estop_reason_t reason) { +static void _set_reason(stat_t reason) { eeprom_update_word(&estop_reason_eeprom, reason); } -static estop_reason_t _get_reason() { +static stat_t _get_reason() { return eeprom_read_word(&estop_reason_eeprom); } static void _switch_callback(switch_id_t id, bool active) { - if (active) estop_trigger(ESTOP_SWITCH); + if (active) estop_trigger(STAT_ESTOP_SWITCH); else estop_clear(); } void estop_init() { - if (switch_is_active(SW_ESTOP)) _set_reason(ESTOP_SWITCH); - if (ESTOP_MAX <= _get_reason()) _set_reason(ESTOP_NONE); - estop.triggered = _get_reason() != ESTOP_NONE; + if (switch_is_active(SW_ESTOP)) _set_reason(STAT_ESTOP_SWITCH); + if (STAT_MAX <= _get_reason()) _set_reason(STAT_OK); + estop.triggered = _get_reason() != STAT_OK; switch_set_callback(SW_ESTOP, _switch_callback); @@ -88,7 +88,8 @@ bool estop_triggered() { } -void estop_trigger(estop_reason_t reason) { +void estop_trigger(stat_t reason) { + if (estop.triggered) return; estop.triggered = true; // Hard stop the motors and the spindle @@ -111,7 +112,7 @@ void estop_trigger(estop_reason_t reason) { void estop_clear() { // Check if estop switch is set if (switch_is_active(SW_ESTOP)) { - if (_get_reason() != ESTOP_SWITCH) _set_reason(ESTOP_SWITCH); + if (_get_reason() != STAT_ESTOP_SWITCH) _set_reason(STAT_ESTOP_SWITCH); return; // Can't clear while estop switch is still active } @@ -121,7 +122,7 @@ void estop_clear() { estop.triggered = false; // Clear reason - _set_reason(ESTOP_NONE); + _set_reason(STAT_OK); // Reboot // Note, hardware.c waits until any spindle stop command has been delivered @@ -136,18 +137,11 @@ bool get_estop() { void set_estop(bool value) { if (value == estop_triggered()) return; - if (value) estop_trigger(ESTOP_USER); + if (value) estop_trigger(STAT_ESTOP_USER); else estop_clear(); } PGM_P get_estop_reason() { - switch (_get_reason()) { - case ESTOP_NONE: return PSTR("none"); - case ESTOP_USER: return PSTR("user"); - case ESTOP_SWITCH: return PSTR("switch"); - case ESTOP_LIMIT: return PSTR("limit"); - case ESTOP_ALARM: return PSTR("alarm"); - default: return PSTR("invalid"); - } + return status_to_pgmstr(_get_reason()); } diff --git a/src/estop.h b/src/estop.h index a493deb..55fdec4 100644 --- a/src/estop.h +++ b/src/estop.h @@ -27,20 +27,12 @@ #pragma once -#include - +#include "status.h" -typedef enum { - ESTOP_NONE, - ESTOP_USER, - ESTOP_SWITCH, - ESTOP_LIMIT, - ESTOP_ALARM, - ESTOP_MAX, -} estop_reason_t; +#include void estop_init(); bool estop_triggered(); -void estop_trigger(estop_reason_t reason); +void estop_trigger(stat_t reason); void estop_clear(); diff --git a/src/gcode_parser.c b/src/gcode_parser.c index 5a7feee..1bc0a6a 100644 --- a/src/gcode_parser.c +++ b/src/gcode_parser.c @@ -481,6 +481,8 @@ static stat_t _parse_gcode_block(char *buf) { case 94: SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_MINUTE_MODE); // case 95: // SET_MODAL(MODAL_GROUP_G5, feed_mode, UNITS_PER_REVOLUTION_MODE); + // case 96: // Spindle Constant Surface Speed (not currently supported) + case 97: break; // Spindle RPM mode (only mode curently supported) default: status = STAT_GCODE_COMMAND_UNSUPPORTED; } break; diff --git a/src/huanyang.c b/src/huanyang.c index 1b2a48d..bd8ade6 100644 --- a/src/huanyang.c +++ b/src/huanyang.c @@ -534,18 +534,18 @@ bool huanyang_stopping() { } -uint8_t get_huanyang_id(int index) {return ha.id;} -void set_huanyang_id(int index, uint8_t value) {ha.id = value;} -bool get_huanyang_debug(int index) {return ha.debug;} -void set_huanyang_debug(int index, uint8_t value) {ha.debug = value;} -bool get_huanyang_connected(int index) {return ha.connected;} -float get_huanyang_freq(int index) {return ha.actual_freq;} -float get_huanyang_current(int index) {return ha.actual_current;} -uint16_t get_huanyang_rpm(int index) {return ha.actual_rpm;} -uint16_t get_huanyang_dcv(int index) {return ha.dc_voltage;} -uint16_t get_huanyang_acv(int index) {return ha.ac_voltage;} -uint16_t get_huanyang_temp(int index) {return ha.temperature;} -float get_huanyang_max_freq(int index) {return ha.max_freq;} -float get_huanyang_min_freq(int index) {return ha.min_freq;} -uint16_t get_huanyang_rated_rpm(int index) {return ha.rated_rpm;} -float get_huanyang_status(int index) {return ha.status;} +uint8_t get_huanyang_id() {return ha.id;} +void set_huanyang_id(uint8_t value) {ha.id = value;} +bool get_huanyang_debug() {return ha.debug;} +void set_huanyang_debug(bool value) {ha.debug = value;} +bool get_huanyang_connected() {return ha.connected;} +float get_huanyang_freq() {return ha.actual_freq;} +float get_huanyang_current() {return ha.actual_current;} +uint16_t get_huanyang_rpm() {return ha.actual_rpm;} +uint16_t get_huanyang_dcv() {return ha.dc_voltage;} +uint16_t get_huanyang_acv() {return ha.ac_voltage;} +uint16_t get_huanyang_temp() {return ha.temperature;} +float get_huanyang_max_freq() {return ha.max_freq;} +float get_huanyang_min_freq() {return ha.min_freq;} +uint16_t get_huanyang_rated_rpm() {return ha.rated_rpm;} +float get_huanyang_status() {return ha.status;} diff --git a/src/machine.c b/src/machine.c index 21f9362..e295827 100644 --- a/src/machine.c +++ b/src/machine.c @@ -948,8 +948,7 @@ void mach_program_end() { mach_set_plane(GCODE_DEFAULT_PLANE); mach_set_distance_mode(GCODE_DEFAULT_DISTANCE_MODE); mach_set_arc_distance_mode(GCODE_DEFAULT_ARC_DISTANCE_MODE); - mach.gm.spindle_mode = SPINDLE_OFF; - spindle_set(SPINDLE_OFF, 0); + mach_set_spindle_mode(SPINDLE_OFF); // M5 mach_flood_coolant_control(false); // M9 mach_set_feed_mode(UNITS_PER_MINUTE_MODE); // G94 mach_set_motion_mode(MOTION_MODE_CANCEL_MOTION_MODE); diff --git a/src/messages.def b/src/messages.def index 72134e4..26059a2 100644 --- a/src/messages.def +++ b/src/messages.def @@ -35,13 +35,23 @@ STAT_MSG(NO_SUCH_DEVICE, "No such device") STAT_MSG(BUFFER_FULL, "Buffer full") STAT_MSG(BUFFER_FULL_FATAL, "Buffer full - fatal") STAT_MSG(EEPROM_DATA_INVALID, "EEPROM data invalid") -STAT_MSG(MOTOR_ERROR, "Motor error") STAT_MSG(STEP_CHECK_FAILED, "Step check failed") -STAT_MSG(MACH_NOT_QUIESCENT, "Cannot perform action while machine is active") +STAT_MSG(MACH_NOT_QUIESCENT, "Cannot perform action while machine active") STAT_MSG(INTERNAL_ERROR, "Internal error") +STAT_MSG(MOTOR_STALLED, "Motor stalled") +STAT_MSG(MOTOR_OVERTEMP_WARN, "Motor over temperature warning") +STAT_MSG(MOTOR_OVERTEMP, "Motor over temperature") +STAT_MSG(MOTOR_SHORTED, "Motor shorted") + STAT_MSG(PREP_LINE_MOVE_TIME_IS_INFINITE, "Move time is infinite") STAT_MSG(PREP_LINE_MOVE_TIME_IS_NAN, "Move time is NAN") +STAT_MSG(MOVE_TARGET_IS_INFINITE, "Move target is infinite") +STAT_MSG(MOVE_TARGET_IS_NAN, "Move target is NAN") +STAT_MSG(EXCESSIVE_MOVE_ERROR, "Excessive move error") + +STAT_MSG(ESTOP_USER, "User triggered EStop") +STAT_MSG(ESTOP_SWITCH, "Switch triggered EStop") // Generic data input errors STAT_MSG(UNRECOGNIZED_NAME, "Unrecognized command or variable name") @@ -73,7 +83,7 @@ STAT_MSG(P_WORD_IS_NOT_A_POSITIVE_INTEGER, "P word is not a positive integer") // Errors and warnings STAT_MSG(MINIMUM_LENGTH_MOVE, "Move less than minimum length") STAT_MSG(MINIMUM_TIME_MOVE, "Move less than minimum time") -STAT_MSG(MACHINE_ALARMED, "Machine is alarmed - Command not processed") +STAT_MSG(MACHINE_ALARMED, "Machine alarmed - Command not processed") STAT_MSG(LIMIT_SWITCH_HIT, "Limit switch hit - Shutdown occurred") STAT_MSG(SOFT_LIMIT_EXCEEDED, "Soft limit exceeded") STAT_MSG(INVALID_AXIS, "Invalid axis") diff --git a/src/motor.c b/src/motor.c index a54ecfd..de38139 100644 --- a/src/motor.c +++ b/src/motor.c @@ -77,17 +77,19 @@ typedef struct { motor_flags_t flags; int32_t encoder; int32_t commanded; - uint16_t steps; // Currently used by the x-axis only + int32_t steps; // Currently used by the x-axis only uint8_t last_clock; bool wasEnabled; + int32_t error; + bool last_negative; // Last step sign // Move prep uint8_t timer_clock; // clock divisor setting or zero for off uint16_t timer_period; // clock period counter - bool positive; // step sign + bool negative; // step sign direction_t direction; // travel direction corrected for polarity int32_t position; - int32_t error; + bool round_up; // toggle rounding direction } motor_t; @@ -228,8 +230,10 @@ int32_t motor_get_encoder(int motor) { void motor_set_encoder(int motor, float encoder) { motor_t *m = &motors[motor]; + cli(); m->encoder = m->position = m->commanded = round(encoder); m->error = 0; + sei(); } @@ -307,12 +311,19 @@ stat_t motor_rtc_callback() { // called by controller void motor_error_callback(int motor, motor_flags_t errors) { - if (motors[motor].power_state != MOTOR_ACTIVE) return; + motor_t *m = &motors[motor]; + + if (m->power_state != MOTOR_ACTIVE) return; - motors[motor].flags |= errors; + m->flags |= errors; report_request(); - if (motor_error(motor)) ALARM(STAT_MOTOR_ERROR); + if (false && motor_error(motor)) { + if (m->flags & MOTOR_FLAG_STALLED_bm) ALARM(STAT_MOTOR_STALLED); + if (m->flags & MOTOR_FLAG_OVERTEMP_WARN_bm) ALARM(STAT_MOTOR_OVERTEMP_WARN); + if (m->flags & MOTOR_FLAG_OVERTEMP_bm) ALARM(STAT_MOTOR_OVERTEMP); + if (m->flags & MOTOR_FLAG_SHORTED_bm) ALARM(STAT_MOTOR_SHORTED); + } } @@ -357,7 +368,9 @@ void motor_load_move(int motor) { m->steps = 0; } if (!m->wasEnabled) steps = 0; - m->encoder += m->positive ? steps : -(int32_t)steps; + + m->encoder += m->last_negative ? -(int32_t)steps : steps; + m->last_negative = m->negative; // Compute error m->error = m->commanded - m->encoder; @@ -376,64 +389,78 @@ void motor_end_move(int motor) { void motor_prep_move(int motor, int32_t seg_clocks, float target) { motor_t *m = &motors[motor]; - int32_t travel = round(target) - m->position + m->error; - m->error = 0; - - // Power motor - switch (m->power_mode) { - case MOTOR_DISABLED: return; - - case MOTOR_POWERED_ONLY_WHEN_MOVING: - if (!travel) return; // Not moving - // Fall through - - case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: - _energize(motor); // TODO is ~5ms enough time to enable the motor? - break; - - case MOTOR_POWER_MODE_MAX_VALUE: break; // Shouldn't get here + // Validate input + if (motor < 0 || MOTORS < motor) {ALARM(STAT_INTERNAL_ERROR); return;} + if (seg_clocks < 0) {ALARM(STAT_INTERNAL_ERROR); return;} + if (isinf(target)) {ALARM(STAT_MOVE_TARGET_IS_INFINITE); return;} + if (isnan(target)) {ALARM(STAT_MOVE_TARGET_IS_NAN); return;} + + // Compute error correction + cli(); + int32_t error = m->error; + int32_t actual_error = error; + if (error < -MAX_STEP_CORRECTION) error = -MAX_STEP_CORRECTION; + else if (MAX_STEP_CORRECTION < error) error = MAX_STEP_CORRECTION; + sei(); + + if (100 < labs(actual_error)) { + STATUS_DEBUG("Motor %d error is %ld", motor, actual_error); + ALARM(STAT_EXCESSIVE_MOVE_ERROR); + return; } // Compute motor timer clock and period. Rounding is performed to eliminate // a negative bias in the uint32_t conversion that results in long-term // negative drift. - int32_t ticks_per_step = labs(seg_clocks / travel); + int32_t travel = round(target) - m->position + error; + uint32_t ticks_per_step = travel ? labs(seg_clocks / travel) : 0; // Find the clock rate that will fit the required number of steps - if (ticks_per_step & 0xffff0000UL) { - ticks_per_step /= 2; - - if (ticks_per_step & 0xffff0000UL) { - ticks_per_step /= 2; + if (ticks_per_step <= 0xffff) m->timer_clock = TC_CLKSEL_DIV1_gc; + else if (ticks_per_step <= 0x1ffff) m->timer_clock = TC_CLKSEL_DIV2_gc; + else if (ticks_per_step <= 0x3ffff) m->timer_clock = TC_CLKSEL_DIV4_gc; + else if (ticks_per_step <= 0x7ffff) m->timer_clock = TC_CLKSEL_DIV8_gc; + else m->timer_clock = 0; // Clock off, too slow + + // Note, we rely on the fact that TC_CLKSEL_DIV1_gc through TC_CLKSEL_DIV8_gc + // equal 1, 2, 3 & 4 respectively. + m->timer_period = ticks_per_step >> (m->timer_clock - 1); + + // Round up if DIV4 or DIV8 and the error is high enough + if (0xffff < ticks_per_step && m->timer_period < 0xffff) { + uint8_t step_error = ticks_per_step & ((1 << (m->timer_clock - 1)) - 1); + uint8_t half_error = 1 << (m->timer_clock - 2); + + if (step_error == half_error) { + if (m->round_up) m->timer_period++; + m->round_up = !m->round_up; + + } else if (half_error < step_error) m->timer_period++; + } - if (ticks_per_step & 0xffff0000UL) { - ticks_per_step /= 2; + if (!m->timer_period) m->timer_clock = 0; + if (!m->timer_clock) m->timer_period = 0; - if (ticks_per_step & 0xffff0000UL) m->timer_clock = 0; // Off, too slow - else m->timer_clock = TC_CLKSEL_DIV8_gc; - } else m->timer_clock = TC_CLKSEL_DIV4_gc; - } else m->timer_clock = TC_CLKSEL_DIV2_gc; - } else m->timer_clock = TC_CLKSEL_DIV1_gc; + // Setup the direction, compensating for polarity. + m->negative = travel < 0; + if (m->negative) m->direction = DIRECTION_CCW ^ m->polarity; + else m->direction = DIRECTION_CW ^ m->polarity; - if (!ticks_per_step) m->timer_clock = 0; - m->timer_period = ticks_per_step; - m->positive = 0 <= travel; + m->position = round(target); - // Setup the direction, compensating for polarity. - if (m->positive) m->direction = DIRECTION_CW ^ m->polarity; - else m->direction = DIRECTION_CCW ^ m->polarity; + // Power motor + switch (m->power_mode) { + case MOTOR_DISABLED: break; - // Compute actual steps - if (m->timer_clock) { - int32_t clocks = seg_clocks >> (m->timer_clock - 1); // Motor timer clocks - int32_t steps = clocks / m->timer_period; + case MOTOR_POWERED_ONLY_WHEN_MOVING: + if (!m->timer_clock) break; // Not moving + // Fall through - // Update position - m->position += m->positive ? steps : -steps; + case MOTOR_ALWAYS_POWERED: case MOTOR_POWERED_IN_CYCLE: + _energize(motor); // TODO is ~5ms enough time to enable the motor? + break; - // Sanity check - int32_t diff = labs(labs(travel) - steps); - if (20 < diff) ALARM(STAT_STEP_CHECK_FAILED); + default: break; // Shouldn't get here } } diff --git a/src/motor.h b/src/motor.h index c1e7007..6c52db1 100644 --- a/src/motor.h +++ b/src/motor.h @@ -41,9 +41,9 @@ typedef enum { MOTOR_FLAG_SHORTED_bm = 1 << 4, MOTOR_FLAG_OPEN_LOAD_bm = 1 << 5, MOTOR_FLAG_ERROR_bm = (//MOTOR_FLAG_STALLED_bm | TODO revisit this + MOTOR_FLAG_SHORTED_bm | MOTOR_FLAG_OVERTEMP_WARN_bm | - MOTOR_FLAG_OVERTEMP_bm | - MOTOR_FLAG_SHORTED_bm) + MOTOR_FLAG_OVERTEMP_bm) } motor_flags_t; diff --git a/src/spindle.c b/src/spindle.c index 59012af..7a57fb5 100644 --- a/src/spindle.c +++ b/src/spindle.c @@ -55,7 +55,7 @@ void spindle_init() { } -void spindle_set(spindle_mode_t mode, float speed) { +void _spindle_set(spindle_mode_t mode, float speed) { spindle.mode = mode; spindle.speed = speed; @@ -98,16 +98,16 @@ void spindle_estop() { } -uint8_t get_spindle_type(int index) {return spindle.type;} +uint8_t get_spindle_type() {return spindle.type;} -void set_spindle_type(int index, uint8_t value) { +void set_spindle_type(uint8_t value) { if (value != spindle.type) { spindle_mode_t mode = spindle.mode; float speed = spindle.speed; - spindle_set(SPINDLE_OFF, 0); + _spindle_set(SPINDLE_OFF, 0); spindle.type = value; - spindle_set(mode, speed); + _spindle_set(mode, speed); } } diff --git a/src/spindle.h b/src/spindle.h index 4542651..b583d55 100644 --- a/src/spindle.h +++ b/src/spindle.h @@ -32,7 +32,6 @@ void spindle_init(); -void spindle_set(spindle_mode_t mode, float speed); void spindle_set_mode(spindle_mode_t mode); void spindle_set_speed(float speed); spindle_mode_t spindle_get_mode(); diff --git a/src/status.c b/src/status.c index ed09d6d..ad06bd4 100644 --- a/src/status.c +++ b/src/status.c @@ -27,6 +27,7 @@ #include "status.h" #include "estop.h" +#include "usart.h" #include #include @@ -70,7 +71,7 @@ stat_t status_message_P(const char *location, status_level_t level, va_list args; // Type - printf_P(PSTR("\n{\"%S\": \""), status_level_pgmstr(level)); + printf_P(PSTR("\n{\"level\":\"%S\",\"msg\":\""), status_level_pgmstr(level)); // Message if (msg) { @@ -86,7 +87,7 @@ stat_t status_message_P(const char *location, status_level_t level, if (code) printf_P(PSTR(", \"code\": %d"), code); // Location - if (location) printf_P(PSTR(", \"location\": \"%S\""), location); + if (location) printf_P(PSTR(", \"where\": \"%S\""), location); putchar('}'); putchar('\n'); @@ -113,6 +114,7 @@ void status_help() { /// Alarm state; send an exception report and stop processing input stat_t status_alarm(const char *location, stat_t code) { status_message_P(location, STAT_LEVEL_ERROR, code, 0); - estop_trigger(ESTOP_ALARM); + estop_trigger(code); + while (!usart_tx_empty()) continue; return code; } diff --git a/src/vars.def b/src/vars.def index d263322..3666c65 100644 --- a/src/vars.def +++ b/src/vars.def @@ -96,11 +96,11 @@ VAR(unit, "u", pstring, 0, 0, 0, "Current unit of measure") VAR(speed, "s", float, 0, 0, 0, "Current spindle speed") VAR(feed, "f", float, 0, 0, 0, "Current feed rate") VAR(tool, "t", uint8_t, 0, 0, 0, "Current tool") -VAR(feed_mode, "fm", pstring, 0, 0, 0, "Current feed rate mode") +VAR(feed_mode, "fm", pstring, 0, 0, 0, "Current feed rate mode") VAR(plane, "pa", pstring, 0, 0, 0, "Current plane") VAR(coord_system, "cs", pstring, 0, 0, 0, "Current coordinate system") VAR(abs_override, "ao", bool, 0, 0, 0, "Absolute override enabled") -VAR(path_mode, "pc", pstring, 0, 0, 0, "Current path control mode") +VAR(path_mode, "pc", pstring, 0, 0, 0, "Current path control mode") VAR(distance_mode, "dm", pstring, 0, 0, 0, "Current distance mode") VAR(arc_dist_mode, "ad", pstring, 0, 0, 0, "Current arc distance mode") VAR(mist_coolant, "mc", bool, 0, 0, 0, "Mist coolant enabled")