Diff: autoMode.h

Revision:

66:14df82661dfa

Parent:

65:cebdfc0184d7

Child:

68:2b2b88ecdcce

Child:

71:4ebc1c0fcb4e

--- a/autoMode.h	Mon Oct 05 07:53:02 2015 +0000
+++ b/autoMode.h	Wed Oct 07 09:01:34 2015 +0000
@@ -2,7 +2,9 @@
 #define AUTOMODE_H
 
 /***PID Controller***/
-PID velocity_controller(36.0,5274.0 ,0.0,RATE);
+//PID velocity_controller(36.0,5274.0 ,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);
 
 #ifdef IM920
@@ -19,10 +21,7 @@
     else if(b==6){  /*start*/
         flagf=1;
         targ_velocity=speed;
-        sendData(4,defoR);
-        wait(0.1);
-        sendData(5,defoL);
-        wait(0.1);
+        sendData(7,defoR);
         stateR = defoR;
         stateL = defoL;
         step   = 0;
@@ -31,20 +30,14 @@
     else if(b==8){ /*middle start*/
         flagf=1;
         targ_velocity=speed;
-        sendData(5,17);
-        wait(0.1);
-        sendData(4,17);
-        wait(0.1);
+        sendData(7,17);
         stateR = 17;
         stateL = 17;
         step   = 4;
         CMode  = 6;
     }   
     else if(b==5){ /*reset*/
-        sendData(4,31);
-        wait(0.1);
-        sendData(5,31);
-        wait(0.1);
+        sendData(7,31);
     }
     else if(b==9){  /*stop*/
         Motor_swing=0;
@@ -101,10 +94,7 @@
     else if(up){  /*start*/
         flagf=1;
         targ_velocity=speed;
-        sendData(5,defoR);
-        wait(0.1);
-        sendData(4,defoL);
-        wait(0.1);
+        sendData(7,defoR);
         stateR = defoR;
         stateL = defoL;
         step   = 0;
@@ -113,26 +103,17 @@
     else if(right){  /*middle start*/
         flagf = 1;
         targ_velocity=speed;
-        sendData(5,17);
-        wait(0.1);
-        sendData(4,17);
-        wait(0.1);
+        sendData(7,17);
         stateR = 17;
         stateL = 17;
         step   = 4;
         CStep  = 6;
     }
     else if(square){ /*reset*/
-        sendData(4,31);
-        wait(0.1);
-        sendData(5,31);
-        wait(0.1);
+        sendData(7,31);
     }
     else if(triangle){ /*set defo*/
-        sendData(5,defoR);
-        wait(0.1);
-        sendData(4,defoL);
-        wait(0.1);
+        sendData(7,defoR);
     }
     else if(cross){  /*stop*/
         Motor_swing=0;
@@ -175,7 +156,8 @@
 /***The function is PID controller initialize.***/
 inline void initializeControllers()
 {
-    velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
+//    velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
+    velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
     direction_controller.setInputLimits(-10.0, 10.0); //x2
 
     //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP