From: Joseph Coffland Date: Thu, 8 Sep 2016 07:29:22 +0000 (-0700) Subject: Implemented axis zeroing. Start AVR in flushing mode, #10 X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=1c94989af85006a3b292ac837ba0b3efad6e38df;p=bbctrl-firmware Implemented axis zeroing. Start AVR in flushing mode, #10 --- diff --git a/src/command.c b/src/command.c index d618d82..16397d2 100644 --- a/src/command.c +++ b/src/command.c @@ -55,31 +55,37 @@ static char *_cmd = 0; -static void _estop() {estop_trigger(ESTOP_USER);} -static void _clear() {estop_clear();} -static void _pause() {mp_request_hold();} -static void _opt_pause() {} // TODO -static void _run() {mp_request_start();} -static void _step() {} // TODO -static void _flush() {mp_request_flush();} -static void _report() {report_request_full();} -static void _home() {} // TODO static void _reboot() {hw_request_hard_reset();} +static unsigned _parse_axis(uint8_t axis) { + switch (axis) { + case 'x': return 0; case 'y': return 1; case 'z': return 2; + case 'a': return 3; case 'b': return 4; case 'c': return 5; + case 'X': return 0; case 'Y': return 1; case 'Z': return 2; + case 'A': return 3; case 'B': return 4; case 'C': return 5; + default: return 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(); break; - case I2C_CLEAR: _clear(); break; - case I2C_PAUSE: _pause(); break; - case I2C_OPTIONAL_PAUSE: _opt_pause(); break; - case I2C_RUN: _run(); break; - case I2C_STEP: _step(); break; - case I2C_FLUSH: _flush(); break; - case I2C_REPORT: _report(); break; - case I2C_HOME: _home(); break; - case I2C_REBOOT: _reboot(); break; + 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: break; // TODO + case I2C_RUN: mp_request_start(); break; + case I2C_STEP: break; // TODO + 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)); + break; } } diff --git a/src/hardware.c b/src/hardware.c index d31a382..65c5c8f 100644 --- a/src/hardware.c +++ b/src/hardware.c @@ -161,6 +161,7 @@ void hw_reset_handler() { if (hw.hard_reset) { while (huanyang_stopping() || !usart_tx_empty() || !eeprom_is_ready()) continue; + hw_hard_reset(); } diff --git a/src/homing.c b/src/homing.c index 48eb2dc..218d1fc 100644 --- a/src/homing.c +++ b/src/homing.c @@ -229,11 +229,11 @@ static void _homing_abort(int8_t axis) { /// set zero and finish up static void _homing_axis_set_zero(int8_t axis) { if (hm.set_coordinates) { - mach_set_position(axis, 0); + mach_set_axis_position(axis, 0); hm.homed[axis] = true; } else // do not set axis if in G28.4 cycle - mach_set_position(axis, mp_runtime_get_work_position(axis)); + mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); mach_set_axis_jerk(axis, hm.saved_jerk); // restore the max jerk value diff --git a/src/i2c.h b/src/i2c.h index 9287249..7ded350 100644 --- a/src/i2c.h +++ b/src/i2c.h @@ -45,6 +45,7 @@ typedef enum { I2C_REPORT, I2C_HOME, I2C_REBOOT, + I2C_ZERO, } i2c_cmd_t; diff --git a/src/machine.c b/src/machine.c index b13fe3a..939bc9d 100644 --- a/src/machine.c +++ b/src/machine.c @@ -688,16 +688,38 @@ void mach_set_coord_system(coord_system_t coord_system) { * (such as homing cycles) when you know there are no more moves in * the planner and that all motion has stopped. */ -void mach_set_position(int axis, float position) { - //if (!mp_is_quiescent()) CM_ALARM(STAT_INTERNAL_ERROR); +void mach_set_axis_position(unsigned axis, float position) { + //if (!mp_is_quiescent()) CM_ALARM(STAT_MACH_NOT_QUIESCENT); + if (AXES <= axis) return; + mach.position[axis] = position; mach.ms.target[axis] = position; - mp_set_planner_position(axis, position); + mp_set_axis_position(axis, position); mp_runtime_set_axis_position(axis, position); mp_runtime_set_steps_from_position(); } +stat_t mach_zero_all() { + for (unsigned axis = 0; axis < AXES; axis++) { + stat_t status = mach_zero_axis(axis); + if (status != STAT_OK) return status; + } + + return STAT_OK; +} + + +stat_t mach_zero_axis(unsigned axis) { + if (!mp_is_quiescent()) return STAT_MACH_NOT_QUIESCENT; + if (AXES <= axis) return STAT_INVALID_AXIS; + + mach_set_axis_position(axis, 0); + + return STAT_OK; +} + + // G28.3 functions and support static void _exec_absolute_origin(float *value, float *flag) { for (int axis = 0; axis < AXES; axis++) @@ -730,7 +752,7 @@ void mach_set_absolute_origin(float origin[], float flag[]) { value[axis] = TO_MILLIMETERS(origin[axis]); mach.position[axis] = value[axis]; // set model position mach.ms.target[axis] = value[axis]; // reset model target - mp_set_planner_position(axis, value[axis]); // set mm position + mp_set_axis_position(axis, value[axis]); // set mm position } mp_queue_command(_exec_absolute_origin, value, flag); diff --git a/src/machine.h b/src/machine.h index 50378c7..4814b8d 100644 --- a/src/machine.h +++ b/src/machine.h @@ -377,9 +377,12 @@ void mach_set_distance_mode(distance_mode_t mode); void mach_set_coord_offsets(coord_system_t coord_system, float offset[], float flag[]); -void mach_set_position(int axis, float position); +void mach_set_axis_position(unsigned axis, float position); void mach_set_absolute_origin(float origin[], float flag[]); +stat_t mach_zero_all(); +stat_t mach_zero_axis(unsigned axis); + void mach_set_coord_system(coord_system_t coord_system); void mach_set_origin_offsets(float offset[], float flag[]); void mach_reset_origin_offsets(); diff --git a/src/messages.def b/src/messages.def index 686de5b..d48da3b 100644 --- a/src/messages.def +++ b/src/messages.def @@ -37,6 +37,7 @@ 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(INTERNAL_ERROR, "Internal error") STAT_MSG(PREP_LINE_MOVE_TIME_IS_INFINITE, "Move time is infinite") @@ -72,6 +73,7 @@ STAT_MSG(MINIMUM_TIME_MOVE, "Move less than minimum time") STAT_MSG(MACHINE_ALARMED, "Machine is 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") // Homing STAT_MSG(HOMING_CYCLE_FAILED, "Homing cycle failed") diff --git a/src/plan/exec.c b/src/plan/exec.c index 7f33053..1f6d112 100644 --- a/src/plan/exec.c +++ b/src/plan/exec.c @@ -518,8 +518,8 @@ stat_t mp_exec_aline(mp_buffer_t *bf) { /// Dequeues buffer and executes move callback stat_t mp_exec_move() { - if (mp_get_state() == STATE_ESTOPPED) return STAT_MACHINE_ALARMED; - if (mp_get_state() == STATE_HOLDING) return STAT_NOOP; + if (mp_get_state() == STATE_ESTOPPED || + mp_get_state() == STATE_HOLDING) return STAT_NOOP; mp_buffer_t *bf = mp_get_run_buffer(); if (!bf) return STAT_NOOP; // Nothing running diff --git a/src/plan/jog.c b/src/plan/jog.c index d652bbf..2d25c94 100644 --- a/src/plan/jog.c +++ b/src/plan/jog.c @@ -89,7 +89,7 @@ static stat_t _exec_jog(mp_buffer_t *bf) { if (done) { // Update machine position for (int axis = 0; axis < AXES; axis++) - mach_set_position(axis, mp_runtime_get_work_position(axis)); + mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); return STAT_NOOP; // Done } diff --git a/src/plan/line.c b/src/plan/line.c index 0718e6d..4376a04 100644 --- a/src/plan/line.c +++ b/src/plan/line.c @@ -56,7 +56,7 @@ move_master_t mm = {{0}}; // context for line planning /// Set planner position for a single axis -void mp_set_planner_position(int axis, const float position) { +void mp_set_axis_position(int axis, float position) { mm.position[axis] = position; } diff --git a/src/plan/line.h b/src/plan/line.h index b49d446..75ce7d5 100644 --- a/src/plan/line.h +++ b/src/plan/line.h @@ -29,5 +29,5 @@ #include "machine.h" -void mp_set_planner_position(int axis, const float position); +void mp_set_axis_position(int axis, float position); stat_t mp_aline(move_state_t *ms); diff --git a/src/plan/planner.c b/src/plan/planner.c index 1fc625a..49eb6da 100644 --- a/src/plan/planner.c +++ b/src/plan/planner.c @@ -596,8 +596,7 @@ void mp_replan_blocks() { * [2] Cannot assume Vf >= Vi due to rounding errors and use of * PLANNER_VELOCITY_TOLERANCE necessitating the introduction of fabs() */ -float mp_get_target_length(const float Vi, const float Vf, - const mp_buffer_t *bf) { +float mp_get_target_length(float Vi, float Vf, const mp_buffer_t *bf) { return fabs(Vi - Vf) * sqrt(fabs(Vi - Vf) * bf->recip_jerk); } @@ -668,8 +667,7 @@ float mp_get_target_length(const float Vi, const float Vf, * * J'(x) = (2 * Vi * x - Vi^2 + 3 * x^2) / L^2 */ -float mp_get_target_velocity(const float Vi, const float L, - const mp_buffer_t *bf) { +float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf) { // 0 iterations (a reasonable estimate) float x = pow(L, 0.66666666) * bf->cbrt_jerk + Vi; // First estimate diff --git a/src/plan/planner.h b/src/plan/planner.h index e5b620e..db1417f 100644 --- a/src/plan/planner.h +++ b/src/plan/planner.h @@ -78,7 +78,5 @@ void mp_flush_planner(); void mp_kinematics(const float travel[], float steps[]); void mp_plan_block_list(mp_buffer_t *bf); void mp_replan_blocks(); -float mp_get_target_length(const float Vi, const float Vf, - const mp_buffer_t *bf); -float mp_get_target_velocity(const float Vi, const float L, - const mp_buffer_t *bf); +float mp_get_target_length(float Vi, float Vf, const mp_buffer_t *bf); +float mp_get_target_velocity(float Vi, float L, const mp_buffer_t *bf); diff --git a/src/plan/state.c b/src/plan/state.c index 310cd2c..09c99fe 100644 --- a/src/plan/state.c +++ b/src/plan/state.c @@ -51,7 +51,9 @@ typedef struct { } planner_state_t; -static planner_state_t ps = {0}; +static planner_state_t ps = { + .flush_requested = true // Start out flushing +}; mp_state_t mp_get_state() {return ps.state;} @@ -186,7 +188,7 @@ void mp_state_callback() { // Reset to actual machine position. Otherwise machine is set to the // position of the last queued move. for (int axis = 0; axis < AXES; axis++) - mach_set_position(axis, mp_runtime_get_axis_position(axis)); + mach_set_axis_position(axis, mp_runtime_get_axis_position(axis)); } // Resume diff --git a/src/probing.c b/src/probing.c index c9e2bee..989ab43 100644 --- a/src/probing.c +++ b/src/probing.c @@ -114,7 +114,7 @@ static void _probing_finish() { for (int axis = 0; axis < AXES; axis++) { // if we got here because of a feed hold keep the model position correct - mach_set_position(axis, mp_runtime_get_work_position(axis)); + mach_set_axis_position(axis, mp_runtime_get_work_position(axis)); // store the probe results pb.results[axis] = mach_get_absolute_position(axis);