Diff: autoMode.h

Revision:

22:3996c3f41922

Parent:

21:79b94cb922f0

Child:

26:8e6c736b6791

--- a/autoMode.h	Fri Sep 18 01:49:16 2015 +0000
+++ b/autoMode.h	Fri Sep 18 02:15:33 2015 +0000
@@ -5,10 +5,67 @@
 PID velocity_controller(18.0,5274.0 ,0.0,RATE);
 PID direction_controller(9.0,2637.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) targ_sita=0.0;
+    else if(l1) targ_sita=PI/2.0;
+    else if(square) if(edge_square) edge_square=0,autoflag=0,Indicator4=0;
+    else if(cross){
+        sendData(1, 4);
+        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();
+        wait(0.3);
+        sita=PI/4.0,x=0.0,y=0.0;
+        velocity_controller.reset();
+        direction_controller.reset();
+    }
+    else if(r2){
+        if(edge_r1){
+            edge_r1=0;
+            sendData(1, 1);
+            wait(0.1);
+            sendData(1, 4);
+            wait(0.01);
+            sendData(1, 2);
+            wait(0.1);
+            sendData(1, 4);
+        }
+    }
+    else if(l2){
+        if(edge_l1){
+            edge_l1=0;
+            sendData(1, 3);
+            wait(0.1);
+            sendData(1, 4);
+            wait(0.01);
+            sendData(1, 5);
+            wait(0.1);
+            sendData(1, 7);
+        }
+    }
+    else if(triangle){
+        if(edge_triangle){
+            edge_triangle=0;
+            if(cylinder_step==3) cylinder_step=0;
+            cylinder_step++;
+            sendData(1, cylinder_step);
+        }
+    }
+}
+
+
+
+
 /***The function is PID controller initialize.***/
 inline void initializeControllers()
 {
-
     velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
     direction_controller.setInputLimits(-10.0, 10.0); //x2