Diff: autoMode.h

Revision:

2:738b28f6a04b

Parent:

0:b613dc16f27d

Child:

3:8d8c25c556ae

--- a/autoMode.h	Wed Oct 28 09:26:31 2015 +0000
+++ b/autoMode.h	Fri Oct 30 09:21:03 2015 +0000
@@ -37,6 +37,7 @@
             resetState();
             flagf=1;
             spcount=0.0;
+            direction_controller.setBias(0.0);
 //            targ_velocity=speed;
 #ifdef BLUE
 //            sendData(5,69); //right
@@ -65,6 +66,7 @@
             resetState();
             flagf = 1;
             spcount=0.0;
+            direction_controller.setBias(0.0);
 //            targ_velocity=speed;
 #ifdef BLUE
             sendData(5,60); //right
@@ -89,6 +91,7 @@
         if(edge8){
             edge8=0;
             resetState();
+            direction_controller.setBias(0.0);
 //            targ_velocity = speed;
 #ifdef BLUE
             sendData(5,69); //right
@@ -298,12 +301,10 @@
 /***The function is PID controller initialize.***/
 inline void initializeControllers()
 {
-//    velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
     velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
-    direction_controller.setInputLimits(-10.0, 10.0); //x2
+    direction_controller.setInputLimits(-PI, PI); //x2
 
     //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
-//    velocity_controller.setOutputLimits(-1.0, 1.0);
     velocity_controller.setOutputLimits(0.0, 1.0);
     direction_controller.setOutputLimits(-1.0, 1.0);
 
@@ -328,8 +329,6 @@
 {
     direction_controller.setSetPoint((float)targ_sita);
     direction_controller.setProcessValue((float)sita);
-    //direction_controller.setSetPoint(0.0);  /*目標値とのずれをなくす*/
-    //direction_controller.setProcessValue(-y);
     x2 = (double)direction_controller.compute();
 }
 
@@ -339,11 +338,19 @@
     sita_following();
     
     if(flagf==0){
-        Vr                  = ( 2.0*(-x1) + x2 ) / 3.0;
-        Vl                  = ( 2.0*(-x1) - x2 ) / 3.0;
+//        Vr                  = ( 2.0*(-x1) + x2 ) / 3.0;
+//        Vl                  = ( 2.0*(-x1) - x2 ) / 3.0;
+        if(x2>0.0){
+            Vr                  = -x1;
+            Vl                  = -x1 - x2;
+        }
+        else{
+            Vr                  = -x1 + x2;
+            Vl                  = -x1;
+        }
     }
     else if(flagf==1){
-        if(x2>0){
+        if(x2>0.0){
             Vr                  = x1;
             Vl                  = x1 - x2;
         }