MainBoard2019_Master_9_22 - aa

Users » yabahiro » Code » MainBoard2019_Master_9_22
大季矢花 / Mbed 2 deprecated MainBoard2019_Master_9_22
Dependencies: mbed TrapezoidControl QEI
Files at this revision
API Documentation at this revision
Revision 41:e1dd6d97f17d, committed 2019-09-29

Comitter:: yabahiro
Date:: Sun Sep 29 07:28:05 2019 +0000
Parent:: 40:7f93984e8ba1
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	Sat Sep 28 13:15:16 2019 +0000
+++ b/System/Process/Process.cpp	Sun Sep 29 07:28:05 2019 +0000
@@ -182,6 +182,8 @@
 bool mtc=false;
 bool mtc2 = false;
 
+bool start_flag = true;
+
 int mode =1;
 int cross=0;//十字数える用
 int cros=0;
@@ -311,6 +313,75 @@
         for(int i = 0; i < 8; i++) {
             g[i] = LineHub::GetPara(i);
         }
+        if(ThSW) {
+            Twsh=1;
+        } else {
+            Twsh=2;
+        }
+        if(StertSW && start_flag) {
+                start_flag = false;
+                lock = false;
+                LedMode(1);
+                if(ZoneSW) {
+                    mode=1;
+                    current = 2;
+                } else {
+                    mode=1;
+                    current = 5;
+                }
+            }
+        if(DWSW||DOSW) {
+            if(DWSW) {
+                zyouge=true;
+            }
+            if(DOSW) {
+                if(DOLS) {
+                    motor[MOTOR_0].pwm = 100;
+                    motor[MOTOR_0].dir = BRAKE;
+                } else {
+                    motor[MOTOR_0].pwm = 50;
+                    motor[MOTOR_0].dir = BACK;
+                }
+            }
+        } else if(zyouge==false){
+            motor[MOTOR_0].pwm = 100;
+            motor[MOTOR_0].dir = BRAKE;
+        }
+        if(zyouge) {
+            if(Twsh==1) {
+                motor[MOTOR_0].pwm = 220;
+                motor[MOTOR_0].dir = FOR;
+                if(UPLS) {
+                    motor[MOTOR_0].pwm = 100;
+                    motor[MOTOR_0].dir = BRAKE;
+                    zyouge=false;
+                }
+            }
+            if(Twsh==2) {
+                motor[MOTOR_0].pwm = 230;
+                motor[MOTOR_0].dir = FOR;
+                if(UPLS2) {
+                    motor[MOTOR_0].pwm = 100;
+                    motor[MOTOR_0].dir = BRAKE;
+                    zyouge=false;
+                }
+            }
+        }
+        if(AIRSW) {
+            if(SW_flag==false) {
+                if(Air_flag==false) {
+                    solenoid.solenoid3 = SOLENOID_ON;
+                    Air_flag=true;
+                    SW_flag=true;
+                } else {
+                    solenoid.solenoid3 = SOLENOID_OFF;
+                    Air_flag=false;
+                    SW_flag=true;
+                }
+            }
+        } else {
+            SW_flag=false;
+        }
 
         //printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 9:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
 
@@ -319,7 +390,7 @@
 #endif
 
 #ifdef USE_ERRORCHECK
-        if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
+        if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & start_flag) {      //& start_flag
             CONTROLLER::Controller::DataReset();
             AllActuatorReset();
             lock = true;
@@ -359,6 +430,7 @@
 #if USE_PROCESS_NUM>0
 static void Process0()
 {
+    ///*
     mode=1;
     LedMode(1);
     if(ThSW) {
@@ -368,6 +440,8 @@
     }
 
     if(StertSW) {
+        //start_flag == false;
+        //lock = false;
         if(ZoneSW) {
             mode=1;
             current = 2;
@@ -427,6 +501,7 @@
             zyougedo=false;
         }
     }
+    //*/
 }
 
 #endif
@@ -434,6 +509,8 @@
 #if USE_PROCESS_NUM>1
 static void Process1()
 {
+    start_flag = true;
+
     mtc=false;
     mtc2 = false;
     LedMode(2);
@@ -2339,16 +2416,16 @@
     LedMode(5);
     count++;
     if(mode==1) {//スタートゾーンから白線検知
-        fast=40;
+        fast=50;
         normal=30;
         slow=20;
-        motor[TIRE_FR].pwm = 40;
+        motor[TIRE_FR].pwm = 35;
         motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 40;
+        motor[TIRE_FL].pwm = 35;
         motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 40;
+        motor[TIRE_BR].pwm = 35;
         motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 40;
+        motor[TIRE_BL].pwm = 32;
         motor[TIRE_BL].dir = BACK;
         if(g[2]==0) {
             count=100000;
@@ -2356,7 +2433,7 @@
             mode=3;
         }
     } else if(mode==3) { //横ライントレースから縦ライントレースへ
-        motor[TIRE_FR].pwm = 25;
+        motor[TIRE_FR].pwm = 27;
         motor[TIRE_FR].dir = FOR;
         motor[TIRE_FL].pwm = 0;
         motor[TIRE_FL].dir = FREE;
@@ -2528,9 +2605,6 @@
             count=0;
             cross=0;
             mode=20;
-            fast=40;
-            normal=30;
-            slow=20;
         }
     } else if(mode==20) {
 
@@ -2562,7 +2636,7 @@
                 motor[TIRE_BR].dir = BACK;
                 motor[TIRE_BL].pwm = normal;
                 motor[TIRE_BL].dir = BACK;
-                if(count>80000) {
+                if(count>50000) {
                     cross++;
                     count=0;
                 }
@@ -2575,6 +2649,7 @@
                 if(cross==2) {
                     mode=21;
                     cross=0;
+                    count=0;
                 }
                 break;
             case 0:
@@ -2690,6 +2765,18 @@
         }
     } else if(mode==21) {
         motor[TIRE_FR].pwm = 20;
+        motor[TIRE_FR].dir = BRAKE;
+        motor[TIRE_FL].pwm = 20;
+        motor[TIRE_FL].dir = BRAKE;
+        motor[TIRE_BR].pwm = 20;
+        motor[TIRE_BR].dir = BRAKE;
+        motor[TIRE_BL].pwm = 20;
+        motor[TIRE_BL].dir = BRAKE;
+        if(count<=100000) {
+            mode=22;
+        }
+    } else if(mode==22) {
+        motor[TIRE_FR].pwm = 20;
         motor[TIRE_FR].dir = BACK;
         motor[TIRE_FL].pwm = 20;
         motor[TIRE_FL].dir = BACK;
@@ -2700,9 +2787,8 @@
         if(g[2]!=98) {
             mode=30;
         }
-
     } else if(mode==30) {
-        motor[TIRE_FR].pwm = 60;
+        motor[TIRE_FR].pwm = 64;
         motor[TIRE_FR].dir = FOR;
         motor[TIRE_FL].pwm = 60;
         motor[TIRE_FL].dir = BACK;
@@ -2712,167 +2798,43 @@
         motor[TIRE_BL].dir = BACK;
         if(g[1]==98) {
             mode=31;
+            count=0;
         }
-        //ゆっくりモードに入れなかった時の保険
-        /*
-        switch(g[1]) {
-            case 98:
-                motor[TIRE_FR].pwm = 30;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 30;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 30;
-                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) {
-                    if(Twsh==2) {
-                        solenoid.solenoid2 = SOLENOID_OFF;
-                    }
-                    mode=31;
-                    count=0;
-                }
-                break;
-            case 0:
-                motor[TIRE_FR].pwm = 60;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 60;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 60;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 60;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = 60;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 20;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 20;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 60;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = 20;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 0;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 0;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 20;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 254:
-                motor[TIRE_FR].pwm = 30;
-                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 = 30;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = 20;
-                motor[TIRE_FR].dir = FOR;
-                motor[TIRE_FL].pwm = 60;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 60;
-                motor[TIRE_BR].dir = FOR;
-                motor[TIRE_BL].pwm = 20;
-                motor[TIRE_BL].dir = BACK;
-                mtc=true;
-                break;
-            case 3:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = 20;
-                motor[TIRE_FL].dir = BACK;
-                motor[TIRE_BR].pwm = 20;
-                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 = 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;
-                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==31) {
-        motor[TIRE_FR].pwm = 15;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 15;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 15;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 15;
-        motor[TIRE_BL].dir = BACK;
-        if(g[3]==0) {
+        motor[TIRE_FR].pwm = 10;
+        motor[TIRE_FR].dir = FREE;
+        motor[TIRE_FL].pwm = 10;
+        motor[TIRE_FL].dir = FREE;
+        motor[TIRE_BR].pwm = 10;
+        motor[TIRE_BR].dir = FREE;
+        motor[TIRE_BL].pwm = 10;
+        motor[TIRE_BL].dir = FREE;
+        if(count>=10000) {
+            mode=32;
             count=0;
-            mode=32;
         }
     } else if(mode==32) {
-        motor[TIRE_FR].pwm = 30;
-        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 = 30;
-        motor[TIRE_BL].dir = FOR;
+        motor[TIRE_FR].pwm = 5;
+        motor[TIRE_FR].dir = BRAKE;
+        motor[TIRE_FL].pwm = 5;
+        motor[TIRE_FL].dir = BRAKE;
+        motor[TIRE_BR].pwm = 5;
+        motor[TIRE_BR].dir = BRAKE;
+        motor[TIRE_BL].pwm = 5;
+        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 = FREE;
+        motor[TIRE_FL].pwm = 25;
+        motor[TIRE_FL].dir = BACK;
+        motor[TIRE_BR].pwm = 25;
+        motor[TIRE_BR].dir = FOR;
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
         if(g[3]==0) {
             count=0;
             if(Twsh==2) {
@@ -2881,8 +2843,7 @@
             mode=40;
         }
     } else if(mode==40) {
-
-        if(LIF) {
+        if(LIF||LIB) {
             mode=42;
         }
 
@@ -3033,6 +2994,9 @@
             }
         }
     } else if(mode==41) {
+        if(LIF||LIB) {
+            mode=42;
+        }
         motor[TIRE_FR].pwm = 30;
         motor[TIRE_FR].dir = BACK;
         motor[TIRE_FL].pwm = 0;
@@ -3049,15 +3013,15 @@
             slow=40;
         }
     } else if(mode==42) {
-        motor[TIRE_FR].pwm = 30;
-        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FR].pwm = 0;
+        motor[TIRE_FR].dir = FREE;
         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[1]==0) {
+        motor[TIRE_BL].pwm = 0;
+        motor[TIRE_BL].dir = FREE;
+        if(g[0]==0) {
             count=0;
             mode=70;
             normal=60;
@@ -3073,7 +3037,7 @@
                     mode=100;
                     count=0;
                 }
-                if(cross==3) {
+                if(cross==2) {
                     cross=0;
                     mode=100;
                 }
Repository toolbox

Export to desktop IDE
Build repository
Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	29 Sep 2019
Imports:	1
Forks:	0
Commits:	42
Dependents:	0
Dependencies:	3
Followers:	1
Revision 41:e1dd6d97f17d, committed 2019-09-29

Changed in this revision

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
