Diff: autoMode.h

Revision:

57:3fbd487e055e

Parent:

55:db1797ac6cb1

Child:

58:6b73e683fa70

Child:

60:4a75f3f3a934

--- a/autoMode.h	Tue Sep 29 03:46:22 2015 +0000
+++ b/autoMode.h	Tue Sep 29 08:23:03 2015 +0000
@@ -5,6 +5,7 @@
 PID velocity_controller(36.0,5274.0 ,0.0,RATE);
 PID direction_controller(36.0,3.0,0.0,RATE);
 
+#ifdef IM920
 /***IM920 correspondence***/
 void autoIM920() {
     if(b==7){  /*mode change*/
@@ -31,9 +32,9 @@
         wait(0.1);
     }
     else if(b==8){ /*set defo*/
-        sendData(4,defoR);
+        sendData(5,defoR);
         wait(0.1);
-        sendData(5,defoL);
+        sendData(4,defoL);
         wait(0.1);
     }
     else if(b==9){  /*stop*/
@@ -48,28 +49,28 @@
         if(edge1){
             edge1=0;
             if(stateL!=1) stateL--;
-            sendData(5,stateL);
+            sendData(4,stateL);
         }
     }
     else if(b==2){  /*L up*/
         if(edge2){
             edge2=0;
             if(stateL!=28) stateL++;
-            sendData(5,stateL);
+            sendData(4,stateL);
         }
     }
     else if(b==3){  /*R down*/
         if(edge3){
             edge3=0;
             if(stateR!=1) stateR--;
-            sendData(4,stateR);
+            sendData(5,stateR);
         }
     }
     else if(b==4){  /*R up*/
         if(edge4){
             edge4=0;
             if(stateR!=28) stateR++;
-            sendData(4,stateR);
+            sendData(5,stateR);
         }
     }
     if(b!=7) edge7=1;
@@ -78,7 +79,75 @@
     if(b!=3) edge3=1;
     if(b!=4) edge4=1;
 }
-
+#else
+void autoPS3(){
+    if(circle){  /*mode change*/
+        if(edge_circle){
+            edge_circle=0;
+            autoflag=0;
+            Indicator4=0;
+            IndicatorAuto=1;
+        }
+    }
+    else if(up){  /*start*/
+        targ_velocity=speed;
+        sendData(5,defoR);
+        wait(0.1);
+        sendData(4,defoL);
+        wait(0.1);
+        stateR = defoR;
+        stateL = defoL;
+    }
+    else if(square){ /*reset*/
+        sendData(4,31);
+        wait(0.1);
+        sendData(5,31);
+        wait(0.1);
+    }
+    else if(triangle){ /*set defo*/
+        sendData(5,defoL);
+        wait(0.1);
+        sendData(4,defoR);
+        wait(0.1);
+    }
+    else if(cross){  /*stop*/
+        Motor_swing=0;
+        sita=PI/4.0,x=0.0,y=0.0;
+        targ_velocity=0.0;
+        targ_sita=PI/4.0;
+        velocity_controller.reset();
+        direction_controller.reset();
+    }
+    else if(l1){  /*L up*/
+        if(edge_l1){
+            edge_l1=0;
+            if(stateL!=2) stateL--;
+            sendData(4,stateL);
+        }
+    }
+    else if(l2){  /*L down*/
+        if(edge_l2){
+            edge_l2=0;
+            if(stateL!=28) stateL++;
+            sendData(4,stateL);
+        }
+    }
+    else if(r1){  /*R up*/
+        if(edge_r1){
+            edge_r1=0;
+            if(stateR!=2) stateR--;
+            sendData(5,stateR);
+        }
+    }
+    else if(r2){  /*R down*/
+        if(edge_r2){
+            edge_r2=0;
+            if(stateR!=28) stateR++;
+            sendData(5,stateR);
+        }
+    }
+}
+#endif
 /***The function is PID controller initialize.***/
 inline void initializeControllers()
 {