diff --git a/Firmware/LICENSE b/Firmware/LICENSE index 9eeab46b..370d37c6 100644 --- a/Firmware/LICENSE +++ b/Firmware/LICENSE @@ -1,6 +1,6 @@ The MIT License (MIT) -Copyright (c) 2016 Oskar Weigl (madcowswe) +Copyright (c) 2016-2018 Oskar Weigl Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 0c5f3f35..ab4a1854 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -1,101 +1,93 @@ #include #include "odrive_main.h" +#include "utils.h" -// Standard sign function, implemented to match the Python impelmentation -template -int sign(T val) { - if (val == T(0)) - return T(0); - else - return (std::signbit(val)) ? -1 : 1; +// A sign function where input 0 has positive sign (not 0) +float sign_hard(float val) { + return (std::signbit(val)) ? -1.0f : 1.0f; } -TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t &config) : config_(config) {} +// Symbol Description +// Ta, Tv and Td Duration of the stages of the AL profile +// Xi and Vi Adapted initial conditions for the AL profile +// Xf Position set-point +// s Direction (sign) of the trajectory +// Vmax, Amax, Dmax and jmax Kinematic bounds +// Ar, Dr and Vr Reached values of acceleration and velocity -float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, - float Vi, float Vmax, - float Amax, float Dmax) { - float dx_stop = (Vi * Vi) / (Dmax * 2.0f); - float dX = Xf - Xi; - int s = sign(dX); +TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t& config) : config_(config) {} - float Ar = s * Amax; // Maximum Acceleration (signed) - float Dr = -1.0f * s * Dmax; // Maximum Deceleration (signed) - float Vr = s * Vmax; // Maximum Velocity (signed) +bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, + float Vmax, float Amax, float Dmax) { + float dX = Xf - Xi; // Distance to travel + float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance + float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement + float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any) + Ar_ = s * Amax; // Maximum Acceleration (signed) + Dr_ = -s * Dmax; // Maximum Deceleration (signed) + Vr_ = s * Vmax; // Maximum Velocity (signed) - float Ta; - float Tv; - float Td; - - // Checking for overshoot on "minimum stop" - if (fabs(dX) <= dx_stop) { - Ta = 0; - Tv = 0; - Vr = Vi; - Dr = -1.0f * sign(Vi) * Dmax; - Td = fabs(Vi) / Dmax; - } else { - // Handle the case where initial velocity > Max velocity - if ((s * Vi) > (s * Vr)) { - Ar = -1.0f * s * Amax; - } - - Ta = (Vr - Vi) / Ar; // Acceleration time - Td = (-Vr) / Dr; // Deceleration time - - // Peak Velocity handling - float dXmin = Ta * (Vr + Vi) / 2.0f + Td * Vr / 2.0f; - - // Short move handling - if (fabs(dX) < fabs(dXmin)) { - Vr = s * sqrt((-((Vi * Vi) / Ar) - 2.0f * dX) / (1.0f / Dr - 1.0f / Ar)); - Ta = std::max(0.0f, (Vr - Vi) / Ar); - Tv = 0; - Td = std::max(0.0f, -Vr / Dr); - } else { - Tv = (dX - dXmin) / Vr; - } + // If we start with a speed faster than cruising, then we need to decel instead of accel + // aka "double deceleration move" in the paper + if ((s * Vi) > (s * Vr_)) { + Ar_ = -s * Amax; } - // Populate object's values + // Time to accel/decel to/from Vr (cruise speed) + Ta_ = (Vr_ - Vi) / Ar_; + Td_ = -Vr_ / Dr_; - Xf_ = Xf; + // Integral of velocity ramps over the full accel and decel times to get + // minimum displacement required to reach cuising speed + float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_; + + // Are we displacing enough to reach cruising speed? + if (s*dX < s*dXmin) { + // Short move (triangle profile) + Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_)); + Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_); + Td_ = std::max(0.0f, -Vr_ / Dr_); + Tv_ = 0.0f; + } else { + // Long move (trapezoidal profile) + Tv_ = (dX - dXmin) / Vr_; + } + + // Fill in the rest of the values used at evaluation-time + Tf_ = Ta_ + Tv_ + Td_; Xi_ = Xi; + Xf_ = Xf; Vi_ = Vi; + yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase - Ar_ = Ar; - Dr_ = Dr; - Vr_ = Vr; - - Ta_ = Ta; - Tv_ = Tv; - Td_ = Td; - - yAccel_ = (Ar * Ta * Ta) / 2.0f + (Vi * Ta) + Xi; - Tav_ = Ta + Tv; - - return Ta + Tv + Td; + return true; } TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { TrapTrajStep_t trajStep; - if (t < 0.0f) { // Initial Conditions - trajStep.Y = Xi_; - trajStep.Yd = Vi_; - trajStep.Ydd = Ar_; + if (t < 0.0f) { // Initial Condition + trajStep.Y = Xi_; + trajStep.Yd = Vi_; + trajStep.Ydd = 0.0f; } else if (t < Ta_) { // Accelerating - trajStep.Y = (Ar_ * (t * t) / 2.0f) + (Vi_ * t) + Xi_; - trajStep.Yd = (Ar_ * t) + Vi_; + trajStep.Y = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t); + trajStep.Yd = Vi_ + Ar_*t; trajStep.Ydd = Ar_; } else if (t < Ta_ + Tv_) { // Coasting - trajStep.Y = yAccel_ + (Vr_ * (t - Ta_)); - trajStep.Yd = Vr_; - trajStep.Ydd = 0; - } else if (t < Ta_ + Tv_ + Td_) { // Deceleration - float Tdc = t - Tav_; - trajStep.Y = yAccel_ + (Vr_ * (t - Ta_)) + Dr_ * (Tdc * Tdc) / 2.0f; - trajStep.Yd = Vr_ + Dr_ * Tdc; + trajStep.Y = yAccel_ + Vr_*(t - Ta_); + trajStep.Yd = Vr_; + trajStep.Ydd = 0.0f; + } else if (t < Tf_) { // Deceleration + float td = t - Tf_; + trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td); + trajStep.Yd = Dr_*td; trajStep.Ydd = Dr_; + } else if (t >= Tf_) { // Final Condition + trajStep.Y = Xf_; + trajStep.Yd = 0.0f; + trajStep.Ydd = 0.0f; + } else { + // TODO: report error here } return trajStep; diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index a30a8616..dec5254d 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -2,10 +2,10 @@ #define _TRAP_TRAJ_H struct TrapTrajConfig_t { - float vel_limit = 20000.0f; - float accel_limit = 5000.0f; - float decel_limit = 5000.0f; - float cpss_to_A = 0.0f; + float vel_limit = 20000.0f; // [count/s] + float accel_limit = 5000.0f; // [count/s^2] + float decel_limit = 5000.0f; // [count/s^2] + float cpss_to_A = 0.0f; // [A/(count/s^2)] }; struct TrapTrajStep_t { @@ -16,15 +16,9 @@ struct TrapTrajStep_t { class TrapezoidalTrajectory { public: - Axis *axis_ = nullptr; // set by Axis constructor - TrapTrajConfig_t &config_; - - TrapezoidalTrajectory(TrapTrajConfig_t &config); - - float planTrapezoidal(float Xf, float Xi, - float Vi, float Vmax, - float Amax, float Dmax); - + TrapezoidalTrajectory(TrapTrajConfig_t& config); + bool planTrapezoidal(float Xf, float Xi, float Vi, + float Vmax, float Amax, float Dmax); TrapTrajStep_t evalTrapTraj(float t); auto make_protocol_definitions() { @@ -32,24 +26,29 @@ class TrapezoidalTrajectory { make_protocol_object("config", make_protocol_property("vel_limit", &config_.vel_limit), make_protocol_property("accel_limit", &config_.accel_limit), - make_protocol_property("decel_limit", &config_.decel_limit))); + make_protocol_property("decel_limit", &config_.decel_limit), + make_protocol_property("cpss_to_A", &config_.cpss_to_A) + ) + ); } - private: - float yAccel_; + Axis* axis_ = nullptr; // set by Axis constructor + TrapTrajConfig_t& config_; float Xi_; float Xf_; float Vi_; float Ar_; - float Dr_; float Vr_; + float Dr_; float Ta_; float Tv_; float Td_; - float Tav_; + float Tf_; + + float yAccel_; }; #endif \ No newline at end of file diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 1cd63327..0ab35668 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -63,6 +63,8 @@ extern "C" { #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) +#define SQ(x) ((x) * (x)) + static const float one_by_sqrt3 = 0.57735026919f; static const float two_by_sqrt3 = 1.15470053838f; static const float sqrt3_by_2 = 0.86602540378f; diff --git a/tools/motion_planning/PlanTrap.py b/tools/motion_planning/PlanTrap.py index 01bc70c6..129da1e6 100644 --- a/tools/motion_planning/PlanTrap.py +++ b/tools/motion_planning/PlanTrap.py @@ -28,15 +28,13 @@ import math import matplotlib.pyplot as plt import random -# Symbol Description -# Ta, Tv and Td Duration of the stages of the AL profile -# q0f , v0f and a0f Initial conditions of the jerk-limited trajectory -# q0 and v0 Adapted initial conditions for the AL profile -# qe Position set-point -# s Direction (sign) of the trajectory -# vmax, amax, dmax and jmax Kinematic bounds -# vr, ar and dr Reached values of velocity and acceleration -# Tj , Tja, Tjv and Tjd Length of the constant jerk stages (FIR filter time) +# Symbol Description +# Ta, Tv and Td Duration of the stages of the AL profile +# Xi and Vi Adapted initial conditions for the AL profile +# Xf Position set-point +# s Direction (sign) of the trajectory +# Vmax, Amax, Dmax and jmax Kinematic bounds +# Ar, Dr and Vr Reached values of acceleration and velocity # Test scales: pos_range = 10000.0 @@ -57,18 +55,18 @@ def PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax): # If we start with a speed faster than cruising, then we need to decel instead of accel # aka "double deceleration move" in the paper if s*Vi > s*Vr: - Ar = -s*Amax print("Handbrake!") + Ar = -s*Amax # Time to accel/decel to/from Vr (cruise speed) - Ta = (Vr - Vi)/Ar + Ta = (Vr-Vi)/Ar Td = -Vr/Dr - # Integrate velocity ramps over the full accel and decel times to get + # Integral of velocity ramps over the full accel and decel times to get # minimum displacement required to reach cuising speed - dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 + dXmin = Ta*(Vr+Vi)/2.0 + Td*(Vr)/2.0 - # Did we displace enough to reach cruising speed? + # Are we displacing enough to reach cruising speed? if s*dX < s*dXmin: print("Short Move:") # From paper: @@ -126,6 +124,8 @@ def EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf): y[i] = Xf yd[i] = 0 ydd[i] = 0 + else: + raise ValueError("t = {} is outside of considered range".format(t)) dy = np.diff(y) dy_max = np.max(np.abs(dy))