Diff: main.cpp

Revision:

35:88dbfefc1bbb

Parent:

34:9a24d0f718ac

Child:

36:a3beb771d8f7

--- a/main.cpp	Mon Nov 16 08:23:00 2020 +0000
+++ b/main.cpp	Mon Nov 16 22:10:05 2020 +0000
@@ -8,7 +8,9 @@
 #include "HardwareSetup.h"
 
 #define BEZIER_ORDER_FOOT    7
-#define NUM_INPUTS (14 + 2*(BEZIER_ORDER_FOOT+1))
+#define BEZIER_ORDER_TORQUE    3 /// CUBIC -> 4 points
+//#define NUM_INPUTS (14 + 2*(BEZIER_ORDER_FOOT+1))
+#define NUM_INPUTS (14 + 2*(BEZIER_ORDER_TORQUE+1))
 #define NUM_OUTPUTS 19
 
 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
@@ -18,7 +20,7 @@
 ExperimentServer server;    // Object that lets us communicate with MATLAB
 Timer t;                    // Timer to measure elapsed time of experiment
 
-QEI encoderA(PE_9,PE_11, NC, 1200, QEI::X4_ENCODING);  // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
+QEI encoderA(PE_9, PE_11, NC, 1200, QEI::X4_ENCODING);  // MOTOR A ENCODER (no index, 1200 counts/rev, Quadrature encoding)
 QEI encoderB(PA_5, PB_3, NC, 1200, QEI::X4_ENCODING);  // MOTOR B ENCODER (no index, 1200 counts/rev, Quadrature encoding)
 QEI encoderC(PC_6, PC_7, NC, 1200, QEI::X4_ENCODING);  // MOTOR C ENCODER (no index, 1200 counts/rev, Quadrature encoding)
 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
@@ -90,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)*30.0f)-15.0f);           // measure current DO WE NEED TO ADJUST GEAR RATIO HERE?
+    current1 = -(((float(motorShield.readCurrentA())/65536.0f)*18.75f)-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
@@ -137,6 +139,7 @@
     
     // Object for 7th order Cartesian foot trajectory
     BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
+    BezierCurve torque_bez(2,BEZIER_ORDER_TORQUE);
     
     // Link the terminal with our server and start it up
     server.attachTerminal(pc);
@@ -173,10 +176,17 @@
             // Get foot trajectory points
             float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
             for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
-              foot_pts[i] = input_params[12+i];    
+              foot_pts[i] = input_params[14+i];    
             }
             rDesFoot_bez.setPoints(foot_pts);
             
+            // Get torque traj points
+            float torque_pts[2*(BEZIER_ORDER_FOOT+1)];
+            for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
+              torque_pts[i] = input_params[14+i];    
+            }
+            torque_bez.setPoints(torque_pts);
+            
             // Attach current loop interrupt
             currentLoop.attach_us(CurrentLoop,current_control_period_us);
                         
@@ -266,6 +276,9 @@
                 vDesFoot[0]*=vMult;
                 vDesFoot[1]*=vMult;
                 
+                float torque_des[2];
+                torque_bez.evaluate(teff/traj_period,torque_des);
+                
                 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles              
                 float xFootd = -rDesFoot[0];
                 float yFootd = rDesFoot[1];                
@@ -306,6 +319,9 @@
                 current_des1 = T1/k_t;          
                 current_des2 = T2/k_t;
                 
+                current_des1 = torque_des[0]/k_t;          
+                current_des2 = torque_des[1]/k_t;
+                
                 
                 // Form output to send to MATLAB     
                 float output_data[NUM_OUTPUTS];
@@ -328,10 +344,14 @@
                 output_data[12] = yFoot;
                 output_data[13] = dxFoot;
                 output_data[14] = dyFoot;
-                output_data[15] = rDesFoot[0];
-                output_data[16] = rDesFoot[1];
-                output_data[17] = vDesFoot[0];
-                output_data[18] = vDesFoot[1];
+                output_data[15] = current1*k_t;
+                output_data[16] = current2*k_t;
+                output_data[17] = torque_des[0];
+                output_data[18] = torque_des[1];
+//                output_data[15] = rDesFoot[0];
+//                output_data[16] = rDesFoot[1];
+//                output_data[17] = 0;//vDesFoot[0];
+//                output_data[18] = 0;//vDesFoot[1];
                 
                 // Send data to MATLAB
                 server.sendData(output_data,NUM_OUTPUTS);