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
} axis_t;
-axis_t axes[MOTORS] = {{0}};
+axis_t axes[MOTORS] = {};
bool axis_is_enabled(int axis) {
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)
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)
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);
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"
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
#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
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;}
// 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;
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
}
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();
#include "util.h"
#include "exec.h"
#include "state.h"
+#include "scurve.h"
#include "config.h"
#include <stdbool.h>
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
}
-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];
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)) {
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;
}
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;
}
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
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++;
#include "exec.h"
#include "axis.h"
#include "command.h"
+#include "scurve.h"
+#include "state.h"
#include "util.h"
#include <math.h>
float target[AXES];
float times[7];
float target_vel;
+ float max_accel;
float max_jerk;
float unit[AXES];
+ float length;
} line_t;
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);
}
// 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;
}
}
+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
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;
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;
}
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;
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);
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;
\******************************************************************************/
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")
--- /dev/null
+/******************************************************************************\
+
+ 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 <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 "scurve.h"
+
+#include <math.h>
+
+
+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;
+}
--- /dev/null
+/******************************************************************************\
+
+ 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 <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
+
+
+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);
// 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;
+}
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
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")
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
projects / bbctrl-firmware / commitdiff