Morgan Mayborne / Mbed OS Bezier_Traj_Follower_Example_OSIC

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MatrixMath Matrix ExperimentServer QEI_pmw MotorShield

Diff: main.cpp

Revision:
27:5d60c6ab6d0a
Parent:
26:5822d4d8dca7
Child:
28:22530fdc149b
--- a/main.cpp	Thu Oct 01 05:21:20 2020 +0000
+++ b/main.cpp	Thu Oct 01 13:54:16 2020 +0000
@@ -72,8 +72,8 @@
 const float l_B_m2=0.0344; 
 const float l_A_m3=0.0622;
 const float l_C_m4=0.0610;
-const float Nmot = 18.75;
-const float Ir = 0.0035/pow(Nmot,2);
+const float N = 18.75;
+const float Ir = 0.0035/pow(N,2);
 
 // Timing parameters
 float current_control_period_us = 200.0f;     // 5kHz current control loop
@@ -295,9 +295,9 @@
                 float fy = 0;
                 
                 // Calculate mass matrix elements
-                float M11 = I1 + I2 + I3 + I4 + Ir + Ir*pow(Nmot,2) + pow(l_AC,2)*m4 + pow(l_A_m3,2)*m3 + pow(l_B_m2,2)*m2 + pow(l_C_m4,2)*m4 + pow(l_OA,2)*m3 + pow(l_OB,2)*m2 + pow(l_OA,2)*m4 + pow(l_O_m1,2)*m1 + 2*l_C_m4*l_OA*m4 + 2*l_AC*l_C_m4*m4*cos(th2) + 2*l_AC*l_OA*m4*cos(th2) + 2*l_A_m3*l_OA*m3*cos(th2) + 2*l_B_m2*l_OB*m2*cos(th2);
-                float M12 = I2 + I3 + pow(l_AC,2)*m4 + pow(l_A_m3,2)*m3 + pow(l_B_m2,2)*m2 + Ir*Nmot + l_AC*l_C_m4*m4*cos(th2) + l_AC*l_OA*m4*cos(th2) + l_A_m3*l_OA*m3*cos(th2) + l_B_m2*l_OB*m2*cos(th2);
-                float M22 = Ir*pow(Nmot,2) + m4*pow(l_AC,2) + m3*pow(l_A_m3,2) + m2*pow(l_B_m2,2) + I2 + I3;
+                float M11 = I1 + I2 + I3 + I4 + Ir + Ir*pow(N,2) + pow(l_AC,2)*m4 + pow(l_A_m3,2)*m3 + pow(l_B_m2,2)*m2 + pow(l_C_m4,2)*m4 + pow(l_OA,2)*m3 + pow(l_OB,2)*m2 + pow(l_OA,2)*m4 + pow(l_O_m1,2)*m1 + 2*l_C_m4*l_OA*m4 + 2*l_AC*l_C_m4*m4*cos(th2) + 2*l_AC*l_OA*m4*cos(th2) + 2*l_A_m3*l_OA*m3*cos(th2) + 2*l_B_m2*l_OB*m2*cos(th2);
+                float M12 = I2 + I3 + pow(l_AC,2)*m4 + pow(l_A_m3,2)*m3 + pow(l_B_m2,2)*m2 + Ir*N + l_AC*l_C_m4*m4*cos(th2) + l_AC*l_OA*m4*cos(th2) + l_A_m3*l_OA*m3*cos(th2) + l_B_m2*l_OB*m2*cos(th2);
+                float M22 = Ir*pow(N,2) + m4*pow(l_AC,2) + m3*pow(l_A_m3,2) + m2*pow(l_B_m2,2) + I2 + I3;
                 
                 // Populate mass matrix
                 MassMatrix.Clear();