Diff: User.cpp

Revision:

22:f0e17f3129cc

Parent:

21:0f12ee2322e4

Child:

23:0c6bc29c8f86

--- a/User.cpp	Thu Mar 12 07:32:03 2020 +0000
+++ b/User.cpp	Fri Mar 13 08:15:38 2020 +0000
@@ -12,7 +12,7 @@
 
 int RSX,RSY,LSX,LSY,BSU,BSL;
 //Digitalout cs(p8);
-int move=5,shoot=0,ball=0,box=0;
+int move=5,shoot=0,ball=0,box=0,ball_count=0,box_count=0;
 CAN controller(p30,p29);//CANpin_name
 
 DigitalOut led1(LED1);
@@ -39,9 +39,9 @@
 int id2 = 1;
 double SERVO2DEG = 270.0 / (11500 - 3500);
 double first = (6800 - 3500) * SERVO2DEG;
-double grab = (3600 - 3500) * SERVO2DEG;
+double grab_ball = (3600 - 3500) * SERVO2DEG;
 double pass = (5000 - 3500) * SERVO2DEG;
-double grab2 = (3550 - 3500) * SERVO2DEG;
+double grab_box = (3550 - 3500) * SERVO2DEG;
 
 void cal()
 {
@@ -126,16 +126,16 @@
         shoot = 1;
         led1 = 1;
     } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
-        shoot = 4;
+        ball = 1;
         led2 = 1;
     } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△
-        shoot = 3;
+        box = 1;
         led3 = 1;
     } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□
-        shoot = 2;
+        ball = 2;
         led4 = 1;
     } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2
-        shoot = 5;
+        box = 2;
         led4 = 1;
     }
     show_angle();
@@ -229,24 +229,22 @@
 
 void catch_ball()
 {
-    switch (shoot) {
-        case 2:
-            if(ball ==0) {
+
+    switch (ball) {
+        case 1: //ball servo
+            if(ball_count ==0) {
                 servo.set_degree(id, first);
                 wait(0.5);
                 shoot =0;
                 ball = 1;
-            } else if(ball ==1) {
-                servo.set_degree(id, grab);
+            } else if(ball_count ==1) {
+                servo.set_degree(id, grab_ball);
                 wait(0.5);
                 shoot = 0;
                 ball =0;
             }
             break;
-        case 3:
-            servo.set_degree(id2, grab2);
-            break;
-        case 4:
+        case 2://ball air
             if (out1==0) {
                 out1=1;
                 shoot=0;
@@ -254,17 +252,32 @@
                 out1=0;
                 shoot=0;
             }
-            break;
-        case 5:
-            if (out3==0) {
-                out3=1;
-            } else if (out3 == 1) {
-                out3=0;
-            }
     }
 }
 
 void catch_box()
 {
+    switch(box) {
+        case 1:// box servo
+            if(box_count ==0) {
+                servo.set_degree(id2, first);
+                wait(0.5);
+                shoot =0;
+                ball = 1;
+            } else if(box_count ==1) {
+                servo.set_degree(id2, grab_box);
+                wait(0.5);
+                shoot = 0;
+                ball =0;
+            }
+        case 2: //box air
+            if (out3==0) {
+                out3=1;
+                shoot = 0;
+            } else if (out3 == 1) {
+                out3=0;
+                shoot = 0;
+            }
+    }
 
 }
\ No newline at end of file