From 26b87bb95be287d48306445f4b3ddb77c86d7a2e Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 00:38:49 -0400 Subject: [PATCH 01/44] Add trajectory planner python script --- tools/Motion Planning/Planner.py | 62 ++++++++++++++++++++++++++++++++ 1 file changed, 62 insertions(+) create mode 100644 tools/Motion Planning/Planner.py diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py new file mode 100644 index 00000000..3a054711 --- /dev/null +++ b/tools/Motion Planning/Planner.py @@ -0,0 +1,62 @@ +import numpy as np +import math + +def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): + + s = np.sign(Xf - Xi) # Sign + + Ar = s*Amax # Maximum Acceleration (signed) + Dr = -s*Dmax # Maximum Deceleration (signed) + Vr = s*Vmax # Maximum Velocity (signed) + + if(s*Vi > s*Vr): + Ar = -s*Amax + + Ta = (Vr - Vi)/Ar # Acceleration Time + Td = (Vf - Vr)/Dr # Deceleration Time + + dX = Xf - Xi # Distance to travel + if Vf == 0: + dXmin = Ta*(Vr + Vi)/2 + Td*(Vr)/2 + elif np.sign(Vf) == np.sign(Vr): + dXmin = Ta*(Vr + Vi)/2 + Td*(Vr-Vf)/2 + Td*Vf + else: + dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - s*Vf)/2 + + if s*dXmin > s*dX: + Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))/math.sqrt(Dr-Ar) + Ta = max(0, (Vr - Vi)/Ar) + Tv = 0 + Td = max(0, (Vf - Vr)/Dr) + else: + Tv = (dX - dXmin)/Vr + + t_traj = np.arange(0, Ta+Tv+Td, dT) + y = [None]*len(t_traj) + yd = [None]*len(t_traj) + ydd = [None]*len(t_traj) + + for i in range(len(t_traj)): + t = t_traj[i] + if(t <= 0): + y[i] = Xi + yd[i] = Vi + ydd[i] = Ai + elif(t <= Ta): + y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT) + yd[i] = yd[i-1] + ydd[i-1]*dT + ydd[i] = Ar + elif(t <= Ta+Tv): + y[i] = y[i-1] + yd[i-1] * dT + yd[i] = yd[i-1] + ydd[i] = 0 + elif(t <= Ta+Tv+Td): + y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT) + yd[i] = yd[i-1] + ydd[i-1]*dT + ydd[i] = Dr + + return (y, yd, ydd) + + +(Y, Yd, Ydd) = trapPlan(10, 0, 0, 0, 0, 0, 10, 20, 20) +print(Y[len(Y)-1]) \ No newline at end of file From 17966fafdeec99c8a2225640e22d82237aa4e18e Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 00:49:41 -0400 Subject: [PATCH 02/44] Add comments to trajplan script --- tools/Motion Planning/Planner.py | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py index 3a054711..edc4f83d 100644 --- a/tools/Motion Planning/Planner.py +++ b/tools/Motion Planning/Planner.py @@ -17,20 +17,22 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): dX = Xf - Xi # Distance to travel if Vf == 0: - dXmin = Ta*(Vr + Vi)/2 + Td*(Vr)/2 + dXmin = Ta*(Vr + Vi)/2 + Td*(Vr)/2 # Basic wedge profile elif np.sign(Vf) == np.sign(Vr): - dXmin = Ta*(Vr + Vi)/2 + Td*(Vr-Vf)/2 + Td*Vf + dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - Vf)/2 + Td*Vf # Wedge profile with an unfinished end else: - dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - s*Vf)/2 + dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - s*Vf)/2 # Wedge profile that crosses Y axis on decel - if s*dXmin > s*dX: - Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))/math.sqrt(Dr-Ar) + if s*dXmin > s*dX: # Short move handling + Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))/math.sqrt(Dr-Ar) # Modified from paper to handle non-zero Vf Ta = max(0, (Vr - Vi)/Ar) Tv = 0 Td = max(0, (Vf - Vr)/Dr) else: - Tv = (dX - dXmin)/Vr + Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v + ## We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory + # Create the time series and preallocate the position, velocity, and acceleration arrays t_traj = np.arange(0, Ta+Tv+Td, dT) y = [None]*len(t_traj) yd = [None]*len(t_traj) @@ -38,22 +40,26 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): for i in range(len(t_traj)): t = t_traj[i] - if(t <= 0): + if(t <= 0): # Initial conditions y[i] = Xi yd[i] = Vi ydd[i] = Ai - elif(t <= Ta): + elif(t <= Ta): # Acceleration y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT) yd[i] = yd[i-1] + ydd[i-1]*dT ydd[i] = Ar - elif(t <= Ta+Tv): + elif(t <= Ta+Tv): # Coasting y[i] = y[i-1] + yd[i-1] * dT yd[i] = yd[i-1] ydd[i] = 0 - elif(t <= Ta+Tv+Td): + elif(t < Ta+Tv+Td): # Deceleration y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT) yd[i] = yd[i-1] + ydd[i-1]*dT ydd[i] = Dr + else: # Final conditions + y[i] = Xf + yd[i] = Vf + ydd[i] = Af return (y, yd, ydd) From d3716af186489284f31dedb8b1d657c6a81c0d21 Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 00:58:47 -0400 Subject: [PATCH 03/44] Comment clean up --- tools/Motion Planning/Planner.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py index edc4f83d..0d009f46 100644 --- a/tools/Motion Planning/Planner.py +++ b/tools/Motion Planning/Planner.py @@ -3,7 +3,8 @@ import math def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): - s = np.sign(Xf - Xi) # Sign + dX = Xf - Xi # Distance to travel + s = np.sign(dX) # Sign Ar = s*Amax # Maximum Acceleration (signed) Dr = -s*Dmax # Maximum Deceleration (signed) @@ -15,7 +16,9 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): Ta = (Vr - Vi)/Ar # Acceleration Time Td = (Vf - Vr)/Dr # Deceleration Time - dX = Xf - Xi # Distance to travel + + + ## Peak velocity handling if Vf == 0: dXmin = Ta*(Vr + Vi)/2 + Td*(Vr)/2 # Basic wedge profile elif np.sign(Vf) == np.sign(Vr): @@ -23,7 +26,8 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): else: dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - s*Vf)/2 # Wedge profile that crosses Y axis on decel - if s*dXmin > s*dX: # Short move handling + ## Short move handling + if s*dXmin > s*dX: Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))/math.sqrt(Dr-Ar) # Modified from paper to handle non-zero Vf Ta = max(0, (Vr - Vi)/Ar) Tv = 0 @@ -38,6 +42,8 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): yd = [None]*len(t_traj) ydd = [None]*len(t_traj) + # We only know acceleration (Ar and Dr), so we integrate to create + # the velocity and position curves for i in range(len(t_traj)): t = t_traj[i] if(t <= 0): # Initial conditions From accd2b6b16af71491fd4b2eef00c3113ec672afc Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 01:13:54 -0400 Subject: [PATCH 04/44] Export the time axis from trapPlan --- tools/Motion Planning/Planner.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py index 0d009f46..dc9710f1 100644 --- a/tools/Motion Planning/Planner.py +++ b/tools/Motion Planning/Planner.py @@ -1,5 +1,6 @@ import numpy as np import math +import matplotlib.pyplot as plt def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): @@ -67,8 +68,11 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): yd[i] = Vf ydd[i] = Af - return (y, yd, ydd) + return (y, yd, ydd, t_traj) -(Y, Yd, Ydd) = trapPlan(10, 0, 0, 0, 0, 0, 10, 20, 20) -print(Y[len(Y)-1]) \ No newline at end of file +(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 0, 0, 10, 20, 20) +plt.plot(t, Y) +plt.plot(t, Yd) +plt.plot(t, Ydd) +plt.show() \ No newline at end of file From 5191335f248fd29493bf24240230ec02a63d56fd Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 18:24:54 -0400 Subject: [PATCH 05/44] Convert to closed-form solution --- tools/Motion Planning/Planner.py | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py index dc9710f1..b594f188 100644 --- a/tools/Motion Planning/Planner.py +++ b/tools/Motion Planning/Planner.py @@ -52,26 +52,24 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): yd[i] = Vi ydd[i] = Ai elif(t <= Ta): # Acceleration - y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT) - yd[i] = yd[i-1] + ydd[i-1]*dT + y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi + yd[i] = (Ar * t) + Vi ydd[i] = Ar elif(t <= Ta+Tv): # Coasting - y[i] = y[i-1] + yd[i-1] * dT - yd[i] = yd[i-1] + y[i] = ((Ar*Ta*Ta) / 2) + (Vi * Ta) + (Vr * (t - Ta)) + Xi + yd[i] = Ar*Ta + Vi ydd[i] = 0 - elif(t < Ta+Tv+Td): # Deceleration - y[i] = y[i-1] + yd[i-1] * dT + (0.5*ydd[i-1]*dT*dT) - yd[i] = yd[i-1] + ydd[i-1]*dT + elif(t <= Ta+Tv+Td): # Deceleration + y[i] = ((Ar*Ta*Ta) / 2) + (Vi * Ta) + (Vr * (t - Ta)) + Xi + Dr*(t - Ta - Tv)*(t - Ta - Tv)/2 + yd[i] = Ar*Ta + Vi + Dr*(t - Ta - Tv) ydd[i] = Dr - else: # Final conditions - y[i] = Xf - yd[i] = Vf - ydd[i] = Af + return (y, yd, ydd, t_traj) -(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 0, 0, 10, 20, 20) +(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 4, 0, 0, 2, 5, 5) +print("Y: ",Y[len(Y)-1],"\nYd: ",Yd[len(Yd)-1]) plt.plot(t, Y) plt.plot(t, Yd) plt.plot(t, Ydd) From 3865510c884b7d6dd9d4ccf16e045c3c53113d44 Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 19:02:09 -0400 Subject: [PATCH 06/44] Compute some planning values ahead of time --- tools/Motion Planning/Planner.py | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py index b594f188..69870425 100644 --- a/tools/Motion Planning/Planner.py +++ b/tools/Motion Planning/Planner.py @@ -45,6 +45,9 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): # We only know acceleration (Ar and Dr), so we integrate to create # the velocity and position curves + y_Accel = (Ar*Ta*Ta) / 2 + (Vi * Ta) + Xi + Tav = Ta + Tv + for i in range(len(t_traj)): t = t_traj[i] if(t <= 0): # Initial conditions @@ -56,20 +59,21 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): yd[i] = (Ar * t) + Vi ydd[i] = Ar elif(t <= Ta+Tv): # Coasting - y[i] = ((Ar*Ta*Ta) / 2) + (Vi * Ta) + (Vr * (t - Ta)) + Xi - yd[i] = Ar*Ta + Vi + y[i] = y_Accel + (Vr * (t - Ta)) + yd[i] = Vr ydd[i] = 0 elif(t <= Ta+Tv+Td): # Deceleration - y[i] = ((Ar*Ta*Ta) / 2) + (Vi * Ta) + (Vr * (t - Ta)) + Xi + Dr*(t - Ta - Tv)*(t - Ta - Tv)/2 - yd[i] = Ar*Ta + Vi + Dr*(t - Ta - Tv) + y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((t - Tav)*(t - Tav))/2 + yd[i] = Vr + Dr*(t - Tav) ydd[i] = Dr - + return (y, yd, ydd, t_traj) (Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 4, 0, 0, 2, 5, 5) -print("Y: ",Y[len(Y)-1],"\nYd: ",Yd[len(Yd)-1]) +print("Y: ",Y[len(Y)-1]) +print("Yd: ",Yd[len(Yd)-1]) plt.plot(t, Y) plt.plot(t, Yd) plt.plot(t, Ydd) From 6149d9733489d04778933492434179c4e515831e Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 20:07:06 -0400 Subject: [PATCH 07/44] Add Trapezoidal Trajectory class and functions --- Firmware/MotorControl/trapTraj.cpp | 90 ++++++++++++++++++++++++++++++ Firmware/MotorControl/trapTraj.hpp | 36 ++++++++++++ 2 files changed, 126 insertions(+) create mode 100644 Firmware/MotorControl/trapTraj.cpp create mode 100644 Firmware/MotorControl/trapTraj.hpp diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp new file mode 100644 index 00000000..a113c897 --- /dev/null +++ b/Firmware/MotorControl/trapTraj.cpp @@ -0,0 +1,90 @@ +#include "odrive_main.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; +} + +float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, + float Vf, float Vi, + float Af, float Ai, + float Vmax, float Amax, float Dmax) { + + float dX = Xf - Xi; + int s = sign(dX); + + float Ar = s*Amax; // Maximum Acceleration (signed) + float Dr = -1.0f*s*Dmax; // Maximum Deceleration (signed) + float Vr = s*Vmax; // Maximum Velocity (signed) + + // Handle the case where initial velocity > Max velocity + if(s*Vi > s*Vr){ + Ar = -1.0f*s*Amax; + } + + float Ta = (Vr - Vi)/Ar; + float Tv; + float Td = (Vf - Vr)/Dr; + + // Peak Velocity handling + float dXmin; + if(Vf == 0.0f){ + dXmin = Ta*(Vr + Vi)/2.0f + (Td*Vr)/2.0f; // Basic wedge profile + } else if(sign(Vf) == sign(Vr)){ + dXmin = Ta*(Vr + Vi)/2.0f + Td*(Vr - Vf)/2.0f + Td*Vf; // Wedge profile with a non-zero end + } else { + dXmin = Ta*(Vr + Vi)/2.0f + Td*(Vr - s*Vf)/2.0f; // Wedge profile that crosses Y axis during decel + } + + // Short move handling + if(s*dXmin > s*dX){ + Vr = s*std::sqrt(-1.0f*Ar*(Vf*Vf-2*Dr*dX))/sqrt(Dr-Ar); + Ta = std::max(0.0f, (Vr - Vi)/Ar); + Tv = 0; + Td = std::max(0.0f, (Vf - Vr)/Dr); + } else { + Tv = (dX - dXmin)/Vr; + } + + // Populate object's values + yAccel_ = (Ar*Ta*Ta) / 2.0f + (Vi * Ta) + Xi; + Xi_ = Xi; + Vi_ = Vi; + Ai_ = Ai; + + Ar_ = Ar; + Dr_ = Dr; + Vr_ = Vr; + + Ta_ = Ta; + Tv_ = Tv; + Td_ = Td; + Tav_ = Ta + Tv; + + return Ta + Tv + Td; +} + +TrapezoidalTrajectory::TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t){ + TrapezoidalTrajectory::TrajectoryStep_t trajStep; + if( t <= 0.0f){ // Initial Conditions + trajStep.Y = Xi_; + trajStep.Yd = Vi_; + trajStep.Ydd = Ai_; + } else if( t <= Ta_){ // Accelerating + trajStep.Y = (Ar_ * (t*t))/2.0f + (Vi_ * t) + Xi_; + trajStep.Yd = (Ar_ * t) + Vi_; + 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 - Ta_; + trajStep.Y = yAccel_ + (Vr_ * (Tdc)) + Dr_*(Tdc*Tdc)/2.0f; + trajStep.Yd = Vr_ + Dr_*Tdc; + trajStep.Ydd = Dr_; + } +} \ No newline at end of file diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp new file mode 100644 index 00000000..94d38dd1 --- /dev/null +++ b/Firmware/MotorControl/trapTraj.hpp @@ -0,0 +1,36 @@ +class TrapezoidalTrajectory { +private: + float yAccel_; + + float Xi_; + float Vi_; + float Ai_; + + float Ar_; + float Dr_; + float Vr_; + + float Ta_; + float Tv_; + float Td_; + float Tav_; + +public: + struct TrajectoryStep_t{ + float Y; + float Yd; + float Ydd; + }; + + TrapezoidalTrajectory(); + + float planTrapezoidal(float Xf, float Xi, + float Vf, float Vi, + float Af, float Ai, + float Vmax, float Amax, float Dmax + ); + + TrajectoryStep_t evalTrapTraj(float t); + + ~TrapezoidalTrajectory(); +} \ No newline at end of file From 94c59515ed192e090ab9408ad8c90e144ff16947 Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 20:08:17 -0400 Subject: [PATCH 08/44] Add trajplan include to odrive_main.h --- Firmware/MotorControl/odrive_main.h | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 4e4db160..c55fe333 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -111,6 +111,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include #include +#include #endif // __cplusplus From b2f8fad5d455ef3d4985c7fb9c9feb5a88b0e7cd Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 20:09:03 -0400 Subject: [PATCH 09/44] Actually return the trajStep struct. --- Firmware/MotorControl/trapTraj.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index a113c897..5d6e732b 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -87,4 +87,6 @@ TrapezoidalTrajectory::TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(floa trajStep.Yd = Vr_ + Dr_*Tdc; trajStep.Ydd = Dr_; } + + return trajStep; } \ No newline at end of file From bea17772a0e33afab0ebf43768a4fa1c7b637e34 Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 20:11:21 -0400 Subject: [PATCH 10/44] Save final conditions also --- Firmware/MotorControl/trapTraj.cpp | 8 ++++++++ Firmware/MotorControl/trapTraj.hpp | 6 +++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 5d6e732b..091721de 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -55,6 +55,10 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, Vi_ = Vi; Ai_ = Ai; + Xf_ = Xf; + Vf_ = Vf; + Af_ = Af; + Ar_ = Ar; Dr_ = Dr; Vr_ = Vr; @@ -86,6 +90,10 @@ TrapezoidalTrajectory::TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(floa trajStep.Y = yAccel_ + (Vr_ * (Tdc)) + Dr_*(Tdc*Tdc)/2.0f; trajStep.Yd = Vr_ + Dr_*Tdc; trajStep.Ydd = Dr_; + } else { // Ending conditions + trajStep.Y = Xf_; + trajStep.Yd = Vf_; + trajStep.Ydd = Af_; } return trajStep; diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index 94d38dd1..65c4589f 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -6,6 +6,10 @@ private: float Vi_; float Ai_; + float Xf_; + float Vf_; + float Af_; + float Ar_; float Dr_; float Vr_; @@ -23,7 +27,7 @@ public: }; TrapezoidalTrajectory(); - + float planTrapezoidal(float Xf, float Xi, float Vf, float Vi, float Af, float Ai, From 3aeaedfe52b2b076fe013423a043c1a8736d241c Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 20:15:17 -0400 Subject: [PATCH 11/44] Add semicolon to class end, add file to tupfile.lua --- Firmware/MotorControl/trapTraj.hpp | 2 +- Firmware/Tupfile.lua | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index 65c4589f..d833c509 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -37,4 +37,4 @@ public: TrajectoryStep_t evalTrapTraj(float t); ~TrapezoidalTrajectory(); -} \ No newline at end of file +}; \ No newline at end of file diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index f242b259..78c52c2b 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -156,6 +156,7 @@ build{ 'MotorControl/controller.cpp', 'MotorControl/sensorless_estimator.cpp', 'MotorControl/main.cpp', + 'MotorControl/trapTraj.cpp', 'communication/communication.cpp', 'communication/ascii_protocol.cpp', 'communication/interface_uart.cpp', From 07bea09d8c575249ec18f8e42caf9801aef9be1d Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 23:03:00 -0400 Subject: [PATCH 12/44] UPdates to planner --- tools/Motion Planning/Planner.py | 41 ++++++++++++++++++-------------- 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py index 69870425..369d0cf7 100644 --- a/tools/Motion Planning/Planner.py +++ b/tools/Motion Planning/Planner.py @@ -1,8 +1,9 @@ import numpy as np import math import matplotlib.pyplot as plt +import random -def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): +def trapPlan(Xf, Vf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): dX = Xf - Xi # Distance to travel s = np.sign(dX) # Sign @@ -17,19 +18,12 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): Ta = (Vr - Vi)/Ar # Acceleration Time Td = (Vf - Vr)/Dr # Deceleration Time - - ## Peak velocity handling - if Vf == 0: - dXmin = Ta*(Vr + Vi)/2 + Td*(Vr)/2 # Basic wedge profile - elif np.sign(Vf) == np.sign(Vr): - dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - Vf)/2 + Td*Vf # Wedge profile with an unfinished end - else: - dXmin = Ta*(Vr + Vi)/2 + Td*(Vr - s*Vf)/2 # Wedge profile that crosses Y axis on decel + dXmin = Ta*(Vr + Vi)/2 + Td*(Vr + Vf)/2 ## Short move handling if s*dXmin > s*dX: - Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))/math.sqrt(Dr-Ar) # Modified from paper to handle non-zero Vf + Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))*math.sqrt(Dr-Ar)/(Dr-Ar) # Modified from paper to handle non-zero Vf Ta = max(0, (Vr - Vi)/Ar) Tv = 0 Td = max(0, (Vf - Vr)/Dr) @@ -67,14 +61,25 @@ def trapPlan(Xf, Xi, Vf, Vi, Af, Ai, Vmax, Amax, Dmax, dT=0.001): yd[i] = Vr + Dr*(t - Tav) ydd[i] = Dr - return (y, yd, ydd, t_traj) -(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 4, 0, 0, 2, 5, 5) -print("Y: ",Y[len(Y)-1]) -print("Yd: ",Yd[len(Yd)-1]) -plt.plot(t, Y) -plt.plot(t, Yd) -plt.plot(t, Ydd) -plt.show() \ No newline at end of file +(Y, Yd, Ydd, t) = trapPlan(0.74, 1.797, 0, 0, 0, 15.122, 22.022, 22.022) +# random.seed() +# for x in range(100): + +# Vmax = random.uniform(0.1, 20) +# Amax = random.uniform(0.1, 40) + +# Xf = random.uniform(-100.0, 100.0) +# Vf = random.uniform(-Vmax+0.001, Vmax-0.001) + +# print(round(Xf, 3), round(Vf, 3), round(Vmax, 3), round(Amax, 3)) +# (Y, Yd, Ydd, t) = trapPlan(Xf, Vf, 0, 0, 0, Vmax, Amax, Amax) + + # print(Xf-Y[-1], Vf-Yd[-1]) + + # plt.plot(t, Y) + # plt.plot(t, Yd) + # plt.plot(t, Ydd) + # plt.show() \ No newline at end of file From 190da53ee9e9ac82ec555eab1475d5842b4f03f6 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 11 Aug 2018 14:31:37 -0400 Subject: [PATCH 13/44] Add FIR Planner python script --- tools/Motion Planning/FIR_Planner.py | 113 +++++++++++++++++++++++++++ tools/Motion Planning/Planner.py | 85 -------------------- 2 files changed, 113 insertions(+), 85 deletions(-) create mode 100644 tools/Motion Planning/FIR_Planner.py delete mode 100644 tools/Motion Planning/Planner.py diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py new file mode 100644 index 00000000..1ec49adc --- /dev/null +++ b/tools/Motion Planning/FIR_Planner.py @@ -0,0 +1,113 @@ +import numpy as np +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) + +def trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): + + dX = Xf - Xi # Distance to travel + s = np.sign(dX) # Sign + + Ar = s*Amax # Maximum Acceleration (signed) + Dr = -s*Dmax # Maximum Deceleration (signed) + Vr = s*Vmax # Maximum Velocity (signed) + + if(s*Vi > s*Vr): + Ar = -s*Amax + + Ta = (Vr - Vi)/Ar # Acceleration Time + Td = (-Vr)/Dr # Deceleration Time + + ## Peak velocity handling + dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 + + ## Short move handling + if s*dXmin > s*dX: + Vr = s*math.sqrt((-(Vi**2 / Ar)-(2*dX))/(1/Dr - 1/Ar)) + Ta = max(0, (Vr - Vi)/Ar) + Tv = 0 + Td = max(0, (-Vr)/Dr) + else: + Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v + + ## We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory + # Create the time series and preallocate the position, velocity, and acceleration arrays + t_traj = np.linspace(0, Ta+Tv+Td, 10000) + y = [None]*len(t_traj) + yd = [None]*len(t_traj) + ydd = [None]*len(t_traj) + + # We only know acceleration (Ar and Dr), so we integrate to create + # the velocity and position curves + y_Accel = (Ar*Ta*Ta) / 2 + (Vi * Ta) + Xi + Tav = Ta + Tv + + for i in range(len(t_traj)): + t = t_traj[i] + if(t <= 0): # Initial conditions + y[i] = Xi + yd[i] = Vi + ydd[i] = Ai + elif(t <= Ta): # Acceleration + y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi + yd[i] = (Ar * t) + Vi + ydd[i] = Ar + elif(t <= Ta+Tv): # Coasting + y[i] = y_Accel + (Vr * (t - Ta)) + yd[i] = Vr + ydd[i] = 0 + elif(t <= Ta+Tv+Td): # Deceleration + y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((t - Tav)*(t - Tav))/2 + yd[i] = Vr + Dr*(t - Tav) + ydd[i] = Dr + + return (y, yd, ydd, t_traj) + + +#(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 15.122, 22.022, 22.022) +fig, axes = plt.subplots(2, 4) +random.seed() +for x in range(8): + + Vmax = random.uniform(0.1, 20) + Amax = random.uniform(0.1, 40) + + Xi = random.uniform(-100.0, 100.0) + Vi = random.uniform(-Vmax, Vmax) + Xf = random.uniform(-100.0, 100.0) + + (Y, Yd, Ydd, t) = trapPlan(Xf, Xi, 0, 0, Vmax, Amax, Amax) + + if(abs(Xf-Y[-1]) > 0.0001): + print("Bad final position: ", Xf, Y[-1], abs(Xf-Y[-1])) + plt.plot(t, Y) + plt.plot(t, Yd) + plt.plot(t, Ydd) + plt.show() + + elif(abs(Yd[-1]) > 0.0001): + print("Bad final Velocity: ", Yd[-1]) + plt.plot(t, Y) + plt.plot(t, Yd) + plt.plot(t, Ydd) + plt.show() + + else: + print("Position Error: {:.6f}\tVelocity Error: {:.6f}".format(abs(Xf-Y[-1]),abs(Yd[-1]))) + + axes[int(x/4), x%4].plot(t, Y) + axes[int(x/4), x%4].plot(t, Yd) + axes[int(x/4), x%4].plot(t, Ydd) + axes[int(x/4), x%4].set_title('Xi: {:.3f} Xf: {:.3f}'.format(Xi, Xf)) + +plt.show() diff --git a/tools/Motion Planning/Planner.py b/tools/Motion Planning/Planner.py deleted file mode 100644 index 369d0cf7..00000000 --- a/tools/Motion Planning/Planner.py +++ /dev/null @@ -1,85 +0,0 @@ -import numpy as np -import math -import matplotlib.pyplot as plt -import random - -def trapPlan(Xf, Vf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): - - dX = Xf - Xi # Distance to travel - s = np.sign(dX) # Sign - - Ar = s*Amax # Maximum Acceleration (signed) - Dr = -s*Dmax # Maximum Deceleration (signed) - Vr = s*Vmax # Maximum Velocity (signed) - - if(s*Vi > s*Vr): - Ar = -s*Amax - - Ta = (Vr - Vi)/Ar # Acceleration Time - Td = (Vf - Vr)/Dr # Deceleration Time - - ## Peak velocity handling - dXmin = Ta*(Vr + Vi)/2 + Td*(Vr + Vf)/2 - - ## Short move handling - if s*dXmin > s*dX: - Vr = s*math.sqrt(-1*Ar*(Vf*Vf-2*Dr*dX))*math.sqrt(Dr-Ar)/(Dr-Ar) # Modified from paper to handle non-zero Vf - Ta = max(0, (Vr - Vi)/Ar) - Tv = 0 - Td = max(0, (Vf - Vr)/Dr) - else: - Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v - - ## We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory - # Create the time series and preallocate the position, velocity, and acceleration arrays - t_traj = np.arange(0, Ta+Tv+Td, dT) - y = [None]*len(t_traj) - yd = [None]*len(t_traj) - ydd = [None]*len(t_traj) - - # We only know acceleration (Ar and Dr), so we integrate to create - # the velocity and position curves - y_Accel = (Ar*Ta*Ta) / 2 + (Vi * Ta) + Xi - Tav = Ta + Tv - - for i in range(len(t_traj)): - t = t_traj[i] - if(t <= 0): # Initial conditions - y[i] = Xi - yd[i] = Vi - ydd[i] = Ai - elif(t <= Ta): # Acceleration - y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi - yd[i] = (Ar * t) + Vi - ydd[i] = Ar - elif(t <= Ta+Tv): # Coasting - y[i] = y_Accel + (Vr * (t - Ta)) - yd[i] = Vr - ydd[i] = 0 - elif(t <= Ta+Tv+Td): # Deceleration - y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((t - Tav)*(t - Tav))/2 - yd[i] = Vr + Dr*(t - Tav) - ydd[i] = Dr - - return (y, yd, ydd, t_traj) - - -(Y, Yd, Ydd, t) = trapPlan(0.74, 1.797, 0, 0, 0, 15.122, 22.022, 22.022) -# random.seed() -# for x in range(100): - -# Vmax = random.uniform(0.1, 20) -# Amax = random.uniform(0.1, 40) - -# Xf = random.uniform(-100.0, 100.0) -# Vf = random.uniform(-Vmax+0.001, Vmax-0.001) - -# print(round(Xf, 3), round(Vf, 3), round(Vmax, 3), round(Amax, 3)) -# (Y, Yd, Ydd, t) = trapPlan(Xf, Vf, 0, 0, 0, Vmax, Amax, Amax) - - # print(Xf-Y[-1], Vf-Yd[-1]) - - # plt.plot(t, Y) - # plt.plot(t, Yd) - # plt.plot(t, Ydd) - # plt.show() \ No newline at end of file From 332ef149c6074faf6765550e93551e73d27a9af3 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 11 Aug 2018 14:41:53 -0400 Subject: [PATCH 14/44] Some tweaks to FIR Planner --- tools/Motion Planning/FIR_Planner.py | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py index 1ec49adc..5cc316e4 100644 --- a/tools/Motion Planning/FIR_Planner.py +++ b/tools/Motion Planning/FIR_Planner.py @@ -13,7 +13,7 @@ import random # vr, ar and dr Reached values of velocity and acceleration # Tj , Tja, Tjv and Tjd Length of the constant jerk stages (FIR filter time) -def trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): +def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): dX = Xf - Xi # Distance to travel s = np.sign(dX) # Sign @@ -39,7 +39,8 @@ def trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): Td = max(0, (-Vr)/Dr) else: Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v - + + ## We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory # Create the time series and preallocate the position, velocity, and acceleration arrays t_traj = np.linspace(0, Ta+Tv+Td, 10000) @@ -75,7 +76,9 @@ def trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): #(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 15.122, 22.022, 22.022) -fig, axes = plt.subplots(2, 4) +numRows = 2 +numCols = 4 +fig, axes = plt.subplots(numRows, numCols, sharey='all') random.seed() for x in range(8): @@ -86,7 +89,7 @@ for x in range(8): Vi = random.uniform(-Vmax, Vmax) Xf = random.uniform(-100.0, 100.0) - (Y, Yd, Ydd, t) = trapPlan(Xf, Xi, 0, 0, Vmax, Amax, Amax) + (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, 0, 0, Vmax, Amax, Amax) if(abs(Xf-Y[-1]) > 0.0001): print("Bad final position: ", Xf, Y[-1], abs(Xf-Y[-1])) @@ -105,9 +108,9 @@ for x in range(8): else: print("Position Error: {:.6f}\tVelocity Error: {:.6f}".format(abs(Xf-Y[-1]),abs(Yd[-1]))) - axes[int(x/4), x%4].plot(t, Y) - axes[int(x/4), x%4].plot(t, Yd) - axes[int(x/4), x%4].plot(t, Ydd) - axes[int(x/4), x%4].set_title('Xi: {:.3f} Xf: {:.3f}'.format(Xi, Xf)) + axes[int(x/numCols), x%numCols].plot(t, Y) + axes[int(x/numCols), x%numCols].plot(t, Yd) + axes[int(x/numCols), x%numCols].plot(t, Ydd) + axes[int(x/numCols), x%numCols].set_title('Xi: {:.3f} Xf: {:.3f}'.format(Xi, Xf)) plt.show() From 5d9ea7c227f06c2a9b0af0deb2b637c7619c913b Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 11 Aug 2018 14:50:38 -0400 Subject: [PATCH 15/44] Remove dT from FIR planner inputs --- tools/Motion Planning/FIR_Planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py index 5cc316e4..089636d1 100644 --- a/tools/Motion Planning/FIR_Planner.py +++ b/tools/Motion Planning/FIR_Planner.py @@ -13,7 +13,7 @@ import random # vr, ar and dr Reached values of velocity and acceleration # Tj , Tja, Tjv and Tjd Length of the constant jerk stages (FIR filter time) -def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax, dT=0.001): +def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax): dX = Xf - Xi # Distance to travel s = np.sign(dX) # Sign From 51501d145c55bc38edfb692bb24991c92637fbdb Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 11 Aug 2018 17:48:19 -0400 Subject: [PATCH 16/44] Further cleanup of FIR Planner --- tools/Motion Planning/FIR_Planner.py | 60 +++++++++++++++------------- 1 file changed, 33 insertions(+), 27 deletions(-) diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py index 089636d1..3e823810 100644 --- a/tools/Motion Planning/FIR_Planner.py +++ b/tools/Motion Planning/FIR_Planner.py @@ -13,7 +13,7 @@ import random # vr, ar and dr Reached values of velocity and acceleration # Tj , Tja, Tjv and Tjd Length of the constant jerk stages (FIR filter time) -def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax): +def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): dX = Xf - Xi # Distance to travel s = np.sign(dX) # Sign @@ -21,9 +21,6 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax): Ar = s*Amax # Maximum Acceleration (signed) Dr = -s*Dmax # Maximum Deceleration (signed) Vr = s*Vmax # Maximum Velocity (signed) - - if(s*Vi > s*Vr): - Ar = -s*Amax Ta = (Vr - Vi)/Ar # Acceleration Time Td = (-Vr)/Dr # Deceleration Time @@ -32,7 +29,7 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax): dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 ## Short move handling - if s*dXmin > s*dX: + if s*dXmin > s*dX: Vr = s*math.sqrt((-(Vi**2 / Ar)-(2*dX))/(1/Dr - 1/Ar)) Ta = max(0, (Vr - Vi)/Ar) Tv = 0 @@ -40,7 +37,6 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax): else: Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v - ## We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory # Create the time series and preallocate the position, velocity, and acceleration arrays t_traj = np.linspace(0, Ta+Tv+Td, 10000) @@ -55,10 +51,10 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax): for i in range(len(t_traj)): t = t_traj[i] - if(t <= 0): # Initial conditions + if(t < 0): # Initial conditions y[i] = Xi yd[i] = Vi - ydd[i] = Ai + ydd[i] = Ar elif(t <= Ta): # Acceleration y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi yd[i] = (Ar * t) + Vi @@ -75,42 +71,52 @@ def FIR_trapPlan(Xf, Xi, Vi, Ai, Vmax, Amax, Dmax): return (y, yd, ydd, t_traj) -#(Y, Yd, Ydd, t) = trapPlan(10, 0, 0, 0, 15.122, 22.022, 22.022) numRows = 2 numCols = 4 fig, axes = plt.subplots(numRows, numCols, sharey='all') random.seed() -for x in range(8): +for x in range(numRows*numCols): Vmax = random.uniform(0.1, 20) - Amax = random.uniform(0.1, 40) + Amax = random.uniform(0.1, 10) - Xi = random.uniform(-100.0, 100.0) - Vi = random.uniform(-Vmax, Vmax) Xf = random.uniform(-100.0, 100.0) + Xi = random.uniform(-100.0, 100.0) + maxVi = math.sqrt(abs(Xf-Xi)*2*Amax) + Vi = random.uniform(-maxVi, maxVi) - (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, 0, 0, Vmax, Amax, Amax) - if(abs(Xf-Y[-1]) > 0.0001): + (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Amax) + + if(abs(Xf-float(Y[-1])) > 0.0001): print("Bad final position: ", Xf, Y[-1], abs(Xf-Y[-1])) - plt.plot(t, Y) - plt.plot(t, Yd) - plt.plot(t, Ydd) - plt.show() + # plt.figure() + # plt.subplot(2,1,1) + # plt.plot(t, Y) + # plt.plot(t, Yd) + # plt.plot(t[-1], Xf, 'b*') + # plt.plot(t[-1], 0, 'r*') + + # plt.subplot(2,1,2) + # plt.plot(t, Ydd) + + # plt.show() elif(abs(Yd[-1]) > 0.0001): print("Bad final Velocity: ", Yd[-1]) - plt.plot(t, Y) - plt.plot(t, Yd) - plt.plot(t, Ydd) - plt.show() - - else: - print("Position Error: {:.6f}\tVelocity Error: {:.6f}".format(abs(Xf-Y[-1]),abs(Yd[-1]))) + # plt.figure() + # plt.plot(t, Y) + # plt.plot(t, Yd) + # # plt.plot(t, Ydd) + # plt.show() axes[int(x/numCols), x%numCols].plot(t, Y) axes[int(x/numCols), x%numCols].plot(t, Yd) axes[int(x/numCols), x%numCols].plot(t, Ydd) - axes[int(x/numCols), x%numCols].set_title('Xi: {:.3f} Xf: {:.3f}'.format(Xi, Xf)) + axes[int(x/numCols), x%numCols].plot(t[-1], Xf, 'b*') + axes[int(x/numCols), x%numCols].plot(t[-1], 0, 'r*') + dX = abs(Xf - Y[-1]) + dV = abs(0 - Yd[-1]) + axes[int(x/numCols), x%numCols].set_title('Xi: {:.3f} Xf: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xi, Xf, dX, dV)) plt.show() From 2eae19a190895133628678cc0dfb88ca9a1587f0 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 11 Aug 2018 20:59:33 -0400 Subject: [PATCH 17/44] FIR Planner working for all conditions --- tools/Motion Planning/FIR_Planner.py | 114 ++++++++++++++++++++------- 1 file changed, 84 insertions(+), 30 deletions(-) diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py index 3e823810..9193c0cf 100644 --- a/tools/Motion Planning/FIR_Planner.py +++ b/tools/Motion Planning/FIR_Planner.py @@ -15,27 +15,54 @@ import random def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): + dX_stop = Vi**2 / (2*Dmax) # Minimum stopping distance dX = Xf - Xi # Distance to travel - s = np.sign(dX) # Sign + + s = np.sign(dX) # Sign of travel direction Ar = s*Amax # Maximum Acceleration (signed) Dr = -s*Dmax # Maximum Deceleration (signed) Vr = s*Vmax # Maximum Velocity (signed) - - Ta = (Vr - Vi)/Ar # Acceleration Time - Td = (-Vr)/Dr # Deceleration Time - ## Peak velocity handling - dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 - - ## Short move handling - if s*dXmin > s*dX: - Vr = s*math.sqrt((-(Vi**2 / Ar)-(2*dX))/(1/Dr - 1/Ar)) - Ta = max(0, (Vr - Vi)/Ar) + if abs(dX) <= dX_stop: # Check for an overshoot condition (decelerate only) + Ta = 0 Tv = 0 - Td = max(0, (-Vr)/Dr) + Vr = Vi + Dr = -np.sign(Vi)*Dmax + Td = abs(Vi) / Dmax + + print("Overshoot Move:") + print("dX: {:.3f}\tdx_Stop: {:.3f}".format(dX, dX_stop)) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) + print("Ta: {:.3f}\tTv: {:.3f}\tTd: {:.3f}".format(Ta, Tv, Td)) + print("Ar: {:.3f}\tDr: {:.3f}\tVr: {:.3f}".format(Ar, Dr, Vr)) + print() + else: - Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v + # Correct initial acceleration direction if needed + if s*Vi > s*Vr: + Ar = -s*Amax + + Ta = (Vr - Vi)/Ar # Acceleration Time + Td = -Vr/Dr # Deceleration Time + + ## Peak velocity handling + dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 + + ## Short move handling + if abs(dX) < abs(dXmin): + print("Short Move:") + print("dX: {:.3f}\tdXmin: {:.3f}".format(dX, dXmin)) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) + print("Ta: {:.3f}\tTd: {:.3f}\tVr: {:.3f}".format(Ta, Td, Vr)) + print() + + Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar)) + Ta = max(0, (Vr - Vi)/Ar) + Tv = 0 + Td = max(0, -Vr/Dr) + else: + Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v ## We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory # Create the time series and preallocate the position, velocity, and acceleration arrays @@ -55,11 +82,11 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): y[i] = Xi yd[i] = Vi ydd[i] = Ar - elif(t <= Ta): # Acceleration + elif(t < Ta): # Acceleration y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi yd[i] = (Ar * t) + Vi ydd[i] = Ar - elif(t <= Ta+Tv): # Coasting + elif(t < Ta+Tv): # Coasting y[i] = y_Accel + (Vr * (t - Ta)) yd[i] = Vr ydd[i] = 0 @@ -73,23 +100,43 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): numRows = 2 numCols = 4 -fig, axes = plt.subplots(numRows, numCols, sharey='all') +fig, axes = plt.subplots(numRows, numCols) random.seed() for x in range(numRows*numCols): Vmax = random.uniform(0.1, 20) - Amax = random.uniform(0.1, 10) + Amax = random.uniform(0.1, 4) + Dmax = Amax Xf = random.uniform(-100.0, 100.0) Xi = random.uniform(-100.0, 100.0) - maxVi = math.sqrt(abs(Xf-Xi)*2*Amax) - Vi = random.uniform(-maxVi, maxVi) + Vi = random.uniform(-Vmax*2, Vmax*2) + # Vmax = .5 + # Amax = .5 + # Dmax = Amax + # Xf = 10 + # Xi = -2 + # Vi = 4 - (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Amax) + (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) - if(abs(Xf-float(Y[-1])) > 0.0001): - print("Bad final position: ", Xf, Y[-1], abs(Xf-Y[-1])) + if(abs(Xf-Xi) <= Vi**2 / (2*Dmax)): + print("Overshoot: ",Xf) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) + print("Y: {:.3f}\tYd: {:.3f}\tYdd: {:.3f}".format(Y[-1], Yd[-1], Ydd[-1])) + print() + (Y2, Yd2, Ydd2, t2) = FIR_trapPlan(Xf, Y[-1], Yd[-1], Vmax, Amax, Dmax) + Y.extend(Y2) + Yd.extend(Yd2) + Ydd.extend(Ydd2) + t2 = t2 + t[-1] + t = np.append(t, t2, axis=0) + + if(abs(Xf-Y[-1]) > 0.0001): + print("Bad Final Position") + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) + print() # plt.figure() # plt.subplot(2,1,1) # plt.plot(t, Y) @@ -101,22 +148,29 @@ for x in range(numRows*numCols): # plt.plot(t, Ydd) # plt.show() - elif(abs(Yd[-1]) > 0.0001): - print("Bad final Velocity: ", Yd[-1]) + print("Bad Final Velocity") + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) + print() # plt.figure() # plt.plot(t, Y) # plt.plot(t, Yd) # # plt.plot(t, Ydd) # plt.show() - axes[int(x/numCols), x%numCols].plot(t, Y) - axes[int(x/numCols), x%numCols].plot(t, Yd) - axes[int(x/numCols), x%numCols].plot(t, Ydd) - axes[int(x/numCols), x%numCols].plot(t[-1], Xf, 'b*') - axes[int(x/numCols), x%numCols].plot(t[-1], 0, 'r*') + ax1 = axes[int(x/numCols), x%numCols] + ax1.plot(t, Y) + ax1.plot(t, Yd) + ax1.plot(t[-1], Xf, 'b*') + ax1.plot(t[-1], 0, 'r*') + ax1.set_ylabel("Pos and Velocity") + + ax2 = ax1.twinx() + ax2.plot(t, Ydd, color='tab:green') + ax2.tick_params(axis='y', labelcolor='tab:green') + ax2.set_ylabel("Acceleration") dX = abs(Xf - Y[-1]) dV = abs(0 - Yd[-1]) - axes[int(x/numCols), x%numCols].set_title('Xi: {:.3f} Xf: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xi, Xf, dX, dV)) + axes[int(x/numCols), x%numCols].set_title('Xf: {:.3f} Xi: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xf, Xi, dX, dV)) plt.show() From 17831b1f432ed026a3a88511615609c32384634b Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 12 Aug 2018 13:03:26 -0400 Subject: [PATCH 18/44] Update trapTraj to match FIR_Planner.py algorithm --- Firmware/MotorControl/trapTraj.cpp | 104 +++++++++++++-------------- Firmware/MotorControl/trapTraj.hpp | 17 ++--- tools/Motion Planning/FIR_Planner.py | 2 - 3 files changed, 58 insertions(+), 65 deletions(-) diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 091721de..bec3cb8e 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -1,63 +1,65 @@ #include "odrive_main.h" +#include // Standard sign function, implemented to match the Python impelmentation -template int sign(T val){ - if(val == T(0)) +template +int sign(T val) { + if (val == T(0)) return T(0); else return (std::signbit(val)) ? -1 : 1; } float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, - float Vf, float Vi, - float Af, float Ai, - float Vmax, float Amax, float Dmax) { - + float Vi, float Vmax, + float Amax, float Dmax) { + float dx_stop = (Vi * Vi) / (Dmax * 2.0f); float dX = Xf - Xi; int s = sign(dX); - float Ar = s*Amax; // Maximum Acceleration (signed) - float Dr = -1.0f*s*Dmax; // Maximum Deceleration (signed) - float Vr = s*Vmax; // Maximum Velocity (signed) + float Ar = s * Amax; // Maximum Acceleration (signed) + float Dr = -1.0f * s * Dmax; // Maximum Deceleration (signed) + float Vr = s * Vmax; // Maximum Velocity (signed) - // Handle the case where initial velocity > Max velocity - if(s*Vi > s*Vr){ - Ar = -1.0f*s*Amax; - } - - float Ta = (Vr - Vi)/Ar; + float Ta; float Tv; - float Td = (Vf - Vr)/Dr; + float Td; - // Peak Velocity handling - float dXmin; - if(Vf == 0.0f){ - dXmin = Ta*(Vr + Vi)/2.0f + (Td*Vr)/2.0f; // Basic wedge profile - } else if(sign(Vf) == sign(Vr)){ - dXmin = Ta*(Vr + Vi)/2.0f + Td*(Vr - Vf)/2.0f + Td*Vf; // Wedge profile with a non-zero end - } else { - dXmin = Ta*(Vr + Vi)/2.0f + Td*(Vr - s*Vf)/2.0f; // Wedge profile that crosses Y axis during decel - } - - // Short move handling - if(s*dXmin > s*dX){ - Vr = s*std::sqrt(-1.0f*Ar*(Vf*Vf-2*Dr*dX))/sqrt(Dr-Ar); - Ta = std::max(0.0f, (Vr - Vi)/Ar); + // Checking for overshoot on "minimum stop" + if (fabs(dX) <= dx_stop) { + Ta = 0; Tv = 0; - Td = std::max(0.0f, (Vf - Vr)/Dr); + Vr = Vi; + Dr = -1.0f * sign(Vi) * Dmax; + Td = fabs(Vi) / Dmax; } else { - Tv = (dX - dXmin)/Vr; + // 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; + } } // Populate object's values - yAccel_ = (Ar*Ta*Ta) / 2.0f + (Vi * Ta) + Xi; + + Xf_ = Xf; Xi_ = Xi; Vi_ = Vi; - Ai_ = Ai; - - Xf_ = Xf; - Vf_ = Vf; - Af_ = Af; Ar_ = Ar; Dr_ = Dr; @@ -66,34 +68,32 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, Ta_ = Ta; Tv_ = Tv; Td_ = Td; + + yAccel_ = (Ar * Ta * Ta) / 2.0f + (Vi * Ta) + Xi; Tav_ = Ta + Tv; return Ta + Tv + Td; } -TrapezoidalTrajectory::TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t){ +TrapezoidalTrajectory::TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { TrapezoidalTrajectory::TrajectoryStep_t trajStep; - if( t <= 0.0f){ // Initial Conditions + if (t < 0.0f) { // Initial Conditions trajStep.Y = Xi_; trajStep.Yd = Vi_; - trajStep.Ydd = Ai_; - } else if( t <= Ta_){ // Accelerating - trajStep.Y = (Ar_ * (t*t))/2.0f + (Vi_ * t) + Xi_; + trajStep.Ydd = Ar_; + } else if (t < Ta_) { // Accelerating + trajStep.Y = (Ar_ * (t * t)/ 2.0f) + (Vi_ * t) + Xi_; trajStep.Yd = (Ar_ * t) + Vi_; trajStep.Ydd = Ar_; - } else if( t <= Ta_ + Tv_){ // Coasting + } 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 + } else if (t < Ta_ + Tv_ + Td_) { // Deceleration float Tdc = t - Ta_; - trajStep.Y = yAccel_ + (Vr_ * (Tdc)) + Dr_*(Tdc*Tdc)/2.0f; - trajStep.Yd = Vr_ + Dr_*Tdc; - trajStep.Ydd = Dr_; - } else { // Ending conditions - trajStep.Y = Xf_; - trajStep.Yd = Vf_; - trajStep.Ydd = Af_; + trajStep.Y = yAccel_ + (Vr_ * (Tdc)) + Dr_ * (Tdc * Tdc) / 2.0f; + trajStep.Yd = Vr_ + Dr_ * Tdc; + trajStep.Ydd = Dr_; } return trajStep; diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index d833c509..fb5a6c58 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -3,13 +3,9 @@ private: float yAccel_; float Xi_; - float Vi_; - float Ai_; - float Xf_; - float Vf_; - float Af_; - + float Vi_; + float Ar_; float Dr_; float Vr_; @@ -28,11 +24,10 @@ public: TrapezoidalTrajectory(); - float planTrapezoidal(float Xf, float Xi, - float Vf, float Vi, - float Af, float Ai, - float Vmax, float Amax, float Dmax - ); + float planTrapezoidal( float Xf, float Xi, + float Vi, float Vmax, + float Amax, float Dmax + ); TrajectoryStep_t evalTrapTraj(float t); diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py index 9193c0cf..7d52232e 100644 --- a/tools/Motion Planning/FIR_Planner.py +++ b/tools/Motion Planning/FIR_Planner.py @@ -163,12 +163,10 @@ for x in range(numRows*numCols): ax1.plot(t, Yd) ax1.plot(t[-1], Xf, 'b*') ax1.plot(t[-1], 0, 'r*') - ax1.set_ylabel("Pos and Velocity") ax2 = ax1.twinx() ax2.plot(t, Ydd, color='tab:green') ax2.tick_params(axis='y', labelcolor='tab:green') - ax2.set_ylabel("Acceleration") dX = abs(Xf - Y[-1]) dV = abs(0 - Yd[-1]) axes[int(x/numCols), x%numCols].set_title('Xf: {:.3f} Xi: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xf, Xi, dX, dV)) From 03bf297bd819da3a938ec7c3a2677e28433aae2c Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 1 Sep 2018 23:39:31 -0400 Subject: [PATCH 19/44] Add TrapTraj to Axis and Controller. Add move_to_pos funciton --- Firmware/MotorControl/axis.cpp | 7 +++- Firmware/MotorControl/axis.hpp | 4 +- Firmware/MotorControl/controller.cpp | 28 ++++++++++++- Firmware/MotorControl/controller.hpp | 32 +++++++++----- Firmware/MotorControl/main.cpp | 3 +- Firmware/MotorControl/odrive_main.h | 3 +- Firmware/MotorControl/trapTraj.cpp | 6 ++- Firmware/MotorControl/trapTraj.hpp | 62 +++++++++++++++------------- Firmware/Tupfile.lua | 2 +- 9 files changed, 98 insertions(+), 49 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 7d79b3d6..8813aa60 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -11,18 +11,21 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, - Motor& motor) + Motor& motor, + TrapezoidalTrajectory& trap) : hw_config_(hw_config), config_(config), encoder_(encoder), sensorless_estimator_(sensorless_estimator), controller_(controller), - motor_(motor) + motor_(motor), + trap_(trap) { encoder_.axis_ = this; sensorless_estimator_.axis_ = this; controller_.axis_ = this; motor_.axis_ = this; + trap_.axis_ = this; } static void step_cb_wrapper(void* ctx) { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 111f9e26..a6b3c746 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -65,7 +65,8 @@ public: Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, - Motor& motor); + Motor& motor, + TrapezoidalTrajectory& trap); void setup(); void start_thread(); @@ -150,6 +151,7 @@ public: SensorlessEstimator& sensorless_estimator_; Controller& controller_; Motor& motor_; + TrapezoidalTrajectory& trap_; osThreadId thread_id_; volatile bool thread_id_valid_ = false; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 43bb206b..6e63e27c 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -44,6 +44,19 @@ void Controller::set_current_setpoint(float current_setpoint) { #endif } +void Controller::move_to_pos(float pos_setpoint) { + planned_move_end_time_ = axis_->trap_.planTrapezoidal(pos_setpoint, axis_->encoder_.pos_estimate_, + axis_->encoder_.vel_estimate_, config_.vel_limit, + config_.accel_lim, config_.deccel_lim); + config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; + TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); + pos_setpoint_ = myTraj.Y; + vel_setpoint_ = myTraj.Yd; + // current_setpoint_ = myTraj.Ydd; + + planned_move_timer_ = axis_->loop_counter_ * current_meas_period; +} + void Controller::start_anticogging_calibration() { // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) { @@ -82,7 +95,20 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { // Only runs if anticogging_.calib_anticogging is true; non-blocking anticogging_calibration(pos_estimate, vel_estimate); - + + // Controlled Move + if (config_.control_mode >= CTRL_MODE_PLANNED_MOVE_CONTROL) { + float time_now = axis_->loop_counter_ * current_meas_period; + if ((time_now - planned_move_timer_) > planned_move_end_time_) { + config_.control_mode = CTRL_MODE_POSITION_CONTROL; + } else { + TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); + pos_setpoint_ = myTraj.Y; + vel_setpoint_ = myTraj.Yd; + // current_setpoint_ = myTraj.Ydd; + } + } + // Position control // TODO Decide if we want to use encoder or pll position here float vel_des = vel_setpoint_; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index f10b6211..89c561b1 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -11,7 +11,8 @@ typedef enum { CTRL_MODE_VOLTAGE_CONTROL = 0, CTRL_MODE_CURRENT_CONTROL = 1, CTRL_MODE_VELOCITY_CONTROL = 2, - CTRL_MODE_POSITION_CONTROL = 3 + CTRL_MODE_POSITION_CONTROL = 3, + CTRL_MODE_PLANNED_MOVE_CONTROL = 4 } Motor_control_mode_t; struct ControllerConfig_t { @@ -21,6 +22,8 @@ struct ControllerConfig_t { // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] + float accel_lim = 5000.0f; + float deccel_lim = 5000.0f; }; class Controller { @@ -31,6 +34,9 @@ public: void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); void set_vel_setpoint(float vel_setpoint, float current_feed_forward); void set_current_setpoint(float current_setpoint); + + // Trajectory-Planned control + void move_to_pos(float pos_setpoint); // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); @@ -71,6 +77,9 @@ public: float vel_integrator_current_ = 0.0f; // [A] float current_setpoint_ = 0.0f; // [A] + float planned_move_timer_ = 0.0f; + float planned_move_end_time_ = 0.0f; + // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( @@ -83,19 +92,20 @@ public: make_protocol_property("pos_gain", &config_.pos_gain), make_protocol_property("vel_gain", &config_.vel_gain), make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), - make_protocol_property("vel_limit", &config_.vel_limit) - ), + make_protocol_property("vel_limit", &config_.vel_limit), + make_protocol_property("accel_lim", &config_.accel_lim), + make_protocol_property("deccel_lim", &config_.deccel_lim)), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, - "pos_setpoint", - "vel_feed_forward", - "current_feed_forward"), + "pos_setpoint", + "vel_feed_forward", + "current_feed_forward"), make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, - "vel_setpoint", - "current_feed_forward"), + "vel_setpoint", + "current_feed_forward"), make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, - "current_setpoint"), - make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) - ); + "current_setpoint"), + make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), + make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration), ); } }; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 18b88433..340767a5 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -162,8 +162,9 @@ int odrive_main(void) { Motor *motor = new Motor(hw_configs[i].motor_config, hw_configs[i].gate_driver_config, motor_configs[i]); + TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(); axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i], - *encoder, *sensorless_estimator, *controller, *motor); + *encoder, *sensorless_estimator, *controller, *motor, *trap); } // Start ADC for temperature measurements and user measurements diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index c55fe333..27be8455 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -109,9 +109,10 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include #include +#include #include #include -#include + #endif // __cplusplus diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index bec3cb8e..3ec1fae2 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -10,6 +10,8 @@ int sign(T val) { return (std::signbit(val)) ? -1 : 1; } +TrapezoidalTrajectory::TrapezoidalTrajectory(){}; + float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax) { @@ -75,8 +77,8 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, return Ta + Tv + Td; } -TrapezoidalTrajectory::TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { - TrapezoidalTrajectory::TrajectoryStep_t trajStep; +TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { + TrajectoryStep_t trajStep; if (t < 0.0f) { // Initial Conditions trajStep.Y = Xi_; trajStep.Yd = Vi_; diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index fb5a6c58..570a22aa 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -1,35 +1,39 @@ +#ifndef _TRAP_TRAJ_H +#define _TRAP_TRAJ_H + +struct TrajectoryStep_t { + float Y; + float Yd; + float Ydd; +}; + class TrapezoidalTrajectory { -private: - float yAccel_; - - float Xi_; - float Xf_; - float Vi_; - - float Ar_; - float Dr_; - float Vr_; - - float Ta_; - float Tv_; - float Td_; - float Tav_; - -public: - struct TrajectoryStep_t{ - float Y; - float Yd; - float Ydd; - }; + public: + Axis* axis_ = nullptr; // set by Axis constructor TrapezoidalTrajectory(); - float planTrapezoidal( float Xf, float Xi, - float Vi, float Vmax, - float Amax, float Dmax - ); - + float planTrapezoidal(float Xf, float Xi, + float Vi, float Vmax, + float Amax, float Dmax); + TrajectoryStep_t evalTrapTraj(float t); - ~TrapezoidalTrajectory(); -}; \ No newline at end of file + private: + float yAccel_; + + float Xi_; + float Xf_; + float Vi_; + + float Ar_; + float Dr_; + float Vr_; + + float Ta_; + float Tv_; + float Td_; + float Tav_; +}; + +#endif \ No newline at end of file diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 78c52c2b..5c573d6a 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -155,8 +155,8 @@ build{ 'MotorControl/encoder.cpp', 'MotorControl/controller.cpp', 'MotorControl/sensorless_estimator.cpp', - 'MotorControl/main.cpp', 'MotorControl/trapTraj.cpp', + 'MotorControl/main.cpp', 'communication/communication.cpp', 'communication/ascii_protocol.cpp', 'communication/interface_uart.cpp', From 7b53d5511b8a59ffa330ce6fa867a0445320e989 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 1 Sep 2018 23:43:47 -0400 Subject: [PATCH 20/44] Fix compilation error, rename to accel_limit and deccel_limit --- Firmware/MotorControl/controller.cpp | 2 +- Firmware/MotorControl/controller.hpp | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 6e63e27c..65dcd595 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -47,7 +47,7 @@ void Controller::set_current_setpoint(float current_setpoint) { void Controller::move_to_pos(float pos_setpoint) { planned_move_end_time_ = axis_->trap_.planTrapezoidal(pos_setpoint, axis_->encoder_.pos_estimate_, axis_->encoder_.vel_estimate_, config_.vel_limit, - config_.accel_lim, config_.deccel_lim); + config_.accel_limit, config_.deccel_limit); config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); pos_setpoint_ = myTraj.Y; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 89c561b1..1ceb8e6a 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -22,8 +22,8 @@ struct ControllerConfig_t { // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] - float accel_lim = 5000.0f; - float deccel_lim = 5000.0f; + float accel_limit = 5000.0f; + float deccel_limit = 5000.0f; }; class Controller { @@ -93,8 +93,8 @@ public: make_protocol_property("vel_gain", &config_.vel_gain), make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), make_protocol_property("vel_limit", &config_.vel_limit), - make_protocol_property("accel_lim", &config_.accel_lim), - make_protocol_property("deccel_lim", &config_.deccel_lim)), + make_protocol_property("accel_limit", &config_.accel_limit), + make_protocol_property("deccel_limit", &config_.deccel_limit)), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, "pos_setpoint", "vel_feed_forward", @@ -105,7 +105,8 @@ public: make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, "current_setpoint"), make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), - make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration), ); + make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) + ); } }; From cd9aac7962af7122eca3afbfb8e4d5ad804c69da Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 1 Sep 2018 23:46:17 -0400 Subject: [PATCH 21/44] Force vel and current setpoints to be 0 after a planned move completes --- Firmware/MotorControl/controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 65dcd595..d7747e3e 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -101,6 +101,8 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s float time_now = axis_->loop_counter_ * current_meas_period; if ((time_now - planned_move_timer_) > planned_move_end_time_) { config_.control_mode = CTRL_MODE_POSITION_CONTROL; + vel_setpoint_ = 0.0f; + current_setpoint_ = 0.0f; } else { TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); pos_setpoint_ = myTraj.Y; From b22bb2ed428ede23e1aaedb1aac0f0a6dca3b9b9 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 1 Sep 2018 23:48:22 -0400 Subject: [PATCH 22/44] Set current_setpoint_ = 0.0f during planned moves for now --- Firmware/MotorControl/controller.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index d7747e3e..fe8fab52 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -53,6 +53,7 @@ void Controller::move_to_pos(float pos_setpoint) { pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; // current_setpoint_ = myTraj.Ydd; + current_setpoint_ = 0.0f; // Temporary, until we have a way to convert from accel to current planned_move_timer_ = axis_->loop_counter_ * current_meas_period; } @@ -108,6 +109,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; // current_setpoint_ = myTraj.Ydd; + current_setpoint_ = 0.0f; // Temporary, until we have a way of converting from accel to current } } From c6b19ac75f1fa6a2f6e784ad0859b966ce82d5ff Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 2 Sep 2018 11:31:57 -0400 Subject: [PATCH 23/44] Deccel -> decel --- Firmware/MotorControl/controller.cpp | 2 +- Firmware/MotorControl/controller.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index fe8fab52..c9b65019 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -47,7 +47,7 @@ void Controller::set_current_setpoint(float current_setpoint) { void Controller::move_to_pos(float pos_setpoint) { planned_move_end_time_ = axis_->trap_.planTrapezoidal(pos_setpoint, axis_->encoder_.pos_estimate_, axis_->encoder_.vel_estimate_, config_.vel_limit, - config_.accel_limit, config_.deccel_limit); + config_.accel_limit, config_.decel_limit); config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); pos_setpoint_ = myTraj.Y; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 1ceb8e6a..0736fbfa 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -23,7 +23,7 @@ struct ControllerConfig_t { float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] float accel_limit = 5000.0f; - float deccel_limit = 5000.0f; + float decel_limit = 5000.0f; }; class Controller { @@ -94,7 +94,7 @@ public: make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), make_protocol_property("vel_limit", &config_.vel_limit), make_protocol_property("accel_limit", &config_.accel_limit), - make_protocol_property("deccel_limit", &config_.deccel_limit)), + make_protocol_property("decel_limit", &config_.decel_limit)), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, "pos_setpoint", "vel_feed_forward", From f29868e844c3ac26d719309ed57a19854351e0d1 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sun, 2 Sep 2018 11:32:19 -0400 Subject: [PATCH 24/44] Fix bug in C++ deceleration planning code --- Firmware/MotorControl/trapTraj.cpp | 4 +- tools/Motion Planning/FIR_Planner.py | 81 +++++++++++++++------------- 2 files changed, 47 insertions(+), 38 deletions(-) diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 3ec1fae2..419cb30e 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -92,8 +92,8 @@ TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { trajStep.Yd = Vr_; trajStep.Ydd = 0; } else if (t < Ta_ + Tv_ + Td_) { // Deceleration - float Tdc = t - Ta_; - trajStep.Y = yAccel_ + (Vr_ * (Tdc)) + Dr_ * (Tdc * Tdc) / 2.0f; + float Tdc = t - Tav_; + trajStep.Y = yAccel_ + (Vr_ * (t - Ta_)) + Dr_ * (Tdc * Tdc) / 2.0f; trajStep.Yd = Vr_ + Dr_ * Tdc; trajStep.Ydd = Dr_; } diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py index 7d52232e..c07eb1f1 100644 --- a/tools/Motion Planning/FIR_Planner.py +++ b/tools/Motion Planning/FIR_Planner.py @@ -13,9 +13,10 @@ import random # vr, ar and dr Reached values of velocity and acceleration # Tj , Tja, Tjv and Tjd Length of the constant jerk stages (FIR filter time) + def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): - dX_stop = Vi**2 / (2*Dmax) # Minimum stopping distance + dX_stop = Vi**2 / (2*Dmax) # Minimum stopping distance dX = Xf - Xi # Distance to travel s = np.sign(dX) # Sign of travel direction @@ -33,27 +34,29 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): print("Overshoot Move:") print("dX: {:.3f}\tdx_Stop: {:.3f}".format(dX, dX_stop)) - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) print("Ta: {:.3f}\tTv: {:.3f}\tTd: {:.3f}".format(Ta, Tv, Td)) print("Ar: {:.3f}\tDr: {:.3f}\tVr: {:.3f}".format(Ar, Dr, Vr)) - print() + print() else: - # Correct initial acceleration direction if needed + # Correct initial acceleration direction if needed if s*Vi > s*Vr: Ar = -s*Amax Ta = (Vr - Vi)/Ar # Acceleration Time Td = -Vr/Dr # Deceleration Time - ## Peak velocity handling + # Peak velocity handling dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 - ## Short move handling + # Short move handling if abs(dX) < abs(dXmin): print("Short Move:") print("dX: {:.3f}\tdXmin: {:.3f}".format(dX, dXmin)) - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) print("Ta: {:.3f}\tTd: {:.3f}\tVr: {:.3f}".format(Ta, Td, Vr)) print() @@ -64,7 +67,7 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): else: Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v - ## We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory + # We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory # Create the time series and preallocate the position, velocity, and acceleration arrays t_traj = np.linspace(0, Ta+Tv+Td, 10000) y = [None]*len(t_traj) @@ -78,7 +81,7 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): for i in range(len(t_traj)): t = t_traj[i] - if(t < 0): # Initial conditions + if(t < 0): # Initial conditions y[i] = Xi yd[i] = Vi ydd[i] = Ar @@ -90,41 +93,44 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): y[i] = y_Accel + (Vr * (t - Ta)) yd[i] = Vr ydd[i] = 0 - elif(t <= Ta+Tv+Td): # Deceleration - y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((t - Tav)*(t - Tav))/2 - yd[i] = Vr + Dr*(t - Tav) + elif(t <= Ta+Tv+Td): # Deceleration + Tdc = t - Tav + y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((Tdc)*(Tdc))/2 + yd[i] = Vr + Dr*(Tdc) ydd[i] = Dr return (y, yd, ydd, t_traj) numRows = 2 -numCols = 4 +numCols = 2 fig, axes = plt.subplots(numRows, numCols) random.seed() for x in range(numRows*numCols): - Vmax = random.uniform(0.1, 20) - Amax = random.uniform(0.1, 4) - Dmax = Amax - - Xf = random.uniform(-100.0, 100.0) - Xi = random.uniform(-100.0, 100.0) - Vi = random.uniform(-Vmax*2, Vmax*2) - - # Vmax = .5 - # Amax = .5 + # Vmax = random.uniform(0.1, 20) + # Amax = random.uniform(0.1, 4) # Dmax = Amax - # Xf = 10 - # Xi = -2 - # Vi = 4 + + # Xf = random.uniform(-100.0, 100.0) + # Xi = random.uniform(-100.0, 100.0) + # Vi = random.uniform(-Vmax*2, Vmax*2) + + Vmax = 100000.0 + Amax = 100000.0 + Dmax = Amax + Xf = 0 + Xi = 1000000 + Vi = 0 (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) if(abs(Xf-Xi) <= Vi**2 / (2*Dmax)): - print("Overshoot: ",Xf) - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) - print("Y: {:.3f}\tYd: {:.3f}\tYdd: {:.3f}".format(Y[-1], Yd[-1], Ydd[-1])) + print("Overshoot: ", Xf) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) + print("Y: {:.3f}\tYd: {:.3f}\tYdd: {:.3f}".format( + Y[-1], Yd[-1], Ydd[-1])) print() (Y2, Yd2, Ydd2, t2) = FIR_trapPlan(Xf, Y[-1], Yd[-1], Vmax, Amax, Dmax) Y.extend(Y2) @@ -135,7 +141,8 @@ for x in range(numRows*numCols): if(abs(Xf-Y[-1]) > 0.0001): print("Bad Final Position") - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) print() # plt.figure() # plt.subplot(2,1,1) @@ -150,7 +157,8 @@ for x in range(numRows*numCols): # plt.show() elif(abs(Yd[-1]) > 0.0001): print("Bad Final Velocity") - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format(Xf, Xi, Vi, Vmax, Amax)) + print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( + Xf, Xi, Vi, Vmax, Amax)) print() # plt.figure() # plt.plot(t, Y) @@ -158,17 +166,18 @@ for x in range(numRows*numCols): # # plt.plot(t, Ydd) # plt.show() - ax1 = axes[int(x/numCols), x%numCols] + ax1 = axes[int(x/numCols), x % numCols] ax1.plot(t, Y) ax1.plot(t, Yd) ax1.plot(t[-1], Xf, 'b*') ax1.plot(t[-1], 0, 'r*') - ax2 = ax1.twinx() - ax2.plot(t, Ydd, color='tab:green') - ax2.tick_params(axis='y', labelcolor='tab:green') + # ax2 = ax1.twinx() + # ax2.plot(t, Ydd, color='tab:green') + # ax2.tick_params(axis='y', labelcolor='tab:green') dX = abs(Xf - Y[-1]) dV = abs(0 - Yd[-1]) - axes[int(x/numCols), x%numCols].set_title('Xf: {:.3f} Xi: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xf, Xi, dX, dV)) + axes[int(x/numCols), x % numCols].set_title( + 'Xf: {:.3f} Xi: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xf, Xi, dX, dV)) plt.show() From 5558dcc8bef5f4f637bbb4ce0ad4d833059ffac7 Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 3 Sep 2018 12:48:35 -0400 Subject: [PATCH 25/44] Add MIT license to FIRPlanner Python script --- tools/Motion Planning/FIR_Planner.py | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/Motion Planning/FIR_Planner.py index c07eb1f1..5cd54468 100644 --- a/tools/Motion Planning/FIR_Planner.py +++ b/tools/Motion Planning/FIR_Planner.py @@ -1,3 +1,23 @@ +# Copyright (c) 2018 Paul Guénette + +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + import numpy as np import math import matplotlib.pyplot as plt From c65e7265e7afd1c6e76b33cd940f8a2a53217bcb Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 3 Sep 2018 17:13:14 -0400 Subject: [PATCH 26/44] Move traj planning config to trap_traj config object --- Firmware/MotorControl/axis.hpp | 3 ++- Firmware/MotorControl/controller.cpp | 8 ++++---- Firmware/MotorControl/controller.hpp | 7 ++----- Firmware/MotorControl/main.cpp | 6 +++++- Firmware/MotorControl/trapTraj.cpp | 20 ++++++++++---------- Firmware/MotorControl/trapTraj.hpp | 23 ++++++++++++++++++++--- 6 files changed, 43 insertions(+), 24 deletions(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index a6b3c746..4eac2c4b 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -190,7 +190,8 @@ public: make_protocol_object("motor", motor_.make_protocol_definitions()), make_protocol_object("controller", controller_.make_protocol_definitions()), make_protocol_object("encoder", encoder_.make_protocol_definitions()), - make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()) + make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()), + make_protocol_object("trap_traj", trap_.make_protocol_definitions()) ); } }; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index c9b65019..89c843f5 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -46,10 +46,10 @@ void Controller::set_current_setpoint(float current_setpoint) { void Controller::move_to_pos(float pos_setpoint) { planned_move_end_time_ = axis_->trap_.planTrapezoidal(pos_setpoint, axis_->encoder_.pos_estimate_, - axis_->encoder_.vel_estimate_, config_.vel_limit, - config_.accel_limit, config_.decel_limit); + axis_->encoder_.vel_estimate_, axis_->trap_.config_.vel_limit, + axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit); config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; - TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); + TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; // current_setpoint_ = myTraj.Ydd; @@ -105,7 +105,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s vel_setpoint_ = 0.0f; current_setpoint_ = 0.0f; } else { - TrajectoryStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); + TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; // current_setpoint_ = myTraj.Ydd; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 0736fbfa..b4e904b6 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -22,8 +22,6 @@ struct ControllerConfig_t { // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] - float accel_limit = 5000.0f; - float decel_limit = 5000.0f; }; class Controller { @@ -92,9 +90,8 @@ public: make_protocol_property("pos_gain", &config_.pos_gain), make_protocol_property("vel_gain", &config_.vel_gain), make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), - 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("vel_limit", &config_.vel_limit) + ), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, "pos_setpoint", "vel_feed_forward", diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 340767a5..815f406e 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -14,6 +14,7 @@ SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; +TrapTrajConfig_t trap_configs[AXIS_COUNT]; bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; @@ -26,6 +27,7 @@ typedef Config< SensorlessEstimator::Config_t[AXIS_COUNT], ControllerConfig_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT], + TrapTrajConfig_t[AXIS_COUNT], AxisConfig_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { @@ -35,6 +37,7 @@ void save_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, + &trap_configs, &axis_configs)) { //printf("saving configuration failed\r\n"); osDelay(5); } else { @@ -51,6 +54,7 @@ void load_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, + &trap_configs, &axis_configs)) { //If loading failed, restore defaults board_config = BoardConfig_t(); @@ -162,7 +166,7 @@ int odrive_main(void) { Motor *motor = new Motor(hw_configs[i].motor_config, hw_configs[i].gate_driver_config, motor_configs[i]); - TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(); + TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]); axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i], *encoder, *sensorless_estimator, *controller, *motor, *trap); } diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 419cb30e..0c5f3f35 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -1,5 +1,5 @@ -#include "odrive_main.h" #include +#include "odrive_main.h" // Standard sign function, implemented to match the Python impelmentation template @@ -10,7 +10,7 @@ int sign(T val) { return (std::signbit(val)) ? -1 : 1; } -TrapezoidalTrajectory::TrapezoidalTrajectory(){}; +TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t &config) : config_(config) {} float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, @@ -40,15 +40,15 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, Ar = -1.0f * s * Amax; } - Ta = (Vr - Vi) / Ar; // Acceleration time - Td = (-Vr) / Dr; // Deceleration time + 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; + 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)); + 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); @@ -58,7 +58,7 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, } // Populate object's values - + Xf_ = Xf; Xi_ = Xi; Vi_ = Vi; @@ -77,14 +77,14 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, return Ta + Tv + Td; } -TrajectoryStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { - TrajectoryStep_t trajStep; +TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { + TrapTrajStep_t trajStep; if (t < 0.0f) { // Initial Conditions trajStep.Y = Xi_; trajStep.Yd = Vi_; trajStep.Ydd = Ar_; } else if (t < Ta_) { // Accelerating - trajStep.Y = (Ar_ * (t * t)/ 2.0f) + (Vi_ * t) + Xi_; + trajStep.Y = (Ar_ * (t * t) / 2.0f) + (Vi_ * t) + Xi_; trajStep.Yd = (Ar_ * t) + Vi_; trajStep.Ydd = Ar_; } else if (t < Ta_ + Tv_) { // Coasting diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index 570a22aa..bd2ae236 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -1,7 +1,14 @@ #ifndef _TRAP_TRAJ_H #define _TRAP_TRAJ_H -struct TrajectoryStep_t { + +struct TrapTrajConfig_t { + float vel_limit = 20000.0f; + float accel_limit = 5000.0f; + float decel_limit = 5000.0f; +}; + +struct TrapTrajStep_t { float Y; float Yd; float Ydd; @@ -10,16 +17,26 @@ struct TrajectoryStep_t { class TrapezoidalTrajectory { public: Axis* axis_ = nullptr; // set by Axis constructor + TrapTrajConfig_t &config_; - TrapezoidalTrajectory(); + TrapezoidalTrajectory(TrapTrajConfig_t &config); float planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax); - TrajectoryStep_t evalTrapTraj(float t); + TrapTrajStep_t evalTrapTraj(float t); + + auto make_protocol_definitions(){ + return make_protocol_member_list( + make_protocol_property("vel_limit", &config_.vel_limit), + make_protocol_property("accel_limit", &config_.accel_limit), + make_protocol_property("decel_limit", &config_.decel_limit) + ); + } private: + float yAccel_; float Xi_; From c014dae5bb35c8e0df536ffb740a84ea40c2aa78 Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 3 Sep 2018 17:16:09 -0400 Subject: [PATCH 27/44] Fix missing config object for trap_traj --- Firmware/MotorControl/trapTraj.hpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index bd2ae236..60001218 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -1,7 +1,6 @@ #ifndef _TRAP_TRAJ_H #define _TRAP_TRAJ_H - struct TrapTrajConfig_t { float vel_limit = 20000.0f; float accel_limit = 5000.0f; @@ -16,7 +15,7 @@ struct TrapTrajStep_t { class TrapezoidalTrajectory { public: - Axis* axis_ = nullptr; // set by Axis constructor + Axis *axis_ = nullptr; // set by Axis constructor TrapTrajConfig_t &config_; TrapezoidalTrajectory(TrapTrajConfig_t &config); @@ -27,16 +26,15 @@ class TrapezoidalTrajectory { TrapTrajStep_t evalTrapTraj(float t); - auto make_protocol_definitions(){ - return make_protocol_member_list( - make_protocol_property("vel_limit", &config_.vel_limit), - make_protocol_property("accel_limit", &config_.accel_limit), - make_protocol_property("decel_limit", &config_.decel_limit) - ); + auto make_protocol_definitions() { + return make_protocol_member_list( + 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))); } private: - float yAccel_; float Xi_; From 97b708d12ccf70b33dabc707e5d5f5e9a89f6b93 Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 7 Sep 2018 18:13:39 -0400 Subject: [PATCH 28/44] Put anticogging in the FF path if we're doing a controlled move --- Firmware/MotorControl/controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 89c843f5..7aae7fb2 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -96,6 +96,7 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { // Only runs if anticogging_.calib_anticogging is true; non-blocking anticogging_calibration(pos_estimate, vel_estimate); + float anticogging_pos = pos_estimate; // Controlled Move if (config_.control_mode >= CTRL_MODE_PLANNED_MOVE_CONTROL) { @@ -111,6 +112,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // current_setpoint_ = myTraj.Ydd; current_setpoint_ = 0.0f; // Temporary, until we have a way of converting from accel to current } + anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } // Position control @@ -133,7 +135,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) if (anticogging_.use_anticogging) { - Iq += anticogging_.cogging_map[mod(static_cast(pos_estimate), axis_->encoder_.config_.cpr)]; + Iq += anticogging_.cogging_map[mod(static_cast(anticogging_pos), axis_->encoder_.config_.cpr)]; } float v_err = vel_des - vel_estimate; From 0ea6a5a37bea707ef8e954b80f35176b5add3e4d Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 15 Sep 2018 23:08:40 -0400 Subject: [PATCH 29/44] Support overshoot moves --- Firmware/MotorControl/trapTraj.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 0c5f3f35..67e0aa6f 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -58,7 +58,6 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, } // Populate object's values - Xf_ = Xf; Xi_ = Xi; Vi_ = Vi; @@ -74,7 +73,23 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, yAccel_ = (Ar * Ta * Ta) / 2.0f + (Vi * Ta) + Xi; Tav_ = Ta + Tv; - return Ta + Tv + Td; + // If it's an overshoot trajectory, we need to re-do our process + // to generate the second-half of the trajectory + if (fabs(dX) <= dx_stop) { + TrapTrajStep_t traj; + traj = evalTrapTraj(Td_); + planTrapezoidal(Xf, traj.Y, traj.Yd, Vmax, Amax, Dmax); + + // Fix initial points + Xi_ = Xi; + Vi_ = Vi; + + // Fix time points + Ta_ += Td; + yAccel_ = (Ar_ * Ta_ * Ta_) / 2.0f + (Vi_ * Ta_) + Xi_; + Tav_ = Ta_ + Tv_; + } + return Ta_ + Tv_ + Td_; } TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { From cd09406133e62222130ea49d50fb79028212f535 Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 15 Sep 2018 23:23:00 -0400 Subject: [PATCH 30/44] Add trapezoidal planner to the changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 32a214f2..69865563 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,9 @@ Please add a note of your changes below this heading if you make a Pull Request. # Unreleased +## Added +* Trapezoidal Trajectory Planner + ## Fixed * Serious reliability issue with USB communication where packets on Native and the CDC interface would collide with each other. From ea510fb8e596e287116200553d058e41ce72c2ab Mon Sep 17 00:00:00 2001 From: Unknown Date: Sat, 15 Sep 2018 23:08:40 -0400 Subject: [PATCH 31/44] Revert "Support overshoot moves" This reverts commit 0ea6a5a37bea707ef8e954b80f35176b5add3e4d. --- Firmware/MotorControl/trapTraj.cpp | 19 ++----------------- 1 file changed, 2 insertions(+), 17 deletions(-) diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 67e0aa6f..0c5f3f35 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -58,6 +58,7 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, } // Populate object's values + Xf_ = Xf; Xi_ = Xi; Vi_ = Vi; @@ -73,23 +74,7 @@ float TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, yAccel_ = (Ar * Ta * Ta) / 2.0f + (Vi * Ta) + Xi; Tav_ = Ta + Tv; - // If it's an overshoot trajectory, we need to re-do our process - // to generate the second-half of the trajectory - if (fabs(dX) <= dx_stop) { - TrapTrajStep_t traj; - traj = evalTrapTraj(Td_); - planTrapezoidal(Xf, traj.Y, traj.Yd, Vmax, Amax, Dmax); - - // Fix initial points - Xi_ = Xi; - Vi_ = Vi; - - // Fix time points - Ta_ += Td; - yAccel_ = (Ar_ * Ta_ * Ta_) / 2.0f + (Vi_ * Ta_) + Xi_; - Tav_ = Ta_ + Tv_; - } - return Ta_ + Tv_ + Td_; + return Ta + Tv + Td; } TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { From 3b931be8cf608f0ada9fc9d49ce1e0fed290da10 Mon Sep 17 00:00:00 2001 From: Unknown Date: Thu, 20 Sep 2018 20:25:38 -0400 Subject: [PATCH 32/44] move_to_pos uses setpoints instead of estimates. Add A_to_cpss --- Firmware/MotorControl/controller.cpp | 12 +++++------- Firmware/MotorControl/controller.hpp | 2 +- Firmware/MotorControl/trapTraj.hpp | 1 + 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 7aae7fb2..d3f9870c 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -44,16 +44,15 @@ void Controller::set_current_setpoint(float current_setpoint) { #endif } -void Controller::move_to_pos(float pos_setpoint) { - planned_move_end_time_ = axis_->trap_.planTrapezoidal(pos_setpoint, axis_->encoder_.pos_estimate_, - axis_->encoder_.vel_estimate_, axis_->trap_.config_.vel_limit, +void Controller::move_to_pos(float goal_point) { + planned_move_end_time_ = axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, + vel_setpoint_, axis_->trap_.config_.vel_limit, axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit); config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; - // current_setpoint_ = myTraj.Ydd; - current_setpoint_ = 0.0f; // Temporary, until we have a way to convert from accel to current + current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.A_to_cpss; planned_move_timer_ = axis_->loop_counter_ * current_meas_period; } @@ -109,8 +108,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; - // current_setpoint_ = myTraj.Ydd; - current_setpoint_ = 0.0f; // Temporary, until we have a way of converting from accel to current + current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.A_to_cpss; } anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index b4e904b6..dd3f76a5 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -34,7 +34,7 @@ public: void set_current_setpoint(float current_setpoint); // Trajectory-Planned control - void move_to_pos(float pos_setpoint); + void move_to_pos(float goal_point); // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index 60001218..cdfe5bc4 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -5,6 +5,7 @@ struct TrapTrajConfig_t { float vel_limit = 20000.0f; float accel_limit = 5000.0f; float decel_limit = 5000.0f; + float A_to_cpss = 0.0f; }; struct TrapTrajStep_t { From 4475c59c8a36685c46c85dc3df11050486958df9 Mon Sep 17 00:00:00 2001 From: Unknown Date: Thu, 20 Sep 2018 20:27:36 -0400 Subject: [PATCH 33/44] Calculating A from cpss, not other way around --- Firmware/MotorControl/controller.cpp | 4 ++-- Firmware/MotorControl/trapTraj.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index d3f9870c..d7740a5e 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -52,7 +52,7 @@ void Controller::move_to_pos(float goal_point) { TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; - current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.A_to_cpss; + current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A; planned_move_timer_ = axis_->loop_counter_ * current_meas_period; } @@ -108,7 +108,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); pos_setpoint_ = myTraj.Y; vel_setpoint_ = myTraj.Yd; - current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.A_to_cpss; + current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A; } anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index cdfe5bc4..a30a8616 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -5,7 +5,7 @@ struct TrapTrajConfig_t { float vel_limit = 20000.0f; float accel_limit = 5000.0f; float decel_limit = 5000.0f; - float A_to_cpss = 0.0f; + float cpss_to_A = 0.0f; }; struct TrapTrajStep_t { From 8392f69b1cb25e1123704f552658917fde0d917f Mon Sep 17 00:00:00 2001 From: Unknown Date: Fri, 21 Sep 2018 18:37:46 -0400 Subject: [PATCH 34/44] Add missing default trapTrajConfig creation --- Firmware/MotorControl/main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 815f406e..1f9b38cb 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -63,6 +63,7 @@ void load_configuration(void) { sensorless_configs[i] = SensorlessEstimator::Config_t(); controller_configs[i] = ControllerConfig_t(); motor_configs[i] = MotorConfig_t(); + trap_configs[i] = TrapTrajConfig_t(); axis_configs[i] = AxisConfig_t(); } } else { From 848b03fb1b9691adf586bfa7f348670264525c68 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 21 Sep 2018 21:53:13 -0700 Subject: [PATCH 35/44] rename path to exclude space --- tools/{Motion Planning => motion_planning}/FIR_Planner.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename tools/{Motion Planning => motion_planning}/FIR_Planner.py (100%) diff --git a/tools/Motion Planning/FIR_Planner.py b/tools/motion_planning/FIR_Planner.py similarity index 100% rename from tools/Motion Planning/FIR_Planner.py rename to tools/motion_planning/FIR_Planner.py From 0447f517ac7f57c338d8eddb7d7635b11b9fcdc4 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 23 Sep 2018 02:00:20 -0700 Subject: [PATCH 36/44] Rewrite of main planning logic --- tools/motion_planning/FIR_Planner.py | 181 +++++++++++---------------- 1 file changed, 73 insertions(+), 108 deletions(-) diff --git a/tools/motion_planning/FIR_Planner.py b/tools/motion_planning/FIR_Planner.py index 5cd54468..ee4b1845 100644 --- a/tools/motion_planning/FIR_Planner.py +++ b/tools/motion_planning/FIR_Planner.py @@ -18,6 +18,10 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE # SOFTWARE. +# This algorithm is based on: +# FIR filter-based online jerk-constrained trajectory generation +# https://www.researchgate.net/profile/Richard_Bearee/publication/304358769_FIR_filter-based_online_jerk-controlled_trajectory_generation/links/5770ccdd08ae10de639c0ff7/FIR-filter-based-online-jerk-controlled-trajectory-generation.pdf + import numpy as np import math import matplotlib.pyplot as plt @@ -35,57 +39,47 @@ import random def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): - - dX_stop = Vi**2 / (2*Dmax) # Minimum stopping distance dX = Xf - Xi # Distance to travel - - s = np.sign(dX) # Sign of travel direction - + stop_dist = Vi**2 / (2*Dmax) # Minimum stopping distance + dX_stop = np.sign(Vi)*stop_dist # Minimum stopping displacement + s = np.sign(dX - dX_stop) # Sign of coast velocity (if any) Ar = s*Amax # Maximum Acceleration (signed) Dr = -s*Dmax # Maximum Deceleration (signed) Vr = s*Vmax # Maximum Velocity (signed) - if abs(dX) <= dX_stop: # Check for an overshoot condition (decelerate only) - Ta = 0 + # 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!") + + # Time to accel/decel to/from Vr (cruise speed) + Ta = (Vr - Vi)/Ar + Td = -Vr/Dr + + # Integrate 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 + + # Did we displace enough to reach cruising speed? + if abs(dX) < abs(dXmin): + print("Short Move:") + # From paper: + # Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar)) + # Simplified for less divisions: + Vr = s*math.sqrt((Dr*Vi**2 + 2*Ar*Dr*dX) / (Dr-Ar)) + Ta = max(0, (Vr - Vi)/Ar) + Td = max(0, -Vr/Dr) Tv = 0 - Vr = Vi - Dr = -np.sign(Vi)*Dmax - Td = abs(Vi) / Dmax - - print("Overshoot Move:") - print("dX: {:.3f}\tdx_Stop: {:.3f}".format(dX, dX_stop)) - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( - Xf, Xi, Vi, Vmax, Amax)) - print("Ta: {:.3f}\tTv: {:.3f}\tTd: {:.3f}".format(Ta, Tv, Td)) - print("Ar: {:.3f}\tDr: {:.3f}\tVr: {:.3f}".format(Ar, Dr, Vr)) - print() - else: - # Correct initial acceleration direction if needed - if s*Vi > s*Vr: - Ar = -s*Amax + print("Long move:") + Tv = (dX - dXmin)/Vr # Coasting time - Ta = (Vr - Vi)/Ar # Acceleration Time - Td = -Vr/Dr # Deceleration Time - - # Peak velocity handling - dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 - - # Short move handling - if abs(dX) < abs(dXmin): - print("Short Move:") - print("dX: {:.3f}\tdXmin: {:.3f}".format(dX, dXmin)) - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( - Xf, Xi, Vi, Vmax, Amax)) - print("Ta: {:.3f}\tTd: {:.3f}\tVr: {:.3f}".format(Ta, Td, Vr)) - print() - - Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar)) - Ta = max(0, (Vr - Vi)/Ar) - Tv = 0 - Td = max(0, -Vr/Dr) - else: - Tv = (dX - dXmin)/Vr # non-short move, coast time at constant v + print("Xi: {:.3f}\tXf: {:.3f}\tVi: {:.3f}".format(Xi, Xf, Vi)) + print("Amax: {:.3f}\tVmax: {:.3f}\tDmax: {:.3f}".format(Amax, Vmax, Dmax)) + print("dX: {:.3f}\tdx_Stop: {:.3f}".format(dX, dX_stop)) + print("Ar: {:.3f}\tDr: {:.3f}\tVr: {:.3f}".format(Ar, Dr, Vr)) + print("Ta: {:.3f}\tTv: {:.3f}\tTd: {:.3f}".format(Ta, Tv, Td)) # We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory # Create the time series and preallocate the position, velocity, and acceleration arrays @@ -122,82 +116,53 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): return (y, yd, ydd, t_traj) -numRows = 2 -numCols = 2 +pos_range = 10000.0 +Vmax_range = 8000.0 +Amax_range = 10000.0 +plot_range = 10000.0 + +numRows = 3 +numCols = 5 fig, axes = plt.subplots(numRows, numCols) -random.seed() +random.seed(2) # Repeatable tests by using specific seed for x in range(numRows*numCols): + rownow = int(x/numCols) + colnow = x % numCols + print("row: {}, col: {}".format(rownow, colnow)) - # Vmax = random.uniform(0.1, 20) - # Amax = random.uniform(0.1, 4) - # Dmax = Amax - - # Xf = random.uniform(-100.0, 100.0) - # Xi = random.uniform(-100.0, 100.0) - # Vi = random.uniform(-Vmax*2, Vmax*2) - - Vmax = 100000.0 - Amax = 100000.0 + Vmax = random.uniform(0.1*Vmax_range, Vmax_range) + Amax = random.uniform(0.1*Amax_range, Amax_range) Dmax = Amax - Xf = 0 - Xi = 1000000 - Vi = 0 + Xf = random.uniform(-pos_range, pos_range) + Xi = random.uniform(-pos_range, pos_range) + Vi = random.uniform(-Vmax*1.5, Vmax*1.5) (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) - if(abs(Xf-Xi) <= Vi**2 / (2*Dmax)): - print("Overshoot: ", Xf) - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( - Xf, Xi, Vi, Vmax, Amax)) - print("Y: {:.3f}\tYd: {:.3f}\tYdd: {:.3f}".format( - Y[-1], Yd[-1], Ydd[-1])) - print() - (Y2, Yd2, Ydd2, t2) = FIR_trapPlan(Xf, Y[-1], Yd[-1], Vmax, Amax, Dmax) - Y.extend(Y2) - Yd.extend(Yd2) - Ydd.extend(Ydd2) - t2 = t2 + t[-1] - t = np.append(t, t2, axis=0) - + if(abs(Xi-Y[0]) > 0.0001): + print("---------- Bad Initial Position ----------") if(abs(Xf-Y[-1]) > 0.0001): - print("Bad Final Position") - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( - Xf, Xi, Vi, Vmax, Amax)) - print() - # plt.figure() - # plt.subplot(2,1,1) - # plt.plot(t, Y) - # plt.plot(t, Yd) - # plt.plot(t[-1], Xf, 'b*') - # plt.plot(t[-1], 0, 'r*') + print("---------- Bad Final Position ----------") + if(abs(Vi-Yd[0]) > 0.0001): + print("---------- Bad Initial Velocity ----------") + if(abs(Yd[-1]) > 0.0001): + print("---------- Bad Final Velocity ----------") - # plt.subplot(2,1,2) - # plt.plot(t, Ydd) + # Plotting + ax1 = axes[rownow, colnow] + # Vel limits (draw first for clearer z-order) + ax1.plot([t[0], t[-1]], [Vmax, Vmax], 'g--') + ax1.plot([t[0], t[-1]], [-Vmax, -Vmax], 'g--') - # plt.show() - elif(abs(Yd[-1]) > 0.0001): - print("Bad Final Velocity") - print("Xf: {:.3f}\tXi: {:.3f}\tVi: {:.3f}\tVmax: {:.3f}\tAmax: {:.3f}\t".format( - Xf, Xi, Vi, Vmax, Amax)) - print() - # plt.figure() - # plt.plot(t, Y) - # plt.plot(t, Yd) - # # plt.plot(t, Ydd) - # plt.show() + ax1.plot(t, Y) # Pos + ax1.plot(t, Yd) # Vel + ax1.plot(t[0], Xi, 'bo') # Pos Initial + ax1.plot(t[0], Vi, 'ro') # Vel Initial + ax1.plot(t[-1], Xf, 'b*') # Pos Final + ax1.plot(t[-1], 0, 'r*') # Vel Final - ax1 = axes[int(x/numCols), x % numCols] - ax1.plot(t, Y) - ax1.plot(t, Yd) - ax1.plot(t[-1], Xf, 'b*') - ax1.plot(t[-1], 0, 'r*') + ax1.set_ylim(-plot_range, plot_range) - # ax2 = ax1.twinx() - # ax2.plot(t, Ydd, color='tab:green') - # ax2.tick_params(axis='y', labelcolor='tab:green') - dX = abs(Xf - Y[-1]) - dV = abs(0 - Yd[-1]) - axes[int(x/numCols), x % numCols].set_title( - 'Xf: {:.3f} Xi: {:.3f}\ndX: {:.3f} dV: {:.3f}'.format(Xf, Xi, dX, dV)) + print() plt.show() From 5478004911fd84b1bf946ff50f8615edb1cdae2b Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 23 Sep 2018 17:38:48 -0700 Subject: [PATCH 37/44] clean up some more --- tools/motion_planning/FIR_Planner.py | 86 +++++++++++++++++----------- 1 file changed, 54 insertions(+), 32 deletions(-) diff --git a/tools/motion_planning/FIR_Planner.py b/tools/motion_planning/FIR_Planner.py index ee4b1845..b8048a9a 100644 --- a/tools/motion_planning/FIR_Planner.py +++ b/tools/motion_planning/FIR_Planner.py @@ -1,4 +1,5 @@ # Copyright (c) 2018 Paul Guénette +# Copyright (c) 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 @@ -41,8 +42,8 @@ import random def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): dX = Xf - Xi # Distance to travel stop_dist = Vi**2 / (2*Dmax) # Minimum stopping distance - dX_stop = np.sign(Vi)*stop_dist # Minimum stopping displacement - s = np.sign(dX - dX_stop) # Sign of coast velocity (if any) + dXstop = np.sign(Vi)*stop_dist # Minimum stopping displacement + s = np.sign(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) @@ -62,7 +63,7 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): dXmin = Ta*(Vr + Vi)/2.0 + Td*(Vr)/2.0 # Did we displace enough to reach cruising speed? - if abs(dX) < abs(dXmin): + if s*dX < s*dXmin: print("Short Move:") # From paper: # Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar)) @@ -75,43 +76,60 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): print("Long move:") Tv = (dX - dXmin)/Vr # Coasting time + Tf = Ta+Tv+Td + print("Xi: {:.3f}\tXf: {:.3f}\tVi: {:.3f}".format(Xi, Xf, Vi)) print("Amax: {:.3f}\tVmax: {:.3f}\tDmax: {:.3f}".format(Amax, Vmax, Dmax)) - print("dX: {:.3f}\tdx_Stop: {:.3f}".format(dX, dX_stop)) + print("dX: {:.3f}\tdXstop: {:.3f}\tdXmin: {:.3f}".format(dX, dXstop, dXmin)) print("Ar: {:.3f}\tDr: {:.3f}\tVr: {:.3f}".format(Ar, Dr, Vr)) print("Ta: {:.3f}\tTv: {:.3f}\tTd: {:.3f}".format(Ta, Tv, Td)) # We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory # Create the time series and preallocate the position, velocity, and acceleration arrays - t_traj = np.linspace(0, Ta+Tv+Td, 10000) + # t_traj = np.linspace(0, Tf, 10000) + t_traj = np.arange(0, Tf+0.1, 1/10000) y = [None]*len(t_traj) yd = [None]*len(t_traj) ydd = [None]*len(t_traj) # We only know acceleration (Ar and Dr), so we integrate to create # the velocity and position curves - y_Accel = (Ar*Ta*Ta) / 2 + (Vi * Ta) + Xi - Tav = Ta + Tv + y_Accel = Xi + Vi*Ta + 0.5*Ar*Ta**2 for i in range(len(t_traj)): t = t_traj[i] - if(t < 0): # Initial conditions - y[i] = Xi - yd[i] = Vi - ydd[i] = Ar - elif(t < Ta): # Acceleration - y[i] = (Ar * (t*t)/2) + (Vi * t) + Xi - yd[i] = (Ar * t) + Vi - ydd[i] = Ar - elif(t < Ta+Tv): # Coasting - y[i] = y_Accel + (Vr * (t - Ta)) - yd[i] = Vr + if t < 0: # Initial conditions + y[i] = Xi + yd[i] = Vi ydd[i] = 0 - elif(t <= Ta+Tv+Td): # Deceleration - Tdc = t - Tav - y[i] = y_Accel + (Vr * (t - Ta)) + Dr*((Tdc)*(Tdc))/2 - yd[i] = Vr + Dr*(Tdc) + elif t < Ta: # Acceleration + y[i] = Xi + Vi*t + 0.5*Ar*t**2 + yd[i] = Vi + Ar*t + ydd[i] = Ar + elif t < Ta+Tv: # Coasting + y[i] = y_Accel + Vr*(t-Ta) + yd[i] = Vr + ydd[i] = 0 + elif t < Tf: # Deceleration + td = t-Tf + y[i] = Xf + 0*td + 0.5*Dr*td**2 + yd[i] = 0 + Dr*td ydd[i] = Dr + elif t >= Tf: # Final condition + y[i] = Xf + yd[i] = 0 + ydd[i] = 0 + + dy = np.diff(y) + dy_max = np.max(np.abs(dy)) + dyd = np.diff(yd) + dyd_max = np.max(np.abs(dyd)) + print("dy_max: {:.3f}\tdyd_max: {:.3f}".format(dy_max, dyd_max)) + if dy_max/np.abs(Xf-Xi) > 0.01: + print("---------- Bad Pos Continuity ----------") + # import ipdb; ipdb.set_trace() + if dyd_max/Vmax > 0.001: + print("---------- Bad Vel Continuity ----------") return (y, yd, ydd, t_traj) @@ -124,7 +142,7 @@ plot_range = 10000.0 numRows = 3 numCols = 5 fig, axes = plt.subplots(numRows, numCols) -random.seed(2) # Repeatable tests by using specific seed +random.seed(3) # Repeatable tests by using specific seed for x in range(numRows*numCols): rownow = int(x/numCols) colnow = x % numCols @@ -135,17 +153,20 @@ for x in range(numRows*numCols): Dmax = Amax Xf = random.uniform(-pos_range, pos_range) Xi = random.uniform(-pos_range, pos_range) - Vi = random.uniform(-Vmax*1.5, Vmax*1.5) + if random.random() <= 0.5: + Vi = random.uniform(-Vmax*1.5, Vmax*1.5) + else: + Vi = 0 (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) - if(abs(Xi-Y[0]) > 0.0001): + if abs(Xi-Y[0]) > 0.0001: print("---------- Bad Initial Position ----------") - if(abs(Xf-Y[-1]) > 0.0001): + if abs(Xf-Y[-1]) > 0.0001: print("---------- Bad Final Position ----------") - if(abs(Vi-Yd[0]) > 0.0001): + if abs(Vi-Yd[0]) > 0.0001: print("---------- Bad Initial Velocity ----------") - if(abs(Yd[-1]) > 0.0001): + if abs(Yd[-1]) > 0.0001: print("---------- Bad Final Velocity ----------") # Plotting @@ -156,10 +177,11 @@ for x in range(numRows*numCols): ax1.plot(t, Y) # Pos ax1.plot(t, Yd) # Vel - ax1.plot(t[0], Xi, 'bo') # Pos Initial - ax1.plot(t[0], Vi, 'ro') # Vel Initial - ax1.plot(t[-1], Xf, 'b*') # Pos Final - ax1.plot(t[-1], 0, 'r*') # Vel Final + ax1.plot(0, Xi, 'bo') # Pos Initial + ax1.plot(0, Vi, 'ro') # Vel Initial + ## TODO: pull out Ta+Td+Td from planner for correct plot points + ax1.plot(t[-1]-0.1, Xf, 'b*') # Pos Final + ax1.plot(t[-1]-0.1, 0, 'r*') # Vel Final ax1.set_ylim(-plot_range, plot_range) From cf6e327c21cf165e785f3f28bbf13e3f6bf1015f Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 23 Sep 2018 18:13:42 -0700 Subject: [PATCH 38/44] added large randomizing non-graphical test --- tools/motion_planning/FIR_Planner.py | 155 ++++++++++++++++----------- 1 file changed, 95 insertions(+), 60 deletions(-) diff --git a/tools/motion_planning/FIR_Planner.py b/tools/motion_planning/FIR_Planner.py index b8048a9a..9158cc87 100644 --- a/tools/motion_planning/FIR_Planner.py +++ b/tools/motion_planning/FIR_Planner.py @@ -38,8 +38,14 @@ import random # vr, ar and dr Reached values of velocity and acceleration # Tj , Tja, Tjv and Tjd Length of the constant jerk stages (FIR filter time) +# Test scales: +pos_range = 10000.0 +Vmax_range = 8000.0 +Amax_range = 10000.0 +plot_range = 10000.0 -def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): + +def TrapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): dX = Xf - Xi # Distance to travel stop_dist = Vi**2 / (2*Dmax) # Minimum stopping distance dXstop = np.sign(Vi)*stop_dist # Minimum stopping displacement @@ -77,16 +83,17 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): Tv = (dX - dXmin)/Vr # Coasting time Tf = Ta+Tv+Td - - print("Xi: {:.3f}\tXf: {:.3f}\tVi: {:.3f}".format(Xi, Xf, Vi)) - print("Amax: {:.3f}\tVmax: {:.3f}\tDmax: {:.3f}".format(Amax, Vmax, Dmax)) - print("dX: {:.3f}\tdXstop: {:.3f}\tdXmin: {:.3f}".format(dX, dXstop, dXmin)) - print("Ar: {:.3f}\tDr: {:.3f}\tVr: {:.3f}".format(Ar, Dr, Vr)) - print("Ta: {:.3f}\tTv: {:.3f}\tTd: {:.3f}".format(Ta, Tv, Td)) - # We've computed Ta, Tv, Td, and Vr. Time to produce a trajectory + print("Xi: {:.2f}\tXf: {:.2f}\tVi: {:.2f}".format(Xi, Xf, Vi)) + print("Amax: {:.2f}\tVmax: {:.2f}\tDmax: {:.2f}".format(Amax, Vmax, Dmax)) + print("dX: {:.2f}\tdXst: {:.2f}\tdXmin: {:.2f}".format(dX, dXstop, dXmin)) + print("Ar: {:.2f}\tVr: {:.2f}\tDr: {:.2f}".format(Ar, Vr, Dr)) + print("Ta: {:.2f}\tTv: {:.2f}\tTd: {:.2f}".format(Ta, Tv, Td)) + + return (Ar, Vr, Dr, Ta, Tv, Td, Tf) + +def EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf): # Create the time series and preallocate the position, velocity, and acceleration arrays - # t_traj = np.linspace(0, Tf, 10000) t_traj = np.arange(0, Tf+0.1, 1/10000) y = [None]*len(t_traj) yd = [None]*len(t_traj) @@ -124,67 +131,95 @@ def FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): dy_max = np.max(np.abs(dy)) dyd = np.diff(yd) dyd_max = np.max(np.abs(dyd)) - print("dy_max: {:.3f}\tdyd_max: {:.3f}".format(dy_max, dyd_max)) - if dy_max/np.abs(Xf-Xi) > 0.01: - print("---------- Bad Pos Continuity ----------") - # import ipdb; ipdb.set_trace() - if dyd_max/Vmax > 0.001: - print("---------- Bad Vel Continuity ----------") + print("dy_max: {:.2f}\tdyd_max: {:.2f}".format(dy_max, dyd_max)) + + error = False + if dy_max/pos_range > 0.001: + print("---------- Bad Pos Continuity --------------------") + error = True + if dyd_max/Vmax_range > 0.001: + print("---------- Bad Vel Continuity --------------------") + error = True + if abs(Xi-y[0]) > 0.0001: + print("---------- Bad Initial Position --------------------") + error = True + if abs(Xf-y[-1]) > 0.0001: + print("---------- Bad Final Position --------------------") + error = True + if abs(Vi-yd[0]) > 0.0001: + print("---------- Bad Initial Velocity --------------------") + error = True + if abs(yd[-1]) > 0.0001: + print("---------- Bad Final Velocity --------------------") + error = True + + if error: + import ipdb; ipdb.set_trace() return (y, yd, ydd, t_traj) +def graphical_test(): + numRows = 3 + numCols = 5 + fig, axes = plt.subplots(numRows, numCols) + random.seed(3) # Repeatable tests by using specific seed + for x in range(numRows*numCols): + rownow = int(x/numCols) + colnow = x % numCols + print("row: {}, col: {}".format(rownow, colnow)) -pos_range = 10000.0 -Vmax_range = 8000.0 -Amax_range = 10000.0 -plot_range = 10000.0 + Vmax = random.uniform(0.1*Vmax_range, Vmax_range) + Amax = random.uniform(0.1*Amax_range, Amax_range) + Dmax = Amax + Xf = random.uniform(-pos_range, pos_range) + Xi = random.uniform(-pos_range, pos_range) + if random.random() <= 0.5: + Vi = random.uniform(-Vmax*1.5, Vmax*1.5) + else: + Vi = 0 -numRows = 3 -numCols = 5 -fig, axes = plt.subplots(numRows, numCols) -random.seed(3) # Repeatable tests by using specific seed -for x in range(numRows*numCols): - rownow = int(x/numCols) - colnow = x % numCols - print("row: {}, col: {}".format(rownow, colnow)) + (Ar, Vr, Dr, Ta, Tv, Td, Tf) = TrapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) + (Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf) - Vmax = random.uniform(0.1*Vmax_range, Vmax_range) - Amax = random.uniform(0.1*Amax_range, Amax_range) - Dmax = Amax - Xf = random.uniform(-pos_range, pos_range) - Xi = random.uniform(-pos_range, pos_range) - if random.random() <= 0.5: - Vi = random.uniform(-Vmax*1.5, Vmax*1.5) - else: - Vi = 0 + # Plotting + ax1 = axes[rownow, colnow] + # Vel limits (draw first for clearer z-order) + ax1.plot([t[0], t[-1]], [Vmax, Vmax], 'g--') + ax1.plot([t[0], t[-1]], [-Vmax, -Vmax], 'g--') - (Y, Yd, Ydd, t) = FIR_trapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) + ax1.plot(t, Y) # Pos + ax1.plot(t, Yd) # Vel + ax1.plot(0, Xi, 'bo') # Pos Initial + ax1.plot(0, Vi, 'ro') # Vel Initial + ## TODO: pull out Ta+Td+Td from planner for correct plot points + ax1.plot(t[-1]-0.1, Xf, 'b*') # Pos Final + ax1.plot(t[-1]-0.1, 0, 'r*') # Vel Final - if abs(Xi-Y[0]) > 0.0001: - print("---------- Bad Initial Position ----------") - if abs(Xf-Y[-1]) > 0.0001: - print("---------- Bad Final Position ----------") - if abs(Vi-Yd[0]) > 0.0001: - print("---------- Bad Initial Velocity ----------") - if abs(Yd[-1]) > 0.0001: - print("---------- Bad Final Velocity ----------") + ax1.set_ylim(-plot_range, plot_range) - # Plotting - ax1 = axes[rownow, colnow] - # Vel limits (draw first for clearer z-order) - ax1.plot([t[0], t[-1]], [Vmax, Vmax], 'g--') - ax1.plot([t[0], t[-1]], [-Vmax, -Vmax], 'g--') + print() - ax1.plot(t, Y) # Pos - ax1.plot(t, Yd) # Vel - ax1.plot(0, Xi, 'bo') # Pos Initial - ax1.plot(0, Vi, 'ro') # Vel Initial - ## TODO: pull out Ta+Td+Td from planner for correct plot points - ax1.plot(t[-1]-0.1, Xf, 'b*') # Pos Final - ax1.plot(t[-1]-0.1, 0, 'r*') # Vel Final + plt.show() - ax1.set_ylim(-plot_range, plot_range) +def large_test(): + random.seed(1) # Repeatable tests by using specific seed + for x in range(100): + print("Test {}".format(x)) + Vmax = random.uniform(0.1*Vmax_range, Vmax_range) + Amax = random.uniform(0.1*Amax_range, Amax_range) + Dmax = Amax + Xf = random.uniform(-pos_range, pos_range) + Xi = random.uniform(-pos_range, pos_range) + if random.random() <= 0.5: + Vi = random.uniform(-Vmax*1.5, Vmax*1.5) + else: + Vi = 0 - print() + (Ar, Vr, Dr, Ta, Tv, Td, Tf) = TrapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) + (Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf) -plt.show() + print() + +if __name__ == '__main__': + large_test() + graphical_test() \ No newline at end of file From 0796a5d5acacc20ba98ebeb926cd4da3776a4d45 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 23 Sep 2018 18:16:14 -0700 Subject: [PATCH 39/44] rename to Plantrap --- tools/motion_planning/{FIR_Planner.py => PlanTrap.py} | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) rename tools/motion_planning/{FIR_Planner.py => PlanTrap.py} (97%) diff --git a/tools/motion_planning/FIR_Planner.py b/tools/motion_planning/PlanTrap.py similarity index 97% rename from tools/motion_planning/FIR_Planner.py rename to tools/motion_planning/PlanTrap.py index 9158cc87..01bc70c6 100644 --- a/tools/motion_planning/FIR_Planner.py +++ b/tools/motion_planning/PlanTrap.py @@ -45,7 +45,7 @@ Amax_range = 10000.0 plot_range = 10000.0 -def TrapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax): +def PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax): dX = Xf - Xi # Distance to travel stop_dist = Vi**2 / (2*Dmax) # Minimum stopping distance dXstop = np.sign(Vi)*stop_dist # Minimum stopping displacement @@ -178,7 +178,7 @@ def graphical_test(): else: Vi = 0 - (Ar, Vr, Dr, Ta, Tv, Td, Tf) = TrapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) + (Ar, Vr, Dr, Ta, Tv, Td, Tf) = PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax) (Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf) # Plotting @@ -215,7 +215,7 @@ def large_test(): else: Vi = 0 - (Ar, Vr, Dr, Ta, Tv, Td, Tf) = TrapPlan(Xf, Xi, Vi, Vmax, Amax, Dmax) + (Ar, Vr, Dr, Ta, Tv, Td, Tf) = PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax) (Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf) print() From 4b6943e401ce3fd62db195ee207cfadd4208e610 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 23 Sep 2018 22:00:00 -0700 Subject: [PATCH 40/44] rewrite cpp trajectory module to match python --- Firmware/LICENSE | 2 +- Firmware/MotorControl/trapTraj.cpp | 144 ++++++++++++++--------------- Firmware/MotorControl/trapTraj.hpp | 35 ++++--- Firmware/MotorControl/utils.h | 2 + tools/motion_planning/PlanTrap.py | 28 +++--- 5 files changed, 102 insertions(+), 109 deletions(-) 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)) From 327ac51abf89b07353b877425ee71880a13c3477 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 24 Sep 2018 22:45:04 -0700 Subject: [PATCH 41/44] rewrite cpp side of trapezoidal trajectory --- CHANGELOG.md | 2 +- Firmware/MotorControl/axis.cpp | 2 +- Firmware/MotorControl/controller.cpp | 38 ++++++++++---------- Firmware/MotorControl/controller.hpp | 54 +++++++++++++--------------- Firmware/MotorControl/main.cpp | 12 +++---- Firmware/MotorControl/odrive_main.h | 1 - Firmware/MotorControl/trapTraj.cpp | 8 ++--- Firmware/MotorControl/trapTraj.hpp | 35 +++++++++--------- 8 files changed, 73 insertions(+), 79 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ff881b35..8b4d531a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,7 +4,7 @@ Please add a note of your changes below this heading if you make a Pull Request. # Unreleased ## Added -* Trapezoidal Trajectory Planner +* **Trapezoidal Trajectory Planner** # Releases diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 8813aa60..52d4c125 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -182,7 +182,7 @@ bool Axis::run_sensorless_spin_up() { bool Axis::run_sensorless_control_loop() { set_step_dir_enabled(config_.enable_step_dir); run_control_loop([this](){ - if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL) + if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL) return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; // Note that all estimators are updated in the loop prefix in run_control_loop diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index d7740a5e..bf50a15a 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -2,7 +2,7 @@ #include "odrive_main.h" -Controller::Controller(ControllerConfig_t& config) : +Controller::Controller(Config_t& config) : config_(config) {} @@ -45,16 +45,12 @@ void Controller::set_current_setpoint(float current_setpoint) { } void Controller::move_to_pos(float goal_point) { - planned_move_end_time_ = axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, - vel_setpoint_, axis_->trap_.config_.vel_limit, - axis_->trap_.config_.accel_limit, axis_->trap_.config_.decel_limit); - config_.control_mode = CTRL_MODE_PLANNED_MOVE_CONTROL; - TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(0.0f); - pos_setpoint_ = myTraj.Y; - vel_setpoint_ = myTraj.Yd; - current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A; - - planned_move_timer_ = axis_->loop_counter_ * current_meas_period; + axis_->trap_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_, + axis_->trap_.config_.vel_limit, + axis_->trap_.config_.accel_limit, + axis_->trap_.config_.decel_limit); + traj_start_loop_count_ = axis_->loop_counter_; + config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL; } void Controller::start_anticogging_calibration() { @@ -97,18 +93,22 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s anticogging_calibration(pos_estimate, vel_estimate); float anticogging_pos = pos_estimate; - // Controlled Move - if (config_.control_mode >= CTRL_MODE_PLANNED_MOVE_CONTROL) { - float time_now = axis_->loop_counter_ * current_meas_period; - if ((time_now - planned_move_timer_) > planned_move_end_time_) { + // Trajectory control + if (config_.control_mode == CTRL_MODE_TRAJECTORY_CONTROL) { + // Note: uint32_t loop count delta is OK across overflow + // Beware of negative deltas, as they will not be well behaved due to uint! + float t = (axis_->loop_counter_ - traj_start_loop_count_) * current_meas_period; + if (t > axis_->trap_.Tf_) { + // Drop into position control mode when done to avoid problems on loop counter delta overflow config_.control_mode = CTRL_MODE_POSITION_CONTROL; + // pos_setpoint already set by trajectory vel_setpoint_ = 0.0f; current_setpoint_ = 0.0f; } else { - TrapTrajStep_t myTraj = axis_->trap_.evalTrapTraj(time_now - planned_move_timer_); - pos_setpoint_ = myTraj.Y; - vel_setpoint_ = myTraj.Yd; - current_setpoint_ = myTraj.Ydd * axis_->trap_.config_.cpss_to_A; + TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t); + pos_setpoint_ = traj_step.Y; + vel_setpoint_ = traj_step.Yd; + current_setpoint_ = traj_step.Ydd * axis_->trap_.config_.A_per_css; } anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index dd3f76a5..b5e45d03 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -5,28 +5,28 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif -// Note: these should be sorted from lowest level of control to -// highest level of control, to allow "<" style comparisons. -typedef enum { - CTRL_MODE_VOLTAGE_CONTROL = 0, - CTRL_MODE_CURRENT_CONTROL = 1, - CTRL_MODE_VELOCITY_CONTROL = 2, - CTRL_MODE_POSITION_CONTROL = 3, - CTRL_MODE_PLANNED_MOVE_CONTROL = 4 -} Motor_control_mode_t; - -struct ControllerConfig_t { - Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t - float pos_gain = 20.0f; // [(counts/s) / counts] - float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] - // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] - float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] - float vel_limit = 20000.0f; // [counts/s] -}; - class Controller { public: - Controller(ControllerConfig_t& config); + // Note: these should be sorted from lowest level of control to + // highest level of control, to allow "<" style comparisons. + enum ControlMode_t{ + CTRL_MODE_VOLTAGE_CONTROL = 0, + CTRL_MODE_CURRENT_CONTROL = 1, + CTRL_MODE_VELOCITY_CONTROL = 2, + CTRL_MODE_POSITION_CONTROL = 3, + CTRL_MODE_TRAJECTORY_CONTROL = 4 + }; + + struct Config_t { + ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t + float pos_gain = 20.0f; // [(counts/s) / counts] + float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] + // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] + float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] + float vel_limit = 20000.0f; // [counts/s] + }; + + Controller(Config_t& config); void reset(); void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); @@ -42,7 +42,7 @@ public: bool update(float pos_estimate, float vel_estimate, float* current_setpoint); - ControllerConfig_t& config_; + Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor // TODO: anticogging overhaul: @@ -75,8 +75,7 @@ public: float vel_integrator_current_ = 0.0f; // [A] float current_setpoint_ = 0.0f; // [A] - float planned_move_timer_ = 0.0f; - float planned_move_end_time_ = 0.0f; + uint32_t traj_start_loop_count_ = 0; // Communication protocol definitions auto make_protocol_definitions() { @@ -93,14 +92,11 @@ public: make_protocol_property("vel_limit", &config_.vel_limit) ), make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, - "pos_setpoint", - "vel_feed_forward", - "current_feed_forward"), + "pos_setpoint", "vel_feed_forward", "current_feed_forward"), make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, - "vel_setpoint", - "current_feed_forward"), + "vel_setpoint", "current_feed_forward"), make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, - "current_setpoint"), + "current_setpoint"), make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) ); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 1f9b38cb..d9f14ebc 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -11,10 +11,10 @@ BoardConfig_t board_config; Encoder::Config_t encoder_configs[AXIS_COUNT]; SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; -ControllerConfig_t controller_configs[AXIS_COUNT]; +Controller::Config_t controller_configs[AXIS_COUNT]; MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; -TrapTrajConfig_t trap_configs[AXIS_COUNT]; +TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT]; bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; @@ -25,9 +25,9 @@ typedef Config< BoardConfig_t, Encoder::Config_t[AXIS_COUNT], SensorlessEstimator::Config_t[AXIS_COUNT], - ControllerConfig_t[AXIS_COUNT], + Controller::Config_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT], - TrapTrajConfig_t[AXIS_COUNT], + TrapezoidalTrajectory::Config_t[AXIS_COUNT], AxisConfig_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { @@ -61,9 +61,9 @@ void load_configuration(void) { for (size_t i = 0; i < AXIS_COUNT; ++i) { encoder_configs[i] = Encoder::Config_t(); sensorless_configs[i] = SensorlessEstimator::Config_t(); - controller_configs[i] = ControllerConfig_t(); + controller_configs[i] = Controller::Config_t(); motor_configs[i] = MotorConfig_t(); - trap_configs[i] = TrapTrajConfig_t(); + trap_configs[i] = TrapezoidalTrajectory::Config_t(); axis_configs[i] = AxisConfig_t(); } } else { diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 27be8455..0165c673 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -113,7 +113,6 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include - #endif // __cplusplus diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index ab4a1854..f1e41aa5 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -15,7 +15,7 @@ float sign_hard(float val) { // Vmax, Amax, Dmax and jmax Kinematic bounds // Ar, Dr and Vr Reached values of acceleration and velocity -TrapezoidalTrajectory::TrapezoidalTrajectory(TrapTrajConfig_t& config) : config_(config) {} +TrapezoidalTrajectory::TrapezoidalTrajectory(Config_t& config) : config_(config) {} bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax) { @@ -63,8 +63,8 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, return true; } -TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { - TrapTrajStep_t trajStep; +TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) { + Step_t trajStep; if (t < 0.0f) { // Initial Condition trajStep.Y = Xi_; trajStep.Yd = Vi_; @@ -82,7 +82,7 @@ TrapTrajStep_t TrapezoidalTrajectory::evalTrapTraj(float t) { trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td); trajStep.Yd = Dr_*td; trajStep.Ydd = Dr_; - } else if (t >= Tf_) { // Final Condition + } else if (t >= Tf_) { // Final Condition trajStep.Y = Xf_; trajStep.Yd = 0.0f; trajStep.Ydd = 0.0f; diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index dec5254d..42dac0ef 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -1,25 +1,24 @@ #ifndef _TRAP_TRAJ_H #define _TRAP_TRAJ_H -struct TrapTrajConfig_t { - 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 { - float Y; - float Yd; - float Ydd; -}; - class TrapezoidalTrajectory { - public: - TrapezoidalTrajectory(TrapTrajConfig_t& config); +public: + struct Config_t { + float vel_limit = 20000.0f; // [count/s] + float accel_limit = 5000.0f; // [count/s^2] + float decel_limit = 5000.0f; // [count/s^2] + float A_per_css = 0.0f; // [A/(count/s^2)] + }; + struct Step_t { + float Y; + float Yd; + float Ydd; + }; + + TrapezoidalTrajectory(Config_t& config); bool planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax); - TrapTrajStep_t evalTrapTraj(float t); + Step_t eval(float t); auto make_protocol_definitions() { return make_protocol_member_list( @@ -27,13 +26,13 @@ class TrapezoidalTrajectory { 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("cpss_to_A", &config_.cpss_to_A) + make_protocol_property("A_per_css", &config_.A_per_css) ) ); } Axis* axis_ = nullptr; // set by Axis constructor - TrapTrajConfig_t& config_; + Config_t& config_; float Xi_; float Xf_; From e88abb56eba892b442b07e9acf256c338f4260c0 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 24 Sep 2018 22:59:36 -0700 Subject: [PATCH 42/44] pull in configs and enum into class for Axis and Motor, for consistency --- Firmware/MotorControl/axis.cpp | 2 +- Firmware/MotorControl/axis.hpp | 78 ++++++++++++------------- Firmware/MotorControl/encoder.cpp | 8 +-- Firmware/MotorControl/main.cpp | 12 ++-- Firmware/MotorControl/motor.cpp | 29 +++++----- Firmware/MotorControl/motor.hpp | 96 +++++++++++++++---------------- 6 files changed, 113 insertions(+), 112 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 52d4c125..4ff660fd 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -7,7 +7,7 @@ #include "odrive_main.h" Axis::Axis(const AxisHardwareConfig_t& hw_config, - AxisConfig_t& config, + Config_t& config, Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 4eac2c4b..6063ee3a 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -5,40 +5,6 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif -// Warning: Do not reorder these enum values. -// The state machine uses ">" comparision on them. -enum AxisState_t { - AXIS_STATE_UNDEFINED = 0, //" comparision on them. + enum State_t { + AXIS_STATE_UNDEFINED = 0, //motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT) voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; - else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) voltage_magnitude = axis_->motor_.config_.calibration_current; else return false; @@ -142,9 +142,9 @@ bool Encoder::run_offset_calibration() { shadow_count_ = count_in_cpr_; float voltage_magnitude; - if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT) voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; - else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) voltage_magnitude = axis_->motor_.config_.calibration_current; else return false; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index d9f14ebc..4550605e 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -12,8 +12,8 @@ BoardConfig_t board_config; Encoder::Config_t encoder_configs[AXIS_COUNT]; SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; Controller::Config_t controller_configs[AXIS_COUNT]; -MotorConfig_t motor_configs[AXIS_COUNT]; -AxisConfig_t axis_configs[AXIS_COUNT]; +Motor::Config_t motor_configs[AXIS_COUNT]; +Axis::Config_t axis_configs[AXIS_COUNT]; TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT]; bool user_config_loaded_; @@ -26,9 +26,9 @@ typedef Config< Encoder::Config_t[AXIS_COUNT], SensorlessEstimator::Config_t[AXIS_COUNT], Controller::Config_t[AXIS_COUNT], - MotorConfig_t[AXIS_COUNT], + Motor::Config_t[AXIS_COUNT], TrapezoidalTrajectory::Config_t[AXIS_COUNT], - AxisConfig_t[AXIS_COUNT]> ConfigFormat; + Axis::Config_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { if (ConfigFormat::safe_store_config( @@ -62,9 +62,9 @@ void load_configuration(void) { encoder_configs[i] = Encoder::Config_t(); sensorless_configs[i] = SensorlessEstimator::Config_t(); controller_configs[i] = Controller::Config_t(); - motor_configs[i] = MotorConfig_t(); + motor_configs[i] = Motor::Config_t(); trap_configs[i] = TrapezoidalTrajectory::Config_t(); - axis_configs[i] = AxisConfig_t(); + axis_configs[i] = Axis::Config_t(); } } else { user_config_loaded_ = true; diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 56817ee8..1782414c 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -6,8 +6,8 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config, - const GateDriverHardwareConfig_t& gate_driver_config, - MotorConfig_t& config) : + const GateDriverHardwareConfig_t& gate_driver_config, + Config_t& config) : hw_config_(hw_config), gate_driver_config_(gate_driver_config), config_(config), @@ -298,10 +298,11 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) { } bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { - Current_control_t* ictrl = ¤t_control_; + // Syntactic sugar + CurrentControl_t& ictrl = current_control_; // For Reporting - ictrl->Iq_setpoint = Iq_des; + ictrl.Iq_setpoint = Iq_des; // Clarke transform float Ialpha = -current_meas_.phB - current_meas_.phC; @@ -312,7 +313,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { float s = arm_sin_f32(phase); float Id = c * Ialpha + s * Ibeta; float Iq = c * Ibeta - s * Ialpha; - ictrl->Iq_measured = Iq; + ictrl.Iq_measured = Iq; // Current error float Ierr_d = Id_des - Id; @@ -320,8 +321,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) // Apply PI control - float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; - float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; + float Vd = ictrl.v_current_control_integral_d + Ierr_d * ictrl.p_gain; + float Vq = ictrl.v_current_control_integral_q + Ierr_q * ictrl.p_gain; float mod_to_V = (2.0f / 3.0f) * vbus_voltage; float V_to_mod = 1.0f / mod_to_V; @@ -335,23 +336,23 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { mod_d *= mod_scalefactor; mod_q *= mod_scalefactor; // TODO make decayfactor configurable - ictrl->v_current_control_integral_d *= 0.99f; - ictrl->v_current_control_integral_q *= 0.99f; + ictrl.v_current_control_integral_d *= 0.99f; + ictrl.v_current_control_integral_q *= 0.99f; } else { - ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); - ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); + ictrl.v_current_control_integral_d += Ierr_d * (ictrl.i_gain * current_meas_period); + ictrl.v_current_control_integral_q += Ierr_q * (ictrl.i_gain * current_meas_period); } // Compute estimated bus current - ictrl->Ibus = mod_d * Id + mod_q * Iq; + ictrl.Ibus = mod_d * Id + mod_q * Iq; // Inverse park transform float mod_alpha = c * mod_d - s * mod_q; float mod_beta = c * mod_q + s * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) - ictrl->final_v_alpha = mod_to_V * mod_alpha; - ictrl->final_v_beta = mod_to_V * mod_beta; + ictrl.final_v_alpha = mod_to_V * mod_alpha; + ictrl.final_v_beta = mod_to_V * mod_beta; // Apply SVM if (!enqueue_modulation_timings(mod_alpha, mod_beta)) diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 07820726..235ca6a8 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -7,51 +7,6 @@ #include "drv8301.h" -typedef enum { - MOTOR_TYPE_HIGH_CURRENT = 0, - // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented - MOTOR_TYPE_GIMBAL = 2 -} Motor_type_t; - -typedef struct { - float phB; - float phC; -} Iph_BC_t; - -typedef struct { - float p_gain; // [V/A] - float i_gain; // [V/As] - float v_current_control_integral_d; // [V] - float v_current_control_integral_q; // [V] - float Ibus; // DC bus current [A] - // Voltage applied at end of cycle: - float final_v_alpha; // [V] - float final_v_beta; // [V] - float Iq_setpoint; - float Iq_measured; - float max_allowed_current; -} Current_control_t; - -// NOTE: for gimbal motors, all units of A are instead V. -// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] -// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. -typedef struct { - bool pre_calibrated = false; // can be set to true to indicate that all values here are valid - int32_t pole_pairs = 7; - float calibration_current = 10.0f; // [A] - float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. - float phase_inductance = 0.0f; // to be set by measure_phase_inductance - float phase_resistance = 0.0f; // to be set by measure_phase_resistance - int32_t direction = 1; // 1 or -1 - Motor_type_t motor_type = MOTOR_TYPE_HIGH_CURRENT; - // Read out max_allowed_current to see max supported value for current_lim. - // float current_lim = 70.0f; //[A] - float current_lim = 10.0f; //[A] - // Value used to compute shunt amplifier gains - float requested_current_range = 70.0f; // [A] - float current_control_bandwidth = 1000.0f; // [rad/s] -} MotorConfig_t; - class Motor { public: enum Error_t { @@ -68,6 +23,51 @@ public: ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200 }; + enum MotorType_t { + MOTOR_TYPE_HIGH_CURRENT = 0, + // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented + MOTOR_TYPE_GIMBAL = 2 + }; + + struct Iph_BC_t { + float phB; + float phC; + }; + + struct CurrentControl_t{ + float p_gain; // [V/A] + float i_gain; // [V/As] + float v_current_control_integral_d; // [V] + float v_current_control_integral_q; // [V] + float Ibus; // DC bus current [A] + // Voltage applied at end of cycle: + float final_v_alpha; // [V] + float final_v_beta; // [V] + float Iq_setpoint; + float Iq_measured; + float max_allowed_current; + }; + + // NOTE: for gimbal motors, all units of A are instead V. + // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] + // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. + struct Config_t { + bool pre_calibrated = false; // can be set to true to indicate that all values here are valid + int32_t pole_pairs = 7; + float calibration_current = 10.0f; // [A] + float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. + float phase_inductance = 0.0f; // to be set by measure_phase_inductance + float phase_resistance = 0.0f; // to be set by measure_phase_resistance + int32_t direction = 1; // 1 or -1 + MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT; + // Read out max_allowed_current to see max supported value for current_lim. + // float current_lim = 70.0f; //[A] + float current_lim = 10.0f; //[A] + // Value used to compute shunt amplifier gains + float requested_current_range = 70.0f; // [A] + float current_control_bandwidth = 1000.0f; // [rad/s] + }; + enum TimingLog_t { TIMING_LOG_GENERAL, TIMING_LOG_ADC_CB_I, @@ -90,7 +90,7 @@ public: Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, - MotorConfig_t& config); + Config_t& config); bool arm(); void disarm(); @@ -119,7 +119,7 @@ public: const MotorHardwareConfig_t& hw_config_; const GateDriverHardwareConfig_t gate_driver_config_; - MotorConfig_t& config_; + Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor //private: @@ -144,7 +144,7 @@ public: Iph_BC_t current_meas_ = {0.0f, 0.0f}; Iph_BC_t DC_calib_ = {0.0f, 0.0f}; float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) - Current_control_t current_control_ = { + CurrentControl_t current_control_ = { .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, From b0f84f7a748ac47877d261804facd255306ad00c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 24 Sep 2018 23:30:06 -0700 Subject: [PATCH 43/44] update traj jason to goal_point --- Firmware/MotorControl/controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index b5e45d03..3db712e4 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -97,7 +97,7 @@ public: "vel_setpoint", "current_feed_forward"), make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, "current_setpoint"), - make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"), + make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "goal_point"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) ); } From d6d6fa58ae80e1bc67053614b75d5d7316cae253 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 25 Sep 2018 22:45:09 -0700 Subject: [PATCH 44/44] add ascii command for trajectory control --- Firmware/communication/ascii_protocol.cpp | 12 ++++++++++++ docs/ascii-protocol.md | 12 ++++++++++++ docs/getting-started.md | 10 ---------- 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 626a7c92..0981cc1d 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -128,6 +128,18 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& axes[motor_number]->controller_.set_current_setpoint(current_setpoint); } + } else if (cmd[0] == 't') { // trapezoidal trajectory + unsigned motor_number; + float goal_point; + int numscan = sscanf(cmd, "t %u %f", &motor_number, &goal_point); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + axes[motor_number]->controller_.move_to_pos(goal_point); + } + } else if (cmd[0] == 'h') { // Help respond(response_channel, use_checksum, "Please see documentation for more details"); respond(response_channel, use_checksum, ""); diff --git a/docs/ascii-protocol.md b/docs/ascii-protocol.md index 49578af8..951ec214 100644 --- a/docs/ascii-protocol.md +++ b/docs/ascii-protocol.md @@ -24,6 +24,18 @@ command *42 ; comment [new line character] ## Command Reference +#### Motor trajectory command +``` +t motor destination +``` +* `t` for trajectory +* `motor` is the motor number, `0` or `1`. +* `destination` is the goal position, in encoder counts. + +Example: `t 0 -20000` + +For general moving around of the axis, this is the recommended command. + #### Motor Position command ``` p motor position velocity_ff current_ff diff --git a/docs/getting-started.md b/docs/getting-started.md index 994009f1..c338e591 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -20,7 +20,6 @@ permalink: / ## Hardware Requirements - You will need: * One or two [brushless motors](https://docs.google.com/spreadsheets/d/12vzz7XVEK6YNIOqH0jAz51F5VUpc-lJEs3mmkWP1H4Y). It is fine, even recommended, to start testing with just a single motor and encoder. @@ -47,7 +46,6 @@ You will need: ## Wiring up the ODrive -
Make sure you have a good mechanical connection between the encoder and the motor, slip can cause disasterous oscillations or runaway.
@@ -61,11 +59,9 @@ All non-power I/O is 3.3V output and 5V tolerant on input, on ODrive v3.3 and ne ![Image of ODrive all hooked up](https://docs.google.com/drawings/d/e/2PACX-1vTCD0P40Cd-wvD7Fl8UYEaxp3_UL81oI4qUVqrrCJPi6tkJeSs2rsffIXQRpdu6rNZs6-2mRKKYtILG/pub?w=1716&h=1281) ## Downloading and Installing Tools - Most instructions in this guide refer to a utility called `odrivetool`, so you should install that first. ### Windows - 1. Install Python 3. We recommend the Anaconda distribution because it packs a lot of useful scientific tools, however you can also install the standalone python. * __Anaconda__: Download the installer from [here](https://www.anaconda.com/download/#windows). Execute the downloaded file and follow the instructions. * __Standalone Python__: Download the installer from [here](https://www.python.org/downloads/). Execute the downloaded file and follow the instructions. @@ -117,7 +113,6 @@ Try step 5 again ### Linux - 1. [Install Python 3](https://www.python.org/downloads/). 2. Install the ODrive tools by opening a terminal and typing `pip install odrive` Enter 3. __Linux__: set up USB permissions @@ -128,7 +123,6 @@ Try step 5 again ``` ## Start `odrivetool` -
__ODrive v3.5 and later:__ Your board should come preflashed with firmware. If you run into problems, follow the instructions [here](odrivetool.md#device-firmware-update) on the DFU procedure before you continue.
__ODrive v3.4 and earlier:__ Your board does __not__ come preflashed with any firmware. Follow the instructions [here](odrivetool.md#device-firmware-update) on the STP Link procedure before you continue.
@@ -173,7 +167,6 @@ For instance, to set the current limit of M0 to 10A you would type: `odrv0.axis0 * You can change `odrv0.axis0.motor.config.calibration_current` [A] to the largest value you feel comfortable leaving running through the motor continously when the motor is stationary. If you are using a small motor (i.e. 15A current rated) you may need to reduce `calibration_current` to a value smaller than the default. ### 2. Set other hardware parameters: - * `odrv0.config.brake_resistance` [Ohm]: This is the resistance of the brake resistor. If you are not using it, you may set it to `0`. Note that there may be some extra resistance in your wiring and in the screw terminals, so if you are getting issues while braking you may want to increase this parameter by around 0.05 ohm. * `odrv0.axis0.motor.config.pole_pairs`: This is the number of **magnet poles** in the rotor, **divided by two**. You can simply count the number of permanent magnets in the rotor, if you can see them. _Note: this is not the same as the number of coils in the stator._ * `odrv0.axis0.motor.config.motor_type`: This is the type of motor being used. Currently two types of motors are supported: High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). @@ -203,7 +196,6 @@ Due to a [known issue](https://github.com/madcowswe/ODrive/issues/183) it is str ## Position control of M0 - Let's get motor 0 up and running. The procedure for motor 1 is exactly the same, so feel free to replace read "axis1" wherever it says "axis0". 1. Type `odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE` Enter. After about 2 seconds should hear a beep. Then the motor will turn slowly in one direction for a few seconds, then back in the other direction. @@ -231,9 +223,7 @@ The ODrive also supports velocity control and current (torque) control. * **Current control**: Set `odrv0.axis0.controller.config.control_mode = CTRL_MODE_CURRENT_CONTROL`. You can now control the current with `odrv0.axis0.controller.current_setpoint = 3`. Units are A. **NOTE**: There is no velocity limiting in current control mode. Make sure that you don't overrev the motor, or exceed the max speed for your encoder. ## What's next? - You can now: - * See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance. * Control the ODrive from your own program or hook it up to an existing system through one of it's [interfaces](interfaces.md). * See how you can improve the behavior during the startup procedure, like [bypassing encoder calibration](encoders.md#encoder-with-index-signal).