三浦 颯太
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Diff: System/Process/Process.cpp
- Revision:
- 26:4c0ce2f05688
- Parent:
- 25:b3a9f34b201d
- Child:
- 27:dd6c9feb7493
--- 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;