From: Joseph Coffland Date: Mon, 27 Mar 2017 00:27:58 +0000 (-0700) Subject: Eliminate forward dif code X-Git-Url: https://git.buildbotics.com/?a=commitdiff_plain;h=0fb1b648704452d776883d5af93bd349001b8d3a;p=bbctrl-firmware Eliminate forward dif code --- diff --git a/avr/src/plan/exec.c b/avr/src/plan/exec.c index 98204bb..29446cd 100644 --- a/avr/src/plan/exec.c +++ b/avr/src/plan/exec.c @@ -36,6 +36,7 @@ #include "motor.h" #include "rtc.h" #include "usart.h" +#include "velocity_curve.h" #include "config.h" #include @@ -82,38 +83,6 @@ typedef struct { 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) { @@ -162,12 +131,8 @@ static stat_t _exec_aline_section(float length, float Vi, float Vt) { 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; } diff --git a/avr/src/plan/forward_dif.c b/avr/src/plan/forward_dif.c deleted file mode 100644 index 80f3339..0000000 --- a/avr/src/plan/forward_dif.c +++ /dev/null @@ -1,204 +0,0 @@ -/******************************************************************************\ - - 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 . - - 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 "forward_dif.h" - -#include "util.h" - -#include - - -/// 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; -} diff --git a/avr/src/plan/forward_dif.h b/avr/src/plan/forward_dif.h deleted file mode 100644 index a0d5c20..0000000 --- a/avr/src/plan/forward_dif.h +++ /dev/null @@ -1,34 +0,0 @@ -/******************************************************************************\ - - 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 - - -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); diff --git a/avr/src/plan/jog.c b/avr/src/plan/jog.c index 9618610..ee4528b 100644 --- a/avr/src/plan/jog.c +++ b/avr/src/plan/jog.c @@ -31,7 +31,7 @@ #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" @@ -48,12 +48,13 @@ typedef struct { 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; @@ -87,46 +88,49 @@ static stat_t _exec_jog(mp_buffer_t *bf) { 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 diff --git a/avr/src/plan/velocity_curve.c b/avr/src/plan/velocity_curve.c new file mode 100644 index 0000000..e8db220 --- /dev/null +++ b/avr/src/plan/velocity_curve.c @@ -0,0 +1,69 @@ +/******************************************************************************\ + + 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 . + + 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 "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; +} diff --git a/avr/src/plan/velocity_curve.h b/avr/src/plan/velocity_curve.h new file mode 100644 index 0000000..8d1e497 --- /dev/null +++ b/avr/src/plan/velocity_curve.h @@ -0,0 +1,32 @@ +/******************************************************************************\ + + 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 . + + 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 velocity_curve(float Vi, float Vt, float t);