version which is easier to read

Dependencies:   mbed KondoServoLibrary Encoder

Files at this revision

API Documentation at this revision

Comitter:
maxnagazumi
Date:
Sat Mar 21 03:15:26 2020 +0000
Parent:
25:4eb2f2795120
Commit message:
aa

Changed in this revision

User.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 4eb2f2795120 -r 3adbabf7d770 User.cpp
--- a/User.cpp	Thu Mar 19 08:35:23 2020 +0000
+++ b/User.cpp	Sat Mar 21 03:15:26 2020 +0000
@@ -41,8 +41,8 @@
 int id2 = 1;//box
 double SERVO2DEG = 270.0 / (11500 - 3500);
 double first_ball = (4500 - 3500) * SERVO2DEG;
-double first_box = (4000 - 3500) * SERVO2DEG;  //X haven't be decided
-double grab_ball = (3830 - 3500) * SERVO2DEG;
+double first_box = (6000 - 3500) * SERVO2DEG;  //X haven't be decided
+double grab_ball = (3850 - 3500) * SERVO2DEG;
 double pass = (7080 - 3500) * SERVO2DEG;
 double grab_box = (3500 - 3500) * SERVO2DEG;
 
@@ -51,8 +51,9 @@
     EC.calOmega();
 }
 
-void shot();
+void ballShot(int data);
 void catch_ball();
+void catch_box();
 void show_angle();
 
 int X_count=0;
@@ -66,12 +67,14 @@
     f.period_us(50);
     b.period_us(50);
     touchSensor.mode(PullUp);
+    X.rise(&Xcount);
+    out1=1;
 }
 void UserLoop(char n,const u8* data)
 {
     static int i=0;
     if(i==0) {
-        wait(2);
+        wait(1);
         i++;
     }
     u16 ButtonState;
@@ -101,8 +104,10 @@
     led3=0;
     led4=0;
     //reset
+
     if((ButtonState >> BUTTONUP)&1 == 1 &&(ButtonState >> BUTTONTRIANGEL)&1 == 1 ) {//R2
         NVIC_SystemReset();
+        move =14;
         printf("reset\r\n");
     }
 
@@ -140,11 +145,19 @@
     }
     //move
     //shoot
-    if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
-        shoot = 1;
-        printf("shoot 1\r\n");
+    if((ButtonState >> BUTTONCIRCLE)&1 == 1 && (ButtonState >> BUTTONLEFT)&1 == 1 ) {//O & left shot reset
+        shoot =0;
+        printf("shoot 0\r\n");
+        led2 = 1;
+    } else if((ButtonState >> BUTTONCIRCLE)&1 == 1) {//O
+        shoot = shoot +1;
+        if(shoot > 3) {
+            shoot =0;
+        }
+        printf("shoot %d\r\n",shoot);
+        wait(0.5);
         led1 = 1;
-    } else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
+    }  else if((ButtonState >> BUTTONCROSS)&1 == 1) {//X
         ball = 1;
         printf("ball 1\r\n");
         led2 = 1;
@@ -162,131 +175,105 @@
         led4 = 1;
     }
 
-    show_angle();
     //CAN通信用プログラム
-    if(shoot==1)shot();
+    ballShot(shoot);
     catch_ball();
+    catch_box();
     can[0] = move;
     controller.write(CANMessage(1,can,1));
     printf("%d\r\n",can[0]);
 }
 
-void show_angle()
+
+void ballShot(int data)
 {
+    static int breaknum=0;
     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()
-{
-    int breaknum=0;
-    X.rise(&Xcount);
-    double a=0,r=0.4,v=0;
     int i=0,count;//rで半径指定 a*r=v
     ticker.attach(&cal,0.05);
-    while(1) {
-        out1=1;
-        printf("%d\r\n",touchSensor.read());
-        f=0;
-        b=0.05;
-        if(touchSensor==0) {//受け渡し
-            f=0;
-            b=0;
-            printf("%d",EC.getCount());
-            EC.reset();
-            printf("%d",EC.getCount());
-            wait(1);
-            out2=0;
-            out2=1;
-            printf("passed\r\n");
-            wait(1);
-            while(1) {
-                f=0.1;
-                b=0;
-                printf("moving to remove servo\r\n");
-                if(EC.getCount()>200) {
-                    printf("servo move\r\n");
+    switch(data) {
+        case 1:
+            if(breaknum==0) {
+                out2=1;
+                out1=1;
+                printf("%d\r\n",touchSensor.read());
+                printf("%d\r\n",EC.getCount());
+                f=0;
+                b=0.09;
+                if(touchSensor==0) {//受け渡し
                     f=0;
                     b=0;
-                    servo.set_degree(id, first_ball);
+                    printf("%d",EC.getCount());
+                    EC.reset();
+                    printf("%d",EC.getCount());
                     breaknum=1;
-                    break;
                 }
-
-            }
-            if(X_count ==1) {
-                EC.reset();
-                X_count =0;
             }
-        }
-        if(breaknum==1)break;
-    }
-    EC.reset();
-    wait(1);
-    X_count=0;
-    double m=0;
-    //角度リセット
-    while(1) {
-        //printf("%.3f %d %d\r\n",v,X_count,i);
-        //a=EC.getOmega();
-        count=EC.getCount();
-        i =count%500;
-        //v=a*r;
-        if(m<1.0) {
-            m=m+0.05;
-        }
-        if(m>1) {
-            m=1;
-        }
-        f=0.1*m;//速度一定
-        b=0.0;
-        if(X_count>2) {
-            if(i > 345 && i < 355) {//離す角度を決める
-                out1=1; //ボールを離す
+            break;
+
+        case 2:
+            if(breaknum ==1) {
+                out1=1;
+                wait(1);
+                out2=0;
+                wait(0.5);
+                breaknum++;
+            }
+            f=0.1;
+            b=0;
+            printf("moving to remove servo\r\n");
+            if(EC.getCount()>200) {
+                printf("servo move\r\n");
                 f=0;
                 b=0;
-                wait(1);
-                printf("shot\r\n");
-                led1=1;
-                led2=1;
-                led3=1;
-                led4=1;
+                servo.set_degree(id, first_ball);
+                EC.reset();
                 break;
             }
-        }
+            break;
+        case 3:
+            X_count=0;
+            double m=0;
+            while(1) {
+                count=EC.getCount();
+                i =count%500;
+                if(m<1.0) {
+                    m=m+0.05;
+                }
+                if(m>1) {
+                    m=1;
+                }
+                f=0.35*m;//速度一定
+                b=0.0;
+                if(X_count>2) {
+                    if(i > 345 && i < 355) {//離す角度を決める
+                        out1=1; //ボールを離す
+                        f=0;
+                        b=0;
+                        printf("shot\r\n");
+                        led1=1;
+                        led2=1;
+                        led3=1;
+                        led4=1;
+                        break;
+                    }
+                }
+            }
+            X.rise(&Xcount);
+            count=EC.getCount();
+            i =count%500;
+            f=0;
+            b=0;
+            led1=0;
+            led2=1;
+            led3=1;
+            led4=0;
+            shoot=0;
+            breaknum=0;
+            break;
     }
-    printf("%.3f %.3f %d\r\n",a,v,i);
-    X.rise(&Xcount);
-    a=EC.getOmega();
-    count=EC.getCount();
-    i =count%500;
-    f=0;
-    b=0;
-    led1=0;
-    led2=1;
-    led3=1;
-    led4=0;
-    shoot=0;
 }
+
 void catch_ball()
 {
     switch (ball) {
@@ -315,9 +302,11 @@
             if (out1==0) {
                 out1=1;
                 ball=0;
+                wait(0.5);
             } else if (out1 == 1) {
                 out1=0;
                 ball=0;
+                wait(0.5);
             }
             break;
     }
@@ -329,13 +318,13 @@
         case 1:// box servo
             if(box_count ==0) {
                 printf("Box first");
-                servo.set_degree(id2, first_box);
+                servo.set_degree(id2,first_box);
                 wait(0.5);
                 box_count =1;
                 box = 0;
             } else if(box_count ==1) {
                 printf("Box grab");
-                servo.set_degree(id2, grab_box);
+                servo.set_degree(id2,grab_box);
                 wait(0.5);
                 box_count = 0;
                 box =0;
@@ -345,9 +334,11 @@
             if (out3==0) {
                 out3=1;
                 box = 0;
+                wait(0.5);
             } else if (out3 == 1) {
                 out3=0;
                 box = 0;
+                wait(0.5);
             }
             break;
     }