From: Joseph Coffland Date: Fri, 5 Jan 2018 09:22:02 +0000 (-0800) Subject: Implemented pause X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=617fbe033234c0f2c056f0c8893e48ae4d767970;p=bbctrl-firmware Implemented pause --- diff --git a/src/avr/src/axis.c b/src/avr/src/axis.c index 3af8ef1..5f151d3 100644 --- a/src/avr/src/axis.c +++ b/src/avr/src/axis.c @@ -41,6 +41,7 @@ int motor_map[AXES] = {-1, -1, -1, -1, -1, -1}; typedef struct { float velocity_max; // max velocity in mm/min or deg/min + float accel_max; // max acceleration in mm/min^2 float travel_max; // max work envelope for soft limits float travel_min; // min work envelope for soft limits float jerk_max; // max jerk (Jm) in km/min^3 @@ -54,7 +55,7 @@ typedef struct { } axis_t; -axis_t axes[MOTORS] = {{0}}; +axis_t axes[MOTORS] = {}; bool axis_is_enabled(int axis) { @@ -124,6 +125,7 @@ float axis_get_vector_length(const float a[], const float b[]) { AXIS_SET(homed, bool) AXIS_GET(velocity_max, float, 0) +AXIS_GET(accel_max, float, 0) AXIS_GET(homed, bool, false) AXIS_GET(homing_mode, homing_mode_t, HOMING_MANUAL) AXIS_GET(radius, float, 0) @@ -149,6 +151,7 @@ AXIS_VAR_GET(jerk_max, float) AXIS_VAR_SET(velocity_max, float) +AXIS_VAR_SET(accel_max, float) AXIS_VAR_SET(radius, float) AXIS_VAR_SET(travel_min, float) AXIS_VAR_SET(travel_max, float) diff --git a/src/avr/src/axis.h b/src/avr/src/axis.h index 810ea2d..866772d 100644 --- a/src/avr/src/axis.h +++ b/src/avr/src/axis.h @@ -55,6 +55,7 @@ void axis_map_motors(); float axis_get_vector_length(const float a[], const float b[]); float axis_get_velocity_max(int axis); +float axis_get_accel_max(int axis); float axis_get_jerk_max(int axis); bool axis_get_homed(int axis); void axis_set_homed(int axis, bool homed); diff --git a/src/avr/src/commands.c b/src/avr/src/commands.c index d639a10..cceec0e 100644 --- a/src/avr/src/commands.c +++ b/src/avr/src/commands.c @@ -63,13 +63,6 @@ unsigned command_out_size() {return 0;} void command_out_exec(void *data) {} -stat_t command_pause(char *cmd) { - if (cmd[1] == '1') state_request_optional_pause(); - else state_request_pause(); - return STAT_OK; -} - - stat_t command_help(char *cmd) { puts_P(PSTR("\nLine editing:\n" " ENTER Submit current command line.\n" diff --git a/src/avr/src/config.h b/src/avr/src/config.h index 5d8ced0..a1ca187 100644 --- a/src/avr/src/config.h +++ b/src/avr/src/config.h @@ -167,8 +167,8 @@ enum { DRV8711_DRIVE_TDRIVEN_250 | DRV8711_DRIVE_OCPDEG_1 | \ DRV8711_DRIVE_OCPTH_250) #define DRV8711_TORQUE DRV8711_TORQUE_SMPLTH_200 -#define DRV8711_CTRL (DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_450 | \ - DRV8711_CTRL_EXSTALL_bm) +#define DRV8711_CTRL (DRV8711_CTRL_ISGAIN_10 | DRV8711_CTRL_DTIME_450 | \ + DRV8711_CTRL_EXSTALL_bm) // Huanyang settings @@ -206,7 +206,7 @@ enum { #define MOTOR_IDLE_TIMEOUT 0.25 // secs, motor off after this time #define MIN_HALF_STEP_CORRECTION 4 -#define JOG_MIN_VELOCITY 10 // mm/min +#define MIN_VELOCITY 10 // mm/min #define CURRENT_SENSE_RESISTOR 0.05 // ohms #define CURRENT_SENSE_REF 2.75 // volts #define MAX_CURRENT 10 // amps diff --git a/src/avr/src/exec.c b/src/avr/src/exec.c index 6111d02..4b531ea 100644 --- a/src/avr/src/exec.c +++ b/src/avr/src/exec.c @@ -82,6 +82,7 @@ float exec_get_axis_position(int axis) {return ex.position[axis];} void exec_set_velocity(float v) {ex.velocity = v;} float exec_get_velocity() {return ex.velocity;} void exec_set_acceleration(float a) {ex.accel = a;} +float exec_get_acceleration() {return ex.accel;} void exec_set_jerk(float j) {ex.jerk = j;} void exec_set_line(int32_t line) {ex.line = line;} int32_t exec_get_line() {return ex.line;} @@ -100,7 +101,7 @@ stat_t exec_move_to_target(float time, const float target[]) { // No move if time is too short if (time < 0.5 * SEGMENT_TIME) { ex.leftover_time += time; - return STAT_NOOP; + return STAT_NOP; } time += ex.leftover_time; @@ -117,8 +118,8 @@ void exec_reset_encoder_counts() {st_set_position(ex.position);} stat_t exec_next() { - if (!ex.cb && !command_exec()) return STAT_NOOP; // Queue empty - if (!ex.cb) return STAT_EAGAIN; // Non-exec command + if (!ex.cb && !command_exec()) return STAT_NOP; // Queue empty + if (!ex.cb) return STAT_AGAIN; // Non-exec command return ex.cb(); // Exec } diff --git a/src/avr/src/exec.h b/src/avr/src/exec.h index 2ae2fbc..03f7b7f 100644 --- a/src/avr/src/exec.h +++ b/src/avr/src/exec.h @@ -45,6 +45,7 @@ float exec_get_axis_position(int axis); void exec_set_velocity(float v); float exec_get_velocity(); void exec_set_acceleration(float a); +float exec_get_acceleration(); void exec_set_jerk(float j); void exec_set_line(int32_t line); int32_t exec_get_line(); diff --git a/src/avr/src/jog.c b/src/avr/src/jog.c index 4a75ab0..bcb8faf 100644 --- a/src/avr/src/jog.c +++ b/src/avr/src/jog.c @@ -31,6 +31,7 @@ #include "util.h" #include "exec.h" #include "state.h" +#include "scurve.h" #include "config.h" #include @@ -76,12 +77,12 @@ static bool _axis_velocity_target(int axis) { float Vi = a->velocity; float Vt = a->target; - if (JOG_MIN_VELOCITY < fabs(Vn)) jr.done = false; // Still jogging + if (MIN_VELOCITY < fabs(Vn)) jr.done = false; // Still jogging if (!fp_ZERO(Vi) && (Vn < 0) != (Vi < 0)) Vn = 0; // Plan to zero on sign change - if (fabs(Vn) < JOG_MIN_VELOCITY) Vn = 0; + if (fabs(Vn) < MIN_VELOCITY) Vn = 0; if (Vt == Vn) return false; // No change @@ -198,17 +199,6 @@ static bool _soft_limit(int axis, float V, float A) { } -static float _next_accel(float Vi, float Vt, float Ai, float jerk) { - float At = sqrt(jerk * fabs(Vt - Vi)) * (Vt < Vi ? -1 : 1); // Target accel - float Ad = jerk * SEGMENT_TIME; // Delta accel - - if (Ai < At) return (Ai < At + Ad) ? At : (Ai + Ad); - if (At < Ai) return (Ai - Ad < At) ? At : (Ai - Ad); - - return Ai; -} - - static float _compute_axis_velocity(int axis) { jog_axis_t *a = &jr.axes[axis]; @@ -216,7 +206,7 @@ static float _compute_axis_velocity(int axis) { float Vt = fabs(a->target); // Apply soft limits - if (_soft_limit(axis, V, a->accel)) Vt = JOG_MIN_VELOCITY; + if (_soft_limit(axis, V, a->accel)) Vt = MIN_VELOCITY; // Check if velocity has reached its target if (fp_EQ(V, Vt)) { @@ -228,7 +218,9 @@ static float _compute_axis_velocity(int axis) { float jerk = axis_get_jerk_max(axis); // Compute next accel - a->accel = _next_accel(V, Vt, a->accel, jerk); + a->accel = scurve_next_accel(SEGMENT_TIME, V, Vt, a->accel, jerk); + + // TODO limit acceleration return V + a->accel * SEGMENT_TIME; } @@ -250,7 +242,7 @@ stat_t jog_exec() { for (int axis = 0; axis < AXES; axis++) { if (!axis_is_enabled(axis)) continue; float V = _compute_axis_velocity(axis); - if (JOG_MIN_VELOCITY < V) jr.done = false; + if (MIN_VELOCITY < V) jr.done = false; velocity_sqr += square(V); jr.axes[axis].velocity = V * jr.axes[axis].sign; } @@ -260,7 +252,7 @@ stat_t jog_exec() { exec_set_cb(0); jr.active = false; - return STAT_NOOP; // Done, no move executed + return STAT_NOP; // Done, no move executed } // Compute target from velocity @@ -283,7 +275,7 @@ stat_t jog_exec() { stat_t command_jog(char *cmd) { // Ignore jog commands when not already idle - if (!jr.active && state_get() != STATE_READY) return STAT_NOOP; + if (!jr.active && state_get() != STATE_READY) return STAT_NOP; // Skip command code cmd++; diff --git a/src/avr/src/line.c b/src/avr/src/line.c index 81dd613..37ab3a1 100644 --- a/src/avr/src/line.c +++ b/src/avr/src/line.c @@ -29,6 +29,8 @@ #include "exec.h" #include "axis.h" #include "command.h" +#include "scurve.h" +#include "state.h" #include "util.h" #include @@ -41,9 +43,11 @@ typedef struct { float target[AXES]; float times[7]; float target_vel; + float max_accel; float max_jerk; float unit[AXES]; + float length; } line_t; @@ -54,22 +58,32 @@ static struct { bool stop_seg; float current_t; - float dist; - float vel; - float accel; + float iD; // Initial segment distance + float iV; // Initial segment velocity + float iA; // Initial segment acceleration float jerk; + float dist; } l = {.current_t = SEGMENT_TIME}; -static float _compute_distance(float t, float v, float a, float j) { - // v * t + 1/2 * a * t^2 + 1/6 * j * t^3 - return t * (v + t * (0.5 * a + 1.0 / 6.0 * j * t)); +static void _segment_target(float target[AXES], float d) { + for (int i = 0; i < AXES; i++) + target[i] = l.line.start[i] + l.line.unit[i] * d; +} + + +static float _segment_distance(float t) { + return l.iD + scurve_distance(t, l.iV, l.iA, l.jerk); +} + + +static float _segment_velocity(float t) { + return l.iV + scurve_velocity(t, l.iA, l.jerk); } -static float _compute_velocity(float t, float a, float j) { - // a * t + 1/2 * j * t^2 - return t * (a + 0.5 * j * t); +static float _segment_accel(float t) { + return l.iA + scurve_acceleration(t, l.jerk); } @@ -96,17 +110,17 @@ static bool _segment_next() { // Acceleration switch (l.seg) { - case 1: case 2: l.accel = l.line.max_jerk * l.line.times[0]; break; - case 5: case 6: l.accel = -l.line.max_jerk * l.line.times[4]; break; - default: l.accel = 0; + case 1: case 2: l.iA = l.line.max_jerk * l.line.times[0]; break; + case 5: case 6: l.iA = -l.line.max_jerk * l.line.times[4]; break; + default: l.iA = 0; } - exec_set_acceleration(l.accel); + exec_set_acceleration(l.iA); // Skip segs which do not land on a SEGMENT_TIME if (seg_t < l.current_t && !l.stop_seg) { l.current_t -= l.line.times[l.seg]; - l.dist += _compute_distance(seg_t, l.vel, l.accel, l.jerk); - l.vel += _compute_velocity(seg_t, l.accel, l.jerk); + l.iD = _segment_distance(seg_t); + l.iV = _segment_velocity(seg_t); continue; } @@ -117,7 +131,73 @@ static bool _segment_next() { } +static stat_t _hold() { + if (state_get() != STATE_HOLDING) { + exec_set_cb(0); + return STAT_AGAIN; + } + + return STAT_NOP; +} + + +static stat_t _pause() { + float t = SEGMENT_TIME - l.current_t; + float v = exec_get_velocity(); + float a = exec_get_acceleration(); + float j = l.line.max_jerk; + + if (v < MIN_VELOCITY) { + exec_set_velocity(0); + exec_set_acceleration(0); + exec_set_jerk(0); + state_holding(); + exec_set_cb(_hold); + return _hold(); + } + + // Compute new velocity and acceleration + a = scurve_next_accel(t, v, 0, a, j); + if (l.line.max_accel < a) a = l.line.max_accel; + v += a * t; + + // Compute distance that will be traveled + l.dist += v * t; + + if (l.line.length < l.dist) { + // Compute time left in current segment + l.current_t = t - (l.dist - l.line.length) / v; + + exec_set_velocity(l.line.target_vel); + exec_set_acceleration(0); + exec_set_jerk(0); + exec_set_cb(0); + + return STAT_AGAIN; + } + + // Compute target position from distance + float target[AXES]; + _segment_target(target, l.dist); + + stat_t status = exec_move_to_target(SEGMENT_TIME, target); + + l.current_t = 0; + exec_set_velocity(v); + exec_set_acceleration(a); + exec_set_jerk(j); + + return status; +} + + static stat_t _line_exec() { + if (state_get() == STATE_STOPPING && (l.seg < 4 || l.line.target_vel)) { + if (SEGMENT_TIME < l.current_t) l.current_t = 0; + exec_set_cb(_pause); + return _pause(); + } + float seg_t = l.line.times[l.seg]; // Adjust time if near a stopping point @@ -127,14 +207,10 @@ static stat_t _line_exec() { l.current_t = seg_t; } - // Compute distance and velocity offsets - float d = l.dist + _compute_distance(l.current_t, l.vel, l.accel, l.jerk); - float v = l.vel + _compute_velocity(l.current_t, l.accel, l.jerk); - - // Compute target position from distance - float target[AXES]; - for (int i = 0; i < AXES; i++) - target[i] = l.line.start[i] + l.line.unit[i] * d; + // Compute distance and velocity (Must be computed before changing time) + float d = _segment_distance(l.current_t); + float v = _segment_velocity(l.current_t); + float a = _segment_accel(l.current_t); // Advance time l.current_t += SEGMENT_TIME; @@ -143,24 +219,35 @@ static stat_t _line_exec() { bool lastSeg = false; if (seg_t < l.current_t) { l.current_t -= seg_t; - l.dist += _compute_distance(seg_t, l.vel, l.accel, l.jerk); - l.vel += _compute_velocity(seg_t, l.accel, l.jerk); + l.iD = _segment_distance(seg_t); + l.iV = _segment_velocity(seg_t); // Next segment lastSeg = !_segment_next(); } - // Do move - // Stop exactly on target to correct for floating-point errors - stat_t status = exec_move_to_target - (delta, (lastSeg && l.stop_seg) ? l.line.target : target); + // Do move & update exec + stat_t status; - // Check if we're done - if (lastSeg) { - exec_set_velocity(l.vel = l.line.target_vel); - exec_set_cb(0); + if (lastSeg && l.stop_seg) { + // Stop exactly on target to correct for floating-point errors + status = exec_move_to_target(delta, l.line.target); + exec_set_velocity(0); + exec_set_acceleration(0); + + } else { + // Compute target position from distance + float target[AXES]; + _segment_target(target, d); - } else exec_set_velocity(v); + status = exec_move_to_target(delta, target); + l.dist = d; + exec_set_velocity(v); + exec_set_acceleration(a); + } + + // Release exec if we are done + if (lastSeg) exec_set_cb(0); return status; } @@ -184,6 +271,10 @@ stat_t command_line(char *cmd) { if (!decode_float(&cmd, &line.target_vel)) return STAT_BAD_FLOAT; if (line.target_vel < 0) return STAT_INVALID_ARGUMENTS; + // Get max accel + if (!decode_float(&cmd, &line.max_accel)) return STAT_BAD_FLOAT; + if (line.max_accel < 0) return STAT_INVALID_ARGUMENTS; + // Get max jerk if (!decode_float(&cmd, &line.max_jerk)) return STAT_BAD_FLOAT; if (line.max_jerk < 0) return STAT_INVALID_ARGUMENTS; @@ -221,17 +312,16 @@ stat_t command_line(char *cmd) { copy_vector(next_start, line.target); // Compute direction vector - float length = 0; for (int i = 0; i < AXES; i++) if (axis_is_enabled(i)) { line.unit[i] = line.target[i] - line.start[i]; - length += line.unit[i] * line.unit[i]; + line.length += line.unit[i] * line.unit[i]; } else line.unit[i] = 0; - length = sqrt(length); + line.length = sqrt(line.length); for (int i = 0; i < AXES; i++) - if (line.unit[i]) line.unit[i] /= length; + if (line.unit[i]) line.unit[i] /= line.length; // Queue command_push(COMMAND_line, &line); @@ -247,8 +337,8 @@ void command_line_exec(void *data) { l.line = *(line_t *)data; // Init dist & velocity - l.dist = 0; - l.vel = exec_get_velocity(); + l.iD = l.dist = 0; + l.iV = exec_get_velocity(); // Find first segment l.seg = -1; diff --git a/src/avr/src/messages.def b/src/avr/src/messages.def index 2613ac0..d1c3070 100644 --- a/src/avr/src/messages.def +++ b/src/avr/src/messages.def @@ -26,8 +26,8 @@ \******************************************************************************/ STAT_MSG(OK, "OK") -STAT_MSG(EAGAIN, "Run command again") -STAT_MSG(NOOP, "No op") +STAT_MSG(AGAIN, "Run command again") +STAT_MSG(NOP, "No op") STAT_MSG(PAUSE, "Pause") STAT_MSG(INTERNAL_ERROR, "Internal error") STAT_MSG(ESTOP_USER, "User triggered EStop") diff --git a/src/avr/src/scurve.c b/src/avr/src/scurve.c new file mode 100644 index 0000000..08efe8e --- /dev/null +++ b/src/avr/src/scurve.c @@ -0,0 +1,56 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 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 "scurve.h" + +#include + + +float scurve_distance(float t, float v, float a, float j) { + // v * t + 1/2 * a * t^2 + 1/6 * j * t^3 + return t * (v + t * (0.5 * a + 1.0 / 6.0 * j * t)); +} + + +float scurve_velocity(float t, float a, float j) { + // a * t + 1/2 * j * t^2 + return t * (a + 0.5 * j * t); +} + + +float scurve_acceleration(float t, float j) {return j * t;} + + +float scurve_next_accel(float dT, float iV, float tV, float iA, float jerk) { + float tA = sqrt(jerk * fabs(tV - iV)) * (tV < iV ? -1 : 1); // Target accel + float dA = jerk * dT; // Delta accel + + if (iA < tA) return (iA < tA + dA) ? tA : (iA + dA); + if (tA < iA) return (iA - dA < tA) ? tA : (iA - dA); + + return iA; +} diff --git a/src/avr/src/scurve.h b/src/avr/src/scurve.h new file mode 100644 index 0000000..58bc836 --- /dev/null +++ b/src/avr/src/scurve.h @@ -0,0 +1,34 @@ +/******************************************************************************\ + + This file is part of the Buildbotics firmware. + + Copyright (c) 2015 - 2017 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 + + +float scurve_distance(float time, float vel, float accel, float jerk); +float scurve_velocity(float time, float accel, float jerk); +float scurve_acceleration(float time, float jerk); +float scurve_next_accel(float dT, float iV, float tV, float iA, float jerk); diff --git a/src/avr/src/state.c b/src/avr/src/state.c index bed09f3..4130349 100644 --- a/src/avr/src/state.c +++ b/src/avr/src/state.c @@ -204,3 +204,11 @@ void state_callback() { // Var callbacks PGM_P get_state() {return state_get_pgmstr(state_get());} PGM_P get_hold_reason() {return state_get_hold_reason_pgmstr(s.hold_reason);} + + +// Command callbacks +stat_t command_pause(char *cmd) { + if (cmd[1] == '1') state_request_optional_pause(); + else state_request_pause(); + return STAT_OK; +} diff --git a/src/avr/src/stepper.c b/src/avr/src/stepper.c index 35464db..a337c1b 100644 --- a/src/avr/src/stepper.c +++ b/src/avr/src/stepper.c @@ -112,8 +112,8 @@ ISR(STEP_LOW_LEVEL_ISR) { stat_t status = exec_next(); switch (status) { - case STAT_NOOP: st.busy = false; break; // No command executed - case STAT_EAGAIN: continue; // No command executed, try again + case STAT_NOP: st.busy = false; break; // No command executed + case STAT_AGAIN: continue; // No command executed, try again case STAT_OK: // Move executed if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued diff --git a/src/avr/src/vars.def b/src/avr/src/vars.def index 840848f..d19fe5d 100644 --- a/src/avr/src/vars.def +++ b/src/avr/src/vars.def @@ -50,6 +50,7 @@ VAR(error, ee, s32, MOTORS, 0, 0, "Motor position error") VAR(motor_fault, fa, bool, 0, 0, 1, "Motor fault status") VAR(velocity_max, vm, f32, MOTORS, 1, 1, "Maxium vel in mm/min") +VAR(accel_max, am, f32, MOTORS, 1, 1, "Maxium accel in mm/min^2") VAR(jerk_max, jm, f32, MOTORS, 1, 1, "Maxium jerk in mm/min^3") VAR(radius, ra, f32, MOTORS, 1, 1, "Axis radius or zero") diff --git a/src/avr/test/planner-test.c b/src/avr/test/planner-test.c index c68fab9..8091ac1 100644 --- a/src/avr/test/planner-test.c +++ b/src/avr/test/planner-test.c @@ -58,8 +58,8 @@ int main(int argc, char *argv[]) { printf("EXEC: %s\n", status_to_pgmstr(status)); switch (status) { - case STAT_NOOP: break; // No command executed - case STAT_EAGAIN: continue; // No command executed, try again + case STAT_NOP: break; // No command executed + case STAT_AGAIN: continue; // No command executed, try again case STAT_OK: // Move executed //if (!st.move_queued) ALARM(STAT_EXPECTED_MOVE); // No move was queued