aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 23:694ecfa97839
- Parent:
- 22:c47f29caded6
- Child:
- 24:41c23c9a5058
--- a/System/Process/Process.cpp Sat Sep 14 00:10:57 2019 +0000 +++ b/System/Process/Process.cpp Sun Sep 15 11:53:56 2019 +0000 @@ -288,7 +288,7 @@ bool dz4=true; bool dz4i=false; -int mode=0; +int mode=10; int mode1=0; int mode1g0=100000; @@ -296,7 +296,7 @@ static void Process1() { mode1g0=100000; - mode=0; + mode=10; mode1=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]); @@ -409,7 +409,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]); mode1g0++; - if(g[0]!=99&&mode==0) { + if(g[0]!=99&&mode==10) { switch(g[0]) { case 98: motor[TIRE_FR].pwm = 30; @@ -426,7 +426,8 @@ } mtc=true; if(mode1==2) { - mode=1; + mode=11; + mode1g0=0; } break; case 0: @@ -540,19 +541,20 @@ break; } } - } else if(mode==1) { - motor[TIRE_FR].pwm = 5; + } else if(mode==11) { + motor[TIRE_FR].pwm = 15; motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].pwm = 5; + motor[TIRE_FL].pwm = 15; motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].pwm = 5; + motor[TIRE_BR].pwm = 15; motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].pwm = 5; + motor[TIRE_BL].pwm = 15; motor[TIRE_BL].dir = FOR; - if(g[2]==0) { - mode=2; + if(mode1g0>5000&&g[2]==0) { + mode1g0=0; + mode=12; } - } else if(mode==2) { + } else if(mode==12) { motor[TIRE_FR].pwm = 15; motor[TIRE_FR].dir = FOR; motor[TIRE_FL].pwm = 15; @@ -563,10 +565,12 @@ motor[TIRE_BL].dir = BACK; mode1g0++; if(mode1g0>10000&&g[2]==0) { - mode=3; + mode1g0=0; + mode1=0; + mode=20; } - } else if(mode==3) { - switch(g[2]) { + } else if(mode==20) { + switch(g[2]) { case 98: motor[TIRE_FR].pwm = 30; motor[TIRE_FR].dir = FOR; @@ -576,18 +580,15 @@ motor[TIRE_BR].dir = BACK; motor[TIRE_BL].pwm = 30; motor[TIRE_BL].dir = BACK; - - mtc2=true; - if(mode1g0>100000) { mode1++; mode1g0=0; } - mtc2=true; - - if(mode1==2) { - mode=3; + if(mode1==3) { + mode=21; + mode1g0=0; + mode1=0; } break; case 0: @@ -701,9 +702,496 @@ break; } } - } - - else { + } else if(mode==21) { + motor[TIRE_FR].pwm = 15; + motor[TIRE_FR].dir = FOR; + 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 = BACK; + if(mode1g0>5000&&g[0]==0) { + mode1g0=0; + mode=22; + } + } else if(mode==22) { + motor[TIRE_FR].pwm = 15; + motor[TIRE_FR].dir = BACK; + 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 = FOR; + if(mode1g0>10000&&g[0]==0) { + mode1g0=0; + mode1=0; + mode=30; + } + } else if(mode==30) { + 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(mode1g0>100000) { + mode1++; + mode1g0=0; + } + mtc=true; + if(mode1==1) { + mode=31; + mode1g0=0; + } + break; + case 0: + motor[TIRE_FR].pwm = 60; + 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 = 60; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 255: + motor[TIRE_FR].pwm = 60; + 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 = 60; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 253: + motor[TIRE_FR].pwm = 20; + 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 = 20; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 254: + motor[TIRE_FR].pwm = 30; + 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 = 30; + motor[TIRE_BL].dir = FOR; + mtc=true; + break; + case 1: + motor[TIRE_FR].pwm = 40; + 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 = 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 = 20; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].pwm = 20; + 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 = 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 = 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 = 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; + mode1g0++; + if(mode1g0>5000&&g[3]==0) { + mode1g0=0; + mode=32; + } + } else if(mode==32) { + 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; + mode1g0++; + if(mode1g0>5000&&g[3]==0) { + mode1g0=0; + mode1=0; + mode=40; + } + } else if(mode==40) { + switch(g[3]) { + case 98: + motor[TIRE_FR].pwm = 30; + motor[TIRE_FR].dir = BACK; + 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 = FOR; + if(mode1g0>100000) { + mode1++; + mode1g0=0; + } + mtc2=true; + if(mode1==1) { + mode=41; + mode1g0=0; + mode1=0; + } + break; + case 0: + motor[TIRE_FR].pwm = 60; + motor[TIRE_FR].dir = BACK; + 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 = FOR; + mtc2=true; + break; + case 255: + motor[TIRE_FR].pwm = 40; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 60; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 60; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 40; + motor[TIRE_BL].dir = FOR; + mtc2=true; + break; + case 253: + 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; + mtc2=true; + break; + case 254: + 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; + mtc2=true; + break; + case 1: + motor[TIRE_FR].pwm = 60; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].pwm = 40; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 40; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].pwm = 60; + motor[TIRE_BL].dir = FOR; + mtc2=true; + break; + case 3: + motor[TIRE_FR].pwm = 20; + 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 = 20; + motor[TIRE_BL].dir = FOR; + mtc2=true; + break; + case 2: + motor[TIRE_FR].pwm = 30; + 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 = 30; + 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) { + motor[TIRE_FR].pwm = 15; + motor[TIRE_FR].dir = BACK; + 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 = FOR; + if(mode1g0>5000&&g[1]==0) { + mode1g0=0; + mode=42; + } + } else if(mode==42) { + motor[TIRE_FR].pwm = 15; + motor[TIRE_FR].dir = FOR; + 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 = BACK; + if(mode1g0>5000&&g[1]==0) { + mode1g0=0; + mode=50; + } else if(mode==50) { + 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(mode1g0>100000) { + mode1++; + mode1g0=0; + } + mtc=true; + if(mode1==1) { + mode=51; + mode1g0=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 = 40; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].pwm = 40; + 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 = FREE; + motor[TIRE_BR].pwm = 0; + motor[TIRE_BR].dir = FREE; + 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 = 40; + 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 = 40; + 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_FR].pwm += 5; + mtc=false; + break; + case 253: + motor[TIRE_FR].pwm += 10; + mtc=false; + break; + case 254: + motor[TIRE_FR].pwm += 20; + motor[TIRE_FL].pwm = 0; + mtc=false; + break; + case 1: + motor[TIRE_FL].pwm += 5; + mtc=false; + break; + case 3: + motor[TIRE_FL].pwm += 10; + mtc=false; + break; + case 2: + motor[TIRE_FL].pwm += 20; + motor[TIRE_FR].pwm = 0; + mtc=false; + break; + } + } + } + } else if(mode==51) { + 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 if(mode==52) { + } else { motor[TIRE_FR].pwm = 0; motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].pwm = 0; @@ -715,11 +1203,13 @@ } } + #endif #if USE_PROCESS_NUM>3 static void Process3() { + } #endif