Revision 26:f67075d64cb3, committed 2021-12-04

Comitter:

davepreiss

Date:

Sat Dec 04 20:46:13 2021 +0000

Parent:

25:52b378e89f42

Commit message:

davetrying to export;

Changed in this revision

--- a/BezierCurve.cpp	Tue Sep 29 21:09:51 2020 +0000
+++ b/BezierCurve.cpp	Sat Dec 04 20:46:13 2021 +0000
@@ -48,6 +48,7 @@
 void BezierCurve::evaluate(float time, float point[]) {
     //float *_point = new float[_dim];
     
+    // fill up point with dim number of zeros
     for(int i=0; i< _dim ; i++) {
         point[i] = 0;
     }        

--- a/main.cpp	Tue Sep 29 21:09:51 2020 +0000
+++ b/main.cpp	Sat Dec 04 20:46:13 2021 +0000
@@ -7,8 +7,11 @@
 #include "MotorShield.h" 
 #include "HardwareSetup.h"
 
-#define BEZIER_ORDER_FOOT    7
-#define NUM_INPUTS (12 + 2*(BEZIER_ORDER_FOOT+1))
+#define BEZIER_ORDER_JUMP    7
+#define BEZIER_ORDER_FLIGHT  7
+#define BEZIER_ORDER_LAND    7
+
+#define NUM_INPUTS (12 + 2*(BEZIER_ORDER_JUMP+1) + 2*(BEZIER_ORDER_FLIGHT+1) + 2*(BEZIER_ORDER_LAND+1) + 2*(BEZIER_ORDER_FLIGHT+1))
 #define NUM_OUTPUTS 19
 
 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
@@ -47,15 +50,16 @@
 float angle2_init;
 
 // Fixed kinematic parameters
-const float l_OA=.011; 
-const float l_OB=.042; 
+const float l_OA=.03673; 
+const float l_OB=.06773; 
 const float l_AC=.096; 
-const float l_DE=.091;
+const float l_DE=.127-(l_OB-l_OA);
 
 // 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;
+float jump_period, flight_period, land_period;
 
 // Control parameters
 float current_Kp = 4.0f;         
@@ -75,11 +79,17 @@
 float k_t = 0.18f;             // motor torque constant
 float nu = 0.0005;             // motor viscous friction
 
+// Foot Sensor Parameters
+bool airborne = 0;
+bool landCheck = false;
+uint8_t gaitState = 0;
+uint8_t hopCount = 0;
+uint8_t hopCountLim = 4; // 0 means 1 hop
+
 // Current control interrupt function
 void CurrentLoop()
 {
     // This loop sets the motor voltage commands using PI current controllers with feedforward terms.
-    
     //use the motor shield as follows:
     //motorShield.motorAWrite(DUTY CYCLE, DIRECTION), DIRECTION = 0 is forward, DIRECTION =1 is backwards.
         
@@ -127,9 +137,10 @@
 
 int main (void)
 {
-    
     // Object for 7th order Cartesian foot trajectory
-    BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
+    BezierCurve rDesJump_bez(2,BEZIER_ORDER_JUMP);
+    BezierCurve rDesFlight_bez(2,BEZIER_ORDER_FLIGHT);
+    BezierCurve rDesLand_bez(2,BEZIER_ORDER_LAND);
     
     // Link the terminal with our server and start it up
     server.attachTerminal(pc);
@@ -139,16 +150,23 @@
     float input_params[NUM_INPUTS];
     pc.printf("%f",input_params[0]);
     
+    // Setup our foot sensor input
+    DigitalIn  footContactPin(PC_6);
+    
+    // NOW WE STAY IN THIS WHILE STATEMENT FOREVER -------------------------------------------------------------------
     while(1) {
-        
-        // If there are new inputs, this code will run
+        // EVERYTHING IS NOW WAITING FOR SERIAL COMMUNICATION TO RUN BELOW, OTHERWISE WE ARE LOOPING THIS IF STATEMENT, this still only runs once per jump
         if (server.getParams(input_params,NUM_INPUTS)) {
             
-                        
-            // 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
+            // Get inputs from MATLAB, note that these are formatted in Experiment_trajectory        
+            gaitState = 0;
+            landCheck = 0;
+            start_period                = 1.5; // removed it as a parameter but we have a 1 second delay before starting the jump
+            traj_period                 = 5.0;
+            end_period                  = 1.0;
+            jump_period                 = input_params[0];    // First buffer time, before trajectory
+            flight_period               = input_params[1];    // Trajectory time/length
+            land_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)
@@ -161,12 +179,46 @@
             D_xy                        = input_params[10];   // Foot damping N/(m/s)
             duty_max                    = input_params[11];   // Maximum duty factor
           
-            // 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];    
+            // Get foot trajectory points - start by initializing an empty array for each curve
+            float jump_pts[2*(BEZIER_ORDER_JUMP+1)]; // foot_pts is a matrix sized for the bezier order
+            float flight_pts[2*(BEZIER_ORDER_FLIGHT+1)]; 
+            float land_pts[2*(BEZIER_ORDER_LAND+1)]; 
+            
+            // Now fill each array up with its respective points
+            for(int i = 0; i<2*(BEZIER_ORDER_JUMP+1);i++) { 
+                jump_pts[i] = input_params[12+i];    // assign the proper input parameters to each foot position
+                flight_pts[i] = input_params[12+BEZIER_ORDER_JUMP+1 +i];
+                land_pts[i] = input_params[12+BEZIER_ORDER_JUMP+1 + BEZIER_ORDER_FLIGHT+1 +i];
+                // Adding this line to try and get the proper flight in
+                //flight_pts[i] = input_params[12+BEZIER_ORDER_JUMP+1 + BEZIER_ORDER_FLIGHT+1 + BEZIER_ORDER_FLIGHT+1 +i];
             }
-            rDesFoot_bez.setPoints(foot_pts);
+            //pc.printf("%f" " = flight points \n\r",flight_pts[1]);
+            //float flight_pts[2][8] = {{0.0651000,0.0650000,0.0650000,0.0417000,-0.0169000,-0.0700000,-0.0700000,-0.0700000},
+            //{-0.215000,-0.215000,-0.215000,-0.159700,-0.147000,-0.175000,-0.175000,-0.175100}};
+            
+            flight_pts[0] = .065000;
+            flight_pts[1] = -.215000;
+            flight_pts[2] = .065000;
+            flight_pts[3] = -.215000;
+            flight_pts[4] = .065000;
+            flight_pts[5] = -.215000;
+            flight_pts[6] = .0417;
+            flight_pts[7] = -.1597;
+            flight_pts[8] = -.0169;
+            flight_pts[9] = -.147;
+            flight_pts[10] = -.0700000;
+            flight_pts[11] = -.175;
+            flight_pts[12] = -.0700000;
+            flight_pts[13] = -.175;
+            flight_pts[14] = -.0700000;
+            flight_pts[15] = -.175;
+            
+            
+            //pc.printf("%f" " = flight points \n\r",flight_pts[1]);
+            
+            rDesJump_bez.setPoints(jump_pts);
+            rDesFlight_bez.setPoints(flight_pts);
+            rDesLand_bez.setPoints(land_pts);
             
             // Attach current loop interrupt
             currentLoop.attach_us(CurrentLoop,current_control_period_us);
@@ -181,14 +233,21 @@
 
             motorShield.motorAWrite(0, 0); //turn motor A off
             motorShield.motorBWrite(0, 0); //turn motor B off
+            
+            hopCount = 0;
                          
-            // Run experiment
-            while( t.read() < start_period + traj_period + end_period) { 
-                 
+            // Run experiment HERE's THE REAL MAIN LOOP ---------------------------------------------------------------------------------------------------------------------------
+            while(gaitState < 5) { 
+            //while( t.read() < 8.0) { 
+                // Check to see if we are airborne or not, where airborne = 1 means we are not touching the ground
+                if(footContactPin == true) {
+                    airborne = true;}
+                else {airborne = false;}
+                // pc.printf("%d" " = Airborne? \n\r",airborne); // confirmed to be working
+                
                 // 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;           
                 
@@ -198,55 +257,128 @@
                 const float dth2= velocity2;
  
                 // Calculate the Jacobian
-                float Jx_th1 = 0;
-                float Jx_th2 = 0;
-                float Jy_th1 = 0;
-                float Jy_th2 = 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);
                                 
                 // Calculate the forward kinematics (position and velocity)
-                float xFoot = 0;
-                float yFoot = 0;
-                float dxFoot = 0;
-                float dyFoot = 0;       
+                float xFoot = l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1);
+                float yFoot = - l_AC*cos(th1 + th2) - l_DE*cos(th1) - l_OB*cos(th1);
+                float dxFoot = dth1*(l_AC*cos(th1 + th2) + l_DE*cos(th1) + l_OB*cos(th1)) + dth2*l_AC*cos(th1 + th2);
+                float dyFoot = dth1*(l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1)) + dth2*l_AC*sin(th1 + th2);       
 
                 // 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;
+                
+                
+                // HERE IS WHERE WE CHECK WHERE WE ARE IN THE GAIT ----------------------------------------------------------------------------------------------
+                // gaitState 0 is just a buffer time before the jump holding the intial position
                 if( t < start_period) {
                     if (K_xx > 0 || K_yy > 0) {
-                        K_xx = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
-                        K_yy = 1; // for joint space control, set this to 1; for Cartesian space control, set this to 50
-                        D_xx = 0.1;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
-                        D_yy = 0.1;  // for joint space control, set this to 0.1; for Cartesian space control, set this to 2
-                        K_xy = 0;
-                        D_xy = 0;
+                        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)
+                        gaitState = 0;
                     }
                     teff = 0;
+                    //pc.printf("%f" " Start Phase \n\r",teff);
                 }
-                else if (t < start_period + traj_period)
-                {
+                // Jump Phase = gait 1
+                else if (t < start_period + jump_period) {
+                    // Lets only update the gains if we need to, this will be the last one for now
                     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;
+                    teff = (t - start_period);
+                    gaitState = 1;
+//                    pc.printf("%f" " Jump Phase \n\r",teff);
+                }
+                // Flight Phase = gait 2, we should not be touching the ground here
+                // If t is lower than expected 
+                else if (t < start_period + jump_period + land_period + 1.6 && landCheck == false) {
+                    teff = (t - start_period - jump_period);
+                    gaitState = 2;
+//                    pc.printf("%f" " Flight Phase \n\r",teff);
+                    
+                    // If we land, or if we are 1 second past the expected flight phase, continue to the land gait
+                    if (airborne == 0 || t > start_period + jump_period + flight_period + 1.5) { 
+//                        pc.printf("%f" " < FLIGHT PHASE OVER, AIRBORNE FOR THIS LONG \n\r",teff);
+                        landCheck = true;
+                        flight_period = (t - start_period - jump_period);
+                        gaitState = 3;
+                        teff = (t - start_period - jump_period - flight_period);
+                    }
                 }
-                else
-                {
-                    teff = traj_period;
-                    vMult = 0;
+                // Land Phase = gait 3
+                else if (gaitState == 3) {
+                    teff = (t - start_period - jump_period - flight_period);
+                    gaitState = 3;
+//                    pc.printf("%f" " Land Phase \n\r",teff);
+                    if (t > start_period + jump_period + flight_period + land_period) {
+                        gaitState = 4;
+                        vMult = 0;
+                    }
+                }
+                else {
+                    if (hopCount <= hopCountLim) {
+                        hopCount = hopCount + 1;
+                        t.reset();
+                        t.start();
+                    }
+                    else {
+                        gaitState = 5;
+                        vMult = 0;
+                    }
+                    hopCount = hopCount + 1;
                 }
                 
-                // Get desired foot positions and velocities
+                // Get desired foot positions and velocities                
                 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;
+                // Evaluate the correct gait (bezier curve) at the % detemined by how far into teff we are. There are 4 gaitStates...
+                if (gaitState == 0){
+                    rDesFoot[0] = jump_pts[0];
+                    rDesFoot[1] = jump_pts[1];
+                    vDesFoot[0] = 0.0;
+                    vDesFoot[1] = 0.0;
+                }
+                else if (gaitState == 1){
+//                    pc.printf("%d" " 1=JUMP \n\r",gaitState);
+                    rDesJump_bez.evaluate(teff/jump_period, rDesFoot);
+                    rDesJump_bez.evaluateDerivative(teff/jump_period, vDesFoot);
+                    vDesFoot[0] /= jump_period;
+                    vDesFoot[1] /= jump_period;
+                }
+                else if (gaitState == 2){
+//                   pc.printf("%d" " 2=FLIGHT \n\r",gaitState);
+                    rDesFlight_bez.evaluate(teff/flight_period, rDesFoot);
+                    rDesFlight_bez.evaluateDerivative(teff/flight_period, vDesFoot);
+                    vDesFoot[0] /= flight_period;
+                    vDesFoot[1] /= flight_period;
+                    if (teff > jump_period) {
+                        rDesFoot[0] = flight_pts[14];
+                        rDesFoot[1] = flight_pts[15];
+                        vDesFoot[0] = 0.0;
+                        vDesFoot[1] = 0.0;
+                    }
+                }
+                else if (gaitState == 3){
+//                    pc.printf("%d" " 3=LAND \n\r",gaitState);
+                    rDesLand_bez.evaluate(teff/land_period, rDesFoot);
+                    rDesLand_bez.evaluateDerivative(teff/land_period, vDesFoot);
+                    vDesFoot[0] /= land_period;
+                    vDesFoot[1] /= land_period;
+                }
+                // if we are past the trajectory period multiply by 0
+                vDesFoot[0]*=vMult; 
                 vDesFoot[1]*=vMult;
                 
                 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles              
@@ -262,34 +394,41 @@
                 float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
         
                 // Calculate error variables
-                float e_x = 0;
-                float e_y = 0;
-                float de_x = 0;
-                float de_y = 0;
+                float e_x = xFoot_inv - xFoot;
+                float e_y = yFoot_inv - yFoot;
+                float de_x = vDesFoot[0] - dxFoot;
+                float de_y = vDesFoot[1] - dyFoot;
         
                 // Calculate virtual force on foot
-                float fx = 0;
-                float fy = 0;
-                                
-                // Set desired currents             
-                current_des1 = 0;          
-                current_des2 = 0;   
-        
+                float fx = K_xx*(e_x) + K_xy*(e_y) + D_xx*(de_x) + D_xy*(de_y);
+                float fy = K_xy*(e_x) + K_yy*(e_y) + D_xy*(de_x) + D_yy*(de_y);
+                
+                float tau1 = Jx_th1*fx + Jy_th1*fy;
+                float tau2 = Jx_th2*fx + Jy_th2*fy;
+                
+                current_des1 = tau1/k_t;
+                current_des2 = tau2/k_t;
+                
+                // All of the above was to set these current_des values which will get fed into the CurrentLoop function running on an interrupt
+                                                
                 // Joint impedance
                 // sub Kxx for K1, Dxx for D1, Kyy for K2, Dyy for D2
                 // Note: Be careful with signs now that you have non-zero desired angles!
                 // Your equations should be of the form i_d = K1*(q1_d - q1) + D1*(dq1_d - dq1)
-//                current_des1 = 0;          
-//                current_des2 = 0;                          
-                           
+                //current_des1 = (K_xx*(angle1_init - angle1) + D_xx*(0.0 - velocity1))/k_t;           
+                //current_des2 = (K_yy*(angle2_init - angle2) + D_yy*(0.0 - velocity2))/k_t;   
+                //current_des1 = 0;
+                //current_des2 = 0;
+                     
                 // Cartesian impedance  
                 // Note: As with the joint space laws, be careful with signs!              
-//                current_des1 = 0;          
-//                current_des2 = 0;   
+                //current_des1 = (K_xx*(th1_des - angle1) + D_xx*(th1_des - velocity1))/k_t;           
+                //current_des2 = (K_yy*(th2_des - angle2) + D_yy*(th2_des - velocity2))/k_t;    
                 
                 
                 // Form output to send to MATLAB     
                 float output_data[NUM_OUTPUTS];
+                
                 // current time
                 output_data[0] = t.read();
                 // motor 1 state
@@ -332,3 +471,24 @@
     
 } // end main
 
+/*
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[0]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[1]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[2]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[3]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[4]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[5]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[6]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[7]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[8]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[9]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[10]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[11]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[12]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[13]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[14]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[15]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[16]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[17]);
+pc.printf("%f" " OUTPUT DATA 0 \n\r",output_data[18]);
+*/
\ No newline at end of file