arm编译多文件Makefile

#include "../ruckig/ruckig.hpp" #include "../ruckig/roots.hpp" namespace ruckig { PositionStep2::PositionStep2(double tf, double p0, double v0, double a0, double pf, double vf, double af, double vMax, double vMin, double aMax, double aMin, double jMax): v0(v0), a0(a0), tf(tf), vf(vf), af(af), _vMax(vMax), _vMin(vMin), _aMax(aMax), _aMin(aMin), _jMax(jMax) { pd = pf - p0; tf_tf = tf * tf; tf_p3 = tf_tf * tf; tf_p4 = tf_tf * tf_tf; vd = vf - v0; vd_vd = vd * vd; v0_v0 = v0 * v0; vf_vf = vf * vf; ad = af - a0; ad_ad = ad * ad; a0_a0 = a0 * a0; af_af = af * af; a0_p3 = a0 * a0_a0; a0_p4 = a0_a0 * a0_a0; a0_p5 = a0_p3 * a0_a0; a0_p6 = a0_p4 * a0_a0; af_p3 = af * af_af; af_p4 = af_af * af_af; af_p5 = af_p3 * af_af; af_p6 = af_p4 * af_af; // max values needs to be invariant to plus minus sign change jMax_jMax = jMax * jMax; g1 = -pd + tf*v0; g2 = -2*pd + tf*(v0 + vf); } bool PositionStep2::time_acc0_acc1_vel(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax) { // Profile UDDU, Solution 1 if ((2*(aMax - aMin) + ad)/jMax < tf) { const double h1 = Sqrt((a0_p4 + af_p4 - 4*a0_p3*(2*aMax + aMin)/3 - 4*af_p3*(aMax + 2*aMin)/3 + 2*(a0_a0 - af_af)*aMax*aMax + (4*a0*aMax - 2*a0_a0)*(af_af - 2*af*aMin + (aMin - aMax)*aMin + 2*jMax*(aMin*tf - vd)) + 2*af_af*(aMin*aMin + 2*jMax*(aMax*tf - vd)) + 4*jMax*(2*aMin*(af*vd + jMax*g1) + (aMax*aMax - aMin*aMin)*vd + jMax*vd_vd) + 8*aMax*jMax_jMax*(pd - tf*vf))/(aMax*aMin) + 4*af_af + 2*a0_a0 + (4*af + aMax - aMin)*(aMax - aMin) + 4*jMax*(aMin - aMax + jMax*tf - 2*af)*tf) * Abs(jMax)/jMax; profile.t[0] = (-a0 + aMax)/jMax; profile.t[1] = (-(af_af - a0_a0 + 2*aMax*aMax + aMin*(aMin - 2*ad - 3*aMax) + 2*jMax*(aMin*tf - vd)) + aMin*h1)/(2*(aMax - aMin)*jMax); profile.t[2] = aMax/jMax; profile.t[3] = (aMin - aMax + h1)/(2*jMax); profile.t[4] = -aMin/jMax; profile.t[5] = tf - (profile.t[0] + profile.t[1] + profile.t[2] + profile.t[3] + 2*profile.t[4] + af/jMax); profile.t[6] = profile.t[4] + af/jMax; if (profile.check_with_timing<JerkSigns::UDDU, Limits::ACC0_ACC1_VEL>(tf, jMax, vMax, vMin, aMax, aMin)) { return true; } } // Profile UDUD if ((-a0 + 4*aMax - af)/jMax < tf) { profile.t[0] = (-a0 + aMax)/jMax; profile.t[1] = (3*(a0_p4 + af_p4) - 4*(a0_p3 + af_p3)*aMax - 4*af_p3*aMax + 24*(a0 + af)*aMax*aMax*aMax - 6*(af_af + a0_a0)*(aMax*aMax - 2*jMax*vd) + 6*a0_a0*(af_af - 2*af*aMax - 2*aMax*jMax*tf) - 12*aMax*aMax*(2*aMax*aMax - 2*aMax*jMax*tf + jMax*vd) - 24*af*aMax*jMax*vd + 12*jMax_jMax*(2*aMax*g1 + vd_vd))/(12*aMax*jMax*(a0_a0 + af_af - 2*(a0 + af)*aMax + 2*(aMax*aMax - aMax*jMax*tf + jMax*vd))); profile.t[2] = aMax/jMax; profile.t[3] = (-a0_a0 - af_af + 2*aMax*(a0 + af - 2*aMax) - 2*jMax*vd)/(2*aMax*jMax) + tf; profile.t[4] = profile.t[2]; profile.t[5] = tf - (profile.t[0] + profile.t[1] + profile.t[2] + profile.t[3] + 2*profile.t[4] - af/jMax); profile.t[6] = profile.t[4] - af/jMax; if (profile.check_with_timing<JerkSigns::UDUD, Limits::ACC0_ACC1_VEL>(tf, jMax, vMax, vMin, aMax, aMin)) { return true; } } return false; } bool PositionStep2::time_acc1_vel(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax) { // Profile UDDU { const double ph1 = a0_a0 + af_af - aMin*(a0 + 2*af - aMin) - 2*jMax*(vd - aMin*tf); const double ph2 = 2*aMin*(jMax*g1 + af*vd) - aMin*aMin*vd + jMax*vd_vd; const double ph3 = af_af + aMin*(aMin - 2*af) - 2*jMax*(vd - aMin*tf); std::array<double, 4> polynom; polynom[0] = (2*(2*a0 - aMin))/jMax; polynom[1] = (4*a0_a0 + ph1 - 3*a0*aMin)/jMax_jMax; polynom[2] = (2*a0*ph1)/(jMax_jMax*jMax); polynom[3] = (3*(a0_p4 + af_p4) - 4*(a0_p3 + 2*af_p3)*aMin + 6*af_af*(aMin*aMin - 2*jMax*vd) + 12*jMax*ph2 + 6*a0_a0*ph3)/(12*jMax_jMax*jMax_jMax); const double t_min = -a0/jMax; const double t_max = std::min((tf + 2*aMin/jMax - (a0 + af)/jMax)/2, (aMax - a0)/jMax); auto roots = roots::solveQuartMonic(polynom); for (double t: roots) { if (t < t_min || t > t_max) { continue; } // Single Newton step (regarding pd) if (std::abs(a0 + jMax*t) > 16*DBL_EPSILON) { const double h0 = jMax*t*t; const double orig = -pd + (3*(a0_p4 + af_p4) - 8*af_p3*aMin - 4*a0_p3*aMin + 6*af_af*(aMin*aMin + 2*jMax*(h0 - vd)) + 6*a0_a0*(af_af - 2*af*aMin + aMin*aMin + 2*aMin*jMax*(-2*t + tf) + 2*jMax*(5*h0 - vd)) + 24*a0*jMax*t*(a0_a0 + af_af - 2*af*aMin + aMin*aMin + 2*jMax*(aMin*(-t + tf) + h0 - vd)) - 24*af*aMin*jMax*(h0 - vd) + 12*jMax*(aMin*aMin*(h0 - vd) + jMax*(h0 - vd)*(h0 - vd)))/(24*aMin*jMax_jMax) + h0*(tf - t) + tf*v0; const double deriv = (a0 + jMax*t)*((a0_a0 + af_af)/(aMin*jMax) + (aMin - a0 - 2*af)/jMax + (4*a0*t + 2*h0 - 2*vd)/aMin + 2*tf - 3*t); t -= orig / deriv; } const double h1 = -((a0_a0 + af_af)/2 + jMax*(-vd + 2*a0*t + jMax*t*t))/aMin; profile.t[0] = t; profile.t[1] = 0; profile.t[2] = a0/jMax + t; profile.t[3] = tf - (h1 - aMin + a0 + af)/jMax - 2*t; profile.t[4] = -aMin/jMax; profile.t[5] = (h1 + aMin)/jMax; profile.t[6] = profile.t[4] + af/jMax; if (profile.check_with_timing<JerkSigns::UDDU, Limits::ACC1_VEL>(tf, jMax, vMax, vMin, aMax, aMin)) { return true; } } } // Profile UDUD { const double ph1 = a0_a0 - af_af + (2*af - a0)*aMax - aMax*aMax - 2*jMax*(vd - aMax*tf); const double ph2 = aMax*aMax + 2*jMax*vd; const double ph3 = af_af + ph2 - 2*aMax*(af + jMax*tf); const double ph4 = 2*aMax*jMax*g1 + aMax*aMax*vd + jMax*vd_vd; std::array<double, 4> polynom; polynom[0] = (4*a0 - 2*aMax)/jMax; polynom[1] = (4*a0_a0 - 3*a0*aMax + ph1)/jMax_jMax; polynom[2] = (2*a0*ph1)/(jMax_jMax*jMax); polynom[3] = (3*(a0_p4 + af_p4) - 4*(a0_p3 + 2*af_p3)*aMax - 24*af*aMax*jMax*vd + 12*jMax*ph4 - 6*a0_a0*ph3 + 6*af_af*ph2)/(12*jMax_jMax*jMax_jMax); const double t_min = -a0/jMax; const double t_max = std::min((tf + ad/jMax - 2*aMax/jMax)/2, (aMax - a0)/jMax); auto roots = roots::solveQuartMonic(polynom); for (double t: roots) { if (t > t_max || t < t_min) { continue; } const double h1 = ((a0_a0 - af_af)/2 + jMax_jMax*t*t - jMax*(vd - 2*a0*t))/aMax; profile.t[0] = t; profile.t[1] = 0; profile.t[2] = t + a0/jMax; profile.t[3] = tf + (h1 + ad - aMax)/jMax - 2*t; profile.t[4] = aMax/jMax; profile.t[5] = -(h1 + aMax)/jMax; profile.t[6] = profile.t[4] - af/jMax; if (profile.check_with_timing<JerkSigns::UDUD, Limits::ACC1_VEL>(tf, jMax, vMax, vMin, aMax, aMin)) { return true; } } } return false; } bool PositionStep2::time_acc0_vel(Profile& profile, double vMax, double vMin, double aMax, double aMin, double jMax) { if (tf < std::max((-a0 + aMax)/jMax, 0.0) + std::max(aMax/jMax, 0.0)) { return false; } const double ph1 = 12*jMax*(-aMax*aMax*vd - jMax*vd_vd + 2*aMax*jMax*(-pd + tf*vf)); // Profile UDDU { std::array<double, 4> polynom; polynom[0] = (2*aMax)/jMax; polynom[1] = (a0_a0 - af_af + 2*ad*aMax + aMax*aMax + 2*jMax*(vd - aMax*tf))/jMax_jMax; polynom[2] = 0; polynom[3] = -(-3*(a0_p4 + af_p4) + 4*(af_p3 + 2*a0_p3)*aMax - 12*a0*aMax*(af_af - 2*jMax*vd) + 6*a0_a0*(af_af - aMax*aMax - 2*jMax*vd) + 6*af_af*(aMax*a