Diff: main.cpp

Revision:

38:8ce2f6edba26

Parent:

37:48a3017a958f

Child:

39:c8da22fcf4f3

diff -r 48a3017a958f -r 8ce2f6edba26 main.cpp
--- a/main.cpp	Mon Nov 16 23:25:57 2020 +0000
+++ b/main.cpp	Tue Nov 17 09:11:44 2020 +0000
@@ -8,7 +8,7 @@
 #include "HardwareSetup.h"
 
 #define BEZIER_ORDER_FOOT    7
-#define BEZIER_ORDER_TORQUE    3 /// CUBIC -> 4 points
+#define BEZIER_ORDER_TORQUE    3 /// if x -> plots x+1 points, so should have x+1 Bezier points for T1 and T2 in MATLAB
 //#define NUM_INPUTS (14 + 2*(BEZIER_ORDER_FOOT+1))
 #define NUM_INPUTS (14 + 2*(BEZIER_ORDER_TORQUE+1))
 #define NUM_OUTPUTS 19
@@ -92,7 +92,7 @@
     //use the motor shield as follows:
     //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
         
-    current1 = -(((float(motorShield.readCurrentA())/65536.0f)*18.75f)-15.0f);           // measure current DO WE NEED TO ADJUST GEAR RATIO HERE?
+    current1 = -(((float(motorShield.readCurrentA())/65536.0f)*30.0f)-15.0f);           // measure current DO WE NEED TO ADJUST GEAR RATIO HERE?
     velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;                                  // measure velocity        
     float err_c1 = current_des1 - current1;                                             // current errror
     current_int1 += err_c1;                                                             // integrate error
@@ -248,11 +248,11 @@
                     float e_dth1= -dth1;
                     float e_dth2= -dth2;
                     
-                    float T1 = K_xx*e_th1 + D_xx*e_dth1;
-                    float T2 = K_yy*e_th2 + D_yy*e_dth2;
+                    torque_des[0] = K_xx*e_th1 + D_xx*e_dth1;
+                    torque_des[1] = K_yy*e_th2 + D_yy*e_dth2;
            
-                    current_des1 = T1/k_t;          
-                    current_des2 = T2/k_t;
+                    current_des1 = torque_des[0]/k_t;          
+                    current_des2 = torque_des[1]/k_t;
                 }
                 else if (t < start_period + traj_period) // torque control to follow Bezier
                 {
@@ -280,11 +280,11 @@
                     float e_dth1= -dth1;
                     float e_dth2= -dth2;
                     
-                    float T1 = K_xx*e_th1 + D_xx*e_dth1;
-                    float T2 = K_yy*e_th2 + D_yy*e_dth2;
+                    torque_des[0] = K_xx*e_th1 + D_xx*e_dth1;
+                    torque_des[1] = K_yy*e_th2 + D_yy*e_dth2;
            
-                    current_des1 = T1/k_t;          
-                    current_des2 = T2/k_t;
+                    current_des1 = torque_des[0]/k_t;          
+                    current_des2 = torque_des[1]/k_t;
                     
 //                    teff = 0;
 //                    float rDesFoot[2] , vDesFoot[2];