Diff: main.cpp

Revision:

3:2730130aa049

Parent:

2:4e581e5b39e8

Child:

4:bb441c9325f4

--- a/main.cpp	Mon Nov 22 18:42:30 2021 +0000
+++ b/main.cpp	Mon Nov 22 20:40:44 2021 +0000
@@ -252,7 +252,7 @@
             D_xx2                       = input_params[17];
             D_yy2                       = input_params[18];
             D_xy2                       = input_params[19];
-            duty_max2                   = input_params[20]
+            duty_max2                   = input_params[20];
           
             // Get foot trajectory points
             float foot_pts[2*(BEZIER_ORDER_FOOT+1)];
@@ -395,7 +395,7 @@
                 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,rDesFoo2);
+                rDesFoot_bez.evaluate(evalPoint,rDesFoot2);
                 rDesFoot_bez.evaluateDerivative(evalPoint,vDesFoot2);
                 vDesFoot2[0]/=traj_period;
                 vDesFoot2[1]/=traj_period;
@@ -413,7 +413,6 @@
                 
                 float xFoot_inv2 = -rDesFoot2[0];
                 float yFoot_inv2 = rDesFoot2[1];                
-                float l_OE = sqrt( (pow(xFoot_inv2,2) + pow(yFoot_inv2,2)) );
                 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) )));
@@ -502,13 +501,13 @@
                 output_data[10]= duty_cycle2;
                 // motor 3 state
                 output_data[11] = angle2;
-                output_data[12 = velocity2;
+                output_data[12] = velocity2;
                 output_data[13] = current2;
                 output_data[14] = current_des2;
                 output_data[15]= duty_cycle2;
                 // motor 4 state
                 output_data[16] = angle2;
-                output_data[17 = velocity2;
+                output_data[17] = velocity2;
                 output_data[18] = current2;
                 output_data[19] = current_des2;
                 output_data[20]= duty_cycle2;