Diff: main.cpp

Revision:

10:bca0274a007b

Parent:

8:1150a13f6967

Child:

11:5513638d3c13

--- a/main.cpp	Fri Oct 03 14:05:41 2014 +0000
+++ b/main.cpp	Fri Oct 03 14:11:17 2014 +0000
@@ -91,13 +91,9 @@
         measurementCallback(&z, &odometry);                
         instance.measurement_Callback( z, dX );
         instance.state_Callback();
-        double vd = instance.getCommand().get(1,1);
-        double wd = instance.getCommand().get(2,1);
-        double phi_r = bicycle.get(3,1)/bicycle.get(1,1)*(wd+vd/bicycle.get(3,1));
-        double phi_l = bicycle.get(3,1)/bicycle.get(2,1)*(wd+vd/bicycle.get(3,1));
+        double phi_r = instance.getCommand().get(1,1);
+        double phi_l = instance.getCommand().get(2,1);
         
-        phi_r = (phi_r >= phi_max ? phi_r : phi_max);
-        phi_l = (phi_l >= phi_max ? phi_l : phi_max);
         
         instance.computeCommand(dX, (double)dt, -1);        
         pc.printf("command : \n phi_r = %d \n phi_l = %d \n", phi_r/phi_max*100, phi_l/phi_max*100);