大季 矢花 / Mbed 2 deprecated 2019_Master_9_30_last

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed TrapezoidControl QEI

Files at this revision

API Documentation at this revision

Revision 42:4f9689c0a349, committed 2019-09-30

Comitter:
yabahiro
Date:
Mon Sep 30 11:33:22 2019 +0000
Parent:
41:e1dd6d97f17d
Commit message:
qq;

Changed in this revision

System/Process/Process.cpp Show annotated file Show diff for this revision Revisions of this file
System/Process/Process.h Show annotated file Show diff for this revision Revisions of this file 
--- a/System/Process/Process.cpp	Sun Sep 29 07:28:05 2019 +0000
+++ b/System/Process/Process.cpp	Mon Sep 30 11:33:22 2019 +0000
@@ -104,63 +104,33 @@
     else return abs(pwmVal);
 }
 
-void LedMode(int led)
+int selectnum[16][4]= {
+//1,2,4,8
+    {0,0,0,0},
+    {1,0,0,0},
+    {0,1,0,0},
+    {1,1,0,0},
+    {0,0,1,0},
+    {1,0,1,0},
+    {0,1,1,0},
+    {1,1,1,0},
+    {0,0,0,1},
+    {1,0,0,1},
+    {0,1,0,1},
+    {1,1,0,1},
+    {0,0,1,1},
+    {1,0,1,1},
+    {0,1,1,1},
+    {1,1,1,1}
+};
+
+void LedMode(int num)
 {
-    /*
-    switch(led) {
-        case 1:
-            POTENTIOMETER::dio[0]= 1;
-            POTENTIOMETER::dio[1]= 0;
-            POTENTIOMETER::dio[2]= 0;
-            POTENTIOMETER::dio[3]= 0;
-            break;
-        case 2:
-            POTENTIOMETER::dio[0]= 0;
-            POTENTIOMETER::dio[1]= 1;
-            POTENTIOMETER::dio[2]= 0;
-            POTENTIOMETER::dio[3]= 0;
-            break;
-        case 3:
-            POTENTIOMETER::dio[0]= 1;
-            POTENTIOMETER::dio[1]= 1;
-            POTENTIOMETER::dio[2]= 0;
-            POTENTIOMETER::dio[3]= 0;
-            break;
-        case 4:
-            POTENTIOMETER::dio[0]= 0;
-            POTENTIOMETER::dio[1]= 0;
-            POTENTIOMETER::dio[2]= 1;
-            POTENTIOMETER::dio[3]= 0;
-            break;
-    }
-    */
-    if(led/8>=1) {
-        POTENTIOMETER::dio[3]= 1;
-        led=led%8;
-    } else {
-        POTENTIOMETER::dio[3]= 0;
-    }
+    POTENTIOMETER::dio[0]= selectnum[num][0];
+    POTENTIOMETER::dio[1]= selectnum[num][1];;
+    POTENTIOMETER::dio[2]= selectnum[num][2];;
+    POTENTIOMETER::dio[3]= selectnum[num][3];;
 
-    if(led/4>=1) {
-        POTENTIOMETER::dio[2]= 1;
-        led=led%4;
-    } else {
-        POTENTIOMETER::dio[2]= 0;
-    }
-
-    if(led/2>=1) {
-        POTENTIOMETER::dio[1]= 1;
-        led=led%2;
-    } else {
-        POTENTIOMETER::dio[1]= 0;
-    }
-
-    if(led/1>=1) {
-        POTENTIOMETER::dio[0]= 1;
-        led=0;
-    } else {
-        POTENTIOMETER::dio[0]= 0;
-    }
 }
 int Twsh;
 bool UP_flag = false;
@@ -184,7 +154,7 @@
 
 bool start_flag = true;
 
-int mode =1;
+int mode =0;
 int cross=0;//十字数える用
 int cros=0;
 int count=100000;//wait代替え
@@ -310,6 +280,13 @@
     SystemProcessInitialize();
 
     while(1) {
+        if(mode==0) {
+            if(ZoneSW) {
+                LedMode(1);
+            } else {
+                LedMode(2);
+            }
+        }
         for(int i = 0; i < 8; i++) {
             g[i] = LineHub::GetPara(i);
         }
@@ -318,18 +295,18 @@
         } 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;
-                }
+            start_flag = false;
+            lock = false;
+            if(ZoneSW) {
+                mode=1;
+                current = 2;
+            } else {
+                mode=1;
+                current = 5;
             }
+        }
         if(DWSW||DOSW) {
             if(DWSW) {
                 zyouge=true;
@@ -343,7 +320,7 @@
                     motor[MOTOR_0].dir = BACK;
                 }
             }
-        } else if(zyouge==false){
+        } else if(zyouge==false) {
             motor[MOTOR_0].pwm = 100;
             motor[MOTOR_0].dir = BRAKE;
         }
@@ -380,7 +357,13 @@
                 }
             }
         } else {
-            SW_flag=false;
+            if( SW_flag==true) {
+                count++;
+                if(count>=5000) {
+                    SW_flag=false;
+                    count=0;
+                }
+            }
         }
 
         //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]);
@@ -430,7 +413,7 @@
 #if USE_PROCESS_NUM>0
 static void Process0()
 {
-    ///*
+    /*
     mode=1;
     LedMode(1);
     if(ThSW) {
@@ -501,7 +484,7 @@
             zyougedo=false;
         }
     }
-    //*/
+    */
 }
 
 #endif
@@ -509,11 +492,10 @@
 #if USE_PROCESS_NUM>1
 static void Process1()
 {
+    LedMode(6);
     start_flag = true;
-
     mtc=false;
     mtc2 = false;
-    LedMode(2);
     mode=1;
     cross=0;
     Twsh=1;
@@ -612,10 +594,13 @@
 #if USE_PROCESS_NUM>2
 static void Process2()
 {
+//   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);
-//   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==1) {//スタートゾーンから白線検知
+        fast=40;
+        normal=30;
+        slow=20;
         motor[TIRE_FR].pwm = normal;
         motor[TIRE_FR].dir = BACK;
         motor[TIRE_FL].pwm = normal;
@@ -632,9 +617,9 @@
     } else if(mode==3) { //横ライントレースから縦ライントレースへ
         motor[TIRE_FR].pwm = 0;
         motor[TIRE_FR].dir = FREE;
-        motor[TIRE_FL].pwm = normal;
+        motor[TIRE_FL].pwm = 25;
         motor[TIRE_FL].dir = FOR;
-        motor[TIRE_BR].pwm = normal;
+        motor[TIRE_BR].pwm = 25;
         motor[TIRE_BR].dir = BACK;
         motor[TIRE_BL].pwm = 0;
         motor[TIRE_BL].dir = FREE;
@@ -833,12 +818,12 @@
                 motor[TIRE_BR].dir = BACK;
                 motor[TIRE_BL].pwm = normal;
                 motor[TIRE_BL].dir = BACK;
-                if(count>80000) {
+                if(count>50000) {
                     cross++;
                     count=0;
                 }
                 mtc2=true;
-                if(cross==2) {
+                if(cross==1) {
                     mode=21;
                     count=0;
                     cross=0;
@@ -956,6 +941,44 @@
             }
         }
     } else if(mode==21) {
+        motor[TIRE_FR].pwm = 26;
+        motor[TIRE_FR].dir = FOR;
+        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FL].dir = FOR;
+        motor[TIRE_BR].pwm = 26;
+        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;
+        }
+    } else if(mode==22) {
+        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=23;
+        }
+    } else if(mode==23) {
+        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<=100000) {
+            mode=30;
+        }
+    } else if(mode==24) {
         motor[TIRE_FR].pwm = 0;
         motor[TIRE_FR].dir = FREE;
         motor[TIRE_FL].pwm = normal;
@@ -967,205 +990,55 @@
         if(g[0]==0) {
             mode=30;
             cros=0;
-            zyouge=true;
         }
     } else if(mode==30) {
-        motor[TIRE_FR].pwm = 62;
+        motor[TIRE_FR].pwm = 63;
         motor[TIRE_FR].dir = BACK;
         motor[TIRE_FL].pwm = 60;
         motor[TIRE_FL].dir = FOR;
         motor[TIRE_BR].pwm = 60;
         motor[TIRE_BR].dir = BACK;
-        motor[TIRE_BL].pwm = 62;
+        motor[TIRE_BL].pwm = 60;
         motor[TIRE_BL].dir = FOR;
         if(g[0]==98) {
             mode=31;
-        }
-        if(g[3]==0) {
-            mode=32;
-            count=0;
-            cros=0;
-        }//ゆっくりモードに入れなかった時の保険
-        /*
-        if(g[0]==98) {
-            if(Twsh==2) {
-                solenoid.solenoid2 = SOLENOID_OFF;
-            }
-            mode=31;
             count=0;
         }
-        switch(g[0]) {
-            case 98:
-                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;
-                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 = 80;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 80;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 80;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 80;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 255:
-                motor[TIRE_FR].pwm = 80;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 40;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 40;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 80;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 253:
-                motor[TIRE_FR].pwm = 80;
-                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 = 40;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 254:
-                motor[TIRE_FR].pwm = 50;
-                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 = 50;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 1:
-                motor[TIRE_FR].pwm = 40;
-                motor[TIRE_FR].dir = BACK;
-                motor[TIRE_FL].pwm = 80;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 80;
-                motor[TIRE_BR].dir = BACK;
-                motor[TIRE_BL].pwm = 40;
-                motor[TIRE_BL].dir = FOR;
-                mtc=true;
-                break;
-            case 3:
-                motor[TIRE_FR].pwm = 0;
-                motor[TIRE_FR].dir = FREE;
-                motor[TIRE_FL].pwm = 40;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 40;
-                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 = 50;
-                motor[TIRE_FL].dir = FOR;
-                motor[TIRE_BR].pwm = 50;
-                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==31) {
-        motor[TIRE_FR].pwm = 15;
-        motor[TIRE_FR].dir = BACK;
-        motor[TIRE_FL].pwm = 10;
-        motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 10;
-        motor[TIRE_BR].dir = FOR;
-        motor[TIRE_BL].pwm = 15;
-        motor[TIRE_BL].dir = FOR;
-        if(g[3]==0) {
-            count=0;
-            mode=40;
-        }
-        */
-
     } else if(mode==31) {
-        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;
-        if(g[3]==0) {
+        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 = 30;
-        motor[TIRE_FR].dir = FOR;
-        motor[TIRE_FL].pwm = 30;
+        motor[TIRE_FR].dir = BACK;
+        motor[TIRE_FL].pwm = 0;
         motor[TIRE_FL].dir = BACK;
-        motor[TIRE_BR].pwm = 30;
+        motor[TIRE_BR].pwm = 0;
         motor[TIRE_BR].dir = FOR;
         motor[TIRE_BL].pwm = 30;
-        motor[TIRE_BL].dir = BACK;
+        motor[TIRE_BL].dir = FOR;
         if(g[3]==0) {
             count=0;
             if(Twsh==2) {
@@ -1206,7 +1079,7 @@
                     count=0;
                 }
                 mtc2=true;
-                if(cross==2) {
+                if(cross==1) {
                     mode=41;
                     count=0;
                     cross=0;
@@ -1324,6 +1197,9 @@
             }
         }
     } else if(mode==41) {
+        if(LIF||LIB) {
+            mode=42;
+        }
         motor[TIRE_FR].pwm = 0;
         motor[TIRE_FR].dir = FREE;
         motor[TIRE_FL].pwm = 30;
@@ -2413,7 +2289,7 @@
 #if USE_PROCESS_NUM>5
 static void Process5()
 {
-    LedMode(5);
+    LedMode(4);
     count++;
     if(mode==1) {//スタートゾーンから白線検知
         fast=50;
@@ -2763,6 +2639,7 @@
                     break;
             }
         }
+
     } else if(mode==21) {
         motor[TIRE_FR].pwm = 20;
         motor[TIRE_FR].dir = BRAKE;
@@ -2775,19 +2652,40 @@
         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;
-        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=30;
+    }
+    if(YELLOW) {
+        if(mode==22) {
+            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=30;
+            }
         }
-    } else if(mode==30) {
+
+    } else {
+        if(mode==22) {
+            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;
+            if(g[1]==0) {
+                count=0;
+                cross=0;
+                mode=30;
+            }
+        }
+    }
+    if(mode==30) {
         motor[TIRE_FR].pwm = 64;
         motor[TIRE_FR].dir = FOR;
         motor[TIRE_FL].pwm = 60;
@@ -2801,28 +2699,28 @@
             count=0;
         }
     } else if(mode==31) {
-        motor[TIRE_FR].pwm = 10;
+        motor[TIRE_FR].pwm = 0;
         motor[TIRE_FR].dir = FREE;
-        motor[TIRE_FL].pwm = 10;
+        motor[TIRE_FL].pwm = 0;
         motor[TIRE_FL].dir = FREE;
-        motor[TIRE_BR].pwm = 10;
+        motor[TIRE_BR].pwm = 0;
         motor[TIRE_BR].dir = FREE;
-        motor[TIRE_BL].pwm = 10;
+        motor[TIRE_BL].pwm = 0;
         motor[TIRE_BL].dir = FREE;
-        if(count>=10000) {
+        if(count>=5000) {
             mode=32;
             count=0;
         }
     } else if(mode==32) {
-        motor[TIRE_FR].pwm = 5;
+        motor[TIRE_FR].pwm = 10;
         motor[TIRE_FR].dir = BRAKE;
-        motor[TIRE_FL].pwm = 5;
+        motor[TIRE_FL].pwm = 10;
         motor[TIRE_FL].dir = BRAKE;
-        motor[TIRE_BR].pwm = 5;
+        motor[TIRE_BR].pwm = 10;
         motor[TIRE_BR].dir = BRAKE;
-        motor[TIRE_BL].pwm = 5;
+        motor[TIRE_BL].pwm = 10;
         motor[TIRE_BL].dir = BRAKE;
-        if(count>=40000) {
+        if(count>=30000) {
             mode=33;
             count=0;
         }

--- a/System/Process/Process.h	Sun Sep 29 07:28:05 2019 +0000
+++ b/System/Process/Process.h	Mon Sep 30 11:33:22 2019 +0000
@@ -23,6 +23,7 @@
 #define AIRSW    LimitSw::IsPressed(2) //上下機構トリガー
 #define UPLS    LimitSw::IsPressed(8) //上下機構リミット１
 #define UPLS2    LimitSw::IsPressed(0) //上下機構リミット２
+#define YELLOW    LimitSw::IsPressed(10) //掛けるときのライントレース切り替え
 #define LIF    LimitSw::IsPressed(11) //フェンス用リミット１
 #define LIB    LimitSw::IsPressed(7) //フェンス用リミット２
 #define DOLS    LimitSw::IsPressed(5) //上下機構下部リミット