大季 矢花
aa
Diff: System/Process/Process.cpp
- Revision:
- 27:dd9f27fce7d1
- Parent:
- 26:4c0ce2f05688
- Child:
- 28:479631c2de29
--- a/System/Process/Process.cpp Sat Sep 28 23:02:17 2019 +0000
+++ b/System/Process/Process.cpp Thu Oct 03 05:25:43 2019 +0000
@@ -1,6 +1,7 @@
#include "mbed.h"
#include "Process.h"
-
+#include "Pulse.h"
+#include <stdlib.h>
#include "../../CommonLibraries/PID/PID.h"
#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
#include "../../Communication/RS485/LineHub/LineHub.h"
@@ -8,7 +9,7 @@
#include "../../Input/ExternalInt/ExternalInt.h"
#include "../../Input/Switch/Switch.h"
#include "../../Input/Encoder/Encoder.h"
-#include "../../Input/Ultrasonic/Ultrasonic.h"
+
#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"
@@ -16,7 +17,7 @@
using namespace SWITCH;
using namespace PID_SPACE;
using namespace ENCODER;
-using namespace ULTRASONIC;
+
using namespace LINEHUB;
@@ -38,10 +39,6 @@
/*Replace here with the definition code of your variables.*/
-USS ultrasonic[] = {
- USS(ECHO_0,TRIG_0,TEMP),
- USS(ECHO_1,TRIG_1,TEMP),
- };
Serial pc(USBTX, USBRX);
@@ -72,12 +69,29 @@
//*************** lift ***************
+//*****************Air********************
+DigitalOut air[]= {
+ DigitalOut(ECD_A_0),
+ DigitalOut(ECD_B_0),
+ DigitalOut(ECD_A_1),
+ DigitalOut(ECD_B_1),
+};
+
+bool Air[4];
+void AirUpdate()
+{
+ for(int i=0; i<=3; i++) {
+ air[i]=Air[i];
+ }
+}
+//*****************Air********************
+
//*************tire*************
PID rotaconPID[] = {
- PID(0.0001,-1,1,0.05,0,0), //LF
- PID(0.0001,-1,1,0.05,0,0), //LB
- PID(0.0001,-1,1,0.05,0,0), //RB
- PID(0.0001,-1,1,0.05,0,0), //RF
+ PID(0.0001,-1,1,0.05,0,0), //LF
+ PID(0.0001,-1,1,0.05,0,0), //LB
+ PID(0.0001,-1,1,0.05,0,0), //RB
+ PID(0.0001,-1,1,0.05,0,0), //RF
};
#define FL 0
@@ -91,9 +105,9 @@
const float ucR = 420.0; //中心からのタイヤの距離
typedef struct {
- float Vx; //X方向の速度
- float Vy; //Y方向の速度
- float Va; //角速度
+ float Vx; //X方向の速度
+ float Vy; //Y方向の速度
+ float Va; //角速度
} Vvector;
Vvector move; //進む速度
@@ -135,6 +149,57 @@
bool countFlag;
//*************tire**************//
+// ************* Ultra ************** //
+//double temp=( Temp.read()* 3.3 - 0.6) / 0.01;
+PulseInOut Echo0(ECHO_0);
+PulseInOut Echo1(ECHO_1);
+PulseInOut Trig0(TRIG_0);
+PulseInOut Trig1(TRIG_1);
+double UltraRead(int num)
+{
+ double Distance=0;
+ double Duration=0;
+ double temp=28;////////////////////////////////////////////////温度
+ if(num==0) {
+ Trig0.write_us(1,10);
+ Duration=Echo0.read_high_us(5000);
+ } else if(num==1) {
+ Trig1.write_us(1,10);
+ Duration=Echo1.read_high_us(5000);
+ }
+ if(Duration>0) {
+ Duration=Duration/2;
+ double sspead=331.5+0.6*temp;
+ Distance=Duration*sspead*100/1000000;
+ } else {
+ return 0;
+ }
+ return Distance;
+}
+//*********Ultra***************
+
+//*********Tape LED**************************
+DigitalOut select1(SELECT_1);
+DigitalOut select2(SELECT_2);
+DigitalOut select3(SELECT_3);
+void LedOut(int num)
+{
+ int selectnum[8][3]= {
+ {0,0,0},
+ {0,0,1},
+ {0,1,0},
+ {0,1,1},
+ {1,0,0},
+ {1,0,1},
+ {1,1,0},
+ {1,1,1}
+ };
+ select1=selectnum[num][0];
+ select2=selectnum[num][1];
+ select3=selectnum[num][2];
+}
+//*********Tape LED**************************
+
// ************* Line ************** //
Timer tow_stop;
@@ -160,38 +225,39 @@
// ************* 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 },
-{ -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 },
-{ -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 },
-{ -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
-{ -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
-{ -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
-{ -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
+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 },
+ { -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 },
+ { -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 },
+ { -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
+ { -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
+ { -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
+ { -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
};
const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
uint8_t SetStatus(int);
-uint8_t SetStatus(int pwmVal) {
- if (pwmVal < 0) return BACK;
- else if (pwmVal > 0) return FOR;
- else if (pwmVal == 0) return BRAKE;
- else return BRAKE;
+uint8_t SetStatus(int pwmVal)
+{
+ if (pwmVal < 0) return BACK;
+ else if (pwmVal > 0) return FOR;
+ else if (pwmVal == 0) return BRAKE;
+ else return BRAKE;
}
uint8_t SetPWM(int);
-uint8_t SetPWM(int pwmVal) {
- if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
- else return abs(pwmVal);
+uint8_t SetPWM(int pwmVal)
+{
+ if (pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255;
+ else return abs(pwmVal);
}
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
@@ -231,461 +297,996 @@
void SystemProcessInitialize()
{
- #pragma region USER-DEFINED_VARIABLE_INIT
- /*Replace here with the initialization code of your variables.*/
- //rotaconPIDtimer.attach(tirePID,0.1);
-
- //DigitalOut Air_16(LS_16);
- //DigitalOut Air_17(LS_17);
- //DigitalOut Air_18(LS_18);
- //DigitalOut Air_19(LS_19);
-
- #pragma endregion USER-DEFINED_VARIABLE_INIT
+ #pragma region USER-DEFINED_VARIABLE_INIT
+ /*Replace here with the initialization code of your variables.*/
+ //rotaconPIDtimer.attach(tirePID,0.1);
- lock = true;
- processChangeComp = true;
- current = DEFAULT_PROCESS;
+ //DigitalOut Air_16(LS_16);
+ //DigitalOut Air_17(LS_17);
+ //DigitalOut Air_18(LS_18);
+ //DigitalOut Air_19(LS_19);
+
+ #pragma endregion USER-DEFINED_VARIABLE_INIT
+
+ lock = true;
+ processChangeComp = true;
+ current = DEFAULT_PROCESS;
- #ifdef USE_SUBPROCESS
- #if USE_PROCESS_NUM>0
- Process[0] = Process0;
- #endif
- #if USE_PROCESS_NUM>1
- Process[1] = Process1;
- #endif
- #if USE_PROCESS_NUM>2
- Process[2] = Process2;
- #endif
- #if USE_PROCESS_NUM>3
- Process[3] = Process3;
- #endif
- #if USE_PROCESS_NUM>4
- Process[4] = Process4;
- #endif
- #if USE_PROCESS_NUM>5
- Process[5] = Process5;
- #endif
- #if USE_PROCESS_NUM>6
- Process[6] = Process6;
- #endif
- #if USE_PROCESS_NUM>7
- Process[7] = Process7;
- #endif
- #if USE_PROCESS_NUM>8
- Process[8] = Process8;
- #endif
- #if USE_PROCESS_NUM>9
- Process[9] = Process9;
- #endif
- #endif
+#ifdef USE_SUBPROCESS
+#if USE_PROCESS_NUM>0
+ Process[0] = Process0;
+#endif
+#if USE_PROCESS_NUM>1
+ Process[1] = Process1;
+#endif
+#if USE_PROCESS_NUM>2
+ Process[2] = Process2;
+#endif
+#if USE_PROCESS_NUM>3
+ Process[3] = Process3;
+#endif
+#if USE_PROCESS_NUM>4
+ Process[4] = Process4;
+#endif
+#if USE_PROCESS_NUM>5
+ Process[5] = Process5;
+#endif
+#if USE_PROCESS_NUM>6
+ Process[6] = Process6;
+#endif
+#if USE_PROCESS_NUM>7
+ Process[7] = Process7;
+#endif
+#if USE_PROCESS_NUM>8
+ Process[8] = Process8;
+#endif
+#if USE_PROCESS_NUM>9
+ Process[9] = Process9;
+#endif
+#endif
}
static void SystemProcessUpdate()
{
- #ifdef USE_SUBPROCESS
- if(controller->Button.HOME) lock = false;
-
- if(controller->Button.START && processChangeComp)
- {
- current++;
- if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
- processChangeComp = false;
- }
- else if(controller->Button.SELECT && processChangeComp)
- {
- current--;
- if (current < 0) current = 0;
- processChangeComp = false;
- }
- else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
- #endif
-
- #ifdef USE_MOTOR
- ACTUATORHUB::MOTOR::Motor::Update(motor);
- #endif
-
- #ifdef USE_SOLENOID
- ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
- #endif
+#ifdef USE_SUBPROCESS
+ if(controller->Button.HOME) lock = false;
+
+ if(controller->Button.START && processChangeComp) {
+ current++;
+ if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
+ processChangeComp = false;
+ } else if(controller->Button.SELECT && processChangeComp) {
+ current--;
+ if (current < 0) current = 0;
+ processChangeComp = false;
+ } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
+#endif
+
+#ifdef USE_MOTOR
+ ACTUATORHUB::MOTOR::Motor::Update(motor);
+#endif
+
+#ifdef USE_SOLENOID
+ ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
+#endif
+
+#ifdef USE_RS485
+ ACTUATORHUB::ActuatorHub::Update();
+ //LINEHUB::LineHub::Update();
+#endif
+
+}
+
+
+double Ult_left=UltraRead(0);///////////////////////////////////////////left sensor
+double Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
+static int Limitphase=0;
+void SystemProcess()
+{
+ SystemProcessInitialize();
+ while(1) {
+ AirUpdate();
+ int g[8];
+ for(int i = 0; i < 8; i++) {
+ g[i] = lineCast(LineHub::GetPara(i));
+ }
+ Ult_left=UltraRead(0);//////////////////////////////////////////left sensor
+ Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
+ pc.printf("%lf,%lf",UltraRead(0),UltraRead(1));
+// 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]);
- #ifdef USE_RS485
- ACTUATORHUB::ActuatorHub::Update();
- //LINEHUB::LineHub::Update();
- #endif
-
+ pc.printf("%d\n\r",current);
+ /*上
+ motor[LIFT_LB].dir = FOR;
+ motor[LIFT_LB].pwm = 180;
+ motor[LIFT_RB].dir = BACK;
+ motor[LIFT_RB].pwm = 200;
+ */
+ if(!(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW))&&startFlag==true) {
+ startFlag=false;
+ lock=false;
+ current=7;
+ } else {
+ motor[LIFT_LB].dir = BRAKE;
+ motor[LIFT_LB].pwm = 180;
+ motor[LIFT_RB].dir = BRAKE;
+ motor[LIFT_RB].pwm = 200;
+ }
+ if(LimitSw::IsPressed(START_SW) && startFlag == true) {
+ LedOut(6);
+ startFlag = false;
+ lock = false;
+ lineFase = 0;
+ lineCount = 0;
+ lineCheck = false;
+ countW = 0;
+ if(LimitSw::IsPressed(REDBLUE_SW)) {
+ current = 4;
+ } else {
+ current = 5;
+ }
+ }
+ buzzer.period(1.0/800);
+
+#ifdef USE_MU
+ controller = CONTROLLER::Controller::GetData();
+#endif
+
+#ifdef USE_ERRORCHECK
+ if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag) {
+ CONTROLLER::Controller::DataReset();
+ AllActuatorReset();
+ lock = true;
+ } else
+#endif
+ {
+
+#ifdef USE_SUBPROCESS
+ if(!lock) {
+ Process[current]();
+ } else
+#endif
+ {
+ //ロック時の処理
+ }
+ }
+
+
+ //Emergency!
+ /*
+ if(!EMG_0 && !EMG_1 && !EMGflag){
+ buzzer = 0;
+ BuzzerTimer.attach(BuzzerTimer_func, 1);
+ EMGflag = true;
+ LED_DEBUG0 = 1;
+ }
+ if(EMG_0 && EMG_1 && EMGflag){
+ buzzer = 1;
+ BuzzerTimer.detach();
+ EMGflag = false;
+ }
+ */
+ SystemProcessUpdate();
+ }
}
-void SystemProcess()
-{
- SystemProcessInitialize();
-
- while(1)
- {
- int g[8];
- for(int i = 0; i < 8; i++){
- g[i] = lineCast(LineHub::GetPara(i));
- }
-
- pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
-
-
- if(LimitSw::IsPressed(START_SW) && startFlag == true) {
- startFlag == false;
- lock = false;
- lineFase = 0;
- lineCount = 0;
- lineCheck = false;
- countW = 0;
- if(LimitSw::IsPressed(REDBLUE_SW)) {
- current = 4;
- } else {
- current = 5;
- }
- }
-
- buzzer.period(1.0/800);
-
- #ifdef USE_MU
- controller = CONTROLLER::Controller::GetData();
- #endif
-
- #ifdef USE_ERRORCHECK
- if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag)
- {
- CONTROLLER::Controller::DataReset();
- AllActuatorReset();
- lock = true;
- }
- else
- #endif
- {
-
- #ifdef USE_SUBPROCESS
- if(!lock)
- {
- Process[current]();
- }
- else
- #endif
- {
- //ロック時の処理
- }
- }
-
-
- //Emergency!
- /*
- if(!EMG_0 && !EMG_1 && !EMGflag){
- buzzer = 0;
- BuzzerTimer.attach(BuzzerTimer_func, 1);
- EMGflag = true;
- LED_DEBUG0 = 1;
- }
- if(EMG_0 && EMG_1 && EMGflag){
- buzzer = 1;
- BuzzerTimer.detach();
- EMGflag = false;
- }
- */
- SystemProcessUpdate();
- }
-}
-
-
-
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
-static void Process0()
-{
- AllActuatorReset();
+static void Process0()
+{
+ AllActuatorReset();
}
#endif
#if USE_PROCESS_NUM>1
static void Process1()
{
-
+
PIDflag = false;
-
+ LedOut(0);
if(controller->Button.UP) {
- motor[LIFT_LB].dir = FOR;
- motor[LIFT_LB].pwm = 180;
- motor[LIFT_RB].dir = BACK;
- motor[LIFT_RB].pwm = 200;
+ motor[LIFT_LB].dir = FOR;
+ motor[LIFT_LB].pwm = 180;
+ motor[LIFT_RB].dir = BACK;
+ motor[LIFT_RB].pwm = 200;
} else if(controller->Button.DOWN) {
- motor[LIFT_LB].dir = BACK;
- motor[LIFT_LB].pwm = 180;
- motor[LIFT_RB].dir = FOR;
- motor[LIFT_RB].pwm = 200;
+ motor[LIFT_LB].dir = BACK;
+ motor[LIFT_LB].pwm = 180;
+ motor[LIFT_RB].dir = FOR;
+ motor[LIFT_RB].pwm = 200;
} else if(controller->Button.LEFT) {
- motor[LIFT_LB].dir = FOR;
- motor[LIFT_LB].pwm = 180;
+ motor[LIFT_LB].dir = FOR;
+ motor[LIFT_LB].pwm = 180;
} else if(controller->Button.RIGHT) {
- motor[LIFT_RB].dir = BACK;
- motor[LIFT_RB].pwm = 180;
+ motor[LIFT_RB].dir = BACK;
+ motor[LIFT_RB].pwm = 180;
} else {
- motor[LIFT_LB].dir = BRAKE;
- motor[LIFT_LB].pwm = 255;
- motor[LIFT_RB].dir = BRAKE;
- motor[LIFT_RB].pwm = 255;
- }
-
-
- if(controller->Button.X) {
- motor[LIFT_U].dir = FOR;
- motor[LIFT_U].pwm = 180;
- } else if(controller->Button.Y) {
- motor[LIFT_U].dir = BACK;
- motor[LIFT_U].pwm = 230;
- } else {
- motor[LIFT_U].dir = BRAKE;
- motor[LIFT_U].pwm = 255;
+ motor[LIFT_LB].dir = BRAKE;
+ motor[LIFT_LB].pwm = 255;
+ motor[LIFT_RB].dir = BRAKE;
+ motor[LIFT_RB].pwm = 255;
}
- if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
- 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_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_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) ;
- } else {
- 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_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;
- }
- if(controller->Button.ZL) {
- solenoid.solenoid4 = SOLENOID_OFF;
- }
+
+ if(controller->Button.X) {
+ motor[LIFT_U].dir = FOR;
+ motor[LIFT_U].pwm = 180;
+ } else if(controller->Button.Y) {
+ motor[LIFT_U].dir = BACK;
+ motor[LIFT_U].pwm = 230;
+ } else {
+ motor[LIFT_U].dir = BRAKE;
+ motor[LIFT_U].pwm = 255;
+ }
+
+ if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
+ 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_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_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) ;
+ } else {
+ 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_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) {
+ Air[CLOTHESPIN] = SOLENOID_ON;
+ }
+ if(controller->Button.ZL) {
+ Air[CLOTHESPIN] = SOLENOID_OFF;
+ }
}
#endif
#if USE_PROCESS_NUM>2
static void Process2()
-{
-
-
-
-
-/*
- if(moving) {
- if(LimitSw::IsPressed(LSW_LB)) {
- if(switchFlag_LB) {
- switchFlag_LB = false;
- motor[LIFT_LB].dir = BRAKE;
- motor[LIFT_LB].pwm = 200;
- } else {
- switchFlag_LB = true;
- }
- }
- if(LimitSw::IsPressed(LSW_RB)) {
- if(switchFlag_RB) {
- switchFlag_RB = false;
- motor[LIFT_RB].dir = BRAKE;
- motor[LIFT_RB].pwm = 200;
- } else {
- switchFlag_RB = true;
- }
- }
- if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false;
- } else {
- if(controller->Button.UP) {
- if(!liftSstate == UPPER)) {
- liftState++;
- motor[LIFT_LB].dir = FOR;
- motor[LIFT_RB].dir = BACK;
- motor[LIFT_LB].pwm = 185;
- motor[LIFT_RB].pwm = 180;
- }
- } else if(controller->Button.DOWN) {
- if(!(liftstate == LOWER)) {
- liftState--;
- moving = true;
- motor[LIFT_LB].dir = BACK;
- motor[LIFT_RB].dir = FOR;
- motor[LIFT_LB].pwm = 180;
- motor[LIFT_RB].pwm = 185;
- }
- } else {
- motor[LIFT_LB].dir = BRAKE;
- motor[LIFT_RB].dir = BRAKE;
- motor[LIFT_LB].pwm = 200;
- motor[LIFT_RB].pwm = 200;
- }
- }
-*/
+{
}
#endif
#if USE_PROCESS_NUM>3
-static void Process3()
+static void Process3()
{
- startFlag = true;
- AllActuatorReset();
- lineFase = 0;
- lineCheck = false;
- lineCount = 0;
- countW = 0;
+ startFlag = true;
+ AllActuatorReset();
+ lineFase = 0;
+ lineCheck = false;
+ lineCount = 0;
+ countW = 0;
}
#endif
#if USE_PROCESS_NUM>4
-static void Process4()
+static void Process4()
{
- LED_DEBUG0 = LED_ON;
-
- /* ************************************** //
-
- 赤ゾーン 赤ゾーン 赤ゾーン
-
- // ************************************** */
-
-
- for(int i = 0; i < 8; i++) {
- linePara[i] = lineCast(LineHub::GetPara(i));
- }
-
- 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;
+ LED_DEBUG0 = LED_ON;
+
+ /* ************************************** //
+
+ 赤ゾーン 赤ゾーン 赤ゾーン
+
+ // ************************************** */
+
+
+ for(int i = 0; i < 8; i++) {
+ linePara[i] = lineCast(LineHub::GetPara(i));
+ }
+
+ if(lineFase == 0) {
+ LedOut(2);
+ 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;
+
+ } else if(lineFase == 1) { // 前 ライントレース
+ switch(linePara[0]) {
+ case -2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = 10;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -10;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 10;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -10;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 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(countW == 3) {
+ countW = 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] >= -1 && linePara[4] <= 1) {
+ lineFase = 3;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+ motor[TIRE_FL].pwm = 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:
+ LedOut(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;
+ }
+
+ targetCount = 1;
+
+ if(countW == targetCount) {
+ countW = 0;
+ lineFase = 4;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase == 4) { // 右 低速
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = FOR;
+ if (linePara[LINE_TOW_2] == 0) { // １と２逆になります。
+ lineFase = 6;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+ if(linePara[LINE_TOW_1] == 0) {
+ if(!LimitSw::IsPressed(SHEETS_SW)) {
+ lineFase=100;
+ } else {
+ 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 = 16;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].pwm = 16;
+ } else if (lineFase == 5) {
+ lineFase = 6;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ } else if(lineFase == 6) { // タオル1 検知
+ LedOut(4);
+ 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 {
+ 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]);
}
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].pwm = 30;
-
- } else if(lineFase == 1) { // 前 ライントレース
- switch(linePara[0]) {
- case -2:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = 10;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -10;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 10;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -10;
- adjAnable = true;
- break;
- case 2:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = 0;
- adjAnable = true;
- break;
- case 'A':
- if(lineCheck == false) {
- lineCheck = true;
- lineCount = 0;
- countW++;
- }
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 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 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;
}
-
- if(adjAnable){
- if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
+ } 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 {
- adj = 0;
+ 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] + adj);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + 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]);
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_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 == 3) {
+ if(countW == 1) {
countW = 0;
- lineFase = 2;
+ lineFase = 10;
lineCount = 0;
lineCheck = false;
}
- } else if(lineFase == 2) { // 前 低速
+ } 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 = 3;
+ lineFase = 11;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -694,9 +1295,9 @@
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]) {
+ motor[TIRE_FR].pwm = 16;
+ } else if(lineFase == 11) {
+ switch(linePara[4]) { // 右 ライントレース
case -2:
tirePWM[TIRE_FL] = 30;
tirePWM[TIRE_BL] = 0;
@@ -792,46 +1393,38 @@
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(countW == targetCount) {
+ if(countW == 2) {
countW = 0;
- lineFase = 4;
+ lineFase = 12;
lineCount = 0;
lineCheck = false;
}
- } else if(lineFase == 4) { // 右 低速
+ } 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 = 6;
+ 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 = 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].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 == 6) { // タオル1 検知
- if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
- lineFase = 7;
+ } 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;
@@ -840,35 +1433,181 @@
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
- } else if(LimitSw::IsPressed(TOW_1L)) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
+ } else if(LimitSw::IsPressed(TOW_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 = 16;
- } else if(LimitSw::IsPressed(TOW_1R)) {
+ motor[TIRE_FR].pwm = 20;
+ } 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_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 = 16;
+ motor[TIRE_FR].pwm = 20;
} 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;
+ if(LimitSw::IsPressed(QF_SW)) {
+ switch(linePara[LINE_TOW_1]) {
+ case -2:
+ tirePWM[TIRE_FL] = -10;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 10;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = -14;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 14;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = -17;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 17;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -17;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 17;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -14;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 14;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -10;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 10;
+ adjAnable = true;
+ break;
+ case 'A':
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
+ tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
+ tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
+ tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+ } else {
+ switch(linePara[LINE_TOW_1]) {
+ case 2:
+ tirePWM[TIRE_FL] = -10;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 10;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = -14;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 14;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = -17;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 17;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -17;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 17;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -14;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 14;
+ adjAnable = true;
+ break;
+ case -2:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -10;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 10;
+ adjAnable = true;
+ break;
+ case 'A':
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
+ tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
+ tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
+ tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
}
- } else if(lineFase == 7) { // ライン 修正
+ } else if(lineFase == 15 ){ // ライン 修正
if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
@@ -897,344 +1636,6 @@
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] = 5;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = 5;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = 10;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -10;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = 15;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -15;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 15;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -15;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 10;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -10;
- adjAnable = true;
- break;
- case -2:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 5;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -5;
- adjAnable = true;
- break;
- case 'A':
- if(lineCheck == false) {
- lineCheck = true;
- lineCount = 0;
- countW++;
- }
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- 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;
- 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){
- adj = 0;//linePara[3];
- } else {
- adj = 0;
- }
-
- motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
- motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
- motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
- motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
- motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
- motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
-
- if(lineCheck == true) {
- lineCount++;
- if(lineCount > 20) lineCheck = false;
- }
- if(countW == 2) {
- countW = 0;
- lineFase = 12;
- lineCount = 0;
- lineCheck = false;
- }
- } else if(lineFase == 12) { // 右 低速
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = FOR;
- if(linePara[LINE_TOW_2] == 0) {
- lineFase = 13;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].pwm = 16;
- } else if (lineFase == 13) {
- lineFase = 14;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- } else if(lineFase == 14) { // タオル2 竿検知
- if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
- lineFase = 15;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 50;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 50;
- } else if(LimitSw::IsPressed(TOW_2L)) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 16;
- } else if(LimitSw::IsPressed(TOW_2R)) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FR].pwm = 16;
- } else {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 16;
- }
- } else if(lineFase == 15 ){ // ライン 修正
- if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 50;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 50;
- } else if(linePara[LINE_TOW_2] > 0) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FR].pwm = 16;
- } else if(linePara[LINE_TOW_2] < 0) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 16;
- } else if(linePara[LINE_TOW_2] == 0) {
lineFase = 16;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
@@ -1255,1813 +1656,2733 @@
motor[TIRE_FR].pwm = 50;
}
} else if(lineFase == 16) { // タオル2 解放
- //solenoid.TOWEL2 = SOLENOID_ON;
+ Air[1] = SOLENOID_ON;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
lineFase = 17;
} else if(lineFase == 17) { // 前
- switch(linePara[LINE_TOW_2]) {
- case 2:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = 10;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -10;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 10;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -10;
- adjAnable = true;
- break;
- case -2:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = 0;
- adjAnable = true;
- break;
- case 'A':
- if(lineCheck == false) {
- lineCheck = true;
- lineCount = 0;
- countW++;
- }
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- if(adjAnable){
- adj = 0;
- } else {
- adj = 0;
- }
-
- motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
- motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
- motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
- motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
- motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
- motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
-
- if(lineCheck == true) {
- lineCount++;
- if(lineCount > 20) lineCheck = false;
- }
- if(countW == 1) {
- countW = 0;
- lineFase = 18;
- lineCount = 0;
- lineCheck = false;
- }
- } else if(lineFase == 18) { // 前 低速
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = BACK;
- if(linePara[4] != 'N') {
- lineFase = 19;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].pwm = 16;
- } else if(lineFase == 19) { // 左
- switch(linePara[3]) {
- case -2:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = 0;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 10;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = -10;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = 40;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = -10;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = 10;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 2:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 'A':
- if(lineCheck == false) {
- lineCheck = true;
- lineCount = 0;
- countW++;
- }
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- if(adjAnable){
- if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
- } else {
- adj = 0;
- }
-
- 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(countW == targetCount) {
- countW = 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;
- countW++;
- }
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- if(adjAnable){
- if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
- } else {
- adj = 0;
- }
-
- motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
- motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
- motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
- motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
- motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
- motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
- motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
-
- if(lineCheck == true) {
- lineCount++;
- if(lineCount > 20) lineCheck = false;
- }
- if(countW == 3) {
- countW = 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;
- }
-
+ if(LimitSw::IsPressed(LSW_UU)) {
+ motor[LIFT_U].dir=BRAKE;
+ motor[LIFT_U].pwm=100;
+ } else {
+ motor[LIFT_U].dir=BACK;
+ motor[LIFT_U].pwm=180;
+ }
+ switch(linePara[LINE_TOW_2]) {
+ case 2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = 10;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -10;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 10;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -10;
+ adjAnable = true;
+ break;
+ case -2:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ if(adjAnable) {
+ adj = 0;
+ } else {
+ adj = 0;
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(countW == 1) {
+ countW = 0;
+ lineFase = 100;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase==100) {
+ LedOut(6);
+ 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_BL].pwm=15;
+ motor[TIRE_BR].pwm=15;
+ if(LimitSw::IsPressed(LSW_UU)) {
+ motor[LIFT_U].dir=BRAKE;
+ motor[LIFT_U].pwm=100;
+ } else {
+ motor[LIFT_U].dir=BACK;
+ motor[LIFT_U].pwm=150;
+ }
+ if((Ult_left>0)&&(Ult_right>0)&&(Ult_left<30)&&(Ult_right<30)) {
+ lineFase=101;
+ }
+ } else if(lineFase==101) {
+ //位置調整
+ //(P制御)
+ 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;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+ lineFase=102;//system lineFase increasing
+ } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
+ if(Ult_right>16) { //Ult_rightが遠い場合
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=0;
+ motor[TIRE_FR].pwm=15;
+ motor[TIRE_BL].pwm=0;
+ motor[TIRE_BR].pwm=15;
+ } 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_BL].pwm=0;
+ motor[TIRE_BR].pwm=15;
+ }
+ } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
+ if(Ult_left>16) { //Ult_leftが遠い場合
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=15;
+ motor[TIRE_FR].pwm=0;
+ motor[TIRE_BL].pwm=15;
+ 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_FR].pwm=0;
+ motor[TIRE_BL].pwm=15;
+ motor[TIRE_BR].pwm=0;
+ }
+ } else { //どっちもあってない場合
+ if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
+ if((Ult_left-Ult_right)>10||((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき
+ if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
+ 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;
+ } 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;
+ }
+ } else { //傾きが大きくなくて離れているとき
+ if((Ult_right+Ult_left)<=25) { //近すぎるとき
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=FOR;
+ 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)>=35) { //離れているとき
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ 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の方が後ろに来ているとき
+ 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;
+ } 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;
+ }
+ }
+ }
+
+ } else {//データを受け取ってないとき
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+ }
+
+ } else if(lineFase==102) {
+ Air[CLOTHESPIN]=SOLENOID_OFF;
+ LedOut(1);
+ //リミットスイッチに当てる
+ static int count2=0;
+ if(count2==0) {
+ if(LimitSw::IsPressed(RIGHTlim)) {
+ count2=1;
+ } else if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
+ if( ((Ult_left+Ult_right)<=25)||((Ult_left+Ult_right)>=35) ) {
+ if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) {
+ if(Ult_left-Ult_right<=0) {
+ //右
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=BACK;
+
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=25;
+
+ motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right));
+ motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right));
+ } else if(Ult_left-Ult_right>0) {
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=BACK;
+
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=25;
+ motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right));
+ motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right));
+ }
+ } else if((Ult_left+Ult_right)<=25) {
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=BACK;
+
+ motor[TIRE_FL].pwm=0;
+ motor[TIRE_FR].pwm=25;
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=0;
+ } else if((Ult_left+Ult_right)>=35) {
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=BACK;
+
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=0;
+ motor[TIRE_BL].pwm=0;
+ motor[TIRE_BR].pwm=25;
+ }
+ } else {
+ if(Ult_left-Ult_right<=0) {
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=BACK;
+
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=25;
+
+ motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right));
+ motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right));
+ } else if(Ult_left-Ult_right>0) {
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=BACK;
+
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=25;
+ motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right));
+ motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right));
+ }
+ }
+
+ } else {
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+
+ }
+ } else if(count2==1) {
+ 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=103;
+ }
+ } else if(lineFase==103) {
+ 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;
+ 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
+ } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
+ if(Ult_right>16) { //Ult_rightが遠い場合
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=0;
+ motor[TIRE_FR].pwm=13;
+ motor[TIRE_BL].pwm=0;
+ motor[TIRE_BR].pwm=13;
+ } 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_BL].pwm=0;
+ motor[TIRE_BR].pwm=13;
+ }
+ } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
+ if(Ult_left>16) { //Ult_leftが遠い場合
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=13;
+ motor[TIRE_FR].pwm=0;
+ motor[TIRE_BL].pwm=13;
+ 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_FR].pwm=0;
+ motor[TIRE_BL].pwm=13;
+ 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の方が後ろに来ているとき
+ 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;
+ } 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;
+ }
+ }
+ } else { //さほど離れてはいないが傾きが大きいとき
+ if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
+ 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;
+ } 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;
+ }
+ }
+ }
+
+ } else {//データを受け取ってないとき
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+ }
+ } else if(lineFase==104) {
+ static int count3=0;
+ static int loop=0;
+ pc.printf("%d\r\n",loop);
+ Air[CLOTHESPIN]=1;
+ if(count3==0) {
+ loop++;
+ if(loop==60) {
+ count3=1;
+ }
+ } else if(count3==1) {
+ Air[CLOTHESPIN]=0;
+ lineFase=105;
+
+ }
+ } else if(lineFase==105) {
+ LedOut(6);
+ Air[CLOTHESPIN]=0;
+ if(!(LimitSw::IsPressed(LEFTlim))) {
+
+ //超音波
+ if(Ult_right>0) {
+ if(Ult_right<15) {
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=FOR;
+ motor[TIRE_FL].pwm=25+(15-Ult_left);
+ motor[TIRE_FR].pwm=25;
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=25+(15-Ult_left);
+ } else if(Ult_right>=15) {
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=FOR;
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=25+(15-Ult_left);
+ motor[TIRE_BL].pwm=25+(15-Ult_left);
+ motor[TIRE_BR].pwm=25;
+ }
+ } else {
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=FOR;
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=25;
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=25;
+ }
+
+
+ } else {
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+ lineFase=106;
+ }
+
+ } else if(lineFase==106) {
+ 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_FR].pwm=15;
+ motor[TIRE_BL].pwm=15;
+ motor[TIRE_BR].pwm=15;
+ if(linePara[4]!='N') {
+ lineFase=19;
+ }
+ } 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) { // 左
+ if(!(LimitSw::IsPressed(LSW_UB))) {
+ motor[LIFT_U].dir=FOR;
+ motor[LIFT_U].pwm=150;
+ } else {
+ motor[LIFT_U].dir=FOR;
+ motor[LIFT_U].pwm=150;
+ }
+ switch(linePara[3]) {
+ case -2:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 10;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = -10;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = 40;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = -10;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = 10;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ if(adjAnable) {
+ if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
+ } else {
+ adj = 0;
+ }
+
+ 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(countW == targetCount) {
+ countW = 0;
+ lineFase = 20;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase == 20) { // 左 低速
+ if(!(LimitSw::IsPressed(LSW_UB))) {
+ motor[LIFT_U].dir=FOR;
+ motor[LIFT_U].pwm=150;
+ } else {
+ motor[LIFT_U].dir=FOR;
+ 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') {
+ //(!!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) {
+ if(!(LimitSw::IsPressed(LSW_UB))) {
+ motor[LIFT_U].dir=FOR;
+ motor[LIFT_U].pwm=150;
+ } else {
+ motor[LIFT_U].dir=FOR;
+ motor[LIFT_U].pwm=150;
+ }
+ switch(linePara[2]) {
+ case -2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = -10;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 10;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = -10;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 10;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ if(adjAnable) {
+ if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
+ } else {
+ adj = 0;
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(countW == 3) {
+ countW = 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;
+ }
+
}
#endif
#if USE_PROCESS_NUM>5
-static void Process5()
-{
-
- /* ************************************** //
-
- 青ゾーン 青ゾーン 青ゾーン
-
- // ************************************** */
-
- for(int i = 0; i < 8; i++) {
- linePara[i] = lineCast(LineHub::GetPara(i));
- }
-
- 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;
- }else if(lineFase == 1) { // 前 ライントレース
- switch(linePara[0]) {
- case -2:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = 10;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -10;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 10;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -10;
- adjAnable = true;
- break;
- case 2:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = 0;
- adjAnable = true;
- break;
- case 'A':
- if(lineCheck == false) {
- lineCheck = true;
- lineCount = 0;
- countW++;
- }
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- if(adjAnable){
- if(linePara[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(countW == 3) {
- countW = 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[3] == 0) {
- lineFase = 3;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].pwm = 16;
- } else if(lineFase == 3) { // 左 ライントレース
- switch(linePara[3]) {
- case -2:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = 0;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 10;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = -10;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 30;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = -30;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = 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[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(countW == 1) {
- countW = 0;
- lineFase = 4;
- lineCount = 0;
- lineCheck = false;
- }
- } else if(lineFase == 4) { // 右 低速
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_FR].dir = BACK;
- if(linePara[LINE_TOW_1] == 0) {
- lineFase = 5;
- motor[TIRE_FL].dir = BRAKE;
- 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 == 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 = 20;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 20;
- } else if(LimitSw::IsPressed(TOW_1R)) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FR].pwm = 20;
- } else {
- switch(linePara[LINE_TOW_1]) {
- case -2:
- tirePWM[TIRE_FL] = -10;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 10;
- tirePWM[TIRE_FR] = 20;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = -14;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 14;
- tirePWM[TIRE_FR] = 20;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = -17;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 17;
- tirePWM[TIRE_FR] = 20;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 20;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -17;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 17;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -14;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 14;
- adjAnable = true;
- break;
- case 2:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -10;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 10;
- adjAnable = true;
- break;
- case 'A':
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 20;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
- motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
- motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
- motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
- motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
- motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
- motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
- }
- } else if(lineFase == 7) { // ライン 修正
- if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
- 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) {
- //tow_stop.reset();
- //tow_stop.start();
- 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] = 10;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -10;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = 14;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -14;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = 17;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -17;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 17;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -17;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 14;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -14;
- adjAnable = true;
- break;
- case -2:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 10;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -10;
- adjAnable = true;
- break;
- case 'A':
- if(lineCheck == false) {
- lineCheck = true;
- lineCount = 0;
- countW++;
- }
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- if(adjAnable){
- adj = 0;
- } else {
- adj = 0;
- }
-
- motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
- motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
- motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
- motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
- motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
- motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
-
- if(lineCheck == true) {
- lineCount++;
- if(lineCount > 20) lineCheck = false;
- }
- if(countW == 1) {
- countW = 0;
- lineFase = 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[3] == 0) {
- lineFase = 11;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].pwm = 16;
- } else if(lineFase == 11) { // 左
- switch(linePara[3]) {
- case -2:
- tirePWM[TIRE_FL] = -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[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(countW == 2) {
- countW = 0;
- lineFase = 12;
- lineCount = 0;
- lineCheck = false;
- }
- } else if(lineFase == 12) { // 左 低速
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_FR].dir = BACK;
- if(linePara[LINE_TOW_2] == 0) {
- lineFase = 13;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].pwm = 16;
- } else if (lineFase == 13) {
- lineFase = 14;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- } else if(lineFase == 14) { // タオル2 竿検知
- if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
- lineFase = 15;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 50;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 50;
- } else if(LimitSw::IsPressed(TOW_2L)) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 20;
- } else if(LimitSw::IsPressed(TOW_2R)) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 20;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FR].pwm = 20;
- } else {
- switch(linePara[LINE_TOW_2]) {
- case -2:
- tirePWM[TIRE_FL] = -10;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 10;
- tirePWM[TIRE_FR] = 20;
- break;
- case -3:
- tirePWM[TIRE_FL] = -14;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 14;
- tirePWM[TIRE_FR] = 20;
- break;
- case -1:
- tirePWM[TIRE_FL] = -17;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 17;
- tirePWM[TIRE_FR] = 20;
- break;
- case 0:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 20;
- break;
- case 1:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -17;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 17;
- break;
- case 3:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -14;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 14;
- break;
- case 2:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -10;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 10;
- break;
- case 'A':
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 20;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- }
-
- motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
- motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
- motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
- motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
- motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
- motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
- motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
- }
- } else if(lineFase == 15 ){ // ライン 修正
- if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 50;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 50;
- } else if(linePara[LINE_TOW_2] > 0) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FR].pwm = 16;
- } else if(linePara[LINE_TOW_2] < 0) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 16;
- } else if(linePara[LINE_TOW_2] == 0) {
- lineFase = 16;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 50;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 50;
- } else {
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 50;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 50;
- }
- } else if(lineFase == 16) { // タオル2 解放
- //solenoid.TOWEL2 = SOLENOID_ON;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- lineFase = 17;
- } else if(lineFase == 17) { // 前
- switch(linePara[LINE_TOW_2]) {
- case 2:
- tirePWM[TIRE_FL] = 10;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -10;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = 14;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -14;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = 17;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -17;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 17;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -17;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 14;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -14;
- adjAnable = true;
- break;
- case -2:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 10;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = 10;
- adjAnable = true;
- break;
- case 'A':
- if(lineCheck == false) {
- lineCheck = true;
- lineCount = 0;
- countW++;
- }
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = 20;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = -20;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- if(adjAnable){
- adj = 0;
- } else {
- adj = 0;
- }
-
- motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
- motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
- motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
- motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
- motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
- motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
-
- if(lineCheck == true) {
- lineCount++;
- if(lineCount > 20) lineCheck = false;
- }
- if(countW == 1) {
- countW = 0;
- lineFase = 18;
- lineCount = 0;
- lineCheck = false;
- }
- } else if(lineFase == 18) { // 前 低速
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = BACK;
- if(linePara[4] == 0) {
- lineFase = 19;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].pwm = 16;
- } else if(lineFase == 19) { // 右
- switch(linePara[4]) {
- case -2:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = 0;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = -10;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = 10;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = 20;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = 20;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = -20;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = 10;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = -10;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case 2:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case 'A':
- if(lineCheck == false) {
- lineCheck = true;
- lineCount = 0;
- countW++;
- }
- tirePWM[TIRE_FL] = 30;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = -30;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- if(adjAnable){
- if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
- } else {
- adj = 0;
- }
-
- motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
- motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
- motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
- motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
- motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
- motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
- motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
-
- if(lineCheck == true) {
- lineCount++;
- if(lineCount > 20) lineCheck = false;
- }
-
- /*
- if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
- targetCount = 3;
- } else if(LimitSw::IsPressed(TOWEL1_SW)) {
- targetCount = 3;
- } else {
- targetCount = 2;
- }
- */
-
- targetCount = 3;
-
- if(countW == targetCount) {
- countW = 0;
- lineFase = 20;
- lineCount = 0;
- lineCheck = false;
- }
- } else if(lineFase == 20) { // 右 低速
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_FR].dir = FOR;
- if (linePara[2] == 0) {
- //(LimitSw::IsPressed(SHEETS_SW)) {
- //ineFase = 20;
- //else if(LimitSw::IsPressed(TOWEL2_SW) {
- //ineFase = 14;
- //else {
- lineFase = 21;
- //
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
- motor[TIRE_FL].pwm = 16;
- motor[TIRE_BL].pwm = 16;
- motor[TIRE_BR].pwm = 16;
- motor[TIRE_FR].pwm = 16;
- } else if(lineFase == 21) {
- switch(linePara[2]) {
- case -2:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case -3:
- tirePWM[TIRE_FL] = -10;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = 10;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case -1:
- tirePWM[TIRE_FL] = -20;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = 20;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case 0:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case 1:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = -20;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = 20;
- adjAnable = true;
- break;
- case 3:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = -10;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = 10;
- adjAnable = true;
- break;
- case 2:
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = 0;
- adjAnable = true;
- break;
- case 'A':
- if(lineCheck == false) {
- lineCheck = true;
- lineCount = 0;
- countW++;
- }
- tirePWM[TIRE_FL] = -30;
- tirePWM[TIRE_BL] = -30;
- tirePWM[TIRE_BR] = 30;
- tirePWM[TIRE_FR] = 30;
- adjAnable = true;
- break;
- case 'N':
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- break;
- default:
- tirePWM[TIRE_FL] = 0;
- tirePWM[TIRE_BL] = 0;
- tirePWM[TIRE_BR] = 0;
- tirePWM[TIRE_FR] = 0;
- adjAnable = false;
- }
-
- if(adjAnable){
- if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
- } else {
- adj = 0;
- }
-
- motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
- motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
- motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
- motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
- motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
- motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
- motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
- motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
-
- if(lineCheck == true) {
- lineCount++;
- if(lineCount > 20) lineCheck = false;
- }
- if(countW == 2) {
- countW = 0;
- lineFase = 22;
- lineCount = 0;
- lineCheck = false;
- }
- } else if(lineFase == 22) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_FR].dir = FOR;
- if (linePara[0] == 'N') {
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].pwm = 30;
- } else{
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_FR].dir = BRAKE;
- }
-
+static void Process5()
+{
+
+ /* ************************************** //
+
+ 青ゾーン 青ゾーン 青ゾーン
+
+ // ************************************** */
+
+ for(int i = 0; i < 8; i++) {
+ linePara[i] = lineCast(LineHub::GetPara(i));
+ }
+
+ 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;
+ } else if(lineFase == 1) { // 前 ライントレース
+ switch(linePara[0]) {
+ case -2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = 10;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -10;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 10;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -10;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ if(adjAnable) {
+ if(linePara[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(countW == 3) {
+ countW = 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[3] == 0) {
+ lineFase = 3;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+ motor[TIRE_FL].pwm = 16;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].pwm = 16;
+ } else if(lineFase == 3) { // 左 ライントレース
+ switch(linePara[3]) {
+ case -2:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 10;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = -10;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 30;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = -30;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = 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[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(countW == 1) {
+ countW = 0;
+ lineFase = 4;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase == 4) { // 右 低速
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_FR].dir = BACK;
+ if(linePara[LINE_TOW_1] == 0) {
+ if(!LimitSw::IsPressed(SHEETS_SW)) {
+ lineFase=100;
+ } else {
+ 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 = 16;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].pwm = 16;
+ } else if (lineFase == 5) {
+ lineFase = 6;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ } else if(lineFase == 6) { // タオル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 = 20;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FR].pwm = 20;
+ } else if(LimitSw::IsPressed(TOW_1R)) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FR].pwm = 20;
+ } else {
+ switch(linePara[LINE_TOW_1]) {
+ case -2:
+ tirePWM[TIRE_FL] = -10;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 10;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = -14;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 14;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = -17;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 17;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -17;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 17;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -14;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 14;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -10;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 10;
+ adjAnable = true;
+ break;
+ case 'A':
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
+ }
+ } else if(lineFase == 7) { // ライン 修正
+ if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
+ 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) {
+ //tow_stop.reset();
+ //tow_stop.start();
+ 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[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] = 10;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -10;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = 14;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -14;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = 17;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -17;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 17;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -17;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 14;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -14;
+ adjAnable = true;
+ break;
+ case -2:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 10;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -10;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ if(adjAnable) {
+ adj = 0;
+ } else {
+ adj = 0;
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(countW == 1) {
+ countW = 0;
+ lineFase = 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[3] == 0) {
+ lineFase = 11;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+ motor[TIRE_FL].pwm = 16;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].pwm = 16;
+ } else if(lineFase == 11) { // 左
+ switch(linePara[3]) {
+ case -2:
+ tirePWM[TIRE_FL] = -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[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(countW == 2) {
+ countW = 0;
+ lineFase = 12;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase == 12) { // 左 低速
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_FR].dir = BACK;
+ if(linePara[LINE_TOW_2] == 0) {
+ lineFase = 13;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+ motor[TIRE_FL].pwm = 16;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].pwm = 16;
+ } else if (lineFase == 13) {
+ lineFase = 14;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ } else if(lineFase == 14) { // タオル2 竿検知
+ if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
+ lineFase = 15;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FR].pwm = 50;
+ } else if(LimitSw::IsPressed(TOW_2L)) {
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FR].pwm = 20;
+ } else if(LimitSw::IsPressed(TOW_2R)) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].pwm = 20;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FR].pwm = 20;
+ } else {
+ switch(linePara[LINE_TOW_2]) {
+ case -2:
+ tirePWM[TIRE_FL] = -10;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 10;
+ tirePWM[TIRE_FR] = 20;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = -14;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 14;
+ tirePWM[TIRE_FR] = 20;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = -17;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 17;
+ tirePWM[TIRE_FR] = 20;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -17;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 17;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -14;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 14;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -10;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 10;
+ break;
+ case 'A':
+
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 20;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
+ }
+ } else if(lineFase == 15 ) { // ライン 修正
+ if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FR].pwm = 50;
+ } else if(linePara[LINE_TOW_2] > 0) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_FL].pwm = 16;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FR].pwm = 16;
+ } else if(linePara[LINE_TOW_2] < 0) {
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_FL].pwm = 16;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FR].pwm = 16;
+ } else if(linePara[LINE_TOW_2] == 0) {
+ lineFase = 16;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FR].pwm = 50;
+ } else {
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BL].pwm = 50;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FR].pwm = 50;
+ }
+ } else if(lineFase == 16) { // タオル2 解放
+ Air[TOWEL2] = SOLENOID_ON;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ lineFase = 17;
+ } else if(lineFase == 17) { // 前
+ switch(linePara[LINE_TOW_2]) {
+ case 2:
+ tirePWM[TIRE_FL] = 10;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -10;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = 14;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -14;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = 17;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -17;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 17;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -17;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 14;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -14;
+ adjAnable = true;
+ break;
+ case -2:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 10;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = 10;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = 20;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = -20;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ if(adjAnable) {
+ adj = 0;
+ } else {
+ adj = 0;
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(countW == 1) {
+ countW = 0;
+ lineFase = 100;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase==100) {
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ if(LimitSw::IsPressed(LSW_UU)) {
+ motor[LIFT_U].dir=BRAKE;
+ motor[LIFT_U].pwm=100;
+ } else {
+ motor[LIFT_U].dir=BACK;
+ motor[LIFT_U].pwm=150;
+ }
+ if((Ult_left>0)&&(Ult_right>0)&&(Ult_left<30)&&(Ult_right<30)) {
+ lineFase=101;
+ }
+ } else if(lineFase==101) {
+ //位置調整
+ //(P制御)
+ 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;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+ lineFase=102;//system lineFase increasing
+ } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
+ if(Ult_right>16) { //Ult_rightが遠い場合
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=0;
+ motor[TIRE_FR].pwm=15;
+ motor[TIRE_BL].pwm=0;
+ motor[TIRE_BR].pwm=15;
+ } 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_BL].pwm=0;
+ motor[TIRE_BR].pwm=15;
+ }
+ } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
+ if(Ult_left>16) { //Ult_leftが遠い場合
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=15;
+ motor[TIRE_FR].pwm=0;
+ motor[TIRE_BL].pwm=15;
+ 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_FR].pwm=0;
+ motor[TIRE_BL].pwm=15;
+ motor[TIRE_BR].pwm=0;
+ }
+ } else { //どっちもあってない場合
+ if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
+ if((Ult_left-Ult_right)>10||((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき
+ if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
+ 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;
+ } 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;
+ }
+ } else { //傾きが大きくなくて離れているとき
+ if((Ult_right+Ult_left)<=25) { //近すぎるとき
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=FOR;
+ 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)>=35) { //離れているとき
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ 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の方が後ろに来ているとき
+ 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;
+ } 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;
+ }
+ }
+ }
+
+ } else {//データを受け取ってないとき
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+ }
+
+ } else if(lineFase==102) {
+ Air[CLOTHESPIN]=SOLENOID_OFF;
+ LedOut(1);
+ //リミットスイッチに当てる
+ static int count2=0;
+ if(count2==0) {
+ if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
+ if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {
+ if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) {
+ if(Ult_left-Ult_right<=0) {
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=FOR;
+
+ motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
+ motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));
+
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=25;
+ } else if(Ult_left-Ult_right>0) {
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=FOR;
+ motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
+ motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=25;
+ }
+ } else if((Ult_left+Ult_right)<=25) {
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=FOR;
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=0;
+ motor[TIRE_BL].pwm=0;
+ motor[TIRE_BR].pwm=25;
+ } else if((Ult_left+Ult_right)>=35) {
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=FOR;
+ motor[TIRE_FL].pwm=0;
+ motor[TIRE_FR].pwm=25;
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=0;
+ }
+ } else {
+ if(Ult_left-Ult_right<=0) {
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=FOR;
+
+ motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
+ motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));
+
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=25;
+ } else if(Ult_left-Ult_right>0) {
+ motor[TIRE_FL].dir=BACK;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=FOR;
+ motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
+ motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=25;
+ }
+ }
+
+ } else {
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+
+ }
+ if(LimitSw::IsPressed(LEFTlim)) {
+ count2=1;
+ }
+
+ } else if(count2==1) {
+ 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=103;
+ }
+ } else if(lineFase==103) {
+ 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;
+ 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
+ } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
+ if(Ult_right>16) { //Ult_rightが遠い場合
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=0;
+ motor[TIRE_FR].pwm=13;
+ motor[TIRE_BL].pwm=0;
+ motor[TIRE_BR].pwm=13;
+ } 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_BL].pwm=0;
+ motor[TIRE_BR].pwm=13;
+ }
+ } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
+ if(Ult_left>16) { //Ult_leftが遠い場合
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=BACK;
+ motor[TIRE_BL].dir=FOR;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=13;
+ motor[TIRE_FR].pwm=0;
+ motor[TIRE_BL].pwm=13;
+ 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_FR].pwm=0;
+ motor[TIRE_BL].pwm=13;
+ 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の方が後ろに来ているとき
+ 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));
+ } 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=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));
+ }
+ }
+ } else { //さほど離れてはいないが傾きが大きいとき
+ if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
+ 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));
+ } 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=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));
+ }
+ }
+ }
+
+ } else {//データを受け取ってないとき
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+ }
+ } else if(lineFase==104) {
+ static int count3=0;
+ static int loop=0;
+ pc.printf("%d\r\n",loop);
+ Air[CLOTHESPIN]=1;
+ if(count3==0) {
+ loop++;
+ if(loop==60) {
+ count3=1;
+ }
+ } else if(count3==1) {
+ Air[CLOTHESPIN]=0;
+ lineFase=105;
+
+ }
+ } else if(lineFase==105) {
+ LedOut(6);
+ Air[CLOTHESPIN]=0;
+ if(!(LimitSw::IsPressed(RIGHTlim))) {
+
+ //超音波
+ if(Ult_right>0) {
+ if(Ult_right<15) {
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=25+(15-Ult_right);
+ motor[TIRE_BL].pwm=25+(15-Ult_right);
+ motor[TIRE_BR].pwm=25;
+ } else if(Ult_right>=15) {
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=25+(Ult_right-15);
+ motor[TIRE_FR].pwm=25;
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=25+(Ult_right-15);
+ }
+ } else {
+ motor[TIRE_FL].dir=FOR;
+ motor[TIRE_FR].dir=FOR;
+ motor[TIRE_BL].dir=BACK;
+ motor[TIRE_BR].dir=BACK;
+ motor[TIRE_FL].pwm=25;
+ motor[TIRE_FR].pwm=25;
+ motor[TIRE_BL].pwm=25;
+ motor[TIRE_BR].pwm=25;
+ }
+
+
+ } else {
+ motor[TIRE_FL].dir=BRAKE;
+ motor[TIRE_FR].dir=BRAKE;
+ motor[TIRE_BL].dir=BRAKE;
+ motor[TIRE_BR].dir=BRAKE;
+ motor[TIRE_FL].pwm=100;
+ motor[TIRE_FR].pwm=100;
+ motor[TIRE_BL].pwm=100;
+ motor[TIRE_BR].pwm=100;
+ lineFase=106;
+ }
+
+ } else if(lineFase==106) {
+ 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_FR].pwm=15;
+ motor[TIRE_BL].pwm=15;
+ motor[TIRE_BR].pwm=15;
+ if(linePara[4]!='N') {
+ lineFase=19;
+ }
+ } else if(lineFase == 18) { // 前 低速
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ if(linePara[4] == 0) {
+ lineFase = 19;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+ motor[TIRE_FL].pwm = 16;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].pwm = 16;
+ } else if(lineFase == 19) { // 右
+ switch(linePara[4]) {
+ case -2:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = -10;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 10;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = 20;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = -20;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = 10;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = -10;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = 30;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = -30;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ if(adjAnable) {
+ if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
+ } else {
+ adj = 0;
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+
+ /*
+ if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
+ targetCount = 3;
+ } else if(LimitSw::IsPressed(TOWEL1_SW)) {
+ targetCount = 3;
+ } else {
+ targetCount = 2;
+ }
+ */
+
+ targetCount = 3;
+
+ if(countW == targetCount) {
+ countW = 0;
+ lineFase = 20;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase == 20) { // 右 低速
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_FR].dir = FOR;
+ if (linePara[2] == 0) {
+ //(!!LimitSw::Ispressed(SHEETS_SW)) {
+ //ineFase = 20;
+ //else if(LimitSw::IsPressed(TOWEL2_SW) {
+ //ineFase = 14;
+ //else {
+ lineFase = 21;
+ //
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+ motor[TIRE_FL].pwm = 16;
+ motor[TIRE_BL].pwm = 16;
+ motor[TIRE_BR].pwm = 16;
+ motor[TIRE_FR].pwm = 16;
+ } else if(lineFase == 21) {
+ switch(linePara[2]) {
+ case -2:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case -3:
+ tirePWM[TIRE_FL] = -10;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 10;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case -1:
+ tirePWM[TIRE_FL] = -20;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 20;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 0:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 1:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = -20;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 20;
+ adjAnable = true;
+ break;
+ case 3:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = -10;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 10;
+ adjAnable = true;
+ break;
+ case 2:
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = true;
+ break;
+ case 'A':
+ if(lineCheck == false) {
+ lineCheck = true;
+ lineCount = 0;
+ countW++;
+ }
+ tirePWM[TIRE_FL] = -30;
+ tirePWM[TIRE_BL] = -30;
+ tirePWM[TIRE_BR] = 30;
+ tirePWM[TIRE_FR] = 30;
+ adjAnable = true;
+ break;
+ case 'N':
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ break;
+ default:
+ tirePWM[TIRE_FL] = 0;
+ tirePWM[TIRE_BL] = 0;
+ tirePWM[TIRE_BR] = 0;
+ tirePWM[TIRE_FR] = 0;
+ adjAnable = false;
+ }
+
+ if(adjAnable) {
+ if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
+ } else {
+ adj = 0;
+ }
+
+ motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
+ motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
+ motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
+ motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
+ motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
+ motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
+
+ if(lineCheck == true) {
+ lineCount++;
+ if(lineCount > 20) lineCheck = false;
+ }
+ if(countW == 2) {
+ countW = 0;
+ lineFase = 22;
+ lineCount = 0;
+ lineCheck = false;
+ }
+ } else if(lineFase == 22) {
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_FR].dir = FOR;
+ if (linePara[0] == 'N') {
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+ motor[TIRE_FL].pwm = 30;
+ motor[TIRE_BL].pwm = 30;
+ motor[TIRE_BR].pwm = 30;
+ motor[TIRE_FR].pwm = 30;
+ } else {
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_FR].dir = BRAKE;
+ }
+
}
#endif
#if USE_PROCESS_NUM>6
-static void Process6()
+static void Process6()
{
- for(int i = 0; i < 8; i++) {
- linePara[i] = lineCast(LineHub::GetPara(i));
- }
-
- if(lineFase == 0) {
- if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
- lineFase = 1;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 30;
- } else if(LimitSw::IsPressed(TOW_1L)) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 12;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 12;
- } else if(LimitSw::IsPressed(TOW_1R)) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 12;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 50;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 50;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FR].pwm = 12;
- } else {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 18;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BL].pwm = 18;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BR].pwm = 18;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 18;
- }
- } else if(lineFase == 1) {
- if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 30;
- } else if(linePara[LINE_TOW_1] > 0) {
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_BL].dir = FOR;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FR].pwm = 15;
- } else if(linePara[LINE_TOW_1] < 0) {
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_FL].pwm = 15;
- motor[TIRE_BL].dir = BACK;
- motor[TIRE_BL].pwm = 15;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BR].pwm = 15;
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FR].pwm = 15;
- } else if(linePara[LINE_TOW_1] == 0) {
- lineFase = 2;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 30;
- } else {
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_FL].pwm = 30;
- motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_BL].pwm = 30;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BR].pwm = 30;
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FR].pwm = 30;
- }
- } else if(lineFase == 2) {
- //solenoid::solenoid1 = SOLENOID_ON
-
- }
+
}
#endif
#if USE_PROCESS_NUM>7
static void Process7()
{
-
+ static int SW_flag=0;
+ static int Processflag=0;
+ if(Processflag==0) {
+ if(Limitphase==0) {
+ //下待機
+ Limitphase=2;
+ } 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) {
+ 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_RB].dir = BACK;
+ motor[LIFT_RB].pwm = 180;
+ 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)) {
+ motor[LIFT_LB].dir = BRAKE;
+ motor[LIFT_LB].pwm = 200;
+ motor[LIFT_RB].dir = BRAKE;
+ motor[LIFT_RB].pwm = 180;
+ Limitphase=4;
+ }
+ }
+ } else if(Limitphase==3) {
+ } else if(Limitphase==4) {
+ //上段待機
+ motor[LIFT_LB].dir = BRAKE;
+ motor[LIFT_LB].pwm = 200;
+ motor[LIFT_RB].dir = BRAKE;
+ motor[LIFT_RB].pwm = 200;
+ current=0;
+ startFlag=true;
+ } else {
+ motor[LIFT_LB].dir = BRAKE;
+ motor[LIFT_LB].pwm = 200;
+ motor[LIFT_RB].dir = BRAKE;
+ motor[LIFT_RB].pwm = 200;
+ startFlag=true;
+ current=0;
+ }
+ 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;
+ Air[TOWEL0]=1;
+ SW_flag=0;
+ Limitphase=0;
+ current=0;
+ 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;
+ 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;
+ current=0;
+ startFlag=true;
+ Processflag=1;
+ }
+ 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;
+ Air[TOWEL0]=1;
+ SW_flag=0;
+ Limitphase=0;
+ current=0;
+ startFlag=true;
+ Processflag=1;
+ }
+ }
}
+
+
+
#endif
-#if USE_PROCESS_NUM>8
+#if USE_PROCESS_NUM>8
static void Process8()
{
- if(controller->Button.A) {
- rotaconSampling.start();
- PIDflag = true;
-
- //linePara_U = LineHub::GetPara(0);
- //linePara_B = LineHub::GetPara(3);
-
-
- pulsePV[FL] = encoder[FL].getPulses();
- pulsePV[BL] = encoder[BL].getPulses();
- pulsePV[BR] = encoder[BR].getPulses();
- pulsePV[FR] = encoder[FR].getPulses();
-
-
- for (int i = 0; i < 4; i++) {
- timeCV[i] = timePV[i];
- timePV[i] = rotaconSampling.read();
- tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
- pulseCV[i] = pulsePV[i];
- }
-
- move.Vx = 0.5;
- move.Vy = 0.5;
- move.Va = 0;
-
- correction_LT.Vx = 0; //0.1 * linePara_U;
- correction_LT.Vy = 0;
- correction_LT.Va = 0;
-
- synthetic.Vx = move.Vx + correction_LT.Vx;
- synthetic.Vy = move.Vy + correction_LT.Vy;
- synthetic.Va = move.Va + correction_LT.Va;
-
- sita = 0;
-
- //タイヤの目標速度算出
- float sinR = 0.7071 * (float)sin(sita);
- float cosR = 0.7071 * (float)cos(sita);
- float nv = (60 * 1000) / ( 2.00 * PI * tireR);
- tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
- tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
- tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
- tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
-
- //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
-
- //PIDによるPWM算出
-
- //モータの駆動
- for (int i = 0; i < 4; i++) {
- if (tirePWM[i] > 255){
- tirePWM[i] = 255;
- } else if (tirePWM[i] < -255) {
- tirePWM[i] = -255;
- }
- }
-
- for(int i = 0;i < 4;i++){
- motor[i].dir = SetStatus(tirePWM[i]);
- motor[i].pwm = SetPWM(tirePWM[i]);
- }
- } else {
- PIDflag = false;
- rotaconSampling.stop();
- rotaconSampling.reset();
- for(int i = 0;i < 4;i++){
- encoder[i].reset();
- pulsePV[i] = 0;
- pulseCV[i] = 0;
- timePV[i] = 0;
- timeCV[i] = 0;
- tirePWM[i] = 0;
- motor[i].dir = SetStatus(tirePWM[i]);
- motor[i].pwm = SetPWM(tirePWM[i]);
- }
- }
+ if(controller->Button.A) {
+ rotaconSampling.start();
+ PIDflag = true;
+
+ //linePara_U = LineHub::GetPara(0);
+ //linePara_B = LineHub::GetPara(3);
+
+
+ pulsePV[FL] = encoder[FL].getPulses();
+ pulsePV[BL] = encoder[BL].getPulses();
+ pulsePV[BR] = encoder[BR].getPulses();
+ pulsePV[FR] = encoder[FR].getPulses();
+
+
+ for (int i = 0; i < 4; i++) {
+ timeCV[i] = timePV[i];
+ timePV[i] = rotaconSampling.read();
+ tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
+ pulseCV[i] = pulsePV[i];
+ }
+
+ move.Vx = 0.5;
+ move.Vy = 0.5;
+ move.Va = 0;
+
+ correction_LT.Vx = 0; //0.1 * linePara_U;
+ correction_LT.Vy = 0;
+ correction_LT.Va = 0;
+
+ synthetic.Vx = move.Vx + correction_LT.Vx;
+ synthetic.Vy = move.Vy + correction_LT.Vy;
+ synthetic.Va = move.Va + correction_LT.Va;
+
+ sita = 0;
+
+ //タイヤの目標速度算出
+ float sinR = 0.7071 * (float)sin(sita);
+ float cosR = 0.7071 * (float)cos(sita);
+ float nv = (60 * 1000) / ( 2.00 * PI * tireR);
+ tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
+ tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
+ tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
+ tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
+
+ //pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
+
+ //PIDによるPWM算出
+
+ //モータの駆動
+ for (int i = 0; i < 4; i++) {
+ if (tirePWM[i] > 255) {
+ tirePWM[i] = 255;
+ } else if (tirePWM[i] < -255) {
+ tirePWM[i] = -255;
+ }
+ }
+
+ for(int i = 0; i < 4; i++) {
+ motor[i].dir = SetStatus(tirePWM[i]);
+ motor[i].pwm = SetPWM(tirePWM[i]);
+ }
+ } else {
+ PIDflag = false;
+ rotaconSampling.stop();
+ rotaconSampling.reset();
+ for(int i = 0; i < 4; i++) {
+ encoder[i].reset();
+ pulsePV[i] = 0;
+ pulseCV[i] = 0;
+ timePV[i] = 0;
+ timeCV[i] = 0;
+ tirePWM[i] = 0;
+ motor[i].dir = SetStatus(tirePWM[i]);
+ motor[i].pwm = SetPWM(tirePWM[i]);
+ }
+ }
}
#endif
#if USE_PROCESS_NUM>9
static void Process9()
{
-
+
}
#endif
#endif
@@ -3070,60 +4391,63 @@
static void AllActuatorReset()
{
- #ifdef USE_SOLENOID
- solenoid.all = ALL_SOLENOID_OFF;
- #endif
+#ifdef USE_SOLENOID
+ solenoid.all = ALL_SOLENOID_OFF;
+#endif
- #ifdef USE_MOTOR
- for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
- {
- motor[i].dir = FREE;
- motor[i].pwm = 0;
- }
- #endif
+#ifdef USE_MOTOR
+ for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
+ motor[i].dir = FREE;
+ motor[i].pwm = 0;
+ }
+#endif
}
-void BuzzerTimer_func(){
+void BuzzerTimer_func()
+{
buzzer = !buzzer;
//LED_DEBUG0 = !LED_DEBUG0;
}
-void TapeLedEms_func() {
+void TapeLedEms_func()
+{
sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}
#pragma region USER-DEFINED-FUNCTIONS
-void tirePID() {
- if(PIDflag == true) {
- //加算するPID値の算出
- rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
- rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
- rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
- rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
- //PID値の加算
- tirePWM[FL] += rotaconPID[0].GetMV();
- tirePWM[BL] += rotaconPID[1].GetMV();
- tirePWM[FR] += rotaconPID[2].GetMV();
- tirePWM[BR] += rotaconPID[3].GetMV();
- }
+void tirePID()
+{
+ if(PIDflag == true) {
+ //加算するPID値の算出
+ rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
+ rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
+ rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
+ rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
+ //PID値の加算
+ tirePWM[FL] += rotaconPID[0].GetMV();
+ tirePWM[BL] += rotaconPID[1].GetMV();
+ tirePWM[FR] += rotaconPID[2].GetMV();
+ tirePWM[BR] += rotaconPID[3].GetMV();
+ }
}
-int lineCast(char k) {
- int l;
- switch(k) {
- case 255:
- l = -1;
- break;
- case 254:
- l = -2;
- break;
- case 253:
- l = -3;
- break;
- default:
- l = k;
- }
- return l;
+int lineCast(char k)
+{
+ int l;
+ switch(k) {
+ case 255:
+ l = -1;
+ break;
+ case 254:
+ l = -2;
+ break;
+ case 253:
+ l = -3;
+ break;
+ default:
+ l = k;
+ }
+ return l;
}
#pragma endregion