Diff: main.cpp

Revision:

16:f9ea2b2d410f

Parent:

15:495333b2ccf1

Child:

17:1bb5aa45826e

--- a/main.cpp	Thu Aug 27 14:55:58 2020 +0000
+++ b/main.cpp	Thu Sep 24 20:16:05 2020 +0000
@@ -3,13 +3,18 @@
 #include "EthernetInterface.h"
 #include "ExperimentServer.h"
 #include "QEI.h"
+#include "BezierCurve.h"
 #include "MotorShield.h" 
 #include "HardwareSetup.h"
 
-#define NUM_INPUTS 2
-#define NUM_OUTPUTS 4
 
+#define BEZIER_ORDER_FOOT    7
+#define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1) + 2*(BEZIER_ORDER_CURRENT+1) )
+#define NUM_OUTPUTS 19
 
+#define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
+
+// Initializations
 Serial pc(USBTX, USBRX);    // USB Serial Terminal
 ExperimentServer server;    // Object that lets us communicate with MATLAB
 Timer t;                    // Timer to measure elapsed time of experiment
@@ -20,6 +25,110 @@
 QEI encoderD(PD_12, PD_13, NC, 1200, QEI::X4_ENCODING);// MOTOR D ENCODER (no index, 1200 counts/rev, Quadrature encoding)
 
 MotorShield motorShield(12000); //initialize the motor shield with a period of 12000 ticks or ~20kHZ
+Ticker currentLoop;
+
+// Variables for q1
+float current1;
+float current_des1 = 0;
+float prev_current_des1 = 0;
+float current_int1 = 0;
+float angle1;
+float angle_des1;
+float velocity1;
+float velocity_des1;
+float duty_cycle1;
+float angle1_init;
+
+// Variables for q2
+float current2;
+float current_des2 = 0;
+float prev_current_des2 = 0;
+float current_int2 = 0;
+float angle2;
+float angle_des2;
+float velocity2;
+float velocity_des2;
+float duty_cycle2;
+float angle2_init;
+
+// Fixed kinematic parameters
+const float l_OA=.011; 
+const float l_OB=.042; 
+const float l_AC=.096; 
+const float l_DE=.091;
+
+// Timing parameters
+float current_control_period_us = 200.0f;     // 5kHz current control loop
+float impedance_control_period_us = 1000.0f;  // 1kHz impedance control loop
+float start_period, traj_period, end_period;
+
+// Control parameters
+float current_Kp = 0.7;           
+float current_Ki = 0.2;           
+float current_int_max = 1.0;      
+float duty_max;      
+float K_xx;
+float K_yy;
+float K_xy;
+float D_xx;
+float D_xy;
+float D_yy;
+float xFoot;
+float yFoot;
+float xDesFoot;
+float yDesFoot;
+
+// Model parameters
+float supply_voltage = 12; // motor supply voltage
+float R;                   // motor resistance
+float k_emf;               // motor torque constant
+float nu1, nu2;            // motor viscous friction
+
+// Current control interrupt function
+void CurrentLoop()
+{
+    // This loop sets the motor voltage commands using PI current controllers.
+    
+    //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
+    float err_c1 = current_des1 - current1;                                         // current errror
+    current_int1 += err_c1;                                                         // integrate error
+    current_int1 = fmaxf( fminf(current_int1, current_int_max), -current_int_max);  // anti-windup
+    duty_cycle1 = current_Kp*err_c1 + current_Ki*current_int1;                      // PI current controller
+    
+    float absDuty1 = abs(duty_cycle1);
+    if (absDuty1 > duty_max) {
+        duty_cycle1 *= duty_max / absDuty1;
+        absDuty1 = duty_max;
+    }    
+    if (duty_cycle1 < 0) { // backwards
+        motorShield.motorAWrite(absDuty1, 1);
+    } else { // forwards
+        motorShield.motorAWrite(absDuty1, 0);
+    }             
+    prev_current_des1 = current_des1; 
+    
+    current2     = -(((float(motorShield.readCurrentB())/65536.0f)*30.0f)-15.0f);   // measure current
+    float err_c2 = current_des2 - current2;                                         // current error
+    current_int2 += err_c2;                                                         // integrate error
+    current_int2 = fmaxf( fminf(current_int2, current_int_max), -current_int_max);  // anti-windup   
+    duty_cycle2 = current_Kp*err_c2 + current_Ki*current_int2;                      // PI current controller
+    
+    float absDuty2 = abs(duty_cycle2);
+    if (absDuty2 > duty_max) {
+        duty_cycle2 *= duty_max / absDuty2;
+        absDuty2 = duty_max;
+    }    
+    if (duty_cycle2 < 0) { // backwards
+        motorShield.motorBWrite(absDuty2, 1);
+    } else { // forwards
+        motorShield.motorBWrite(absDuty2, 0);
+    }             
+    prev_current_des2 = current_des2; 
+    
+}
 
 int main (void)
 {
@@ -32,10 +141,37 @@
     pc.printf("%f",input_params[0]);
     
     while(1) {
+        
+        // If there are new inputs, this code will run
         if (server.getParams(input_params,NUM_INPUTS)) {
-            float v1   = input_params[0]; // Duty cycle for first second
-            float v2   = input_params[1]; // Duty cycle for second second
+            
+                        
+            // Get inputs from MATLAB            
+            start_period                = input_params[0];    // First buffer time, before trajectory
+            traj_period                 = input_params[1];    // Trajectory time/length
+            end_period                  = input_params[2];    // Second buffer time, after trajectory
+    
+            angle1_init                 = input_params[3];    // Initial angle for q1 (rad)
+            angle2_init                 = input_params[4];    // Initial angle for q2 (rad)
 
+            K_xx                        = input_params[5];    // Foot stiffness N/m
+            K_yy                        = input_params[6];    // Foot stiffness N/m
+            K_xy                        = input_params[7];    // Foot stiffness N/m
+            D_xx                        = input_params[8];    // Foot damping N/(m/s)
+            D_yy                        = input_params[9];    // Foot damping N/(m/s)
+            D_xy                        = input_params[10];   // Foot damping N/(m/s)
+            duty_max                    = input_params[11];   // Maximum duty factor
+          
+            // TODO: check that this gets inputs correctly?
+            float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
+            for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
+              foot_pts[i] = input_params[21+i];    
+            }
+            rDesFoot_bez.setPoints(foot_pts);
+            
+            // Attach current loop interrupt
+            currentLoop.attach_us(CurrentLoop,current_control_period_us);
+                        
             // Setup experiment
             t.reset();
             t.start();
@@ -45,35 +181,151 @@
             encoderD.reset();
 
             motorShield.motorAWrite(0, 0); //turn motor A off
-            
-            //use the motor shield as follows:
-            //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
-             
+            motorShield.motorBWrite(0, 0); //turn motor B off
+                         
             // Run experiment
-            while( t.read() < 2 ) {
-                // Perform control loop logic
-                if (t.read() < 1)
-                    motorShield.motorAWrite(v1, 0); //run motor A at "v1" duty cycle and in the forward direction 
+            while( t.read() < start_period + traj_period + end_period) { 
+                 
+                // Read encoders to get motor states
+                angle1 = encoderA.getPulses() *PULSE_TO_RAD + angle1_init;       
+                velocity1 = encoderA.getVelocity() * PULSE_TO_RAD;
+                 
+                angle2 = encoderB.getPulses() * PULSE_TO_RAD + angle2_init;       
+                velocity2 = encoderB.getVelocity() * PULSE_TO_RAD;           
+                
+                const float th1 = angle1;
+                const float th2 = angle2;
+                const float dth1= velocity1;
+                const float dth2= velocity2;
+                
+                // ADD YOUR CONTROL CODE HERE (CALCULATE AND SET DESIRED CURRENTS)
+ 
+                // Calculate the Jacobian
+                float Jx_th1 = 0;
+                float Jx_th2 = 0;
+                float Jy_th1 = 0;
+                float Jx_th2 = 0;
+                
+                // Calculate the forware kinematics (position and velocity)
+                float xLeg = 0;
+                float yLeg = 0;
+                float dxLeg = 0;
+                float fyLeg = 0;       
+                               
+//                float Jx_th1 = l_AC*cos(th1 + th2) + l_DE*cos(th1) + l_OB*cos(th1);
+//                float Jx_th2 =  l_AC*cos(th1 + th2);
+//                float Jy_th1 = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
+//                float Jy_th2 =  l_AC*sin(th1 + th2);
+//        
+//                float xLeg = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
+//                float yLeg = - l_AC*cos(th1 + th2) - l_DE*cos(th1) - l_OB*cos(th1);
+//                float dxLeg = Jx_th1 * dth1 + Jx_th2 * dth2;
+//                float dyLeg = Jy_th1 * dth1 + Jy_th2 * dth2;
+
+                // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
+                float teff  = 0;
+                float vMult = 0;
+                if( t < start_period) {
+                    if (K_xx > 0 || K_yy > 0) {
+                        K_xx = 180;
+                        K_yy = 180;
+                        D_xx = 2;
+                        D_yy = 2;
+                        K_xy = 0;
+                        D_xy = 0;
+                    }
+                    teff = 0;
+                }
+                else if (t < start_period + traj_period)
+                {
+                    K_xx = input_params[5]; // Foot stiffness N/m
+                    K_yy = input_params[6]; // Foot stiffness N/m
+                    K_xy = input_params[7]; // Foot stiffness N/m
+                    D_xx = input_params[8]; // Foot damping N/(m/s)
+                    D_yy = input_params[9]; // Foot damping N/(m/s)
+                    D_xy = input_params[10]; // Foot damping N/(m/s)
+                    teff = (t-start_period);
+                    vMult = 1;
+                }
                 else
-                    motorShield.motorAWrite(v2, 0); //run motor A at "v2" duty cycle and in the forward direction 
+                {
+                    teff = traj_period;  
+                    // TODO: reset gains and vMult here?  
+                }
+                
+                float rDesFoot[2] , vDesFoot[2];
+                rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
+                rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
+                vDesFoot[0]/=traj_period;
+                vDesFoot[1]/=traj_period;
+                vDesFoot[0]*=vMult;
+                vDesFoot[1]*=vMult;
 
-                // Form output to send to MATLAB
-                float output_data[NUM_OUTPUTS];
+                // Calculate error variables
+                float e_x = 0;
+                float e_y = 0;
+                float de_x = 0;
+                float de_y = 0;
+
+//                float e_x = ( xLeg - rDesFoot[0]);
+//                float e_y = ( yLeg - rDesFoot[1]); 
+//                float de_x = ( dxLeg - vDesFoot[0]);
+//                float de_y = ( dyLeg - vDesFoot[1]);
+//        
+                // Calculate virtual force on foot
+                float fx = 0;
+                float fy = 0;
+                
+//                float fx   = -K_xx * e_x - K_xy * e_y - D_xx * de_x -D_xy * de_y;
+//                float fy   = -K_yy * e_y - K_xy * e_x - D_yy * de_y -D_xy * de_x;
                 
+                // Set desired currents
+                current_des1 = 0;
+                current_des2 = 0;                
+                
+//                current_des1 = (nu1*velocity1 + Jx_th1*fx + Jy_th1*fy)/k_emf;
+//                current_des2 = (nu2*velocity2 + Jx_th2*fx + Jy_th2*fy)/k_emf;         
+                
+                //Form output to send to MATLAB     
+                float output_data[NUM_OUTPUTS];
+                // current time
                 output_data[0] = t.read();
-                output_data[1] = encoderA.getPulses(); 
-                output_data[2] = encoderA.getVelocity();
-                output_data[3] = motorShield.readCurrentA();
+                // motor 1 state
+                output_data[1] = angle1;
+                output_data[2] = velocity1;  
+                output_data[3] = current1;
+                output_data[4] = current_des1;
+                output_data[5] = duty_cycle1;
+                // motor 2 state
+                output_data[6] = angle2;
+                output_data[7] = velocity2;
+                output_data[8] = current2;
+                output_data[9] = current_des2;
+                output_data[10]= duty_cycle2;
+                // foot state
+                output_data[11] = xFoot;
+                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];
                 
                 // Send data to MATLAB
                 server.sendData(output_data,NUM_OUTPUTS);
-                
-                wait(.001); //run control loop at 1kHz
+
+                wait_us(impedance_control_period_us);   
             }
+            
             // Cleanup after experiment
             server.setExperimentComplete();
+            currentLoop.detach();
             motorShield.motorAWrite(0, 0); //turn motor A off
+            motorShield.motorBWrite(0, 0); //turn motor B off
+        
         } // end if
+        
     } // end while
     
 } // end main