From 26b87bb95be287d48306445f4b3ddb77c86d7a2e Mon Sep 17 00:00:00 2001 From: Unknown Date: Mon, 6 Aug 2018 00:38:49 -0400 Subject: [PATCH 01/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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/34] 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 {