Diff: User.cpp

Revision:

20:dd4634500bb3

Parent:

19:510b87211851

Child:

21:0f12ee2322e4

--- a/User.cpp	Wed Mar 11 09:08:05 2020 +0000
+++ b/User.cpp	Thu Mar 12 03:04:51 2020 +0000
@@ -32,18 +32,20 @@
 DigitalOut out2(p20);//エアシリンダー（投擲）
 
 
-    int id = 0;
+int id = 0;
 double SERVO2DEG = 270.0 / (11500 - 3500);
 double first = (6800 - 3500) * SERVO2DEG;
-double grab = (3800 - 3500) * SERVO2DEG;
+double grab = (3600 - 3500) * SERVO2DEG;
 double pass = (5000 - 3500) * SERVO2DEG;
 
 void cal()
 {
     EC.calOmega();
 }
+
 void shot();
 void catch_ball();
+void show_angle();
 
 int X_count=0;
 void Xcount()
@@ -119,15 +121,19 @@
         shoot = 1;
         led1 = 1;
     } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
-        shoot = 0;
+        shoot = 4;
         led2 = 1;
     } else if((ButtonState >> BUTTONTRIANGEL)&1 == 1) {//△
         shoot = 3;
         led3 = 1;
     } else if((ButtonState >> BUTTONSQUARE)&1 == 1) {//□
-        shoot = 4;
+        shoot = 2;
+        led4 = 1;
+    } else if((ButtonState >> BUTTONR2)&1 == 1) {//R2
+        shoot = 5;
         led4 = 1;
     }
+    show_angle();
     //CAN通信用プログラム
     if(shoot==1)shot();
     if(shoot>=2)catch_ball();
@@ -136,6 +142,31 @@
     printf("%d\r\n",can[0]);
 }
 
+void show_angle()
+{
+    X.rise(&Xcount);
+    f.period_us(50);
+    b.period_us(50);
+    double a=0,r=0.4,v=0;
+    int i=0,count;//rで半径指定 a*r=v
+    //ticker.attach(&cal,0.05);
+    if(X_count ==1) {
+        printf("set ok");
+        EC.reset();
+    }
+    //角度リセット
+    a=EC.getOmega();
+    count=EC.getCount();
+    i =count%500;
+    printf("%.3f %.3f %d %d stop\r\n",a,v,X_count,count);
+    f=0;
+    b=0;
+    led1=0;
+    led2=1;
+    led3=1;
+    led4=0;
+
+}
 void shot()
 {
     X.rise(&Xcount);
@@ -144,10 +175,13 @@
     double a=0,r=0.4,v=0;
     int i=0,count;//rで半径指定 a*r=v
     ticker.attach(&cal,0.05);
-    printf("set");
-    if(X_count ==1) {
-        EC.reset();
-        X_count =0;
+    while(1) {
+        printf("set");
+        if(X_count ==1) {
+            EC.reset();
+            X_count =0;
+        }
+        break;
     }
     //角度リセット
     while(1) {
@@ -192,17 +226,42 @@
 {
     switch (shoot) {
         case 2:
-        servo.set_degree(id, pass);
-        break;
+            if(ball ==0) {
+                servo.set_degree(id, first);
+                wait(0.5);
+                shoot =0;
+                ball = 1;
+            } else if(ball ==1) {
+                servo.set_degree(id, grab);
+                wait(0.5);
+                shoot = 0;
+                ball =0;
+            }
+            break;
         case 3:
-        servo.set_degree(id, grab);
-        break;
+            f=0.01;
+            wait (0.1);
+            f=0;
+            shoot = 0;
+            break;
         case 4:
-        if (out1==0){
-            out1=1;
-        }else if (out1 == 1){
-            out1=0;
-        }
+            if (out1==0) {
+                out1=1;
+                shoot=0;
+            } else if (out1 == 1) {
+                out1=0;
+                shoot=0;
+            }
+            break;
+        case 5:
+            if (out2==0) {
+                out2=1;
+                shoot=0;
+            } else if (out2 == 1) {
+                out2=0;
+                shoot=0;
+            }
+
     }
 }