三浦 颯太
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Revision 27:dd6c9feb7493, committed 2019-10-02
- Comitter:
- M_souta
- Date:
- Wed Oct 02 11:08:26 2019 +0000
- Parent:
- 26:4c0ce2f05688
- Commit message:
- pp;
Changed in this revision
diff -r 4c0ce2f05688 -r dd6c9feb7493 Input/Encoder/Encoder.cpp
--- a/Input/Encoder/Encoder.cpp Sat Sep 28 23:02:17 2019 +0000
+++ b/Input/Encoder/Encoder.cpp Wed Oct 02 11:08:26 2019 +0000
@@ -2,13 +2,14 @@
#include "mbed.h"
const int PerRev = 256;
-
+ /*
QEI encoder[] = {
QEI(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING),
QEI(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING),
QEI(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING),
QEI(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING),
};
+ */
namespace ENCODER {
diff -r 4c0ce2f05688 -r dd6c9feb7493 Input/Encoder/Encoder.h
--- a/Input/Encoder/Encoder.h Sat Sep 28 23:02:17 2019 +0000
+++ b/Input/Encoder/Encoder.h Wed Oct 02 11:08:26 2019 +0000
@@ -3,7 +3,7 @@
#include "QEI.h"
-extern QEI encoder[];
+//extern QEI encoder[];
namespace ENCODER {
#define ECD_A_0 PB_1
diff -r 4c0ce2f05688 -r dd6c9feb7493 System/Process/Process.cpp
--- a/System/Process/Process.cpp Sat Sep 28 23:02:17 2019 +0000
+++ b/System/Process/Process.cpp Wed Oct 02 11:08:26 2019 +0000
@@ -160,6 +160,26 @@
// ************* Line ************** //
+// ************* Air *********** //
+
+#define AIR_NUM 4
+
+DigitalOut air[] = {
+ DigitalOut (ECD_A_0),
+ DigitalOut (ECD_B_0),
+ DigitalOut (ECD_A_1),
+ DigitalOut (ECD_B_1),
+};
+
+bool Air[AIR_NUM];
+
+void AirUpdate() {
+ for(int i = 0; i < AIR_NUM; i++) {
+ air[i] = Air[i];
+ }
+}
+// ************* Air ************ //
+
const int omni[15][15] =
{
{ 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
@@ -235,11 +255,6 @@
/*Replace here with the initialization code of your variables.*/
//rotaconPIDtimer.attach(tirePID,0.1);
- //DigitalOut Air_16(LS_16);
- //DigitalOut Air_17(LS_17);
- //DigitalOut Air_18(LS_18);
- //DigitalOut Air_19(LS_19);
-
#pragma endregion USER-DEFINED_VARIABLE_INIT
lock = true;
@@ -323,27 +338,41 @@
while(1)
{
- int g[8];
- for(int i = 0; i < 8; i++){
- g[i] = lineCast(LineHub::GetPara(i));
- }
- pc.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]);
-
-
+ int g[8];
+ for(int i = 0; i < 8; i++){
+ g[i] = lineCast(LineHub::GetPara(i));
+ }
+ pc.printf("0:%d 1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
+ /*
+ if(LimitSw::IsPressed(UNFOLD_SW) && startFlag) {
+ startFlag = false;
+ lock = false;
+ current = 6;
+
+ }
+ */
+
+ /*
if(LimitSw::IsPressed(START_SW) && startFlag == true) {
- startFlag == false;
+ startFlag = false;
lock = false;
lineFase = 0;
lineCount = 0;
lineCheck = false;
countW = 0;
+ for(int i = 0; i < 4; i++) {
+ Air[i] = 0;
+ }
if(LimitSw::IsPressed(REDBLUE_SW)) {
current = 4;
} else {
current = 5;
}
}
+ */
+
+ AirUpdate(); // エアー 更新
buzzer.period(1.0/800);
@@ -402,6 +431,7 @@
static void Process0()
{
AllActuatorReset();
+
}
#endif
@@ -468,12 +498,28 @@
motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
}
- if(controller->Button.ZR) {
- solenoid.solenoid4 = SOLENOID_ON;
+ if(controller->Button.L) {
+ air[0] = SOLENOID_ON;
+ } else {
+ air[0] = SOLENOID_OFF;
}
if(controller->Button.ZL) {
- solenoid.solenoid4 = SOLENOID_OFF;
+ air[1] = SOLENOID_ON;
+ } else {
+ air[1] = SOLENOID_OFF;
}
+ if(controller->Button.R) {
+ air[2] = SOLENOID_ON;
+ } else {
+ air[2] = SOLENOID_OFF;
+ }
+ if(controller->Button.ZR) {
+ air[3] = SOLENOID_ON;
+ } else {
+ air[3] = SOLENOID_OFF;
+ }
+
+
}
#endif
@@ -544,13 +590,16 @@
lineCheck = false;
lineCount = 0;
countW = 0;
+ for(int i = 0; i < 4; i++) {
+ air[i] = 0;
+ Air[i] = 0;
+ }
}
#endif
#if USE_PROCESS_NUM>4
static void Process4()
{
- LED_DEBUG0 = LED_ON;
/* ************************************** //
@@ -684,17 +733,17 @@
motor[TIRE_BL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_FR].dir = BACK;
- if(linePara[4] == 0) {
+ if(linePara[4] >= -1 && linePara[4] <= 1){
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 = 16;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].pwm = 16;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_FR].pwm = 20;
} else if(lineFase == 3) { // 右 ライントレース
switch(linePara[4]) {
case -2:
@@ -793,13 +842,7 @@
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 = 1;
if(countW == targetCount) {
countW = 0;
@@ -812,17 +855,17 @@
motor[TIRE_BL].dir = BACK;
motor[TIRE_BR].dir = BACK;
motor[TIRE_FR].dir = FOR;
- if (linePara[LINE_TOW_1] == 0) {
+ if (linePara[LINE_TOW_2] == 0) { // １と２逆になります。
lineFase = 6;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
}
- motor[TIRE_FL].pwm = 14;
- motor[TIRE_BL].pwm = 14;
- motor[TIRE_BR].pwm = 14;
- motor[TIRE_FR].pwm = 14;
+ 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;
@@ -840,35 +883,738 @@
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
- } else if(LimitSw::IsPressed(TOW_1L)) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
+ } else if(LimitSw::IsPressed(TOW_1L)) {
+ motor[TIRE_FL].dir = FOR;
+ 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 = FOR;
motor[TIRE_FR].pwm = 16;
} else if(LimitSw::IsPressed(TOW_1R)) {
motor[TIRE_FL].dir = BACK;
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 = BACK;
+ motor[TIRE_FR].pwm = 16;
+ } else {
+ if(LimitSw::IsPressed(QF_SW)) {
+ 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':
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
+ tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
+ tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
+ tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+ } else {
+ 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':
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
+ tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
+ tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
+ tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
+ 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_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 = 8;
+ 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 = BACK;
- motor[TIRE_FR].pwm = 16;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FR].pwm = 50;
+ }
+ } else if(lineFase == 8) { // タオル1 解放
+ Air[0] = SOLENOID_ON;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ lineFase = 9;
+ } else if(lineFase == 9) { // 前 ライントレース
+ if(LimitSw::IsPressed(QF_SW)) {
+ 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;
+ }
} else {
+ 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;
+ }
+ }
+
+ 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]);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(countW == 1) {
+ countW = 0;
+ lineFase = 10;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } 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) {
+ 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 = 16;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].pwm = 16;
+ } else if(lineFase == 11) {
+ 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(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;
+ if(linePara[LINE_TOW_1] == 0) {
+ lineFase = 13;
+ 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 == 13) {
+ lineFase = 14;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ } 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 = FOR;
+ 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;
+ } else if(LimitSw::IsPressed(TOW_2R)) {
motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 16;
+ motor[TIRE_FL].pwm = 20;
motor[TIRE_BL].dir = BACK;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 16;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FR].pwm = 20;
+ } else {
+ if(LimitSw::IsPressed(QF_SW)) {
+ 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] = tirePWM[TIRE_FL];
+ tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
+ tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
+ tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+ } else {
+ 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] = tirePWM[TIRE_FL];
+ tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
+ tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
+ tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
+ 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) { // ライン 修正
+ } else if(lineFase == 15 ){ // ライン 修正
if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
@@ -897,7 +1643,7 @@
motor[TIRE_FR].dir = FOR;
motor[TIRE_FR].pwm = 16;
} else if(linePara[LINE_TOW_1] == 0) {
- lineFase = 8;
+ lineFase = 16;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
@@ -916,89 +1662,168 @@
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
}
- } else if(lineFase == 8) { // タオル1 解放
- //solenoid.TOWEL1 = SOLENOID_ON;
+ } else if(lineFase == 16) { // タオル2 解放
+ Air[1] = SOLENOID_ON;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
- lineFase = 9;
- } else if(lineFase == 9) { // 前
- switch(linePara[LINE_TOW_1]) {
- case 2:
- 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] = 20;
- tirePWM[TIRE_BR] = -10;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = 15;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -15;
- 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] = 15;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -15;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 10;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -10;
- adjAnable = true;
- break;
- case -2:
- 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;
- 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;
+ lineFase = 17;
+ } else if(lineFase == 17) { // 前 ライントレース
+ if(LimitSw::IsPressed(QF_SW)) {
+ switch(linePara[LINE_TOW_1]) {
+ 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;
+ }
+ } else {
+ switch(linePara[LINE_TOW_1]) {
+ 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){
@@ -1022,17 +1847,19 @@
}
if(countW == 1) {
countW = 0;
- lineFase = 10;
+ lineFase = 18;
lineCount = 0;
lineCheck = false;
}
- } else if(lineFase == 10) {
+ } 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 = 11;
+ if(linePara[4] != 'N') {
+ lineFase = 19;
+ Air[0] = SOLENOID_OFF;
+ Air[1] = SOLENOID_OFF;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -1041,9 +1868,9 @@
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[4]) {
+ motor[TIRE_FR].pwm = 16;
+ } else if(lineFase == 19) { // 左
+ switch(linePara[3]) {
case -2:
tirePWM[TIRE_FL] = -30;
tirePWM[TIRE_BL] = 0;
@@ -1121,344 +1948,6 @@
}
if(adjAnable){
- adj = 0;//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(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;
- if(linePara[LINE_TOW_2] == 0) {
- lineFase = 13;
- 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 == 13) {
- lineFase = 14;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- } 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 = 16;
- 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;
- } else if(LimitSw::IsPressed(TOW_2R)) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 16;
- 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;
- } else {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 16;
- }
- } 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] = 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){
- 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] != 'N') {
- 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[3]) {
- 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] = 40;
- 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[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
} else {
adj = 0;
@@ -1478,16 +1967,6 @@
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) {
@@ -1502,23 +1981,17 @@
motor[TIRE_BR].dir = FOR;
motor[TIRE_FR].dir = BACK;
if (linePara[2] == 'N') {
- //(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) {
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_FR].pwm = 20;
+ } else if(lineFase == 21) { // 後 ライントレース
switch(linePara[2]) {
case -2:
tirePWM[TIRE_FL] = 0;
@@ -1931,18 +2404,18 @@
motor[TIRE_FL].dir = FOR;
motor[TIRE_FL].pwm = 20;
motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BL].pwm = 20;
motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
+ motor[TIRE_BR].pwm = 20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FR].pwm = 20;
} else if(LimitSw::IsPressed(TOW_1R)) {
motor[TIRE_FL].dir = BACK;
motor[TIRE_FL].pwm = 20;
motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BL].pwm = 20;
motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
+ motor[TIRE_BR].pwm = 20;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FR].pwm = 20;
} else {
@@ -2004,10 +2477,10 @@
adjAnable = true;
break;
case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
+ tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
+ tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
+ tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
+ tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
adjAnable = false;
break;
default:
@@ -2078,7 +2551,7 @@
motor[TIRE_FR].pwm = 50;
}
} else if(lineFase == 8) { // タオル1 解放
- //solenoid.TOWEL1 = SOLENOID_ON;
+ Air[0] = SOLENOID_ON;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -2342,18 +2815,18 @@
motor[TIRE_FL].dir = FOR;
motor[TIRE_FL].pwm = 20;
motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BL].pwm = 20;
motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
+ motor[TIRE_BR].pwm = 20;
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_BL].pwm = 20;
motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
+ motor[TIRE_BR].pwm = 20;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FR].pwm = 20;
} else {
@@ -2407,10 +2880,10 @@
tirePWM[TIRE_FR] = 20;
break;
case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
+ tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
+ tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
+ tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
+ tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
break;
default:
tirePWM[TIRE_FL] = 0;
@@ -2477,7 +2950,7 @@
motor[TIRE_FR].pwm = 50;
}
} else if(lineFase == 16) { // タオル2 解放
- //solenoid.TOWEL2 = SOLENOID_ON;
+ Air[1] = SOLENOID_ON;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -2593,6 +3066,8 @@
motor[TIRE_FR].dir = BACK;
if(linePara[4] == 0) {
lineFase = 19;
+ Air[0] = SOLENOID_OFF;
+ Air[1] = SOLENOID_OFF;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -2871,101 +3346,15 @@
#if USE_PROCESS_NUM>6
static void Process6()
{
- for(int i = 0; i < 8; i++) {
- linePara[i] = lineCast(LineHub::GetPara(i));
- }
+ /*
+ static bool stop_flag_L, stop_flag_R;
+ if() {
+ if(LimitSw::IsPressed(LSW_LB) {
+ if() {
+ } else {
- if(lineFase == 0) {
- if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
- lineFase = 1;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 30;
- } else if(LimitSw::IsPressed(TOW_1L)) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 12;
- 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 = 12;
- } else if(LimitSw::IsPressed(TOW_1R)) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 12;
- 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 = 12;
- } else {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 18;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BL].pwm = 18;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BR].pwm = 18;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 18;
- }
- } else if(lineFase == 1) {
- if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 30;
- } else if(linePara[LINE_TOW_1] > 0) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FR].pwm = 15;
- } else if(linePara[LINE_TOW_1] < 0) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 15;
- } else if(linePara[LINE_TOW_1] == 0) {
- lineFase = 2;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 30;
- } else {
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 30;
- }
- } else if(lineFase == 2) {
- //solenoid::solenoid1 = SOLENOID_ON
+ */
- }
}
#endif
@@ -2979,6 +3368,7 @@
#if USE_PROCESS_NUM>8
static void Process8()
{
+ /*
if(controller->Button.A) {
rotaconSampling.start();
PIDflag = true;
@@ -3055,6 +3445,7 @@
motor[i].pwm = SetPWM(tirePWM[i]);
}
}
+ */
}
#endif
@@ -3125,5 +3516,7 @@
}
return l;
}
+
+
#pragma endregion