Diff: main.cpp

Revision:

1:bc34fdc4e16b

Parent:

0:6e2abd0956f1

diff -r 6e2abd0956f1 -r bc34fdc4e16b main.cpp
--- a/main.cpp	Sat Dec 14 12:42:13 2019 +0000
+++ b/main.cpp	Wed Dec 18 02:33:45 2019 +0000
@@ -37,32 +37,34 @@
 {
     b=0;
 }
-void move()
+void move_motor()
 {
-    double old_turn=turn;
-    a=EC_backdrop.getRad();
-    if(a-b>=0.1) {
-        turn=0.1;
-        //pc.printf("F");
-
-    } else if (a-b>=0.05) {
-        turn=10*(a-b)*(a-b);
-        //pc.printf("f");
-    } else if (b-a>=0.1) {
-        turn=-0.1;
-        //pc.printf("B");
-    } else if (b-a>=0.05) {
-        turn=-10*(a-b)*(a-b);
-        //pc.printf("b");
-    } else {
-        backdrop.stop();
-        backdrop.turn(0);
-        turn=0;
-        //break;
-        //pc.printf("s");
+    while(1) {
+        double old_turn=turn;
+        a=EC_backdrop.getRad();
+        if(a-b>=0.1) {
+            turn=0.1;
+            pc.printf("F");
+        } else if (a-b>=0.05) {
+            turn=10*(a-b)*(a-b);
+            pc.printf("f");
+        } else if (b-a>=0.1) {
+            turn=-0.1;
+            pc.printf("B");
+        } else if (b-a>=0.05) {
+            turn=-10*(a-b)*(a-b);
+            pc.printf("b");
+        } else {
+            backdrop.stop();
+            backdrop.turn(0);
+            turn=0;
+            pc.printf("s");
+        }
+        if(turn*old_turn<0)turn=0;
+        backdrop.turn(turn);
+        pc.printf("%lf",EC_backdrop.getRad());
+        if(b-a<0.05&&a-b<0.05)break;
     }
-    if(turn*old_turn<0)turn=0;
-    backdrop.turn(turn);
 }
 
 int q = 0;
@@ -109,9 +111,8 @@
 
 int main()
 {
-    move();
-
     pc.printf("setting please");
+    move_motor();
     while(1) {
 
 
@@ -218,16 +219,22 @@
 
     if(T2 == 1) {  //ボール掴んで投げる
         //printf("a-T = 1  T2 = %d  t2_r = %d\n\r",T2,t2_r);
+        snatch=1;
         tsukami();
-        move();
+        printf("tsukami");
+        move_motor();
         wait(3);
         snatch=0;
         wait(1);
         put();
+        printf("put");
+        move_motor();
         wait(3);
         snatch=1;
         wait(1);
         top();
+        printf("top");
+        move_motor();
         wait(5);
         pass1=0;
         wait(5);