Revision 5:3d30627ae76e, committed 2021-11-30

Comitter:

lschwend

Date:

Tue Nov 30 21:52:04 2021 +0000

Parent:

4:bb441c9325f4

Child:

6:7f39aa2c97da

Commit message:

Added ellipse and some other edits;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.lib	Tue Nov 30 21:52:04 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/saloutos/code/Matrix/#444fdf9b7d4c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MatrixMath.lib	Tue Nov 30 21:52:04 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/saloutos/code/MatrixMath/#1c6ba87fa6a3

--- a/main.cpp	Mon Nov 29 20:49:05 2021 +0000
+++ b/main.cpp	Tue Nov 30 21:52:04 2021 +0000
@@ -10,8 +10,8 @@
 #include "HardwareSetup.h"
 
 #define BEZIER_ORDER_FOOT    7
-#define NUM_INPUTS (21 + 2*(BEZIER_ORDER_FOOT+1))
-#define NUM_OUTPUTS 29
+#define NUM_INPUTS (27)
+#define NUM_OUTPUTS 33
 
 #define PULSE_TO_RAD (2.0f*3.14159f / 1200.0f)
 
@@ -112,6 +112,14 @@
 float k_t = 0.18f;             // motor torque constant
 float nu = 0.0005;             // motor viscous friction
 
+// ellipse stuff
+float y0; 
+float x0; 
+float Omega; 
+float a; 
+float b; 
+float phase;
+
 // Current control interrupt function
 void CurrentLoop()
 {
@@ -211,7 +219,7 @@
     // Object for 7th order Cartesian foot trajectory. 
     
     //CREATE A TRAJECTORY 
-    BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
+    //BezierCurve rDesFoot_bez(2,BEZIER_ORDER_FOOT);
     
     // Link the terminal with our server and start it up
     server.attachTerminal(pc);
@@ -253,13 +261,21 @@
             D_yy2                       = input_params[18];
             D_xy2                       = input_params[19];
             duty_max2                   = input_params[20];
+            
+            a = input_params[21]; 
+            b = input_params[22]; 
+            Omega = input_params[23]; 
+            y0 = input_params[24]; 
+            x0 = input_params[25];
+            phase = input_params[26]; 
+            
           
             // 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[21+i];    
-            }
-            rDesFoot_bez.setPoints(foot_pts);
+            //float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
+//            for(int i = 0; i<2*(BEZIER_ORDER_FOOT+1);i++) {
+//              foot_pts[i] = input_params[27+i];    
+//            }
+//            rDesFoot_bez.setPoints(foot_pts);
             
             // Attach current loop interrupt
             currentLoop.attach_us(CurrentLoop,current_control_period_us);
@@ -301,10 +317,10 @@
                 const float dth2= velocity2;
  
  
-                const float th3 = angle3;
-                const float th4 = angle4;
-                const float dth3= velocity3;
-                const float dth4= velocity4;
+                const float th3 = -angle3;
+                const float th4 = -angle4;
+                const float dth3= -velocity3;
+                const float dth4= -velocity4;
                 
                 
                 
@@ -384,73 +400,85 @@
                 }
                 
                 // 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;
-                vDesFoot[1]*=vMult;
+                //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;
                 
-                float rDesFoot2[2] , vDesFoot2[2];
-                float evalPoint = teff/traj_period+traj_period/2;
-                if(evalPoint>traj_period) evalPoint=evalPoint-traj_period;
-                rDesFoot_bez.evaluate(evalPoint,rDesFoot2);
-                rDesFoot_bez.evaluateDerivative(evalPoint,vDesFoot2);
-                vDesFoot2[0]/=traj_period;
-                vDesFoot2[1]/=traj_period;
-                vDesFoot2[0]*=vMult;
-                vDesFoot2[1]*=vMult;
+               // float rDesFoot2[2] , vDesFoot2[2];
+//                float evalPoint = teff/traj_period+traj_period/2;
+//                if(evalPoint>traj_period) evalPoint=evalPoint-traj_period;
+//                rDesFoot_bez.evaluate(evalPoint,rDesFoot2);
+//                rDesFoot_bez.evaluateDerivative(evalPoint,vDesFoot2);
+//                vDesFoot2[0]/=traj_period;
+//                vDesFoot2[1]/=traj_period;
+//                vDesFoot2[0]*=vMult;
+//                vDesFoot2[1]*=vMult;
                 
                 // Calculate the inverse kinematics (joint positions and velocities) for desired joint angles              
-                float xFoot_inv = -rDesFoot[0];
-                float yFoot_inv = rDesFoot[1];                
-                float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
-                float alpha = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
-                float th2_des = -(3.14159f - alpha); 
-                float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
-                
-                
-                float xFoot_inv2 = -rDesFoot2[0];
-                float yFoot_inv2 = rDesFoot2[1];                
-                float alpha2 = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
-                float th3_des = -(3.14159f - alpha2); 
-                float th4_des = -((3.14159f/2.0f) + atan2(yFoot_inv2,xFoot_inv2) - abs(asin( (l_AC/l_OE)*sin(alpha2) )));
-                
-                
+                //float xFoot_inv = -rDesFoot[0];
+//                float yFoot_inv = rDesFoot[1];                
+//                float l_OE = sqrt( (pow(xFoot_inv,2) + pow(yFoot_inv,2)) );
+//                float alpha = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
+//                float th2_des = -(3.14159f - alpha); 
+//                float th1_des = -((3.14159f/2.0f) + atan2(yFoot_inv,xFoot_inv) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
+//                
+//                
+//                float xFoot_inv2 = -rDesFoot2[0];
+//                float yFoot_inv2 = rDesFoot2[1];                
+//                float alpha2 = abs(acos( (pow(l_OE,2) - pow(l_AC,2) - pow((l_OB+l_DE),2))/(-2.0f*l_AC*(l_OB+l_DE)) ));
+//                float th3_des = -(3.14159f - alpha2); 
+//                float th4_des = -((3.14159f/2.0f) + atan2(yFoot_inv2,xFoot_inv2) - abs(asin( (l_AC/l_OE)*sin(alpha2) )));
+//                
+//                
+//                
+//                float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
+//                float dth1_des = (1.0f/dd) * (  Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
+//                float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
+//                
+//                float dd2 = (Jx_th3*Jy_th4 - Jx_th4*Jy_th3);
+//                float dth3_des = (1.0f/dd2) * (  Jy_th4*vDesFoot[0] - Jx_th4*vDesFoot[1] );
+//                float dth4_des = (1.0f/dd2) * ( -Jy_th3*vDesFoot[0] + Jx_th3*vDesFoot[1] );
+        
+                // Calculate error variables and the desired position in ellispe
+                float xDes = b*cos(Omega*teff) + x0; 
+                float yDes = a*sin(Omega*teff) + y0;
+                float vxDes = 0;//-b*Omega*sin(Omega*teff);
+                float vyDes = 0; //a*Omega*cos(Omega*teff); 
                 
-                float dd = (Jx_th1*Jy_th2 - Jx_th2*Jy_th1);
-                float dth1_des = (1.0f/dd) * (  Jy_th2*vDesFoot[0] - Jx_th2*vDesFoot[1] );
-                float dth2_des = (1.0f/dd) * ( -Jy_th1*vDesFoot[0] + Jx_th1*vDesFoot[1] );
+                float e_x = xDes - xFoot;
+                float e_y = yDes - yFoot;
+                float de_x = vxDes - dxFoot;
+                float de_y = vyDes - dyFoot;
                 
-                float dd2 = (Jx_th3*Jy_th4 - Jx_th4*Jy_th3);
-                float dth3_des = (1.0f/dd2) * (  Jy_th4*vDesFoot[0] - Jx_th4*vDesFoot[1] );
-                float dth4_des = (1.0f/dd2) * ( -Jy_th3*vDesFoot[0] + Jx_th3*vDesFoot[1] );
-        
-                // Calculate error variables
-                float e_x = 0;
-                float e_y = 0;
-                float de_x = 0;
-                float de_y = 0;
+                float xDes2 = b*cos(Omega*teff + phase) + x0; 
+                float yDes2 = a*sin(Omega*teff + phase) + y0;
+                float vxDes2 = 0;//-b*Omega*sin(Omega*teff + phase);
+                float vyDes2 = 0; //a*Omega*cos(Omega*teff + phase); 
                 
-                float e_x2 = 0;
-                float e_y2 = 0;
-                float de_x2 = 0;
-                float de_y2 = 0;
+                float e_x2 = xDes2 - xFoot2;
+                float e_y2 = yDes2 - yFoot2;
+                float de_x2 = vxDes2 - dxFoot2;
+                float de_y2 = vyDes2 - dyFoot2;
+                
+               
         
                 // Calculate virtual force on foot
-                float fx = K_xx*(rDesFoot[0]-xFoot) +K_xy*(rDesFoot[1]-yFoot)+D_xx*(vDesFoot[0]-dxFoot)+D_xy*(vDesFoot[1]-dyFoot);
-                float fy = K_xy*(rDesFoot[0]-xFoot) + K_yy*(rDesFoot[1]-yFoot) + D_xy*(vDesFoot[0]-xFoot)+D_yy*(vDesFoot[1]-dyFoot);
+                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 fx2 = K_xx2*(rDesFoot2[0]-xFoot2) +K_xy*(rDesFoot2[1]-yFoot2)+D_xx2*(vDesFoot2[0]-dxFoot2)+D_xy2*(vDesFoot2[1]-dyFoot2);
-                float fy2 = K_xy2*(rDesFoot2[0]-xFoot2) + K_yy*(rDesFoot2[1]-yFoot2) + D_xy2*(vDesFoot2[0]-xFoot2)+D_yy2*(vDesFoot2[1]-dyFoot2);
+                float fx2 = K_xx2*(e_x2) +K_xy2*(e_y2)+D_xx2*(de_x2)+D_xy2*(de_y2);
+                float fy2 = K_xy2*(e_x2) + K_yy2*(e_y2) + D_xy2*(de_x2)+D_yy2*(de_y2);
                 
                                                 
                 // Set desired currents             
                 current_des1 = (Jx_th1*fx+Jy_th1*fy)/k_t;          
                 current_des2 = (Jx_th2*fx+Jy_th2*fy)/k_t;   
-                current_des3 = (Jx_th1*fx+Jy_th1*fy)/k_t;   
-                current_des4 = (Jx_th2*fx+Jy_th2*fy)/k_t;          
+                current_des3 = (Jx_th1*fx2+Jy_th1*fy2)/k_t;   
+                current_des4 = (Jx_th2*fx2+Jy_th2*fy2)/k_t;          
        
 
         
@@ -500,26 +528,32 @@
                 output_data[9] = current_des2;
                 output_data[10]= duty_cycle2;
                 // motor 3 state
-                output_data[11] = angle2;
-                output_data[12] = velocity2;
-                output_data[13] = current2;
-                output_data[14] = current_des2;
-                output_data[15]= duty_cycle2;
+                output_data[11] = angle3;
+                output_data[12] = velocity3;
+                output_data[13] = current3;
+                output_data[14] = current_des3;
+                output_data[15]= duty_cycle3;
                 // motor 4 state
-                output_data[16] = angle2;
-                output_data[17] = velocity2;
-                output_data[18] = current2;
-                output_data[19] = current_des2;
-                output_data[20]= duty_cycle2;
+                output_data[16] = angle4;
+                output_data[17] = velocity4;
+                output_data[18] = current4;
+                output_data[19] = current_des4;
+                output_data[20]= duty_cycle4;
                 // foot state
                 output_data[21] = xFoot;
                 output_data[22] = yFoot;
                 output_data[23] = dxFoot;
                 output_data[24] = dyFoot;
-                output_data[25] = rDesFoot[0];
-                output_data[26] = rDesFoot[1];
-                output_data[27] = vDesFoot[0];
-                output_data[28] = vDesFoot[1];
+                output_data[25] = xDes;
+                output_data[26] = yDes;
+                output_data[27] = vxDes;
+                output_data[28] = vyDes;
+                
+                output_data[29] = xFoot2; 
+                output_data[30] = yFoot2; 
+                output_data[31] = xDes2; 
+                output_data[32] = yDes2; 
+                
                 
                 // Send data to MATLAB
                 server.sendData(output_data,NUM_OUTPUTS);