From 3ea9718d46cbc356a0f09c97699e970b2940c74f Mon Sep 17 00:00:00 2001 From: Joseph Coffland Date: Tue, 13 Mar 2018 15:37:30 -0700 Subject: [PATCH] More work on jogging --- src/avr/src/SCurve.cpp | 52 ++++++++++++++++++++++++++++-- src/avr/src/SCurve.h | 5 ++- src/avr/src/jog.c | 25 ++++++++++++--- src/avr/src/scurve.c | 72 ------------------------------------------ src/avr/src/scurve.h | 35 -------------------- 5 files changed, 73 insertions(+), 116 deletions(-) delete mode 100644 src/avr/src/scurve.c delete mode 100644 src/avr/src/scurve.h diff --git a/src/avr/src/SCurve.cpp b/src/avr/src/SCurve.cpp index 88680b4..08fad3c 100644 --- a/src/avr/src/SCurve.cpp +++ b/src/avr/src/SCurve.cpp @@ -38,6 +38,14 @@ SCurve::SCurve(float maxV, float maxA, float maxJ) : 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; @@ -55,9 +63,10 @@ unsigned SCurve::getPhase() const { 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; @@ -76,6 +85,43 @@ float SCurve::next(float t, float targetV) { } +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; @@ -129,7 +175,7 @@ float SCurve::stoppingDist(float v, float a, float maxA, float maxJ) { 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 diff --git a/src/avr/src/SCurve.h b/src/avr/src/SCurve.h index b06ae20..bcdf9b4 100644 --- a/src/avr/src/SCurve.h +++ b/src/avr/src/SCurve.h @@ -27,6 +27,8 @@ #pragma once +#include + class SCurve { float maxV; @@ -53,7 +55,8 @@ public: 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, diff --git a/src/avr/src/jog.c b/src/avr/src/jog.c index 7227569..db2a513 100644 --- a/src/avr/src/jog.c +++ b/src/avr/src/jog.c @@ -73,15 +73,30 @@ stat_t jog_exec() { 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; diff --git a/src/avr/src/scurve.c b/src/avr/src/scurve.c deleted file mode 100644 index ddf973b..0000000 --- a/src/avr/src/scurve.c +++ /dev/null @@ -1,72 +0,0 @@ -/******************************************************************************\ - - 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 . - - 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 -#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 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; - } -} diff --git a/src/avr/src/scurve.h b/src/avr/src/scurve.h deleted file mode 100644 index 0f3c5ab..0000000 --- a/src/avr/src/scurve.h +++ /dev/null @@ -1,35 +0,0 @@ -/******************************************************************************\ - - 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 . - - 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 time, float Vi, float Vt, float accel, float aMax, - float jerk); -- 2.27.0