Diff: main.cpp

Revision:

2:90b53995cb1f

Parent:

1:bc34fdc4e16b

--- a/main.cpp	Wed Dec 18 02:33:45 2019 +0000
+++ b/main.cpp	Fri Dec 20 05:52:05 2019 +0000
@@ -119,7 +119,7 @@
 
 
 #ifdef NHK2020_TEST_MODE
-
+        printf("T2 = %d\n\r",T2);
         pc.printf("%lf",EC_backdrop.getRad());
         if(pc.readable()) {
             char sel=pc.getc();
@@ -173,51 +173,54 @@
     can1.frequency(1000000);
     can_ticker.attach(&can_read,0.01);
     printf("wait_mode\r\n");
-    while(1) {
-        char sel=pc.getc();
-        if(sel=='1') {
-            snatch=0;
-            printf("snatch_off\r\n");
-        }
-        if(sel=='2') {
-            snatch=1;
-            printf("snatch_on\r\n");
-        }
-        if(sel=='3') {
-            pass1=0;
-            printf("pass1_off\r\n");
-        }
-        if(sel=='4') {
-            pass1=1;
-            printf("pass1_on\r\n");
-        }
-        if(sel=='5') {
-            pass2=0;
-            printf("pass2_off\r\n");
-        }
-        if(sel=='6') {
-            pass2=1;
-            printf("pass2_on\r\n");
-        }
-        if(sel=='q') {
-            printf("end_wait_mode\r\n");
-            break;
+    //printf("T2 = %d\n\r",T2);
+    if(T2 == 0) {
+        while(1) {
+            char sel=pc.getc();
+            if(sel=='1') {
+                snatch=0;
+                printf("snatch_off\r\n");
+            }
+            if(sel=='2') {
+                snatch=1;
+                printf("snatch_on\r\n");
+            }
+            if(sel=='3') {
+                pass1=0;
+                printf("pass1_off\r\n");
+            }
+            if(sel=='4') {
+                pass1=1;
+                printf("pass1_on\r\n");
+            }
+            if(sel=='5') {
+                pass2=0;
+                printf("pass2_off\r\n");
+            }
+            if(sel=='6') {
+                pass2=1;
+                printf("pass2_on\r\n");
+            }
+            if(sel=='q') {
+                printf("end_wait_mode\r\n");
+                T2=1;
+                break;
 
+            }
         }
     }
-
-    if(T2 == 0) {  //スタート位置からボール前まで移動（アーム待機）
-        T2=1;
+    if(T2 == 1) {  //スタート位置からボール前まで移動（アーム待機）
+        //T2=1;
         while(1) {
-            //printf("a-T = 0  T2 = %d  t2_r = %d\n\r",T2,t2_r);
+            printf("a-T = 0  T2 = %d  t2_r = %d\n\r",T2,t2_r);
             wait(0.1);
-            if(T2 == 1) {
+            if(T2 == 2) {
                 break;
             }
         }
     }
 
-    if(T2 == 1) {  //ボール掴んで投げる
+    if(T2 == 2) {  //ボール掴んで投げる
         //printf("a-T = 1  T2 = %d  t2_r = %d\n\r",T2,t2_r);
         snatch=1;
         tsukami();
@@ -242,11 +245,11 @@
 
         T2++;
     }
-    if(T2 == 2) {  //スタートゾーンに戻る（アーム待機）
+    if(T2 == 3) {  //スタートゾーンに戻る（アーム待機）
         while(1) {
-            //printf("a-T = 2  T2 = %d  t2_r = %d\n\r",T2,t2_r);
+            printf("a-T = 2  T2 = %d  t2_r = %d\n\r",T2,t2_r);
             wait(0.1);
-            if(T2 == 1) {
+            if(T2 == 4) {
                 break;
             }
         }