Diff: autoMode.h

Revision:

26:760f1bce8214

Parent:

24:6d2573d6f2b6

Child:

27:88863fab46c0

--- a/autoMode.h	Sun Nov 08 04:26:26 2015 +0000
+++ b/autoMode.h	Sun Nov 08 10:56:27 2015 +0000
@@ -2,10 +2,9 @@
 #define AUTOMODE_H
 
 /***PID Controller***/
-//PID velocity_controller(36.0,5274.0 ,0.0,RATE);
+PID velocity_controller(9.0,5274.0,0.0,RATE);
+PID direction_controller(36.0,3.5,0.0,RATE);
 //PID direction_controller(36.0,3.0,0.0,RATE);
-PID velocity_controller(9.0,5274.0,0.0,RATE);
-PID direction_controller(36.0,3.0,0.0,RATE);
 
 Timeout OpStart;
 
@@ -21,17 +20,15 @@
 void autoIM920() {
     if(b==7){  /*mode change*/
         if(edge7) {
-            Com.detach();
             edge7=0;
             autoflag=0;
             Indicator4=0;
             IndicatorAuto=1;
             direction_controller.setTunings(12.0,9.0,0.0);
+            targ_velocity=0.0;
             flaga=0;
-            Move_l(0.0);
-            Move_r(0.0);
-            wait(0.1);
-            mCom.attach(&mCall,RATE);
+            spcount=speed;
+            dpcount=0.0;
         }
     }
     else if((b==6)&&(!flaga)){ /*start*/
@@ -40,8 +37,8 @@
             resetState();
             flagf=1;
             spcount=0.0;
+            dpcount=0.0;
             direction_controller.setBias(0.0);
-//            targ_velocity=speed;
 #ifdef BLUE
             sendData(5,69); //right
             wait(0.05);
@@ -81,78 +78,6 @@
     if(b!=8) edge8=1;
     if(b!=9) edge9=1;
 }
-/***The function is PID controller initialize.***/
-inline void initializeControllers()
-{
-    velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
-//    velocity_controller.setInputLimits(-1500.0, 1500.0); //x1
-    direction_controller.setInputLimits(-PI, PI); //x2
 
-    //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
-    velocity_controller.setOutputLimits(0.0, 1.0);
-    direction_controller.setOutputLimits(-1.0, 1.0);
-
-    //set bias. 初期値
-    velocity_controller.setBias(0.0);
-    direction_controller.setBias(0.0);
-
-    //set mode.
-    velocity_controller.setMode(AUTO_MODE);
-    direction_controller.setMode(AUTO_MODE);
-}
-
-/***The function is following move speed.***/
-inline void velocity_following()
-{
-    velocity_controller.setSetPoint((float)targ_velocity);
-    velocity_controller.setProcessValue((float)velocity);
-    x1 = (double)velocity_controller.compute();
-}
-
-inline void sita_following()
-{
-    direction_controller.setSetPoint((float)targ_sita);
-    direction_controller.setProcessValue((float)sita);
-    x2 = (double)direction_controller.compute();
-}
-
-inline void move_following()
-{
-    velocity_following();
-    sita_following();
-    
-    if(flagf==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;
-        }*/
-        if(x2>0.0){
-            Vr                  = -x1 + x2;
-            Vl                  = -x1;
-        }
-        else{
-            Vr                  = -x1;
-            Vl                  = -x1 - x2;
-        }
-    }
-    else if(flagf==1){
-        if(x2>0.0){
-            Vr                  = x1;
-            Vl                  = x1 - x2;
-        }
-        else{
-            Vr                  = x1 + x2;
-            Vl                  = x1;
-        }
-    }
-    Move_r( ( float ) Vr );
-    Move_l( ( float ) Vl );
-}
 
 #endif /*autoMode.h*/