Diff: main.cpp

Revision:

9:337fe0747940

Parent:

8:4bdfac5b52e1

--- a/main.cpp	Tue Mar 05 04:29:49 2019 +0000
+++ b/main.cpp	Wed Mar 06 03:51:08 2019 +0000
@@ -41,18 +41,19 @@
 #ifdef HARUROBO_TEST_MODE
     T1 = 0;
     printf("start\n\r");
-    printf("T1 = %d\n\r",T1);
+    //printf("T1 = %d\n\r",T1);
     if(T1 == 0) {
 
         //スタート位置からみかんの木まで移動
-        printf("t = 0");
+        printf("T1 = 0\n\r");
         T1++;
         //printf("t = %d\n\r",T1);
     }
 
     if(T1 == 1) {
         while(1) {
-            //printf("wait\n\r");
+            printf("T1 = 1\n\r");
+            wait(0.5);
             if(T1 == 2) {
                 break;
             }
@@ -62,13 +63,15 @@
     if(T1 == 2) {
 
         //みかんの木から三宝置き場まで移動
-        printf("t = 2");
+        printf("T1 = 2\n\r");
         T1++;
     }
 
     if(T1 == 3) {
         while(1) {
+            printf("T1 = 3\n\r");
             if(T1 == 4) {
+                
                 break;
             }
         }
@@ -118,19 +121,20 @@
                         if(go_waitmode == 0) {
 
 
-                            
+
                             //can_start();
                             gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0);
                             purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
-                            purecurve(8,1,1,-2317,2500,-1610,2000,9,1000,5,0.1,10,0.1,600,0);
-                            gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0);
+                            purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0);
+                            gogo_straight(1,1,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0);
                             MaxonControl(0,0,0,0);
-                            set_cond(2,0,-1243,1,800);
-                            pos_correction(-1610,1250,0,1,0);
+                            set_cond(2,0,-1243,1,1076);
+                            pos_correction(-1672,1400,0,0,0);
+                            enc_correction(1,1);
                             wait(0.5);
 
-                            gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0);
-                            purecurve(3,1,1,-1610,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
+                            gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0);
+                            purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0);
                             purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
                             gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90);
                             gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90);
@@ -140,7 +144,8 @@
                             set_cond(0,0,-2462,0,0);
                             gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
                             MaxonControl(0,0,0,0);
-                            pos_correction(-2850,4500,-90,1,1);
+                            pos_correction(-2850,4500,-90,0,1);
+                            enc_correction(1,0);
                             wait(0.5);
 
                             gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90);
@@ -151,19 +156,20 @@
                             set_cond(2,1,-3500,1,6324);
                             gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
                             MaxonControl(0,0,0,0);
-                            pos_correction(-2700,6647,-90,1,1);
+                            pos_correction(-2700,6647,-90,0,0);
+                            enc_correction(1,1);
                             wait(0.5);
 
                             gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90);
                             gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90);
-                            purecurve(8,1,1,-1000,6647,-350,6000,9,1000,5,0.1,10,0.1,600,-180);
-                            gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180);
+                            purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180);
+                            gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180);
                             set_cond(2,1,1243,1,4000);
-                            gogo_straight(0,0,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
+                            gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180);
                             MaxonControl(0,0,0,0);
-                            pos_correction(-400,4500,-180,1,1);
+                            pos_correction(-519,4500,-180,0,0);
                             MaxonControl(0,0,0,0);
-                            
+
                             go_waitmode = 1;
 
                         } else if(go_waitmode == 1) {