aa
Dependencies: mbed TrapezoidControl QEI
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) //上下機構リミット1 #define UPLS2 LimitSw::IsPressed(0) //上下機構リミット2 +#define YELLOW LimitSw::IsPressed(10) //掛けるときのライントレース切り替え #define LIF LimitSw::IsPressed(11) //フェンス用リミット1 #define LIB LimitSw::IsPressed(7) //フェンス用リミット2 #define DOLS LimitSw::IsPressed(5) //上下機構下部リミット