#include "motor.h"
#include "rtc.h"
#include "usart.h"
+#include "velocity_curve.h"
#include "config.h"
#include <string.h>
static mp_exec_t ex = {{0}};
-/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity
-/// curve. This yields a constant pop; with pop being the sixth derivative
-/// of position:
-///
-/// 1st - velocity
-/// 2nd - acceleration
-/// 3rd - jerk
-/// 4th - snap
-/// 5th - crackle
-/// 6th - pop
-///
-/// The Bezier curve takes the form:
-///
-/// f(t) = P_0(1 - t)^5 + 5P_1(1 - t)^4 t + 10P_2(1 - t)^3 t^2 +
-/// 10P_3(1 - t)^2 t^3 + 5P_4(1 - t)t^4 + P_5t^5
-///
-/// Where 0 <= t <= 1, f(t) is the velocity and P_0 through P_5 are the control
-/// points. In our case:
-///
-/// P_0 = P_1 = P2 = Vi
-/// P_3 = P_4 = P5 = Vt
-///
-/// Where Vi is the initial velocity and Vt is the target velocity.
-///
-/// After substitution, expanding the polynomial and collecting terms we have:
-///
-/// f(t) = (Vt - Vi)(6t^5 - 15t^4 + 10t^3) + Vi
-///
-/// Computing this directly using 32bit float-point on a 32MHz AtXMega processor
-/// takes about 60uS or about 1,920 clocks. The code was compiled with avr-gcc
-/// v4.9.2 with -O3.
-
/// Common code for head and tail sections
static stat_t _exec_aline_section(float length, float Vi, float Vt) {
if (ex.section_new) {
if (Vi == Vt) ex.segment_dist += ex.segment_delta;
else {
// Compute quintic Bezier curve
- const float t = ex.segment * ex.segment_delta;
- const float t2 = t * t;
- const float t3 = t2 * t;
-
ex.segment_velocity =
- (Vt - Vi) * (6 * t2 - 15 * t + 10) * t3 + Vi;
+ velocity_curve(Vi, Vt, ex.segment * ex.segment_delta);
ex.segment_dist += ex.segment_velocity * ex.segment_time;
}
+++ /dev/null
-/******************************************************************************\
-
- This file is part of the Buildbotics firmware.
-
- Copyright (c) 2015 - 2017 Buildbotics LLC
- Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
- Copyright (c) 2012 - 2015 Rob Giseburt
- 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 "forward_dif.h"
-
-#include "util.h"
-
-#include <math.h>
-
-
-/// Forward differencing math
-///
-/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity
-/// curve. This gives us a constant "pop"; with pop being the sixth derivative
-/// of position:
-///
-/// 1st - velocity
-/// 2nd - acceleration
-/// 3rd - jerk
-/// 4th - snap
-/// 5th - crackle
-/// 6th - pop
-///
-/// The Bezier curve takes the form:
-///
-/// V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) +
-/// P_4 * B_4(t) + P_5 * B_5(t)
-///
-/// Where 0 <= t <= 1, and V(t) is the velocity. P_0 through P_5 are
-/// the control points, and B_0(t) through B_5(t) are the Bernstein
-/// basis as follows:
-///
-/// B_0(t) = (1 - t)^5 = -t^5 + 5t^4 - 10t^3 + 10t^2 - 5t + 1
-/// B_1(t) = 5(1 - t)^4 * t = 5t^5 - 20t^4 + 30t^3 - 20t^2 + 5t
-/// B_2(t) = 10(1 - t)^3 * t^2 = -10t^5 + 30t^4 - 30t^3 + 10t^2
-/// B_3(t) = 10(1 - t)^2 * t^3 = 10t^5 - 20t^4 + 10t^3
-/// B_4(t) = 5(1 - t) * t^4 = -5t^5 + 5t^4
-/// B_5(t) = t^5 = t^5
-///
-/// ^ ^ ^ ^ ^ ^
-/// A B C D E F
-///
-/// We use forward-differencing to calculate each position through the curve.
-/// This requires a formula of the form:
-///
-/// V_f(t) = A * t^5 + B * t^4 + C * t^3 + D * t^2 + E * t + F
-///
-/// Looking at the above B_0(t) through B_5(t) expanded forms, if we take the
-/// coefficients of t^5 through t of the Bezier form of V(t), we can determine
-/// that:
-///
-/// A = -P_0 + 5 * P_1 - 10 * P_2 + 10 * P_3 - 5 * P_4 + P_5
-/// B = 5 * P_0 - 20 * P_1 + 30 * P_2 - 20 * P_3 + 5 * P_4
-/// C = -10 * P_0 + 30 * P_1 - 30 * P_2 + 10 * P_3
-/// D = 10 * P_0 - 20 * P_1 + 10 * P_2
-/// E = - 5 * P_0 + 5 * P_1
-/// F = P_0
-///
-/// Since we want the initial acceleration and jerk values to be 0, We set
-///
-/// P_i = P_0 = P_1 = P_2 (initial velocity)
-/// P_t = P_3 = P_4 = P_5 (target velocity)
-///
-/// which, after simplification, resolves to:
-///
-/// A = - 6 * P_i + 6 * P_t
-/// B = 15 * P_i - 15 * P_t
-/// C = -10 * P_i + 10 * P_t
-/// D = 0
-/// E = 0
-/// F = P_i
-///
-/// Given an interval count of segments to get from P_i to P_t, we get the
-/// parametric "step" size:
-///
-/// h = 1 / segs
-///
-/// We need to calculate the initial value of forward differences (F_0 - F_5)
-/// such that the inital velocity V = P_i, then we iterate over the following
-/// segs times:
-///
-/// V += F_5
-/// F_5 += F_4
-/// F_4 += F_3
-/// F_3 += F_2
-/// F_2 += F_1
-///
-/// See
-/// http://www.drdobbs.com/forward-difference-calculation-of-bezier/184403417
-/// for an example of how to calculate F_0 - F_5 for a cubic bezier curve. Since
-/// this is a quintic bezier curve, we need to extend the formulas somewhat.
-/// I'll not go into the long-winded step-by-step here, but it gives the
-/// resulting formulas:
-///
-/// a = A, b = B, c = C, d = D, e = E, f = F
-///
-/// F_5(t + h) - F_5(t) = (5ah)t^4 + (10ah^2 + 4bh)t^3 +
-/// (10ah^3 + 6bh^2 + 3ch)t^2 + (5ah^4 + 4bh^3 + 3ch^2 + 2dh)t + ah^5 +
-/// bh^4 + ch^3 + dh^2 + eh
-///
-/// a = 5ah
-/// b = 10ah^2 + 4bh
-/// c = 10ah^3 + 6bh^2 + 3ch
-/// d = 5ah^4 + 4bh^3 + 3ch^2 + 2dh
-///
-/// After substitution, simplification, and rearranging:
-///
-/// F_4(t + h) - F_4(t) = (20ah^2)t^3 + (60ah^3 + 12bh^2)t^2 +
-/// (70ah^4 + 24bh^3 + 6ch^2)t + 30ah^5 + 14bh^4 + 6ch^3 + 2dh^2
-///
-/// a = 20ah^2
-/// b = 60ah^3 + 12bh^2
-/// c = 70ah^4 + 24bh^3 + 6ch^2
-///
-/// After substitution, simplification, and rearranging:
-///
-/// F_3(t + h) - F_3(t) = (60ah^3)t^2 + (180ah^4 + 24bh^3)t + 150ah^5 +
-/// 36bh^4 + 6ch^3
-///
-/// You get the picture...
-///
-/// F_2(t + h) - F_2(t) = (120ah^4)t + 240ah^5 + 24bh^4
-/// F_1(t + h) - F_1(t) = 120ah^5
-///
-/// Normally, we could then assign t = 0, use the A-F values from above, and get
-/// out initial F_* values. However, for the sake of "averaging" the velocity
-/// of each segment, we actually want to have the initial V be be at t = h/2 and
-/// iterate segs-1 times. So, the resulting F_* values are (steps not shown):
-///
-/// F_5 = 121Ah^5 / 16 + 5Bh^4 + 13Ch^3 / 4 + 2Dh^2 + Eh
-/// F_4 = 165Ah^5 / 2 + 29Bh^4 + 9Ch^3 + 2Dh^2
-/// F_3 = 255Ah^5 + 48Bh^4 + 6Ch^3
-/// F_2 = 300Ah^5 + 24Bh^4
-/// F_1 = 120Ah^5
-///
-/// Note that with our current control points, D and E are actually 0.
-///
-/// This can be simplified even further by subsituting Ah, Bh & Ch back in and
-/// reducing to:
-///
-/// F_5 = (32.5 * s^2 - 75 * s + 45.375)(Vt - Vi) * h^5
-/// F_4 = (90.0 * s^2 - 435 * s + 495.0 )(Vt - Vi) * h^5
-/// F_3 = (60.0 * s^2 - 720 * s + 1530.0 )(Vt - Vi) * h^5
-/// F_2 = ( - 360 * s + 1800.0 )(Vt - Vi) * h^5
-/// F_1 = ( 720.0 )(Vt - Vi) * h^5
-///
-float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float segs) {
- const float h = 1 / segs;
- const float segs2 = square(segs);
- const float Vdxh5 = (Vt - Vi) * h * h * h * h * h;
-
- fdif[4] = (32.5 * segs2 - 75.0 * segs + 45.375) * Vdxh5;
- fdif[3] = (90.0 * segs2 - 435.0 * segs + 495.0 ) * Vdxh5;
- fdif[2] = (60.0 * segs2 - 720.0 * segs + 1530.0 ) * Vdxh5;
- fdif[1] = ( - 360.0 * segs + 1800.0 ) * Vdxh5;
- fdif[0] = ( 720.0 ) * Vdxh5;
-
- // Calculate the initial velocity by calculating:
- //
- // V(h / 2) =
- //
- // ( -6Vi + 6Vt) / (2^5 * s^8) +
- // ( 15Vi - 15Vt) / (2^4 * s^8) +
- // (-10Vi + 10Vt) / (2^3 * s^8) + Vi =
- //
- // (Vt - Vi) * 1/2 * h^8 + Vi
- return (Vt - Vi) * 0.5 * square(square(square(h))) + Vi;
-}
-
-
-float mp_next_forward_dif(forward_dif_t fdif) {
- float delta = fdif[4];
-
- for (int i = 4; i; i--)
- fdif[i] += fdif[i - 1];
-
- return delta;
-}
+++ /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
-
-
-typedef float forward_dif_t[5];
-
-float mp_init_forward_dif(forward_dif_t fdif, float Vi, float Vt, float s);
-float mp_next_forward_dif(forward_dif_t fdif);
#include "planner.h"
#include "buffer.h"
#include "line.h"
-#include "forward_dif.h"
+#include "velocity_curve.h"
#include "runtime.h"
#include "machine.h"
#include "state.h"
bool writing;
float Vi;
float Vt;
- forward_dif_t fdifs[AXES];
- unsigned segments[AXES];
+ float velocity_delta[AXES];
+ float velocity_t[AXES];
int sign[AXES];
float velocity[AXES];
float next_velocity[AXES];
+ float initial_velocity[AXES];
float target_velocity[AXES];
} jog_runtime_t;
float velocity_sqr = 0;
+ // Compute per axis velocities
for (int axis = 0; axis < AXES; axis++) {
- float Vi = fabs(jr.velocity[axis]);
+ float V = fabs(jr.velocity[axis]);
float Vt = fabs(jr.target_velocity[axis]);
if (changed) {
- if (fp_EQ(Vi, Vt)) {
- Vi = Vt;
- jr.segments[axis] = 0;
+ if (fp_EQ(V, Vt)) {
+ V = Vt;
+ jr.velocity_t[axis] = 1;
} else {
// Compute axis max jerk
float jerk = axis_get_jerk_max(axis) * JERK_MULTIPLIER;
// Compute length to velocity given max jerk
- float length = mp_get_target_length(Vi, Vt, jerk * JOG_JERK_MULT);
+ float length = mp_get_target_length(V, Vt, jerk * JOG_JERK_MULT);
// Compute move time
- float move_time = 2 * length / (Vi + Vt);
+ float move_time = 2 * length / (V + Vt);
+
if (move_time < MIN_SEGMENT_TIME) {
- Vi = Vt;
- jr.segments[axis] = 0;
+ V = Vt;
+ jr.velocity_t[axis] = 1;
} else {
- jr.segments[axis] = ceil(move_time / NOM_SEGMENT_TIME);
-
- Vi = mp_init_forward_dif(jr.fdifs[axis], Vi, Vt, jr.segments[axis]);
- jr.segments[axis]--;
+ jr.initial_velocity[axis] = V;
+ jr.velocity_t[axis] = jr.velocity_delta[axis] =
+ NOM_SEGMENT_TIME / move_time;
}
}
+ }
- } else if (jr.segments[axis]) {
- Vi += mp_next_forward_dif(jr.fdifs[axis]);
- jr.segments[axis]--;
+ if (jr.velocity_t[axis] < 1) {
+ // Compute quintic Bezier curve
+ V = velocity_curve(jr.initial_velocity[axis], Vt, jr.velocity_t[axis]);
+ jr.velocity_t[axis] += jr.velocity_delta[axis];
- } else Vi = Vt;
+ } else V = Vt;
- if (JOG_MIN_VELOCITY < Vi || JOG_MIN_VELOCITY < Vt) done = false;
+ if (JOG_MIN_VELOCITY < V || JOG_MIN_VELOCITY < Vt) done = false;
- velocity_sqr += square(Vi);
- jr.velocity[axis] = Vi * jr.sign[axis];
+ velocity_sqr += square(V);
+ jr.velocity[axis] = V * jr.sign[axis];
}
// Check if we are done
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ 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 "velocity_curve.h"
+
+
+/// We are using a quintic (fifth-degree) Bezier polynomial for the velocity
+/// curve. This yields a constant pop; with pop being the sixth derivative
+/// of position:
+///
+/// 1st - velocity
+/// 2nd - acceleration
+/// 3rd - jerk
+/// 4th - snap
+/// 5th - crackle
+/// 6th - pop
+///
+/// The Bezier curve takes the form:
+///
+/// f(t) = P_0(1 - t)^5 + 5P_1(1 - t)^4 t + 10P_2(1 - t)^3 t^2 +
+/// 10P_3(1 - t)^2 t^3 + 5P_4(1 - t)t^4 + P_5t^5
+///
+/// Where 0 <= t <= 1, f(t) is the velocity and P_0 through P_5 are the control
+/// points. In our case:
+///
+/// P_0 = P_1 = P2 = Vi
+/// P_3 = P_4 = P5 = Vt
+///
+/// Where Vi is the initial velocity and Vt is the target velocity.
+///
+/// After substitution, expanding the polynomial and collecting terms we have:
+///
+/// f(t) = (Vt - Vi)(6t^5 - 15t^4 + 10t^3) + Vi
+///
+/// Computing this directly using 32bit float-point on a 32MHz AtXMega processor
+/// takes about 60uS or about 1,920 clocks. The code was compiled with avr-gcc
+/// v4.9.2 with -O3.
+float velocity_curve(float Vi, float Vt, float t) {
+ const float t2 = t * t;
+ const float t3 = t2 * t;
+
+ return (Vt - Vi) * (6 * t2 - 15 * t + 10) * t3 + Vi;
+}
--- /dev/null
+/******************************************************************************\
+
+ This file is part of the Buildbotics firmware.
+
+ Copyright (c) 2015 - 2017 Buildbotics LLC
+ Copyright (c) 2010 - 2015 Alden S. Hart, Jr.
+ Copyright (c) 2012 - 2015 Rob Giseburt
+ 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 velocity_curve(float Vi, float Vt, float t);
projects / bbctrl-firmware / commitdiff