Revision 25:906ee0226ebf, committed 2020-11-18

Comitter:

jawyatt

Date:

Wed Nov 18 23:06:30 2020 +0000

Parent:

24:bf92a281beb8

Commit message:

Updated 6:03pm 11/18

Changed in this revision

--- a/Bezier_Traj_Follower_Example.lib	Wed Nov 18 20:41:13 2020 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://os.mbed.com/users/saloutos/code/Bezier_Traj_Follower_Example/#80e05d939f8c

--- a/main.cpp	Wed Nov 18 20:41:13 2020 +0000
+++ b/main.cpp	Wed Nov 18 23:06:30 2020 +0000
@@ -129,7 +129,7 @@
 {
     
     // Object for 7th order Cartesian foot 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);
@@ -170,7 +170,7 @@
               //pc.printf(foot_pts);
               //foot_pts2[i] = input_params[13];    
             }
-            rDesFoot_bez.setPoints(foot_pts);
+//            rDesFoot_bez.setPoints(foot_pts);
             
             // Attach current loop interrupt
             currentLoop.attach_us(CurrentLoop,current_control_period_us);
@@ -211,7 +211,10 @@
                 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);       
+                float dyFoot = dth1*(l_AC*sin(th1 + th2) + l_DE*sin(th1) + l_OB*sin(th1)) + dth2*l_AC*sin(th1 + th2); 
+                
+                float xFoot_des;
+                float yFoot_des;     
 
                 // Set gains based on buffer and traj times, then calculate desired x,y from Bezier trajectory at current time if necessary
                 float teff  = 0;
@@ -227,6 +230,8 @@
                         //rDesFoot_bez.setPoints(foot_pts);
                     }
                     teff = 0;
+                    xFoot_des = 0;
+                    yFoot_des = -0.1;
                 }
                 else if (t < start_period + traj_period)
                 {
@@ -254,7 +259,10 @@
                     foot_pts[13] = -0.15;
                     foot_pts[14] = -0.15;
                     foot_pts[15] = -0.15;
-                    rDesFoot_bez.setPoints(foot_pts);
+//                    rDesFoot_bez.setPoints(foot_pts);
+                    
+                    xFoot_des = -0.15;
+                    yFoot_des = -0.17;
                 }
                 else
                 {
@@ -263,8 +271,8 @@
                 }
                 
                 float rDesFoot[2] , vDesFoot[2];
-                rDesFoot_bez.evaluate(teff/traj_period,rDesFoot);
-                rDesFoot_bez.evaluateDerivative(teff/traj_period,vDesFoot);
+//                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;
@@ -277,23 +285,23 @@
                 float xFootd = foot_pts[0];
                 float yFootd = foot_pts[1];
                 //float yFootd = rDesFoot[1];                
-                float l_OE = sqrt( (pow(xFootd,2) + pow(yFootd,2)) );
+                float l_OE = sqrt( (pow(xFoot_des,2) + pow(yFoot_des,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(yFootd,xFootd) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
+                float th1_des = -((3.14159f/2.0f) + atan2(yFoot_des,xFoot_des) - abs(asin( (l_AC/l_OE)*sin(alpha) )));
                 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] );
         
                 // Calculate error variables
-                float e_x = rDesFoot[0]-xFoot;
-                float e_y = rDesFoot[1]-yFoot;
+                float e_x = xFoot_des-xFoot;
+                float e_y = yFoot_des-yFoot;
                 float de_x = vDesFoot[0]-dxFoot;
                 float de_y = vDesFoot[1]-dyFoot;
         
                 // 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]-dxFoot)+D_yy*(vDesFoot[1]-dyFoot);
+                float fx = K_xx*(xFoot_des-xFoot)+K_xy*(yFoot_des-yFoot)+D_xx*(vDesFoot[0]-dxFoot)+D_xy*(vDesFoot[1]-dyFoot);
+                float fy = K_xy*(xFoot_des-xFoot)+K_yy*(yFoot_des-yFoot)+D_xy*(vDesFoot[0]-dxFoot)+D_yy*(vDesFoot[1]-dyFoot);
                 
                 // torque
                 float T1 = Jx_th1*fx+Jy_th1*fy;
@@ -337,8 +345,8 @@
                 output_data[12] = yFoot;
                 output_data[13] = dxFoot;
                 output_data[14] = dyFoot;
-                output_data[15] = rDesFoot[0];
-                output_data[16] = rDesFoot[1];
+                output_data[15] = xFoot_des;
+                output_data[16] = yFoot_des;
                 output_data[17] = vDesFoot[0];
                 output_data[18] = vDesFoot[1];