Diff: System/Process/Process.cpp

Revision:

28:9cd36721cf83

Parent:

27:545148741493

Child:

29:c78a46f51209

--- a/System/Process/Process.cpp	Wed Sep 18 03:07:18 2019 +0000
+++ b/System/Process/Process.cpp	Wed Sep 18 11:36:53 2019 +0000
@@ -274,13 +274,8 @@
 static void Process0()
 {
     if(StertSW) {
-        if(ZoneSW) {
-            current = 1;
-            Twsh=2;
-        } else {
-            current = 1;
-            Twsh=3;
-        }
+        current = 2;
+        Twsh=3;
     }
 }
 #endif
@@ -304,7 +299,7 @@
 static void Process1()
 {
     count=100000;
-    mode=1;
+    mode=0;
     cross=0;
     motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
     motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
@@ -417,6 +412,15 @@
 {
 //    printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
     count++;
+    if(mode==0&&StertSW) {
+        if(ThSW) {
+            Twsh=1;
+            mode=1;
+        } else {
+            Twsh=2;
+            mode=1;
+        }
+    }
     if(mode==1) {//スタートゾーンから白線検知
         motor[TIRE_FR].pwm = 30;
         motor[TIRE_FR].dir = BACK;
@@ -490,7 +494,7 @@
                     count=0;
                 }
                 mtc=true;
-                if(cross==1) {
+                if(cross==Twsh) {
                     mode=11;
                     count=0;
                 }
@@ -644,7 +648,7 @@
                 motor[TIRE_BR].dir = BACK;
                 motor[TIRE_BL].pwm = 30;
                 motor[TIRE_BL].dir = BACK;
-                if(count>50000) {
+                if(count>70000) {
                     cross++;
                     count=0;
                 }
@@ -775,7 +779,7 @@
         motor[TIRE_BR].dir = BACK;
         motor[TIRE_BL].pwm = 0;
         motor[TIRE_BL].dir = FREE;
-        if(count>5000&&g[0]==0) {
+        if(count>2000&&g[0]==0) {
             count=0;
             mode=22;
         }
@@ -788,7 +792,7 @@
         motor[TIRE_BR].dir = FOR;
         motor[TIRE_BL].pwm = 15;
         motor[TIRE_BL].dir = FOR;
-        if(count>10000&&g[0]==0) {
+        if(count>5000&&g[0]==0) {
             count=0;
             cross=0;
             mode=30;
@@ -1431,6 +1435,11 @@
         }
     } else if(mode==70) {
         switch(g[1]) {
+            case 99:
+                if(count>=100000) {
+                    mode=100;
+                }
+                break;
             case 98:
                 motor[TIRE_FR].pwm = 30;
                 motor[TIRE_FR].dir = FOR;
@@ -1440,15 +1449,6 @@
                 motor[TIRE_BR].dir = FOR;
                 motor[TIRE_BL].pwm = 30;
                 motor[TIRE_BL].dir = BACK;
-                if(count>100000) {
-                    cross++;
-                    count=0;
-                }
-                mtc=true;
-                if(cross==1) {
-                    mode=71;
-                    count=0;
-                }
                 break;
             case 0:
                 motor[TIRE_FR].pwm = 60;
@@ -1561,55 +1561,15 @@
                     break;
             }
         }
-    } else if(mode==71) {
-        motor[TIRE_FR].pwm = 15;
+    } else if(mode==100) {
+        motor[TIRE_FR].pwm = 20;
         motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 15;
+        motor[TIRE_FL].pwm = 20;
         motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 15;
+        motor[TIRE_BR].pwm = 23;
         motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 15;
+        motor[TIRE_BL].pwm = 20;
         motor[TIRE_BL].dir = BACK;
-        if(count>5000&&g[3]==0) {
-            count=0;
-            mode=72;
-        }
-    } else if(mode==72) {
-        motor[TIRE_FR].pwm = 15;
-        motor[TIRE_FR].dir = BACK;
-        motor[TIRE_FL].pwm = 15;
-        motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = 15;
-        motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = 15;
-        motor[TIRE_BL].dir = FOR;
-        count++;
-        if(count>50000&&g[3]==0) {
-            count=0;
-            cross=0;
-            mode=80;
-        }
-    } else if(mode==80) {
-        if(count>=400000) {
-            mode=100;
-        }
-        motor[TIRE_FR].pwm = 40;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 40;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 40;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 40;
-        motor[TIRE_BL].dir = BACK;
-    } else if(mode==100) {
-        motor[TIRE_FR].pwm = 0;
-        motor[TIRE_FR].dir = BRAKE;
-        motor[TIRE_FL].pwm = 0;
-        motor[TIRE_FL].dir = BRAKE;
-        motor[TIRE_BR].pwm = 0;
-        motor[TIRE_BR].dir = BRAKE;
-        motor[TIRE_BL].pwm = 0;
-        motor[TIRE_BL].dir = BRAKE;
     } else {
         motor[TIRE_FR].pwm = 0;
         motor[TIRE_FR].dir = BRAKE;