Diff: autoMode.h

Revision:

52:d9e1629da852

Parent:

51:cb430192b28b

Child:

55:db1797ac6cb1

--- a/autoMode.h	Sun Sep 27 05:02:15 2015 +0000
+++ b/autoMode.h	Sun Sep 27 10:04:52 2015 +0000
@@ -5,39 +5,80 @@
 PID velocity_controller(36.0,5274.0 ,0.0,RATE);
 PID direction_controller(36.0,3.0,0.0,RATE);
 
-/***Ps3 correspondence***/
-void autoPs3()
-{
-    /*if(up) targ_velocity=speed;
-    else if(down) targ_velocity=-speed;
-    else if(right) targ_velocity=0.0,targ_sita=0.0;
-    else if(left) targ_velocity=0.0,targ_sita=PI/2.0;
-    else if(r1) sendData(1,4);
-    else if(l1) sendData(1,5);
-//    else if(r1) sendData(4,18);
-//    else if(l1) sendData(5,18);
-    else if(circle) {
-        if(edge_circle) edge_circle=0,autoflag=0,Indicator4=0,IndicatorAuto=1;
-    } else if(cross) {
+/***IM920 correspondence***/
+void autoIM920() {
+    if(b==7){  /*mode change*/
+        if(edge7){
+            edge7=0;
+            autoflag=0;
+            Indicator4=0;
+            IndicatorAuto=1;
+        }
+    }
+    else if(b==6){  /*start*/
+        targ_velocity=speed;
+        sendData(4,defoR);
+        wait(0.1);
+        sendData(5,defoL);
+        wait(0.1);
+        stateR = defoR;
+        stateL = defoL;
+    }
+    else if(b==5){ /*reset*/
+        sendData(4,31);
+        wait(0.1);
+        sendData(5,31);
+        wait(0.1);
+    }
+    else if(b==8){ /*set defo*/
+        sendData(4,defoR);
+        wait(0.1);
+        sendData(5,defoL);
+        wait(0.1);
+    }
+    else if(b==9){  /*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(triangle) {
-        if(edge_triangle) {
-            edge_triangle=0;
-            if(cylinderStep==3) cylinderStep=0;
-            cylinderStep++;
-            sendData(1, cylinderStep);
+    }
+    else if(b==1){  /*L down*/
+        if(edge1){
+            edge1=0;
+            if(stateL!=1) stateL--;
+            sendData(5,stateL);
+        }
+    }
+    else if(b==2){  /*L up*/
+        if(edge2){
+            edge2=0;
+            if(stateL!=28) stateL++;
+            sendData(5,stateL);
         }
-    }*/
+    }
+    else if(b==3){  /*R down*/
+        if(edge3){
+            edge3=0;
+            if(stateR!=1) stateR--;
+            sendData(4,stateR);
+        }
+    }
+    else if(b==4){  /*R up*/
+        if(edge4){
+            edge4=0;
+            if(stateR!=28) stateR++;
+            sendData(4,stateR);
+        }
+    }
+    if(b!=7) edge7=1;
+    if(b!=1) edge1=1;
+    if(b!=2) edge2=1;
+    if(b!=3) edge3=1;
+    if(b!=4) edge4=1;
 }
 
-
-
-
 /***The function is PID controller initialize.***/
 inline void initializeControllers()
 {