![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Diff: System/Process/Process.cpp
- Revision:
- 26:4c0ce2f05688
- Parent:
- 25:b3a9f34b201d
--- a/System/Process/Process.cpp Fri Sep 27 07:51:32 2019 +0000 +++ b/System/Process/Process.cpp Sat Sep 28 23:02:17 2019 +0000 @@ -137,6 +137,8 @@ // ************* Line ************** // +Timer tow_stop; + float pw = 0; int lineFase = 0; bool lineCheck = false; @@ -144,7 +146,7 @@ bool adjAnable = false; int adj = 0; -int count = 0; +int countW = 0; int lineCount = 0; int targetCount = 0; @@ -335,15 +337,12 @@ lineFase = 0; lineCount = 0; lineCheck = false; - count = 0; - /* - if(LimitSw::IsPressed(ISREDBLUE_SW) { + countW = 0; + if(LimitSw::IsPressed(REDBLUE_SW)) { current = 4; } else { current = 5; } - */ - current = 4; } buzzer.period(1.0/800); @@ -414,14 +413,14 @@ if(controller->Button.UP) { motor[LIFT_LB].dir = FOR; - motor[LIFT_LB].pwm = 190; + motor[LIFT_LB].pwm = 180; motor[LIFT_RB].dir = BACK; - motor[LIFT_RB].pwm = 180; + motor[LIFT_RB].pwm = 200; } else if(controller->Button.DOWN) { motor[LIFT_LB].dir = BACK; motor[LIFT_LB].pwm = 180; motor[LIFT_RB].dir = FOR; - motor[LIFT_RB].pwm = 190; + motor[LIFT_RB].pwm = 200; } else if(controller->Button.LEFT) { motor[LIFT_LB].dir = FOR; motor[LIFT_LB].pwm = 180; @@ -431,8 +430,8 @@ } else { motor[LIFT_LB].dir = BRAKE; motor[LIFT_LB].pwm = 255; - motor[LIFT_RB].dir = BACK; - motor[LIFT_RB].pwm = 20; + motor[LIFT_RB].dir = BRAKE; + motor[LIFT_RB].pwm = 255; } @@ -471,7 +470,8 @@ if(controller->Button.ZR) { solenoid.solenoid4 = SOLENOID_ON; - } else { + } + if(controller->Button.ZL) { solenoid.solenoid4 = SOLENOID_OFF; } } @@ -486,42 +486,42 @@ /* if(moving) { - if(LimiSw::IsPressed(LSW_LB)) { + if(LimitSw::IsPressed(LSW_LB)) { if(switchFlag_LB) { switchFlag_LB = false; motor[LIFT_LB].dir = BRAKE; motor[LIFT_LB].pwm = 200; } else { - seitchFlag_LB = true; + switchFlag_LB = true; } } - if(LimiSw::IsPressed(LSW_RB)) { + if(LimitSw::IsPressed(LSW_RB)) { if(switchFlag_RB) { switchFlag_RB = false; motor[LIFT_RB].dir = BRAKE; motor[LIFT_RB].pwm = 200; } else { - seitchFlag_RB = true; + switchFlag_RB = true; } } if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false; } else { if(controller->Button.UP) { - if(!(state == UPPER)) { - state++; - motor[LIFT_LB].dir = BACK; - motor[LIFT_RB].dir = FOR; - motor[LIFT_LB].pwm = 200; - motor[LIFT_RB].pwm = 200; + if(!liftSstate == UPPER)) { + liftState++; + motor[LIFT_LB].dir = FOR; + motor[LIFT_RB].dir = BACK; + motor[LIFT_LB].pwm = 185; + motor[LIFT_RB].pwm = 180; } } else if(controller->Button.DOWN) { - if(!(state == LOWER)) { - state--; + if(!(liftstate == LOWER)) { + liftState--; moving = true; - motor[LIFT_LB].dir = FOR; - motor[LIFT_RB].dir = BACK; - motor[LIFT_LB].pwm = 200; - motor[LIFT_RB].pwm = 200; + motor[LIFT_LB].dir = BACK; + motor[LIFT_RB].dir = FOR; + motor[LIFT_LB].pwm = 180; + motor[LIFT_RB].pwm = 185; } } else { motor[LIFT_LB].dir = BRAKE; @@ -531,6 +531,7 @@ } } */ + } #endif @@ -542,7 +543,7 @@ lineFase = 0; lineCheck = false; lineCount = 0; - count = 0; + countW = 0; } #endif @@ -630,7 +631,7 @@ if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 30; @@ -672,8 +673,8 @@ lineCount++; if(lineCount > 20) lineCheck = false; } - if(count == 3) { - count = 0; + if(countW == 3) { + countW = 0; lineFase = 2; lineCount = 0; lineCheck = false; @@ -749,7 +750,7 @@ if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = -30; @@ -800,8 +801,8 @@ targetCount = 2; } - if(count == targetCount) { - count = 0; + if(countW == targetCount) { + countW = 0; lineFase = 4; lineCount = 0; lineCheck = false; @@ -925,64 +926,64 @@ } else if(lineFase == 9) { // 前 switch(linePara[LINE_TOW_1]) { case 2: - tirePWM[TIRE_FL] = 0; - tirePWM[TIRE_BL] = 30; - tirePWM[TIRE_BR] = 0; - tirePWM[TIRE_FR] = -30; + tirePWM[TIRE_FL] = 5; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = 5; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 3: tirePWM[TIRE_FL] = 10; - tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BL] = 20; tirePWM[TIRE_BR] = -10; - tirePWM[TIRE_FR] = -30; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 1: - tirePWM[TIRE_FL] = 20; - tirePWM[TIRE_BL] = 30; - tirePWM[TIRE_BR] = -20; - tirePWM[TIRE_FR] = -30; + tirePWM[TIRE_FL] = 15; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -15; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: - tirePWM[TIRE_FL] = 30; - tirePWM[TIRE_BL] = 30; - tirePWM[TIRE_BR] = -30; - tirePWM[TIRE_FR] = -30; + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case -1: - tirePWM[TIRE_FL] = 30; - tirePWM[TIRE_BL] = 20; - tirePWM[TIRE_BR] = -30; - tirePWM[TIRE_FR] = -20; + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 15; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -15; adjAnable = true; break; case -3: - tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_FL] = 20; tirePWM[TIRE_BL] = 10; - tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_BR] = -20; tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -2: - tirePWM[TIRE_FL] = 30; - tirePWM[TIRE_BL] = 0; - tirePWM[TIRE_BR] = -30; - tirePWM[TIRE_FR] = 0; + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 5; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -5; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } - tirePWM[TIRE_FL] = 30; - tirePWM[TIRE_BL] = 30; - tirePWM[TIRE_BR] = -30; - tirePWM[TIRE_FR] = -30; + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 'N': @@ -1019,8 +1020,8 @@ lineCount++; if(lineCount > 20) lineCheck = false; } - if(count == 1) { - count = 0; + if(countW == 1) { + countW = 0; lineFase = 10; lineCount = 0; lineCheck = false; @@ -1044,64 +1045,64 @@ } else if(lineFase == 11) { switch(linePara[4]) { case -2: - tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 0; - tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case -3: - tirePWM[TIRE_FL] = 30; - tirePWM[TIRE_BL] = -10; - tirePWM[TIRE_BR] = -30; - tirePWM[TIRE_FR] = 10; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 10; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -1: - tirePWM[TIRE_FL] = 30; - tirePWM[TIRE_BL] = -20; - tirePWM[TIRE_BR] = -30; - tirePWM[TIRE_FR] = 20; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: - tirePWM[TIRE_FL] = 30; - tirePWM[TIRE_BL] = -30; - tirePWM[TIRE_BR] = -30; - tirePWM[TIRE_FR] = 30; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: - tirePWM[TIRE_FL] = 20; - tirePWM[TIRE_BL] = -30; - tirePWM[TIRE_BR] = -20; - tirePWM[TIRE_FR] = 30; + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 3: - tirePWM[TIRE_FL] = 10; - tirePWM[TIRE_BL] = -30; - tirePWM[TIRE_BR] = -10; - tirePWM[TIRE_FR] = 30; + tirePWM[TIRE_FL] = -10; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 10; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 0; - tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; - tirePWM[TIRE_FR] = 30; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } - tirePWM[TIRE_FL] = 30; - tirePWM[TIRE_BL] = -30; - tirePWM[TIRE_BR] = -30; - tirePWM[TIRE_FR] = 30; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': @@ -1129,17 +1130,17 @@ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); - motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj); - motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); + motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj); + motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); - motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]); + motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } - if(count == 1) { - count = 0; + if(countW == 2) { + countW = 0; lineFase = 12; lineCount = 0; lineCheck = false; @@ -1315,7 +1316,7 @@ if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 30; @@ -1357,8 +1358,8 @@ lineCount++; if(lineCount > 20) lineCheck = false; } - if(count == 1) { - count = 0; + if(countW == 1) { + countW = 0; lineFase = 18; lineCount = 0; lineCheck = false; @@ -1398,8 +1399,8 @@ case -1: tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 20; - tirePWM[TIRE_BR] = 30 - tirePWM[TIRE_FR] = -2; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: @@ -1434,7 +1435,7 @@ if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 30; @@ -1489,8 +1490,8 @@ targetCount = 3; - if(count == targetCount) { - count = 0; + if(countW == targetCount) { + countW = 0; lineFase = 20; lineCount = 0; lineCheck = false; @@ -1572,7 +1573,7 @@ if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = -30; @@ -1614,8 +1615,8 @@ lineCount++; if(lineCount > 20) lineCheck = false; } - if(count == 3) { - count = 0; + if(countW == 3) { + countW = 0; lineFase = 22; lineCount = 0; lineCheck = false; @@ -1660,65 +1661,65 @@ } if(lineFase == 0) { - motor[TIRE_FL].dir = BACK; - motor[TIRE_BL].dir = BACK; - motor[TIRE_BR].dir = FOR; - motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BL].dir = FOR; + motor[TIRE_BR].dir = BACK; + motor[TIRE_FR].dir = BACK; if(linePara[0] != 'N' && linePara[0] != 'A') { lineFase = 1; } - motor[TIRE_FL].pwm = 20; - motor[TIRE_BL].pwm = 20; - motor[TIRE_BR].pwm = 20; - motor[TIRE_FR].pwm = 20; + motor[TIRE_FL].pwm = 30; + motor[TIRE_BL].pwm = 30; + motor[TIRE_BR].pwm = 30; + motor[TIRE_FR].pwm = 30; }else if(lineFase == 1) { // 前 ライントレース switch(linePara[0]) { case -2: tirePWM[TIRE_FL] = 0; - tirePWM[TIRE_BL] = 50; + tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case -3: - tirePWM[TIRE_FL] = 20; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = -30; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = 10; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = -10; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case -1: tirePWM[TIRE_FL] = 20; - tirePWM[TIRE_BL] = 50; + tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = -20; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 0: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = 40; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = -40; + tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 3: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = 20; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = -20; + tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_BL] = 10; + tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_FR] = -10; adjAnable = true; break; case 2: - tirePWM[TIRE_FL] = 50; + tirePWM[TIRE_FL] = 30; tirePWM[TIRE_BL] = 0; - tirePWM[TIRE_BR] = -50; + tirePWM[TIRE_BR] = -30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; @@ -1726,12 +1727,12 @@ if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': @@ -1759,98 +1760,98 @@ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); - motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]) * 0.8; - motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj) * 0.8; - motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj) * 0.8; - motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]) * 0.8; + motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); + motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); + motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); + motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } - if(count == 3) { - count = 0; + if(countW == 3) { + countW = 0; lineFase = 2; lineCount = 0; lineCheck = false; } } else if(lineFase == 2) { // 前 低速 - motor[TIRE_FL].dir = BACK; - motor[TIRE_BL].dir = BACK; - motor[TIRE_BR].dir = FOR; - motor[TIRE_FR].dir = FOR; - if(linePara[3] != 'N') { + motor[TIRE_FL].dir = FOR; + motor[TIRE_BL].dir = FOR; + motor[TIRE_BR].dir = BACK; + motor[TIRE_FR].dir = BACK; + if(linePara[3] == 0) { lineFase = 3; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } - motor[TIRE_FL].pwm = 20; - motor[TIRE_BL].pwm = 20; - motor[TIRE_BR].pwm = 20; - motor[TIRE_FR].pwm = 20; + motor[TIRE_FL].pwm = 16; + motor[TIRE_BL].pwm = 16; + motor[TIRE_BR].pwm = 16; + motor[TIRE_FR].pwm = 16; } else if(lineFase == 3) { // 左 ライントレース - switch(linePara[4]) { + switch(linePara[3]) { case -2: - tirePWM[TIRE_FL] = -50; + tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 0; - tirePWM[TIRE_BR] = 50; + tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case -3: - tirePWM[TIRE_FL] = -50; - tirePWM[TIRE_BL] = 20; - tirePWM[TIRE_BR] = 50; - tirePWM[TIRE_FR] = -20; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 10; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -1: - tirePWM[TIRE_FL] = -50; - tirePWM[TIRE_BL] = 40; - tirePWM[TIRE_BR] = 50; - tirePWM[TIRE_FR] = -40; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: - tirePWM[TIRE_FL] = -50; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = 50; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: - tirePWM[TIRE_FL] = -40; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = 40; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 3: - tirePWM[TIRE_FL] = -20; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = 20; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = -10; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 10; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 0; - tirePWM[TIRE_BL] = 50; + tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } - tirePWM[TIRE_FL] = -50; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = 50; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': @@ -1887,17 +1888,17 @@ lineCount++; if(lineCount > 20) lineCheck = false; } - if(count == 2) { - count = 0; + if(countW == 1) { + countW = 0; lineFase = 4; lineCount = 0; lineCheck = false; } } else if(lineFase == 4) { // 右 低速 - motor[TIRE_FL].dir = FOR; - motor[TIRE_BL].dir = BACK; - motor[TIRE_BR].dir = BACK; - motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BL].dir = FOR; + motor[TIRE_BR].dir = FOR; + motor[TIRE_FR].dir = BACK; if(linePara[LINE_TOW_1] == 0) { lineFase = 5; motor[TIRE_FL].dir = BRAKE; @@ -1905,17 +1906,17 @@ motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } - motor[TIRE_FL].pwm = 20; - motor[TIRE_BL].pwm = 20; - motor[TIRE_BR].pwm = 20; - motor[TIRE_FR].pwm = 20; + motor[TIRE_FL].pwm = 16; + motor[TIRE_BL].pwm = 16; + motor[TIRE_BR].pwm = 16; + motor[TIRE_FR].pwm = 16; } else if (lineFase == 5) { lineFase = 6; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; - } else if(lineFase == 6) { // タオル 竿検知 + } else if(lineFase == 6) { // タオル1 竿検知 if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) { lineFase = 7; motor[TIRE_FL].dir = BRAKE; @@ -1928,31 +1929,103 @@ motor[TIRE_FR].pwm = 50; } else if(LimitSw::IsPressed(TOW_1L)) { motor[TIRE_FL].dir = FOR; - motor[TIRE_FL].pwm = 16; + motor[TIRE_FL].pwm = 20; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = FOR; - motor[TIRE_FR].pwm = 16; + motor[TIRE_FR].pwm = 20; } else if(LimitSw::IsPressed(TOW_1R)) { motor[TIRE_FL].dir = BACK; - motor[TIRE_FL].pwm = 16; + motor[TIRE_FL].pwm = 20; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BACK; - motor[TIRE_FR].pwm = 16; + motor[TIRE_FR].pwm = 20; } else { - motor[TIRE_FL].dir = BACK; - motor[TIRE_FL].pwm = 20; - motor[TIRE_BL].dir = BACK; - motor[TIRE_BL].pwm = 20; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BR].pwm = 20; - motor[TIRE_FR].dir = FOR; - motor[TIRE_FR].pwm = 20; + switch(linePara[LINE_TOW_1]) { + case -2: + tirePWM[TIRE_FL] = -10; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 10; + tirePWM[TIRE_FR] = 20; + adjAnable = true; + break; + case -3: + tirePWM[TIRE_FL] = -14; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 14; + tirePWM[TIRE_FR] = 20; + adjAnable = true; + break; + case -1: + tirePWM[TIRE_FL] = -17; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 17; + tirePWM[TIRE_FR] = 20; + adjAnable = true; + break; + case 0: + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 20; + adjAnable = true; + break; + case 1: + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -17; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 17; + adjAnable = true; + break; + case 3: + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -14; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 14; + adjAnable = true; + break; + case 2: + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -10; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 10; + adjAnable = true; + break; + case 'A': + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 20; + adjAnable = true; + break; + case 'N': + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + adjAnable = false; + break; + default: + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + adjAnable = false; + } + + motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); + motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); + motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); + motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); + motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); + motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); + motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); + motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); } } else if(lineFase == 7) { // ライン 修正 if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') { @@ -1983,6 +2056,8 @@ motor[TIRE_FR].dir = FOR; motor[TIRE_FR].pwm = 16; } else if(linePara[LINE_TOW_1] == 0) { + //tow_stop.reset(); + //tow_stop.start(); lineFase = 8; motor[TIRE_FL].dir = BRAKE; motor[TIRE_FL].pwm = 50; @@ -1994,13 +2069,13 @@ motor[TIRE_FR].pwm = 50; } else { motor[TIRE_FL].dir = BRAKE; - motor[TIRE_FL].pwm = 30; + motor[TIRE_FL].pwm = 50; motor[TIRE_BL].dir = BRAKE; - motor[TIRE_BL].pwm = 30; + motor[TIRE_BL].pwm = 50; motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BR].pwm = 30; + motor[TIRE_BR].pwm = 50; motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FR].pwm = 30; + motor[TIRE_FR].pwm = 50; } } else if(lineFase == 8) { // タオル1 解放 //solenoid.TOWEL1 = SOLENOID_ON; @@ -2012,64 +2087,64 @@ } else if(lineFase == 9) { // 前 switch(linePara[LINE_TOW_1]) { case 2: - tirePWM[TIRE_FL] = 0; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = 0; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = 10; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -10; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 3: - tirePWM[TIRE_FL] = 20; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = -20; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = 14; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -14; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 1: - tirePWM[TIRE_FL] = 40; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = -40; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = 17; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -17; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case -1: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = 40; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = -40; + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 17; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -17; adjAnable = true; break; case -3: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = 20; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = -20; + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 14; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -14; adjAnable = true; break; case -2: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = 0; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = 0; + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 10; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -10; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = 50; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = -50; + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 'N': @@ -2106,95 +2181,95 @@ lineCount++; if(lineCount > 20) lineCheck = false; } - if(count == 1) { - count = 0; + if(countW == 1) { + countW = 0; lineFase = 10; lineCount = 0; lineCheck = false; } - } else if(lineFase == 10) { + } else if(lineFase == 10) { // 前 低速 motor[TIRE_FL].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_FR].dir = BACK; - if(linePara[4] == 0) { + if(linePara[3] == 0) { lineFase = 11; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; } - motor[TIRE_FL].pwm = 20; - motor[TIRE_BL].pwm = 20; - motor[TIRE_BR].pwm = 20; - motor[TIRE_FR].pwm = 20; - } else if(lineFase == 11) { - switch(linePara[4]) { + motor[TIRE_FL].pwm = 16; + motor[TIRE_BL].pwm = 16; + motor[TIRE_BR].pwm = 16; + motor[TIRE_FR].pwm = 16; + } else if(lineFase == 11) { // 左 + switch(linePara[3]) { case -2: - tirePWM[TIRE_FL] = 50; + tirePWM[TIRE_FL] = -30; tirePWM[TIRE_BL] = 0; - tirePWM[TIRE_BR] = -50; + tirePWM[TIRE_BR] = 30; tirePWM[TIRE_FR] = 0; adjAnable = true; break; case -3: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = -30; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = 30; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 10; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -10; adjAnable = true; break; case -1: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = -40; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = 40; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -20; adjAnable = true; break; case 0: - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = -50; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = 50; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 1: - tirePWM[TIRE_FL] = 40; - tirePWM[TIRE_BL] = -50; - tirePWM[TIRE_BR] = -40; - tirePWM[TIRE_FR] = 50; + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 3: - tirePWM[TIRE_FL] = 20; - tirePWM[TIRE_BL] = -50; - tirePWM[TIRE_BR] = -20; - tirePWM[TIRE_FR] = 50; + tirePWM[TIRE_FL] = -10; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 10; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 2: tirePWM[TIRE_FL] = 0; - tirePWM[TIRE_BL] = -50; + tirePWM[TIRE_BL] = 30; tirePWM[TIRE_BR] = 0; - tirePWM[TIRE_FR] = 50; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'A': if(lineCheck == false) { lineCheck = true; lineCount = 0; - count++; + countW++; } - tirePWM[TIRE_FL] = 50; - tirePWM[TIRE_BL] = -50; - tirePWM[TIRE_BR] = -50; - tirePWM[TIRE_FR] = 50; + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 30; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = -30; adjAnable = true; break; case 'N': tirePWM[TIRE_FL] = 0; tirePWM[TIRE_BL] = 0; - tirePWM[TIRE_BR] = 0.; + tirePWM[TIRE_BR] = 0; tirePWM[TIRE_FR] = 0; adjAnable = false; break; @@ -2207,35 +2282,34 @@ } if(adjAnable){ - adj = 0;//linePara[3]; + if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4]; } else { adj = 0; } - motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj); - motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); - motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); - motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); - motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj); - motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); - motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); - motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]); - + motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); + motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); + motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); + motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj); + motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); + motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); + motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); + motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj); if(lineCheck == true) { lineCount++; if(lineCount > 20) lineCheck = false; } - if(count == 3) { - count = 0; + if(countW == 2) { + countW = 0; lineFase = 12; lineCount = 0; lineCheck = false; } - } else if(lineFase == 12) { // 右 低速 - motor[TIRE_FL].dir = FOR; - motor[TIRE_BL].dir = BACK; - motor[TIRE_BR].dir = BACK; - motor[TIRE_FR].dir = FOR; + } else if(lineFase == 12) { // 左 低速 + motor[TIRE_FL].dir = BACK; + motor[TIRE_BL].dir = FOR; + motor[TIRE_BR].dir = FOR; + motor[TIRE_FR].dir = BACK; if(linePara[LINE_TOW_2] == 0) { lineFase = 13; motor[TIRE_FL].dir = BRAKE; @@ -2253,7 +2327,538 @@ motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_FR].dir = BRAKE; - } else { + } else if(lineFase == 14) { // タオル2 竿検知 + if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) { + lineFase = 15; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_FL].pwm = 50; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BL].pwm = 50; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BR].pwm = 50; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FR].pwm = 50; + } else if(LimitSw::IsPressed(TOW_2L)) { + motor[TIRE_FL].dir = FOR; + motor[TIRE_FL].pwm = 20; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BL].pwm = 50; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BR].pwm = 50; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FR].pwm = 20; + } else if(LimitSw::IsPressed(TOW_2R)) { + motor[TIRE_FL].dir = BACK; + motor[TIRE_FL].pwm = 20; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BL].pwm = 50; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BR].pwm = 50; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FR].pwm = 20; + } else { + switch(linePara[LINE_TOW_2]) { + case -2: + tirePWM[TIRE_FL] = -10; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 10; + tirePWM[TIRE_FR] = 20; + break; + case -3: + tirePWM[TIRE_FL] = -14; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 14; + tirePWM[TIRE_FR] = 20; + break; + case -1: + tirePWM[TIRE_FL] = -17; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 17; + tirePWM[TIRE_FR] = 20; + break; + case 0: + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 20; + break; + case 1: + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -17; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 17; + break; + case 3: + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -14; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 14; + break; + case 2: + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -10; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 10; + break; + case 'A': + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 20; + break; + case 'N': + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + break; + default: + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + } + + motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); + motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); + motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); + motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); + motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); + motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); + motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); + motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); + } + } else if(lineFase == 15 ){ // ライン 修正 + if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') { + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_FL].pwm = 50; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BL].pwm = 50; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BR].pwm = 50; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FR].pwm = 50; + } else if(linePara[LINE_TOW_2] > 0) { + motor[TIRE_FL].dir = BACK; + motor[TIRE_FL].pwm = 16; + motor[TIRE_BL].dir = FOR; + motor[TIRE_BL].pwm = 16; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BR].pwm = 16; + motor[TIRE_FR].dir = BACK; + motor[TIRE_FR].pwm = 16; + } else if(linePara[LINE_TOW_2] < 0) { + motor[TIRE_FL].dir = FOR; + motor[TIRE_FL].pwm = 16; + motor[TIRE_BL].dir = BACK; + motor[TIRE_BL].pwm = 16; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BR].pwm = 16; + motor[TIRE_FR].dir = FOR; + motor[TIRE_FR].pwm = 16; + } else if(linePara[LINE_TOW_2] == 0) { + lineFase = 16; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_FL].pwm = 50; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BL].pwm = 50; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BR].pwm = 50; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FR].pwm = 50; + } else { + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_FL].pwm = 50; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BL].pwm = 50; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BR].pwm = 50; + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FR].pwm = 50; + } + } else if(lineFase == 16) { // タオル2 解放 + //solenoid.TOWEL2 = SOLENOID_ON; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_FR].dir = BRAKE; + lineFase = 17; + } else if(lineFase == 17) { // 前 + switch(linePara[LINE_TOW_2]) { + case 2: + tirePWM[TIRE_FL] = 10; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -10; + tirePWM[TIRE_FR] = -20; + adjAnable = true; + break; + case 3: + tirePWM[TIRE_FL] = 14; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -14; + tirePWM[TIRE_FR] = -20; + adjAnable = true; + break; + case 1: + tirePWM[TIRE_FL] = 17; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -17; + tirePWM[TIRE_FR] = -20; + adjAnable = true; + break; + case 0: + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -20; + adjAnable = true; + break; + case -1: + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 17; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -17; + adjAnable = true; + break; + case -3: + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 14; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -14; + adjAnable = true; + break; + case -2: + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 10; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = 10; + adjAnable = true; + break; + case 'A': + if(lineCheck == false) { + lineCheck = true; + lineCount = 0; + countW++; + } + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = 20; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = -20; + adjAnable = true; + break; + case 'N': + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + adjAnable = false; + break; + default: + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + adjAnable = false; + } + + if(adjAnable){ + adj = 0; + } else { + adj = 0; + } + + motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]); + motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); + motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj); + motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); + motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]); + motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); + motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj); + motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); + + if(lineCheck == true) { + lineCount++; + if(lineCount > 20) lineCheck = false; + } + if(countW == 1) { + countW = 0; + lineFase = 18; + lineCount = 0; + lineCheck = false; + } + } else if(lineFase == 18) { // 前 低速 + motor[TIRE_FL].dir = FOR; + motor[TIRE_BL].dir = FOR; + motor[TIRE_BR].dir = BACK; + motor[TIRE_FR].dir = BACK; + if(linePara[4] == 0) { + lineFase = 19; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_FR].dir = BRAKE; + } + motor[TIRE_FL].pwm = 16; + motor[TIRE_BL].pwm = 16; + motor[TIRE_BR].pwm = 16; + motor[TIRE_FR].pwm = 16; + } else if(lineFase == 19) { // 右 + switch(linePara[4]) { + case -2: + tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_FR] = 0; + adjAnable = true; + break; + case -3: + tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_BL] = -10; + tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_FR] = 10; + adjAnable = true; + break; + case -1: + tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_FR] = 20; + adjAnable = true; + break; + case 0: + tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case 1: + tirePWM[TIRE_FL] = 20; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = -20; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case 3: + tirePWM[TIRE_FL] = 10; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = -10; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case 2: + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case 'A': + if(lineCheck == false) { + lineCheck = true; + lineCount = 0; + countW++; + } + tirePWM[TIRE_FL] = 30; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = -30; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case 'N': + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + adjAnable = false; + break; + default: + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + adjAnable = false; + } + + if(adjAnable){ + if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3]; + } else { + adj = 0; + } + + motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj); + motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj); + motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); + motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]); + motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj); + motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj); + motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); + motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]); + + if(lineCheck == true) { + lineCount++; + if(lineCount > 20) lineCheck = false; + } + + /* + if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) { + targetCount = 3; + } else if(LimitSw::IsPressed(TOWEL1_SW)) { + targetCount = 3; + } else { + targetCount = 2; + } + */ + + targetCount = 3; + + if(countW == targetCount) { + countW = 0; + lineFase = 20; + lineCount = 0; + lineCheck = false; + } + } else if(lineFase == 20) { // 右 低速 + motor[TIRE_FL].dir = FOR; + motor[TIRE_BL].dir = BACK; + motor[TIRE_BR].dir = BACK; + motor[TIRE_FR].dir = FOR; + if (linePara[2] == 0) { + //(LimitSw::IsPressed(SHEETS_SW)) { + //ineFase = 20; + //else if(LimitSw::IsPressed(TOWEL2_SW) { + //ineFase = 14; + //else { + lineFase = 21; + // + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_FR].dir = BRAKE; + } + motor[TIRE_FL].pwm = 16; + motor[TIRE_BL].pwm = 16; + motor[TIRE_BR].pwm = 16; + motor[TIRE_FR].pwm = 16; + } else if(lineFase == 21) { + switch(linePara[2]) { + case -2: + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case -3: + tirePWM[TIRE_FL] = -10; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = 10; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case -1: + tirePWM[TIRE_FL] = -20; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = 20; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case 0: + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case 1: + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = -20; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = 20; + adjAnable = true; + break; + case 3: + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = -10; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = 10; + adjAnable = true; + break; + case 2: + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = 0; + adjAnable = true; + break; + case 'A': + if(lineCheck == false) { + lineCheck = true; + lineCount = 0; + countW++; + } + tirePWM[TIRE_FL] = -30; + tirePWM[TIRE_BL] = -30; + tirePWM[TIRE_BR] = 30; + tirePWM[TIRE_FR] = 30; + adjAnable = true; + break; + case 'N': + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + adjAnable = false; + break; + default: + tirePWM[TIRE_FL] = 0; + tirePWM[TIRE_BL] = 0; + tirePWM[TIRE_BR] = 0; + tirePWM[TIRE_FR] = 0; + adjAnable = false; + } + + if(adjAnable){ + if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0]; + } else { + adj = 0; + } + + motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj); + motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]); + motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]); + motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj); + motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj); + motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]); + motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]); + motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj); + + if(lineCheck == true) { + lineCount++; + if(lineCount > 20) lineCheck = false; + } + if(countW == 2) { + countW = 0; + lineFase = 22; + lineCount = 0; + lineCheck = false; + } + } else if(lineFase == 22) { + motor[TIRE_FL].dir = BACK; + motor[TIRE_BL].dir = BACK; + motor[TIRE_BR].dir = FOR; + motor[TIRE_FR].dir = FOR; + if (linePara[0] == 'N') { + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_FR].dir = BRAKE; + } + motor[TIRE_FL].pwm = 30; + motor[TIRE_BL].pwm = 30; + motor[TIRE_BR].pwm = 30; + motor[TIRE_FR].pwm = 30; + } else{ motor[TIRE_FL].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE;