Diff: main.cpp

Revision:

8:4bdfac5b52e1

Parent:

7:44ce34007499

Child:

9:337fe0747940

--- a/main.cpp	Sat Mar 02 08:27:05 2019 +0000
+++ b/main.cpp	Tue Mar 05 04:29:49 2019 +0000
@@ -12,6 +12,8 @@
 #define PI 3.141592
 
 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
+#define HARUROBO_TEST_MODE
+//#define HARUROBO_MAIN_MODE
 
 int go_waitmode = 0;
 
@@ -36,12 +38,74 @@
 
 #endif
 
+#ifdef HARUROBO_TEST_MODE
+    T1 = 0;
+    printf("start\n\r");
+    printf("T1 = %d\n\r",T1);
+    if(T1 == 0) {
+
+        //スタート位置からみかんの木まで移動
+        printf("t = 0");
+        T1++;
+        //printf("t = %d\n\r",T1);
+    }
+
+    if(T1 == 1) {
+        while(1) {
+            //printf("wait\n\r");
+            if(T1 == 2) {
+                break;
+            }
+        }
+    }
+
+    if(T1 == 2) {
+
+        //みかんの木から三宝置き場まで移動
+        printf("t = 2");
+        T1++;
+    }
+
+    if(T1 == 3) {
+        while(1) {
+            if(T1 == 4) {
+                break;
+            }
+        }
+    }
+
+    if(T1 == 4) {
+
+        //三宝置き場からりんごの木まで移動
+        printf("t = 4");
+        T1++;
+    }
+
+    if(T1 == 5) {
+        while(1) {
+            if(T1 == 6) {
+                break;
+            }
+        }
+    }
+
+    if(T1 == 6) {
+
+        //りんごの木からお供え台まで移動
+        printf("t = 6");
+        T1++;
+    }
+#endif
+
+#ifdef HARUROBO_MAIN_MODE
     while(1) {
 
+        id1_value[0] = 1;
         switch(id1_value[0]) {
 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
             case 1:
 
+                id1_value[6] = 1;
                 switch(id1_value[6]) {
                     case 0:
                         //-----right mode-------------------------------------------------------------------------------------------------------------//
@@ -50,16 +114,18 @@
                     case 1:
                         //-----left mode--------------------------------------------------------------------------------------------------------------//
 
+                        go_waitmode = 0;
                         if(go_waitmode == 0) {
 
+
+                            
                             //can_start();
-                            //set_cond(2,1,-700,1,-700);
                             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);
                             MaxonControl(0,0,0,0);
-                            set_cond(2,0,-1243,1,1080);
+                            set_cond(2,0,-1243,1,800);
                             pos_correction(-1610,1250,0,1,0);
                             wait(0.5);
 
@@ -72,7 +138,7 @@
                             pos_correction(-2962,4500,-90,1,1);
 
                             set_cond(0,0,-2462,0,0);
-                            gogo_straight(1,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90);
+                            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);
                             wait(0.5);
@@ -83,7 +149,7 @@
                             purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90);
                             gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90);
                             set_cond(2,1,-3500,1,6324);
-                            gogo_straight(1,1,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90);
+                            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);
                             wait(0.5);
@@ -93,10 +159,11 @@
                             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);
                             set_cond(2,1,1243,1,4000);
-                            gogo_straight(1,1,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
+                            gogo_straight(0,0,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180);
                             MaxonControl(0,0,0,0);
                             pos_correction(-400,4500,-180,1,1);
-
+                            MaxonControl(0,0,0,0);
+                            
                             go_waitmode = 1;
 
                         } else if(go_waitmode == 1) {
@@ -125,4 +192,6 @@
         }
 //------------------------------------------------------------------------------------------------------------------------------------//
     }
-}
\ No newline at end of file
+#endif
+}
+