unsigned SCurve::getPhase() const {
if (!v) return 0;
+ // Handle negative velocity
+ float v = this->v;
+ float a = this->a;
+ if (v < 0) {
+ v = -v;
+ a = -a;
+ }
+
if (0 < a) {
if (0 < j) return 1;
if (!j) return 2;
float SCurve::getStoppingDist() const {return stoppingDist(v, a, maxA, maxJ);}
-float SCurve::next(float t, float targetV) {
+float SCurve::next(float t, float targetV, float overrideJ) {
// Compute next acceleration
- float nextA = nextAccel(t, targetV, v, a, maxA, maxJ);
+ float nextA =
+ nextAccel(t, targetV, v, a, maxA, isfinite(overrideJ) ? overrideJ : maxJ);
// Compute next velocity
float deltaV = nextA * t;
}
+float SCurve::adjustedJerkForStoppingDist(float d) const {
+ // Only make adjustments in phase 7
+ if (getPhase() != 7) return NAN;
+
+ // Handle negative velocity
+ float v = this->v;
+ float a = this->a;
+ if (v < 0) {
+ v = -v;
+ a = -a;
+ }
+
+ // Compute distance to zero vel
+ float actualD = distance(-a / maxJ, v, 0, maxJ);
+ const float fudge = 0.05;
+ if (actualD < d) return maxJ * (1 + fudge);
+ else if (d < actualD) return maxJ * (1 - fudge);
+ return maxJ;
+
+ // Compute jerk to hit distance exactly
+ //
+ // S-curve distance formula
+ // d = vt + 1/2 at^2 + 1/6 jt^3
+ //
+ // t = -a/j when a < 0
+ //
+ // Substitute and simplify
+ // (d = -va/j + (a^3)/(2j^2) - (a^3)/(6j^2)) * j^2
+ // dj^2 = -jva + 1/2 a^3 - 1/6 a^3
+ // dj^2 + vaj - 1/3 a^3 = 0
+ //
+ // Apply quadratic formula
+ // j = (-va - sqrt((va)^2 + 4/3 da^3)) / (2d)
+ return -a * (v + sqrt(v * v + 1.333333 * d * a)) / (2 * d);
+}
+
+
float SCurve::stoppingDist(float v, float a, float maxA, float maxJ) {
// Already stopped
if (!v) return 0;
float SCurve::nextAccel(float t, float targetV, float v, float a, float maxA,
float maxJ) {
bool increasing = v < targetV;
- float deltaA = t * maxJ;
+ float deltaA = acceleration(t, maxJ);
if (increasing && a < -deltaA)
return a + deltaA; // negative accel, increasing speed
#pragma once
+#include <math.h>
+
class SCurve {
float maxV;
unsigned getPhase() const;
float getStoppingDist() const;
- float next(float t, float targetV);
+ float next(float t, float targetV, float overrideJ = NAN);
+ float adjustedJerkForStoppingDist(float d) const;
static float stoppingDist(float v, float a, float maxA, float maxJ);
static float nextAccel(float t, float targetV, float v, float a, float maxA,
bool softLimited = min != max && axis_get_homed(axis);
// Apply soft limits, if enabled and homed
+ float jerk = jr.scurves[axis].getMaxJerk();
if (softLimited && MIN_VELOCITY < fabs(targetV)) {
- float dist = jr.scurves[axis].getStoppingDist() * 1.01;
-
- if (vel < 0 && p - dist <= min) targetV = -MIN_VELOCITY;
- if (0 < vel && max <= p + dist) targetV = MIN_VELOCITY;
+ float dist = jr.scurves[axis].getStoppingDist();
+
+ float targetDist = 0;
+ if (vel < 0 && p - dist <= min) {
+ targetV = -MIN_VELOCITY;
+ targetDist = p - min;
+ }
+
+ if (0 < vel && max <= p + dist) {
+ targetV = MIN_VELOCITY;
+ targetDist = max - p;
+ }
+
+ if (targetDist) {
+ float adjustedJ =
+ jr.scurves[axis].adjustedJerkForStoppingDist(targetDist);
+ if (isfinite(adjustedJ)) jerk = adjustedJ;
+ }
}
// Compute next velocity
- float v = jr.scurves[axis].next(SEGMENT_TIME, targetV);
+ float v = jr.scurves[axis].next(SEGMENT_TIME, targetV, jerk);
// Don't overshoot soft limits
float deltaP = v * SEGMENT_TIME;
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2018, 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>
-#include <stdbool.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 time, float Vi, float Vt, float accel, float aMax,
- float jerk) {
- bool increasing = Vi < Vt;
- float deltaA = time * jerk;
-
- if (increasing && accel < -deltaA)
- return accel + deltaA; // negative accel, increasing speed
-
- if (!increasing && deltaA < accel)
- return accel - deltaA; // positive accel, decreasing speed
-
- float deltaV = fabs(Vt - Vi);
- float targetA = sqrt(2 * deltaV * jerk);
- if (aMax < targetA) targetA = aMax;
-
- if (increasing) {
- if (targetA < accel + deltaA) return targetA;
- return accel + deltaA;
-
- } else {
- if (accel - deltaA < -targetA) return -targetA;
- return accel - deltaA;
- }
-}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2018, 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 time, float Vi, float Vt, float accel, float aMax,
- float jerk);
projects / bbctrl-firmware / commitdiff