2019_Master_10_2_last - aa

Users » yabahiro » Code » 2019_Master_10_2_last
大季矢花 / Mbed 2 deprecated 2019_Master_10_2_last
Dependencies: mbed TrapezoidControl QEI
Files at this revision
API Documentation at this revision
Revision 44:c208a908583c, committed 2019-10-03

Comitter:: yabahiro
Date:: Thu Oct 03 06:22:38 2019 +0000
Parent:: 43:677f7446612e
Commit message:: aa;
Changed in this revision

System/Process/Process.cpp
Show annotated file Show diff for this revision Revisions of this file
--- a/System/Process/Process.cpp	Wed Oct 02 01:53:59 2019 +0000
+++ b/System/Process/Process.cpp	Thu Oct 03 06:22:38 2019 +0000
@@ -154,6 +154,7 @@
 bool mtc=false;
 bool mtc2 = false;
 bool emcs=false;
+bool kiri=true;
 
 bool start_flag = true;
 
@@ -338,7 +339,7 @@
             lock = false;
             if(ZoneSW) {
                 mode=1;
-                current = 2;
+                current = 6;
             } else {
                 mode=1;
                 current = 4;
@@ -620,198 +621,7 @@
 //   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]);
     LedMode(3);
     count++;
-    if(mode==1) {//スタートゾーンから白線検知
-        fast=40;
-        normal=30;
-        slow=20;
-        motor[TIRE_FR].pwm = normal;
-        motor[TIRE_FR].dir = BACK;
-        motor[TIRE_FL].pwm = normal;
-        motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = normal;
-        motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = normal;
-        motor[TIRE_BL].dir = FOR;
-        if(g[2]==0) {
-            count=100000;
-            cross=0;
-            mode=3;
-        }
-    } else if(mode==3) { //横ライントレースから縦ライントレースへ
-        motor[TIRE_FR].pwm = 0;
-        motor[TIRE_FR].dir = FREE;
-        motor[TIRE_FL].pwm = 25;
-        motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = 25;
-        motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = 0;
-        motor[TIRE_BL].dir = FREE;
-        if(g[0]==0) {
-            mode=10;
-            count=100000;
-        }
-    } else if(mode==10) { //縦ライントレース
-
-        if(counts==false&&g[2]==0) {
-            cros++;
-            counts=true;
-        }
-        if(counts==true&&g[2]==99) {
-            counts=false;
-        }
-
-        if(cros==Twsh) {
-            mode=11;
-            count=0;
-            cros=0;
-        }//ゆっくりモードに入れなかった時の保険
-        switch(g[0]) {
-            case 98:
-                motor[TIRE_FR].pwm = normal;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = normal;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = normal;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = normal;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                if(count>80000) {
-                    cross++;
-                    count=0;
-                }
-                if(cross==Twsh) {
-                    mode=11;
-                    count=0;
-                    cross=0;
-                }
-                break;
-            case 0:
-                motor[TIRE_FR].pwm = fast;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = fast;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = fast;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = fast;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = fast;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = slow;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = slow;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = fast;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = slow;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = FREE;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FREE;
-                motor[TIRE_BL].pwm = slow;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 254:
-                motor[TIRE_FR].pwm = normal;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = FREE;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FREE;
-                motor[TIRE_BL].pwm = normal;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = slow;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = fast;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = fast;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = slow;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 3:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = slow;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = slow;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc=true;
-                break;
-            case 2:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = normal;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = normal;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 0;
-                motor[TIRE_BL].dir = FREE;
-                mtc=true;
-                break;
-        }
-        if(g[0]!=98&&mtc==true) {
-            switch(g[1]) {
-                case 0:
-                    mtc=false;
-                    break;
-                case 255:
-                    motor[TIRE_BR].pwm += 5;
-                    mtc=false;
-                    break;
-                case 253:
-                    motor[TIRE_BR].pwm += 10;
-                    mtc=false;
-                    break;
-                case 254:
-                    motor[TIRE_BR].pwm += 20;
-                    motor[TIRE_BL].pwm = 0;
-                    mtc=false;
-                    break;
-                case 1:
-                    motor[TIRE_BL].pwm += 5;
-                    mtc=false;
-                    break;
-                case 3:
-                    motor[TIRE_BL].pwm += 10;
-                    mtc=false;
-                    break;
-                case 2:
-                    motor[TIRE_BL].pwm += 20;
-                    motor[TIRE_BR].pwm = 0;
-                    mtc=false;
-                    break;
-            }
-        }
-    } else if(mode==11) {
-        motor[TIRE_FR].pwm = 0;
-        motor[TIRE_FR].dir = FREE;
-        motor[TIRE_FL].pwm = normal;
-        motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = normal;
-        motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = 0;
-        motor[TIRE_BL].dir = FREE;
-        if(g[2]==0) {
-            count=0;
-            cross=0;
-            mode=20;
-        }
-    } else if(mode==20) {
+    if(mode==0||mode==1) {
 
         if(Twsh==2) {
             solenoid.solenoid2 = SOLENOID_ON;
@@ -830,7 +640,11 @@
             // count=0;
             // cros=0;
         }//ゆっくりモードに入れなかった時の保険
-
+        if(g[0]==0) {
+            fast=35;
+            normal=20;
+            slow=15;
+        }
         switch(g[2]) {
             case 98:
                 motor[TIRE_FR].pwm = normal;
@@ -841,7 +655,7 @@
                 motor[TIRE_BR].dir = BACK;
                 motor[TIRE_BL].pwm = normal;
                 motor[TIRE_BL].dir = BACK;
-                if(count>50000) {
+                if(count>30000) {
                     cross++;
                     count=0;
                 }
@@ -850,6 +664,9 @@
                     mode=22;
                     count=0;
                     cross=0;
+                    fast=40;
+                    normal=30;
+                    slow=20;
                 }
                 break;
             case 0:
@@ -977,17 +794,17 @@
             count=0;
         }
     }
-    if(YELLOW) {
+    if(YELLOW==true) {
         if(mode==22) {
-            motor[TIRE_FR].pwm = 10;
+            motor[TIRE_FR].pwm = 100;
             motor[TIRE_FR].dir = BRAKE;
-            motor[TIRE_FL].pwm = 10;
+            motor[TIRE_FL].pwm = 100;
             motor[TIRE_FL].dir = BRAKE;
-            motor[TIRE_BR].pwm = 10;
+            motor[TIRE_BR].pwm = 100;
             motor[TIRE_BR].dir = BRAKE;
-            motor[TIRE_BL].pwm = 10;
+            motor[TIRE_BL].pwm = 100;
             motor[TIRE_BL].dir = BRAKE;
-            if(count<=100000) {
+            if(count>=10000) {
                 mode=23;
             }
         } else if(mode==23) {
@@ -999,11 +816,24 @@
             motor[TIRE_BR].dir = FOR;
             motor[TIRE_BL].pwm = 20;
             motor[TIRE_BL].dir = FOR;
+            if(g[2]==98) {
+                mode=24;
+                count=0;
+            }
+        } else if(mode==24) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
             if(g[2]!=98) {
                 count=0;
-                mode=24;
+                mode=25;
             }
-        } else if(mode==24) {
+        } else if(mode==25) {
             motor[TIRE_FR].pwm = 10;
             motor[TIRE_FR].dir = BRAKE;
             motor[TIRE_FL].pwm = 10;
@@ -1012,19 +842,35 @@
             motor[TIRE_BR].dir = BRAKE;
             motor[TIRE_BL].pwm = 10;
             motor[TIRE_BL].dir = BRAKE;
-            if(count<=100000) {
-                mode=25;
+            if(count>=10000) {
+                mode=26;
+                count=0;
             }
-        } else if(mode==25) {
-            motor[TIRE_FR].pwm = 15;
+        } else if(mode==26) {
+            motor[TIRE_FR].pwm = 20;
             motor[TIRE_FR].dir = FOR;
-            motor[TIRE_FL].pwm = 15;
+            motor[TIRE_FL].pwm = 20;
             motor[TIRE_FL].dir = FOR;
-            motor[TIRE_BR].pwm = 15;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = BACK;
+            if(count>=5000) {
+                mode=27;
+                count=0;
+            }
+        } else if(mode==27) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 20;
             motor[TIRE_BR].dir = FOR;
-            motor[TIRE_BL].pwm = 15;
+            motor[TIRE_BL].pwm = 20;
             motor[TIRE_BL].dir = FOR;
-            if(count<=100000) {
+            //どれくらい回転するかって値
+            if(count>=2500) {
+                count=0;
                 mode=30;
             }
         } else if(mode==24) {
@@ -1036,8 +882,9 @@
             motor[TIRE_BR].dir = BRAKE;
             motor[TIRE_BL].pwm = 10;
             motor[TIRE_BL].dir = BRAKE;
-            if(count<=100000) {
+            if(count>=10000) {
                 mode=30;
+                count=0;
             }
         }
     } else {
@@ -1057,13 +904,13 @@
         }
     }
     if(mode==30) {
-        motor[TIRE_FR].pwm = 60;
+        motor[TIRE_FR].pwm = 59;
         motor[TIRE_FR].dir = BACK;
-        motor[TIRE_FL].pwm = 60;
+        motor[TIRE_FL].pwm = 62;
         motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = 60;
+        motor[TIRE_BR].pwm = 59;
         motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = 60;
+        motor[TIRE_BL].pwm = 62;
         motor[TIRE_BL].dir = FOR;
         if(g[0]==98) {
             mode=31;
@@ -1096,13 +943,13 @@
             count=0;
         }
     } else if(mode==33) {
-        motor[TIRE_FR].pwm = 30;
+        motor[TIRE_FR].pwm = 0;
         motor[TIRE_FR].dir = BACK;
-        motor[TIRE_FL].pwm = 0;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 0;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 30;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
         motor[TIRE_BL].dir = FOR;
         if(g[3]==0) {
             count=0;
@@ -1144,7 +991,7 @@
                     count=0;
                 }
                 mtc2=true;
-                if(cross==1) {
+                if(cross==2) {
                     mode=41;
                     count=0;
                     cross=0;
@@ -2248,7 +2095,855 @@
 #if USE_PROCESS_NUM>6
 static void Process6()
 {
+//   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]);
+    LedMode(3);
+    count++;
+    if(mode==1) {//スタートゾーンから白線検知
+        fast=40;
+        normal=30;
+        slow=20;
+        motor[TIRE_FR].pwm = normal;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = normal;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = normal;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = normal;
+        motor[TIRE_BL].dir = FOR;
+        if(g[2]==0) {
+            count=100000;
+            cross=0;
+            mode=3;
+        }
+    } else if(mode==3) { //横ライントレースから縦ライントレースへ
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 25;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 25;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(g[0]==0) {
+            mode=10;
+            count=100000;
+        }
+    } else if(mode==10) { //縦ライントレース
+        if(counts==false&&g[2]==0) {
+            cros++;
+            counts=true;
+        }
+        if(counts==true&&g[2]==99) {
+            counts=false;
+        }
 
+        if(cros==Twsh) {
+            mode=11;
+            count=0;
+            cros=0;
+        }//ゆっくりモードに入れなかった時の保険
+        switch(g[0]) {
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                if(count>80000) {
+                    cross++;
+                    count=0;
+                }
+                if(cross==Twsh) {
+                    mode=11;
+                    count=0;
+                    cross=0;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+        }
+        if(g[0]!=98&&mtc==true) {
+            switch(g[1]) {
+                case 0:
+                    mtc=false;
+                    break;
+                case 255:
+                    motor[TIRE_BR].pwm += 5;
+                    mtc=false;
+                    break;
+                case 253:
+                    motor[TIRE_BR].pwm += 10;
+                    mtc=false;
+                    break;
+                case 254:
+                    motor[TIRE_BR].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc=false;
+                    break;
+                case 1:
+                    motor[TIRE_BL].pwm += 5;
+                    mtc=false;
+                    break;
+                case 3:
+                    motor[TIRE_BL].pwm += 10;
+                    mtc=false;
+                    break;
+                case 2:
+                    motor[TIRE_BL].pwm += 20;
+                    motor[TIRE_BR].pwm = 0;
+                    mtc=false;
+                    break;
+            }
+        }
+    } else if(mode==11) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = normal;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = normal;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(g[2]==0) {
+            count=0;
+            cross=0;
+            mode=20;
+        }
+    } else if(mode==20) {
+
+        if(Twsh==2) {
+            solenoid.solenoid2 = SOLENOID_ON;
+        }
+        if(counts==false&&g[0]==0) {
+            cros++;
+            counts=true;
+        }
+        if(counts==true&&g[0]==99) {
+            counts=false;
+        }
+
+        if(cros==2) {
+
+            // mode=21;
+            // count=0;
+            // cros=0;
+        }//ゆっくりモードに入れなかった時の保険
+        if(g[0]==0) {
+            fast=35;
+            normal=20;
+            slow=15;
+        }
+        switch(g[2]) {
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                if(count>30000) {
+                    cross++;
+                    count=0;
+                }
+                mtc2=true;
+                if(cross==2) {
+                    mode=22;
+                    count=0;
+                    cross=0;
+                    fast=40;
+                    normal=30;
+                    slow=20;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = FOR;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = BACK;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[3]) {
+                case 0:
+                    mtc2=false;
+                    break;
+                case 255:
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 253:
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 254:
+                    motor[TIRE_BL].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc2=false;
+                    break;
+                case 1:
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 3:
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 2:
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc2=false;
+                    break;
+            }
+        }
+    } else if(mode==21) {
+        motor[TIRE_FR].pwm = 30;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 30;
+        motor[TIRE_BL].dir = BACK;
+        if(g[2]==98&&count>=10000) {
+            mode=22;
+            count=0;
+        }
+    }
+    kiri=true;
+    if(kiri) {
+        if(mode==22) {
+            motor[TIRE_FR].pwm = 100;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 100;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 100;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 100;
+            motor[TIRE_BL].dir = BRAKE;
+            if(count>=10000) {
+                mode=23;
+            }
+        } else if(mode==23) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
+            if(g[2]==98) {
+                mode=24;
+                count=0;
+            }
+        } else if(mode==24) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = BACK;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = BACK;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
+            if(g[2]!=98) {
+                count=0;
+                mode=25;
+            }
+        } else if(mode==25) {
+            motor[TIRE_FR].pwm = 10;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 10;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 10;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 10;
+            motor[TIRE_BL].dir = BRAKE;
+            if(count>=10000) {
+                mode=26;
+                count=0;
+            }
+        } else if(mode==26) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = BACK;
+            if(count>=5000) {
+                mode=27;
+                count=0;
+            }
+        } else if(mode==27) {
+            motor[TIRE_FR].pwm = 20;
+            motor[TIRE_FR].dir = FOR;
+            motor[TIRE_FL].pwm = 20;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 20;
+            motor[TIRE_BR].dir = FOR;
+            motor[TIRE_BL].pwm = 20;
+            motor[TIRE_BL].dir = FOR;
+            //どれくらい回転するかって値
+            if(count>=3300) {
+                count=0;
+                mode=30;
+            }
+        } else if(mode==24) {
+            motor[TIRE_FR].pwm = 10;
+            motor[TIRE_FR].dir = BRAKE;
+            motor[TIRE_FL].pwm = 10;
+            motor[TIRE_FL].dir = BRAKE;
+            motor[TIRE_BR].pwm = 10;
+            motor[TIRE_BR].dir = BRAKE;
+            motor[TIRE_BL].pwm = 10;
+            motor[TIRE_BL].dir = BRAKE;
+            if(count>=10000) {
+                mode=30;
+                count=0;
+            }
+        }
+    } else {
+        if(mode==22) {
+            motor[TIRE_FR].pwm = 0;
+            motor[TIRE_FR].dir = FREE;
+            motor[TIRE_FL].pwm = 25;
+            motor[TIRE_FL].dir = FOR;
+            motor[TIRE_BR].pwm = 25;
+            motor[TIRE_BR].dir = BACK;
+            motor[TIRE_BL].pwm = 0;
+            motor[TIRE_BL].dir = FREE;
+            if(g[0]==0) {
+                mode=30;
+                cros=0;
+            }
+        }
+    }
+    if(mode==30) {
+        motor[TIRE_FR].pwm = 59;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 62;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 59;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 62;
+        motor[TIRE_BL].dir = FOR;
+        if(g[0]==98) {
+            mode=31;
+            count=0;
+        }
+    } else if(mode==31) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(count>=5000) {
+            count=0;
+            mode=32;
+        }
+    } else if(mode==32) {
+        motor[TIRE_FR].pwm = 10;
+        motor[TIRE_FR].dir = BRAKE;
+        motor[TIRE_FL].pwm = 10;
+        motor[TIRE_FL].dir = BRAKE;
+        motor[TIRE_BR].pwm = 10;
+        motor[TIRE_BR].dir = BRAKE;
+        motor[TIRE_BL].pwm = 10;
+        motor[TIRE_BL].dir = BRAKE;
+        if(count>=40000) {
+            mode=33;
+            count=0;
+        }
+    } else if(mode==33) {
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = BACK;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FOR;
+        if(g[3]==0) {
+            count=0;
+            if(Twsh==2) {
+                solenoid.solenoid2 = SOLENOID_OFF;
+            }
+            mode=40;
+        }
+    } else if(mode==40) {
+        if(LIB) {
+            mode=42;
+        }
+        if(counts==false&&g[0]==0) {
+            cros++;
+            counts=true;
+        }
+        if(counts==true&&g[0]==99) {
+            counts=false;
+        }
+
+        if(cros==1) {
+            // mode=41;
+            //count=0;
+            //cros=0;
+        }//ゆっくりモードに入れなかった時の保険
+
+        switch(g[3]) {
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                if(count>20000) {
+                    cross++;
+                    count=0;
+                }
+                mtc2=true;
+                if(cross==2) {
+                    mode=41;
+                    count=0;
+                    cross=0;
+                }
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc2=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = BACK;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = FOR;
+                mtc2=true;
+                break;
+        }
+        if(g[2]!=98&&mtc2==true) {
+            switch(g[2]) {
+                case 0:
+                    mtc2=false;
+                    break;
+                case 255:
+                    motor[TIRE_FL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 253:
+                    motor[TIRE_FL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 254:
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_BL].pwm = 0;
+                    mtc2=false;
+                    break;
+                case 1:
+                    motor[TIRE_BL].pwm += 5;
+                    mtc2=false;
+                    break;
+                case 3:
+                    motor[TIRE_BL].pwm += 10;
+                    mtc2=false;
+                    break;
+                case 2:
+                    motor[TIRE_BL].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc2=false;
+                    break;
+            }
+        }
+    } else if(mode==41) {
+        if(LIF||LIB) {
+            mode=42;
+        }
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(g[1]==0) {
+            count=0;
+            mode=70;
+        }
+    } else if(mode==42) {
+        motor[TIRE_FR].pwm = 20;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 0;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 0;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = 20;
+        motor[TIRE_BL].dir = BACK;
+        if(g[1]==0) {
+            count=0;
+            mode=70;
+        }
+    } else if(mode==70) {
+        switch(g[1]) {
+            case 99:
+                if(count>=100000) {
+                    mode=100;
+                }
+                break;
+            case 98:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                break;
+            case 0:
+                motor[TIRE_FR].pwm = 80;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 80;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = 80;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 80;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 255:
+                motor[TIRE_FR].pwm = fast;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = fast;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 253:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 254:
+                motor[TIRE_FR].pwm = normal;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = 0;
+                motor[TIRE_FL].dir = FREE;
+                motor[TIRE_BR].pwm = 0;
+                motor[TIRE_BR].dir = FREE;
+                motor[TIRE_BL].pwm = normal;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 1:
+                motor[TIRE_FR].pwm = slow;
+                motor[TIRE_FR].dir = FOR;
+                motor[TIRE_FL].pwm = fast;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = fast;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = slow;
+                motor[TIRE_BL].dir = BACK;
+                mtc=true;
+                break;
+            case 3:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = slow;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = slow;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+            case 2:
+                motor[TIRE_FR].pwm = 0;
+                motor[TIRE_FR].dir = FREE;
+                motor[TIRE_FL].pwm = normal;
+                motor[TIRE_FL].dir = BACK;
+                motor[TIRE_BR].pwm = normal;
+                motor[TIRE_BR].dir = FOR;
+                motor[TIRE_BL].pwm = 0;
+                motor[TIRE_BL].dir = FREE;
+                mtc=true;
+                break;
+        }
+        if(g[1]!=98&&mtc==true) {
+            switch(g[0]) {
+                case 0:
+                    mtc=false;
+                    break;
+                case 255:
+                    motor[TIRE_FL].pwm += 5;
+                    mtc=false;
+                    break;
+                case 253:
+                    motor[TIRE_FL].pwm += 10;
+                    mtc=false;
+                    break;
+                case 254:
+                    motor[TIRE_FL].pwm += 20;
+                    motor[TIRE_FR].pwm = 0;
+                    mtc=false;
+                    break;
+                case 1:
+                    motor[TIRE_FR].pwm += 5;
+                    mtc=false;
+                    break;
+                case 3:
+                    motor[TIRE_FR].pwm += 10;
+                    mtc=false;
+                    break;
+                case 2:
+                    motor[TIRE_FR].pwm += 20;
+                    motor[TIRE_FL].pwm = 0;
+                    mtc=false;
+                    break;
+            }
+        }
+    } else if(mode==100) {
+        motor[TIRE_FR].pwm = 20;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 20;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 23;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 20;
+        motor[TIRE_BL].dir = BACK;
+    } else {
+    }
 }
 #endif
Repository toolbox

Export to desktop IDE
Build repository
Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	03 Oct 2019
Imports:	2
Forks:	0
Commits:	45
Dependents:	0
Dependencies:	3
Followers:	1
Revision 44:c208a908583c, committed 2019-10-03

Changed in this revision

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
