大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 25:b3a9f34b201d
- Parent:
- 24:370616a56815
- Child:
- 26:4c0ce2f05688
--- a/System/Process/Process.cpp Wed Sep 18 02:03:56 2019 +0000
+++ b/System/Process/Process.cpp Fri Sep 27 07:51:32 2019 +0000
@@ -1,4 +1,3 @@
-
#include "mbed.h"
#include "Process.h"
@@ -141,14 +140,22 @@
float pw = 0;
int lineFase = 0;
bool lineCheck = false;
+bool lineFlag = false;
+bool adjAnable = false;
+int adj = 0;
+
+int count = 0;
+int lineCount = 0;
+int targetCount = 0;
+
+bool startFlag = true;
+
int linePWM;
int adj_F;
int adj_B;
int mode = 0;
-int lineCount = 0;
-
// ************* Line ************** //
const int omni[15][15] =
@@ -224,7 +231,12 @@
{
#pragma region USER-DEFINED_VARIABLE_INIT
/*Replace here with the initialization code of your variables.*/
- rotaconPIDtimer.attach(tirePID,0.1);
+ //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
@@ -315,13 +327,24 @@
}
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]);
-
- //float a = ultrasonic[0].ReadDis();
- //pc.printf("%f\n\r",a);
-
- //int ppap = encoder[0].getPulses();
- //pc.printf("%d\n\r",ppap);
-
+
+
+ if(LimitSw::IsPressed(START_SW) && startFlag == true) {
+ startFlag == false;
+ lock = false;
+ lineFase = 0;
+ lineCount = 0;
+ lineCheck = false;
+ count = 0;
+ /*
+ if(LimitSw::IsPressed(ISREDBLUE_SW) {
+ current = 4;
+ } else {
+ current = 5;
+ }
+ */
+ current = 4;
+ }
buzzer.period(1.0/800);
@@ -330,7 +353,7 @@
#endif
#ifdef USE_ERRORCHECK
- if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
+ if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag)
{
CONTROLLER::Controller::DataReset();
AllActuatorReset();
@@ -406,10 +429,10 @@
motor[LIFT_RB].dir = BACK;
motor[LIFT_RB].pwm = 180;
} else {
- motor[LIFT_LB].dir = FREE;
+ motor[LIFT_LB].dir = BRAKE;
motor[LIFT_LB].pwm = 255;
motor[LIFT_RB].dir = BACK;
- motor[LIFT_RB].pwm = 10;
+ motor[LIFT_RB].pwm = 20;
}
@@ -418,39 +441,49 @@
motor[LIFT_U].pwm = 180;
} else if(controller->Button.Y) {
motor[LIFT_U].dir = BACK;
- motor[LIFT_U].pwm = 180;
+ motor[LIFT_U].pwm = 230;
} else {
motor[LIFT_U].dir = BRAKE;
- motor[LIFT_U].pwm = 180;
+ motor[LIFT_U].pwm = 255;
}
if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
- motor[TIRE_BL].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
+ motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
- motor[TIRE_FR].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
+ motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
- motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
+ motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
- motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
+ motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
} else {
- motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
+ motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
- motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
+ motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
}
+
+ if(controller->Button.ZR) {
+ solenoid.solenoid4 = SOLENOID_ON;
+ } else {
+ solenoid.solenoid4 = SOLENOID_OFF;
+ }
}
#endif
#if USE_PROCESS_NUM>2
static void Process2()
{
+
+
+
+
/*
if(moving) {
if(LimiSw::IsPressed(LSW_LB)) {
@@ -504,116 +537,1109 @@
#if USE_PROCESS_NUM>3
static void Process3()
{
+ startFlag = true;
AllActuatorReset();
lineFase = 0;
+ lineCheck = false;
+ lineCount = 0;
+ count = 0;
}
#endif
#if USE_PROCESS_NUM>4
static void Process4()
{
+ LED_DEBUG0 = LED_ON;
- static int x,y;
- static int count = 0;
+ /* ************************************** //
+
+ 赤ゾーン 赤ゾーン 赤ゾーン
+
+ // ************************************** */
+
+
+ for(int i = 0; i < 8; i++) {
+ linePara[i] = lineCast(LineHub::GetPara(i));
+ }
- linePara_U = lineCast(LineHub::GetPara(0));
- linePara_B = lineCast(LineHub::GetPara(2));
- linePara_L = lineCast(LineHub::GetPara(3));
- linePara_R = lineCast(LineHub::GetPara(4));
+ if(lineFase == 0) {
+ 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 = 30;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_FR].pwm = 30;
- if(linePara_B == 'A' && count == 0) {
- lineFase = 1;
- }
-
- if(lineFase == 0) {
- pw = 0.5;
- switch(linePara_U) {
+ } else if(lineFase == 1) { // 前 ライントレース
+ switch(linePara[0]) {
case -2:
- x = 5;
- y = 3;
+ 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;
+ count++;
+ }
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -30;
+ 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;
+ }
+
+ if(adjAnable){
+ if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
+ } 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(count == 3) {
+ count = 0;
+ lineFase = 2;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase == 2) { // 前 低速
+ 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 = 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;
+ } else if(lineFase == 3) { // 右 ライントレース
+ 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:
- x = 5;
- y = 3;
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = -10;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 10;
+ adjAnable = true;
break;
case -1:
- x = 6;
- y = 3;
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
break;
case 0:
- x = 7;
- y = 3;
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
break;
case 1:
- x = 8;
- y = 3;
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
break;
case 3:
- x = 9;
- y = 3;
+ tirePWM[TIRE_FL] = 10;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = -10;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
break;
case 2:
- x = 9;
- y = 3;
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
break;
case 'A':
- lineCheck = true;
- x = x;
- y = y;
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ count++;
+ }
+ 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;
+ }
+
+ if(count == targetCount) {
+ count = 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;
+ if (linePara[LINE_TOW_1] == 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;
+ } 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) { // タオル1 検知
+ if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
+ lineFase = 7;
+ 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_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;
+ 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 = 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 == 7) { // ライン 修正
+ if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == '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_1] > 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_1] < 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_1] == 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 = BRAKE;
+ motor[TIRE_FR].pwm = 50;
+ }
+ } else if(lineFase == 8) { // タオル1 解放
+ //solenoid.TOWEL1 = 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] = 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;
+ count++;
+ }
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
break;
case 'N':
- x = 7;
- y = 7;
+ 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(count == 1) {
+ count = 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;
- x = 7;
- y = 7;
+ 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;
+ count++;
+ }
+ 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:
- x = 9;
- y = 9;
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
}
- if(lineCheck == true && (!(linePara_U) == 'A')) {
- count++;
+
+ 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_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]);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(count == 1) {
+ count = 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;
}
- } else if(lineFase == 1) {
- pw = 0.3;
- x = 7;
- y = 9;
- if(linePara_R == 0) {
- lineFase = 2;
- x = 7;
- y = 7;
+ 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 == 2) {
- x = 7;
- y = 7;
- } else {
- x = 7;
- y = 7;
- }
-
- int t = 0;
- if((linePara_U + linePara_B) > 3) t = 1;
-
- if(controller->Button.A) {
- motor[TIRE_BL].dir = SetStatus(-omni[y][14-x] );
- motor[TIRE_FL].dir = SetStatus(omni[y][x]);
- motor[TIRE_BR].dir = SetStatus(-omni[14-x][14-y]);
- motor[TIRE_FR].dir = SetStatus(omni[x][14-y]);
-
- motor[TIRE_FR].pwm = SetPWM((omni[y][14-x])) * pw;
- motor[TIRE_FL].pwm = SetPWM((omni[y][x])) * pw;
- motor[TIRE_BR].pwm = SetPWM((omni[14-x][14-y])) * pw;
- motor[TIRE_BL].pwm = SetPWM((omni[x][14-y])) * pw;
- } else {
- motor[TIRE_BL].dir = SetStatus(0);
- motor[TIRE_FL].dir = SetStatus(0);
- motor[TIRE_BR].dir = SetStatus(0);
- motor[TIRE_FR].dir = SetStatus(0);
-
- motor[TIRE_FR].pwm = SetPWM(0);
- motor[TIRE_FL].pwm = SetPWM(0);
- motor[TIRE_BR].pwm = SetPWM(0);
- motor[TIRE_BL].pwm = SetPWM(0);
+ } 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;
+ count++;
+ }
+ 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(count == 1) {
+ count = 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] = -2;
+ 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;
+ count++;
+ }
+ 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;
+ }
+
+ 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(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
+ targetCount = 3;
+ } else if(LimitSw::IsPressed(TOWEL1_SW)) {
+ targetCount = 3;
+ } else {
+ targetCount = 2;
+ }
+ */
+
+ targetCount = 3;
+
+ if(count == targetCount) {
+ count = 0;
+ lineFase = 20;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase == 20) { // 左 低速
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].dir = FOR;
+ 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) {
+ 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;
+ count++;
+ }
+ 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(count == 3) {
+ count = 0;
+ lineFase = 22;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase == 22) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_FR].dir = BACK;
+ if (linePara[2] == '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;
+ motor[TIRE_FR].dir = BRAKE;
}
}
@@ -622,158 +1648,719 @@
#if USE_PROCESS_NUM>5
static void Process5()
{
- lineFase = 0;
- lineCheck = true;
+
+ /* ************************************** //
+
+ 青ゾーン 青ゾーン 青ゾーン
+
+ // ************************************** */
+
+ for(int i = 0; i < 8; i++) {
+ linePara[i] = lineCast(LineHub::GetPara(i));
+ }
+
+ if(lineFase == 0) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_FR].dir = FOR;
+ 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;
+ }else if(lineFase == 1) { // 前 ライントレース
+ switch(linePara[0]) {
+ case -2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 40;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = -40;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ count++;
+ }
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = -50;
+ 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[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
+ } 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]) * 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;
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(count == 3) {
+ count = 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') {
+ 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;
+ } else if(lineFase == 3) { // 左 ライントレース
+ switch(linePara[4]) {
+ case -2:
+ tirePWM[TIRE_FL] = -50;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 50;
+ 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;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = -50;
+ tirePWM[TIRE_BL] = 40;
+ tirePWM[TIRE_BR] = 50;
+ tirePWM[TIRE_FR] = -40;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = -50;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = 50;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = -40;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = 40;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ count++;
+ }
+ tirePWM[TIRE_FL] = -50;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = 50;
+ tirePWM[TIRE_FR] = -50;
+ 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;
+ }
+
+ 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 == 2) {
+ count = 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;
+ if(linePara[LINE_TOW_1] == 0) {
+ lineFase = 5;
+ 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 == 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) { // タオル 竿検知
+ if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
+ lineFase = 7;
+ 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_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;
+ 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 = 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 = 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;
+ }
+ } else if(lineFase == 7) { // ライン 修正
+ if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == '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_1] > 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_1] < 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_1] == 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 = 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 == 8) { // タオル1 解放
+ //solenoid.TOWEL1 = 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] = 0;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = 40;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = -40;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = -50;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 40;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = -40;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case -2:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ count++;
+ }
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 50;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = -50;
+ 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(count == 1) {
+ count = 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 = 20;
+ motor[TIRE_BL].pwm = 20;
+ motor[TIRE_BR].pwm = 20;
+ motor[TIRE_FR].pwm = 20;
+ } else if(lineFase == 11) {
+ switch(linePara[4]) {
+ case -2:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = -50;
+ 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;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = -40;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = 40;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = -50;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = 50;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = 40;
+ tirePWM[TIRE_BL] = -50;
+ tirePWM[TIRE_BR] = -40;
+ tirePWM[TIRE_FR] = 50;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = -50;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = 50;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = -50;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 50;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ count++;
+ }
+ tirePWM[TIRE_FL] = 50;
+ tirePWM[TIRE_BL] = -50;
+ tirePWM[TIRE_BR] = -50;
+ tirePWM[TIRE_FR] = 50;
+ 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;//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_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]);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(count == 3) {
+ count = 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 {
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+
}
#endif
#if USE_PROCESS_NUM>6
static void Process6()
{
-
- for(int i = 0; i < 8; i++){
+ for(int i = 0; i < 8; i++) {
linePara[i] = lineCast(LineHub::GetPara(i));
}
- static int count = 100000;
- count++;
-
- if(count < 10000) {
- lineCheck = false;
- } else {
- lineCheck = true;
+ 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
+
}
-
- if(lineFase == 0) { // 前進
- switch(linePara[0]) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FR].pwm = 0;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BL].pwm = 0;
- break;
- case -3:
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FR].pwm = 10;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BL].pwm = 10;
- break;
- case -1:
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FR].pwm = 20;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BL].pwm = 20;
- break;
- case 0:
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BL].pwm = 30;
- break;
- case 1:
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_BR].pwm = 20;
- motor[TIRE_BL].pwm = 30;
- break;
- case 3:
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 10;
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_BR].pwm = 10;
- motor[TIRE_BL].pwm = 30;
- break;
- case 2:
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 0;
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_BR].pwm = 0;
- motor[TIRE_BL].pwm = 30;
- break;
- case 'A':
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BL].pwm = 30;
- if(lineCheck == true) {
- lineCount++;
- count = 0;
- }
- default:
- 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_FR].pwm = 30;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BL].pwm = 30;
- }
- if(lineCount == 1) {
- lineFase = 1;
- }
- } else if(lineFase == 1) { // 前進 低速
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_FR].pwm = 15;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_BL].pwm = 15;
- if(linePara[4] == 0) {
- lineFase = 2;
- 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_FR].pwm = 30;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BL].pwm = 30;
- }
- } else if(lineFase == 2){ // 位置調整
- lineFase = 3;
- } else if(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 = 30;
- motor[TIRE_FR].pwm = 30;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_BL].pwm = 30;
- }
-
}
#endif