Miya Miyagawa
The last version programs
Dependencies: mbed TrapezoidControl Pulse QEI
Diff: System/Process/Process.cpp
- Revision:
- 29:5365ee0521f6
- Parent:
- 28:479631c2de29
- Child:
- 30:76b140d176f0
--- a/System/Process/Process.cpp Thu Oct 03 05:33:48 2019 +0000
+++ b/System/Process/Process.cpp Sun Oct 06 03:53:06 2019 +0000
@@ -159,7 +159,7 @@
{
double Distance=0;
double Duration=0;
- double temp=28;////////////////////////////////////////////////温度
+ double temp=26;////////////////////////////////////////////////温度
if(num==0) {
Trig0.write_us(1,10);
Duration=Echo0.read_high_us(5000);
@@ -226,14 +226,14 @@
// ************* Line ************** //
const int omni[15][15] = {
- { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
- { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
+ { 0, 5, 21, 47, 83, 130, 207, 255, 255, 255, 255, 255, 255, 255, 255 },
+ { -5, 0, 5, 21, 47, 83, 130, 207, 193, 208, 234, 255, 255, 255, 255 },
{ -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
{ -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
{ -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
{ -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
- { -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
- { -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
+ { -207, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
+ { -255, -207, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
{ -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
{ -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
{ -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
@@ -378,8 +378,8 @@
}
-double Ult_left=UltraRead(0);///////////////////////////////////////////left sensor
-double Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
+double Ult_left;///////////////////////////////////////////left sensor
+double Ult_right;//////////////////////////////////////////right sensor
static int Limitphase=0;
void SystemProcess()
{
@@ -392,10 +392,15 @@
}
Ult_left=UltraRead(0);//////////////////////////////////////////left sensor
Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
- pc.printf("%lf,%lf",UltraRead(0),UltraRead(1));
+ for(int i=0; i<19; i++) {
+ pc.printf("%d",LimitSw::IsPressed(i));
+ if(i==18) {
+ pc.printf("\r\n");
+ }
+ }
+// pc.printf("%lf,%lf\r\n",Ult_left,Ult_right);
// 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]);
-
- pc.printf("%d\n\r",current);
+// pc.printf("%d\n\r",current);
/*上
motor[LIFT_LB].dir = FOR;
motor[LIFT_LB].pwm = 180;
@@ -423,7 +428,7 @@
if(LimitSw::IsPressed(REDBLUE_SW)) {
current = 4;
} else {
- current = 5;
+ current = 4;
}
}
buzzer.period(1.0/800);
@@ -487,7 +492,7 @@
{
PIDflag = false;
- LedOut(0);
+ LedOut(0);
if(controller->Button.UP) {
motor[LIFT_LB].dir = FOR;
motor[LIFT_LB].pwm = 180;
@@ -545,12 +550,27 @@
motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
}
- if(controller->Button.ZR) {
- Air[CLOTHESPIN] = SOLENOID_ON;
+ if(controller->Button.L) {
+ air[0] = SOLENOID_ON;
+ } else {
+ air[0] = SOLENOID_OFF;
}
if(controller->Button.ZL) {
- Air[CLOTHESPIN] = 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
@@ -725,7 +745,7 @@
} else if(lineFase == 3) { // 右 ライントレース
switch(linePara[4]) {
case -2:
- LedOut(2);
+ LedOut(2);
tirePWM[TIRE_FL] = 30;
tirePWM[TIRE_BL] = 0;
tirePWM[TIRE_BR] = -30;
@@ -841,7 +861,7 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
}
- if(linePara[LINE_TOW_1] == 0) {
+ if(linePara[LINE_TOW_2] == 0) {
if(!LimitSw::IsPressed(SHEETS_SW)) {
lineFase=100;
} else {
@@ -863,17 +883,30 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
} else if(lineFase == 6) { // タオル1 検知
- LedOut(4);
+ LedOut(4);
+ static int Towlcount0=0;
+ Towlcount0++;
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;
+ if(Towlcount0<=20) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FR].pwm = 50;
+ } else {
+ 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;
@@ -893,826 +926,338 @@
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 = 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) { // 前 ライントレース
- LedOut(2);
- 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 竿検知
- LedOut(4);
- 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 = 20;
- motor[TIRE_BL].dir = BACK;
- 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 == 15 ){ // ライン 修正
- 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 = 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 解放
- 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 = 17;
- } else if(lineFase == 17) { // 前
- if(LimitSw::IsPressed(LSW_UU)) {
- motor[LIFT_U].dir=BRAKE;
- motor[LIFT_U].pwm=100;
+ 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[LIFT_U].dir=BACK;
- motor[LIFT_U].pwm=180;
+ 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 解放
+ 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) { // 前 ライントレース
+ LedOut(2);
switch(linePara[LINE_TOW_2]) {
case 2:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = -30;
+ tirePWM[TIRE_FL] = 10;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -10;
+ tirePWM[TIRE_FR] = -20;
adjAnable = true;
break;
case 3:
- tirePWM[TIRE_FL] = 10;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -10;
- tirePWM[TIRE_FR] = -30;
+ tirePWM[TIRE_FL] = 14;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -14;
+ tirePWM[TIRE_FR] = -20;
adjAnable = true;
break;
case 1:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -30;
+ tirePWM[TIRE_FL] = 17;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -17;
+ tirePWM[TIRE_FR] = -20;
adjAnable = true;
break;
case 0:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -30;
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -20;
adjAnable = true;
break;
case -1:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -20;
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 17;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -17;
adjAnable = true;
break;
case -3:
- tirePWM[TIRE_FL] = 30;
+ 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] = -30;
+ tirePWM[TIRE_BR] = -20;
tirePWM[TIRE_FR] = -10;
adjAnable = true;
break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ if(adjAnable) {
+ adj = 0;
+ } else {
+ adj = 0;
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(countW == 1) {
+ countW = 0;
+ lineFase = 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;
@@ -1720,6 +1265,48 @@
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;
@@ -1727,9 +1314,389 @@
countW++;
}
tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BL] = -30;
tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -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 竿検知
+ LedOut(4);
+ static int Towlcount1=0;
+ Towlcount1++;
+ if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
+ if(Towlcount1<=20) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].pwm = 50;
+
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FR].pwm = 50;
+ } else {
+ 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 = 20;
+ motor[TIRE_BL].dir = BACK;
+ 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 == 15 ) { // ライン 修正
+ 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 = 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 解放
+ 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 = 17;
+ } else if(lineFase == 17) { // 前
+ 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':
+ 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':
@@ -1778,10 +1745,10 @@
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_BL].pwm=15;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
if(LimitSw::IsPressed(LSW_UU)) {
motor[LIFT_U].dir=BRAKE;
motor[LIFT_U].pwm=100;
@@ -1813,18 +1780,18 @@
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=BACK;
motor[TIRE_FL].pwm=0;
- motor[TIRE_FR].pwm=15;
+ motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=0;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_BR].pwm=20;
} else if(Ult_right<14) { //Ult_rightが近い場合
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=0;
- motor[TIRE_FR].pwm=15;
+ motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=0;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_BR].pwm=20;
}
} else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
if(Ult_left>16) { //Ult_leftが遠い場合
@@ -1832,18 +1799,18 @@
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=BACK;
- motor[TIRE_FL].pwm=15;
+ motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=0;
- motor[TIRE_BL].pwm=15;
+ motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=0;
} else if(Ult_left<14) { //Ult_leftが近い場合
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=FOR;
- motor[TIRE_FL].pwm=15;
+ motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=0;
- motor[TIRE_BL].pwm=15;
+ motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=0;
}
} else { //どっちもあってない場合
@@ -1854,19 +1821,19 @@
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
- motor[TIRE_FL].pwm=15;
- motor[TIRE_FR].pwm=15;
- motor[TIRE_BL].pwm=15;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
- motor[TIRE_FL].pwm=15;
- motor[TIRE_FR].pwm=15;
- motor[TIRE_BL].pwm=15;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
}
} else { //傾きが大きくなくて離れているとき
if((Ult_right+Ult_left)<=25) { //近すぎるとき
@@ -1896,19 +1863,19 @@
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
- motor[TIRE_FL].pwm=15;
- motor[TIRE_FR].pwm=15;
- motor[TIRE_BL].pwm=15;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
- motor[TIRE_FL].pwm=15;
- motor[TIRE_FR].pwm=15;
- motor[TIRE_BL].pwm=15;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
}
}
}
@@ -2027,6 +1994,19 @@
lineFase=103;
}
} else if(lineFase==103) {
+ static int PuluPulu=0;
+ if(PuluPulu>100) {
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=255;
+ motor[TIRE_FR].pwm=255;
+ motor[TIRE_BL].pwm=255;
+ motor[TIRE_BR].pwm=255;
+
+ lineFase=104;//system lineFase increasing
+ }
if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
motor[TIRE_FL].dir=BRAKE;
@@ -2045,18 +2025,18 @@
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=BACK;
motor[TIRE_FL].pwm=0;
- motor[TIRE_FR].pwm=13;
+ motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=0;
- motor[TIRE_BR].pwm=13;
+ motor[TIRE_BR].pwm=20;
} else if(Ult_right<14) { //Ult_rightが近い場合
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=0;
- motor[TIRE_FR].pwm=13;
+ motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=0;
- motor[TIRE_BR].pwm=13;
+ motor[TIRE_BR].pwm=20;
}
} else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
if(Ult_left>16) { //Ult_leftが遠い場合
@@ -2064,62 +2044,66 @@
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=BACK;
- motor[TIRE_FL].pwm=13;
+ motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=0;
- motor[TIRE_BL].pwm=13;
+ motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=0;
} else if(Ult_left<14) { //Ult_leftが近い場合
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=FOR;
- motor[TIRE_FL].pwm=13;
+ motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=0;
- motor[TIRE_BL].pwm=13;
+ motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=0;
}
} else { //どっちもあってない場合
if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
+ PuluPulu++;
motor[TIRE_FL].dir=FOR;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
- motor[TIRE_FL].pwm=15;
- motor[TIRE_FR].pwm=15;
- motor[TIRE_BL].pwm=15;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
+ PuluPulu++;
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
- motor[TIRE_FL].pwm=15;
- motor[TIRE_FR].pwm=15;
- motor[TIRE_BL].pwm=15;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
}
}
} else { //さほど離れてはいないが傾きが大きいとき
if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
+ PuluPulu++;
motor[TIRE_FL].dir=FOR;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
- motor[TIRE_FL].pwm=15;
- motor[TIRE_FR].pwm=15;
- motor[TIRE_BL].pwm=15;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
+ PuluPulu++;
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
- motor[TIRE_FL].pwm=15;
- motor[TIRE_FR].pwm=15;
- motor[TIRE_BL].pwm=15;
- motor[TIRE_BR].pwm=15;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
}
}
}
@@ -2137,7 +2121,7 @@
} else if(lineFase==104) {
static int count3=0;
static int loop=0;
- pc.printf("%d\r\n",loop);
+// pc.printf("%d\r\n",loop);
Air[CLOTHESPIN]=1;
if(count3==0) {
loop++;
@@ -2211,7 +2195,7 @@
if(linePara[4]!='N') {
lineFase=19;
}
- } else if(lineFase == 18) { // 前 低速
+ } else if(lineFase == 20) { // 前 低速
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].dir = FOR;
motor[TIRE_BR].dir = BACK;
@@ -2342,9 +2326,9 @@
}
*/
- targetCount = 3;
+ //targetCount = 3;
- if(countW == targetCount) {
+ if(countW == 1) {
countW = 0;
lineFase = 20;
lineCount = 0;
@@ -2355,14 +2339,14 @@
motor[LIFT_U].dir=FOR;
motor[LIFT_U].pwm=150;
} else {
- motor[LIFT_U].dir=FOR;
+ motor[LIFT_U].dir=BACK;
motor[LIFT_U].pwm=150;
}
motor[TIRE_FL].dir = BACK;
motor[TIRE_BL].dir = FOR;
motor[TIRE_BR].dir = FOR;
motor[TIRE_FR].dir = BACK;
- if (linePara[2] == 'N') {
+ if (linePara[2] == 0) {
//(!!LimitSw::Ispressed(SHEETS_SW)) {
//ineFase = 20;
//else if(LimitSw::IsPressed(TOWEL2_SW) {
@@ -2523,7 +2507,7 @@
青ゾーン 青ゾーン 青ゾーン
// ************************************** */
-
+ LED_DEBUG0 = LED_ON;
for(int i = 0; i < 8; i++) {
linePara[i] = lineCast(LineHub::GetPara(i));
}
@@ -2790,17 +2774,31 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
} else if(lineFase == 6) { // タオル1 竿検知
- LedOut(4);
+ LedOut(4);
+ static int Towlcount0=0;
+ Towlcount0++;
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;
+ if(Towlcount0<=20) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].pwm = 50;
+
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FR].pwm = 50;
+ } else {
+ 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 = 20;
@@ -2959,7 +2957,7 @@
motor[TIRE_FR].dir = BRAKE;
lineFase = 9;
} else if(lineFase == 9) { // 前
- LedOut(2);
+ LedOut(2);
switch(linePara[LINE_TOW_1]) {
case 2:
tirePWM[TIRE_FL] = 10;
@@ -3203,17 +3201,31 @@
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
} else if(lineFase == 14) { // タオル2 竿検知
- LedOut(4);
+ LedOut(4);
+ static int Towlcount1=0;
+ Towlcount1++;
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;
+ if(Towlcount1<=20) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].pwm = 50;
+
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FR].pwm = 50;
+ } else {
+ 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;
@@ -3469,6 +3481,10 @@
motor[TIRE_BL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
if(LimitSw::IsPressed(LSW_UU)) {
motor[LIFT_U].dir=BRAKE;
motor[LIFT_U].pwm=100;
@@ -3711,6 +3727,18 @@
lineFase=103;
}
} else if(lineFase==103) {
+ static int PuluPulu=0;
+ if(PuluPulu>=100) {
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=255;
+ motor[TIRE_FR].pwm=255;
+ motor[TIRE_BL].pwm=255;
+ motor[TIRE_BR].pwm=255;
+ lineFase=104;//system lineFase increasing
+ }
if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
motor[TIRE_FL].dir=BRAKE;
@@ -3766,44 +3794,48 @@
if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
+ PuluPulu++;
motor[TIRE_FL].dir=FOR;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
- motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
+ PuluPulu++;
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
- motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
}
}
} else { //さほど離れてはいないが傾きが大きいとき
if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
+ PuluPulu++;
motor[TIRE_FL].dir=FOR;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
- motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
+ PuluPulu++;
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
- motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
- motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
+ motor[TIRE_FL].pwm=20;
+ motor[TIRE_FR].pwm=20;
+ motor[TIRE_BL].pwm=20;
+ motor[TIRE_BR].pwm=20;
}
}
}
@@ -3895,7 +3927,7 @@
if(linePara[4]!='N') {
lineFase=19;
}
- } else if(lineFase == 18) { // 前 低速
+ } else if(lineFase == 20) { // 前 低速
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].dir = FOR;
motor[TIRE_BR].dir = BACK;
@@ -4019,9 +4051,9 @@
}
*/
- targetCount = 3;
+
- if(countW == targetCount) {
+ if(countW == 1) {
countW = 0;
lineFase = 20;
lineCount = 0;
@@ -4039,7 +4071,6 @@
//ineFase = 14;
//else {
lineFase = 21;
- //
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -4146,7 +4177,7 @@
lineCount++;
if(lineCount > 20) lineCheck = false;
}
- if(countW == 2) {
+ if(countW == 1) {
countW = 0;
lineFase = 22;
lineCount = 0;
@@ -4196,53 +4227,41 @@
} else if(Limitphase==1) {
} else if(Limitphase==2) {
//下→上とタオル展開
- pc.printf("%d\r\n",SW_flag);
-
- motor[LIFT_LB].dir = FOR;
- motor[LIFT_LB].pwm = 200;
- motor[LIFT_RB].dir = BACK;
- motor[LIFT_RB].pwm = 180;
-
- if(SW_flag==0) {
+ static int countZYOUGE=0;
+ countZYOUGE++;
+ if(countZYOUGE<=20) {
motor[LIFT_LB].dir = FOR;
- motor[LIFT_LB].pwm = 200;
- motor[LIFT_RB].dir = BACK;
- motor[LIFT_RB].pwm = 180;
- if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
- SW_flag=1;
- }
- } else if(SW_flag==1) {
- motor[LIFT_LB].dir = FOR;
- motor[LIFT_LB].pwm = 200;
+ motor[LIFT_LB].pwm = 180;
motor[LIFT_RB].dir = BACK;
- motor[LIFT_RB].pwm = 180;
+ motor[LIFT_RB].pwm = 200;
+ } else {
if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
- SW_flag=2;
- }
- } else if(SW_flag==2) {
- motor[LIFT_LB].dir = FOR;
- motor[LIFT_LB].pwm = 200;
- motor[LIFT_RB].dir = BACK;
- motor[LIFT_RB].pwm = 180;
- if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
- SW_flag=3;
- }
- } else if(SW_flag==3) {
- motor[LIFT_LB].dir = FOR;
- motor[LIFT_LB].pwm = 200;
- motor[LIFT_RB].dir = BACK;
- motor[LIFT_RB].pwm = 180;
- if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
+ Limitphase=4;
motor[LIFT_LB].dir = BRAKE;
motor[LIFT_LB].pwm = 200;
motor[LIFT_RB].dir = BRAKE;
- motor[LIFT_RB].pwm = 180;
- Limitphase=4;
+ motor[LIFT_RB].pwm = 200;
+ } else if(!(LimitSw::IsPressed(LSW_LB))&&!(LimitSw::IsPressed(LSW_RB))) {
+ motor[LIFT_LB].dir = FOR;
+ motor[LIFT_LB].pwm = 180;
+ motor[LIFT_RB].dir = BACK;
+ motor[LIFT_RB].pwm = 200;
+ } else if(!(LimitSw::IsPressed(LSW_LB))&&LimitSw::IsPressed(LSW_RB)) {
+ motor[LIFT_LB].dir = FOR;
+ motor[LIFT_LB].pwm = 180;
+ motor[LIFT_RB].dir = BRAKE;
+ motor[LIFT_RB].pwm = 200;
+ } else if(LimitSw::IsPressed(LSW_LB)&&!(LimitSw::IsPressed(LSW_RB))) {
+ motor[LIFT_LB].dir = BRAKE;
+ motor[LIFT_LB].pwm = 150;
+ motor[LIFT_RB].dir = BACK;
+ motor[LIFT_RB].pwm = 200;
}
}
} else if(Limitphase==3) {
} else if(Limitphase==4) {
//上段待機
+ Air[TOWEL0]=1;
motor[LIFT_LB].dir = BRAKE;
motor[LIFT_LB].pwm = 200;
motor[LIFT_RB].dir = BRAKE;
@@ -4269,22 +4288,19 @@
startFlag=true;
Processflag=1;
}
- } else if(Processflag==1) {
- motor[LIFT_LB].dir = FOR;
- motor[LIFT_LB].pwm = 200;
- motor[LIFT_RB].dir = BACK;
- motor[LIFT_RB].pwm = 180;
+
+ motor[LIFT_RB].pwm = 200;
if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
motor[LIFT_LB].dir = BRAKE;
motor[LIFT_LB].pwm = 200;
motor[LIFT_RB].dir = BRAKE;
- motor[LIFT_RB].pwm = 180;
+ motor[LIFT_RB].pwm = 200;
current=0;
startFlag=true;
Processflag=1;
}
- if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)){
- motor[LIFT_LB].dir = BRAKE;
+ if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)) {
+ motor[LIFT_LB].dir = BRAKE;
motor[LIFT_LB].pwm = 200;
motor[LIFT_RB].dir = BRAKE;
motor[LIFT_RB].pwm = 200;
@@ -4294,7 +4310,7 @@
current=0;
startFlag=true;
Processflag=1;
- }
+ }
}
}