Miya Miyagawa
The last version programs
Dependencies: mbed TrapezoidControl Pulse QEI
System/Process/Process.cpp
- Committer:
- Ryosei
- Date:
- 2020-03-27
- Revision:
- 30:76b140d176f0
- Parent:
- 29:5365ee0521f6
File content as of revision 30:76b140d176f0:
#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"
#include "../../Communication/Controller/Controller.h"
#include "../../Input/ExternalInt/ExternalInt.h"
#include "../../Input/Switch/Switch.h"
#include "../../Input/Encoder/Encoder.h"
#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"
using namespace SWITCH;
using namespace PID_SPACE;
using namespace ENCODER;
using namespace LINEHUB;
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
static bool lock;
static bool processChangeComp;
static int current;
static void AllActuatorReset();
#ifdef USE_SUBPROCESS
static void (*Process[USE_PROCESS_NUM])(void);
#endif
#pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
/*Replace here with the definition code of your variables.*/
Serial pc(USBTX, USBRX);
//**************Buzzer****************
//DigitalOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
bool EMGflag = false;
PwmOut buzzer(BUZZER_PIN);
//**************Buzzer****************
//************TapeLed*****************
void TapeLedEms_func();
TapeLedData tapeLED;
TapeLedData sendLedData;
TapeLED_Mode ledMode = Normal;
Ticker tapeLedTimer;
//************TapaLed*****************
//*************** lift ***************
#define LOWER 1
#define MIDDLRE 2
#define UPPER 3
uint8_t liftState = LOWER;
bool moving = false;
bool switchFlag_LB = false;
bool switchFlag_RB = false;
//*************** 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
};
#define FL 0
#define BL 1
#define BR 2
#define FR 3
#define PI 3.141592
const float tireR = 101.6; //タイヤの半径
const float ucR = 420.0; //中心からのタイヤの距離
typedef struct {
float Vx; //X方向の速度
float Vy; //Y方向の速度
float Va; //角速度
} Vvector;
Vvector move; //進む速度
Vvector correction_LT; //ライントレースの補正速度
Vvector synthetic; //合成速度
float sita = 0;
bool PIDflag = false;
int linePara[8];
int linePara_U;
int linePara_B;
int linePara_L;
int linePara_R;
#define FL 0
#define BL 1
#define BR 2
#define FR 3
float tireProcessRPM[4];
float tireTargetMaxRPM[4];
float tireTargetRPM[4];
float tirePWM[4];
float timePV[4];
float timeCV[4];
float pulsePV[4];
float pulseCV[4];
void tirePID();
int lineCast(char k);
Timer rotaconSampling;
Ticker rotaconPIDtimer;
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=10;////////////////////////////////////////////////温度
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-1.5;
}
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;
float pw = 0;
int lineFase = 0;
bool lineCheck = false;
bool lineFlag = false;
bool adjAnable = false;
int adj = 0;
int countW = 0;
int lineCount = 0;
int targetCount = 0;
bool startFlag = true;
int linePWM;
int adj_F;
int adj_B;
int mode = 0;
// ************* Line ************** //
const int omni[15][15] = {
{ 0, 5, 21, 47, 83, 130, 207, 255, 255, 255, 255, 255, 255, 255, 255 },
{ -5, 0, 5, 21, 47, 83, 130, 207, 193, 208, 234, 255, 255, 255, 255 },
{ -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
{ -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
{ -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
{ -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
{ -207, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
{ -255, -207, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
{ -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
{ -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
{ -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
{ -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 SetPWM(int);
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
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0(void);
#endif
#if USE_PROCESS_NUM>1
static void Process1(void);
#endif
#if USE_PROCESS_NUM>2
static void Process2(void);
#endif
#if USE_PROCESS_NUM>3
static void Process3(void);
#endif
#if USE_PROCESS_NUM>4
static void Process4(void);
#endif
#if USE_PROCESS_NUM>5
static void Process5(void);
#endif
#if USE_PROCESS_NUM>6
static void Process6(void);
#endif
#if USE_PROCESS_NUM>7
static void Process7(void);
#endif
#if USE_PROCESS_NUM>8
static void Process8(void);
#endif
#if USE_PROCESS_NUM>9
static void Process9(void);
#endif
#endif
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
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
}
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_RS485
ACTUATORHUB::ActuatorHub::Update();
//LINEHUB::LineHub::Update();
#endif
}
double Ult_left;///////////////////////////////////////////left sensor
double Ult_right;//////////////////////////////////////////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
//for(int i=0; i<19; i++) {
// pc.printf("%d",LimitSw::IsPressed(i));
// if(i==18) {
// pc.printf("\r\n");
// }
// }
//
pc.printf("%lf,%lf\r\n",Ult_left,Ult_right);
// pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
// pc.printf("%d\n\r",current);
/*上
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 = 100;
lineCount = 0;
lineCheck = false;
countW = 0;
if(LimitSw::IsPressed(REDBLUE_SW)) {
current = 5;
} 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();
}
#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;
} 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;
} else if(controller->Button.LEFT) {
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;
} 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;
}
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.L) {
air[0] = SOLENOID_ON;
} else {
air[0] = SOLENOID_OFF;
}
if(controller->Button.ZL) {
air[1] = SOLENOID_ON;
} else {
air[1] = SOLENOID_OFF;
}
if(controller->Button.R) {
air[2] = SOLENOID_ON;
} else {
air[2] = SOLENOID_OFF;
}
if(controller->Button.ZR) {
air[3] = SOLENOID_ON;
} else {
air[3] = SOLENOID_OFF;
}
}
#endif
#if USE_PROCESS_NUM>2
static void Process2()
{
}
#endif
#if USE_PROCESS_NUM>3
static void Process3()
{
startFlag = true;
AllActuatorReset();
lineFase = 0;
lineCheck = false;
lineCount = 0;
countW = 0;
}
#endif
#if USE_PROCESS_NUM>4
static void Process4()
{
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] ==0) {
lineFase = 3;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
}
motor[TIRE_FL].pwm = 16;
motor[TIRE_BL].pwm = 16;
motor[TIRE_BR].pwm = 16;
motor[TIRE_FR].pwm = 16;
} else if(lineFase == 3) { // 右 ライントレース
switch(linePara[4]) {
case -2:
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) {
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);
static int Towlcount0=0;
Towlcount0++;
if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
if(Towlcount0<=20) {
motor[TIRE_FL].dir = BACK;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BACK;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FR].pwm = 50;
} else {
lineFase = 7;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
}
} else if(LimitSw::IsPressed(TOW_1L)) {
motor[TIRE_FL].dir = FOR;
motor[TIRE_FL].pwm = 16;
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]);
}
} else if(lineFase == 7) { // ライン 修正
if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
} else if(linePara[LINE_TOW_2] > 0) {
motor[TIRE_FL].dir = BACK;
motor[TIRE_FL].pwm = 16;
motor[TIRE_BL].dir = FOR;
motor[TIRE_BL].pwm = 16;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BR].pwm = 16;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FR].pwm = 16;
} else if(linePara[LINE_TOW_2] < 0) {
motor[TIRE_FL].dir = FOR;
motor[TIRE_FL].pwm = 16;
motor[TIRE_BL].dir = BACK;
motor[TIRE_BL].pwm = 16;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BR].pwm = 16;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FR].pwm = 16;
} else if(linePara[LINE_TOW_2] == 0) {
lineFase = 8;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
} else {
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
}
} else if(lineFase == 8) { // タオル1 解放
Air[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) { // 前 ライントレース
LedOut(2);
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 = 10;
lineCount = 0;
lineCheck = false;
}
} else if(lineFase == 10) { // 前 低速
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_FR].dir = BACK;
if(linePara[4] == 0) {
lineFase = 11;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
}
motor[TIRE_FL].pwm = 16;
motor[TIRE_BL].pwm = 16;
motor[TIRE_BR].pwm = 16;
motor[TIRE_FR].pwm = 16;
} else if(lineFase == 11) {
switch(linePara[4]) { // 右 ライントレース
case -2:
tirePWM[TIRE_FL] = 30;
tirePWM[TIRE_BL] = 0;
tirePWM[TIRE_BR] = -30;
tirePWM[TIRE_FR] = 0;
adjAnable = true;
break;
case -3:
tirePWM[TIRE_FL] = 30;
tirePWM[TIRE_BL] = -10;
tirePWM[TIRE_BR] = -30;
tirePWM[TIRE_FR] = 10;
adjAnable = true;
break;
case -1:
tirePWM[TIRE_FL] = 30;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = -30;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case 0:
tirePWM[TIRE_FL] = 30;
tirePWM[TIRE_BL] = -30;
tirePWM[TIRE_BR] = -30;
tirePWM[TIRE_FR] = 30;
adjAnable = true;
break;
case 1:
tirePWM[TIRE_FL] = 20;
tirePWM[TIRE_BL] = -30;
tirePWM[TIRE_BR] = -20;
tirePWM[TIRE_FR] = 30;
adjAnable = true;
break;
case 3:
tirePWM[TIRE_FL] = 10;
tirePWM[TIRE_BL] = -30;
tirePWM[TIRE_BR] = -10;
tirePWM[TIRE_FR] = 30;
adjAnable = true;
break;
case 2:
tirePWM[TIRE_FL] = 0;
tirePWM[TIRE_BL] = -30;
tirePWM[TIRE_BR] = 0;
tirePWM[TIRE_FR] = 30;
adjAnable = true;
break;
case 'A':
if(lineCheck == false) {
lineCheck = true;
lineCount = 0;
countW++;
}
tirePWM[TIRE_FL] = 30;
tirePWM[TIRE_BL] = -30;
tirePWM[TIRE_BR] = -30;
tirePWM[TIRE_FR] = 30;
adjAnable = true;
break;
case 'N':
tirePWM[TIRE_FL] = 0;
tirePWM[TIRE_BL] = 0;
tirePWM[TIRE_BR] = 0;
tirePWM[TIRE_FR] = 0;
adjAnable = false;
break;
default:
tirePWM[TIRE_FL] = 0;
tirePWM[TIRE_BL] = 0;
tirePWM[TIRE_BR] = 0;
tirePWM[TIRE_FR] = 0;
adjAnable = false;
}
if(adjAnable) {
if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
} else {
adj = 0;
}
motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
if(lineCheck == true) {
lineCount++;
if(lineCount > 20) lineCheck = false;
}
if(countW == 2) {
countW = 0;
lineFase = 12;
lineCount = 0;
lineCheck = false;
}
} else if(lineFase == 12) { // 右 低速
motor[TIRE_FL].dir = FOR;
motor[TIRE_BL].dir = BACK;
motor[TIRE_BR].dir = BACK;
motor[TIRE_FR].dir = FOR;
if(linePara[LINE_TOW_1] == 0) {
lineFase = 13;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
}
motor[TIRE_FL].pwm = 16;
motor[TIRE_BL].pwm = 16;
motor[TIRE_BR].pwm = 16;
motor[TIRE_FR].pwm = 16;
} else if (lineFase == 13) {
lineFase = 14;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_FR].dir = BRAKE;
} else if(lineFase == 14) { // タオル2 竿検知
LedOut(4);
static int Towlcount1=0;
Towlcount1++;
if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
if(Towlcount1<=20) {
motor[TIRE_FL].dir = BACK;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BACK;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FR].pwm = 50;
} else {
lineFase = 15;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
}
} else if(LimitSw::IsPressed(TOW_2L)) {
motor[TIRE_FL].dir = FOR;
motor[TIRE_FL].pwm = 20;
motor[TIRE_BL].dir = FOR;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BR].pwm = 20;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FR].pwm = 20;
} else if(LimitSw::IsPressed(TOW_2R)) {
motor[TIRE_FL].dir = BACK;
motor[TIRE_FL].pwm = 20;
motor[TIRE_BL].dir = BACK;
motor[TIRE_BL].pwm = 20;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BR].pwm = 20;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FR].pwm = 20;
} else {
if(!(LimitSw::IsPressed(QF_SW))) {
switch(linePara[LINE_TOW_1]) {
case -2:
tirePWM[TIRE_FL] = -10;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 10;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case -3:
tirePWM[TIRE_FL] = -14;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 14;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case -1:
tirePWM[TIRE_FL] = -17;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 17;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case 0:
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case 1:
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -17;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 17;
adjAnable = true;
break;
case 3:
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -14;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 14;
adjAnable = true;
break;
case 2:
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -10;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 10;
adjAnable = true;
break;
case 'A':
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case 'N':
tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
adjAnable = false;
break;
default:
tirePWM[TIRE_FL] = 0;
tirePWM[TIRE_BL] = 0;
tirePWM[TIRE_BR] = 0;
tirePWM[TIRE_FR] = 0;
adjAnable = false;
}
} else {
switch(linePara[LINE_TOW_1]) {
case 2:
tirePWM[TIRE_FL] = -10;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 10;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case 3:
tirePWM[TIRE_FL] = -14;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 14;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case 1:
tirePWM[TIRE_FL] = -17;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 17;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case 0:
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case -1:
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -17;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 17;
adjAnable = true;
break;
case -3:
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -14;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 14;
adjAnable = true;
break;
case -2:
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -10;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 10;
adjAnable = true;
break;
case 'A':
tirePWM[TIRE_FL] = -20;
tirePWM[TIRE_BL] = -20;
tirePWM[TIRE_BR] = 20;
tirePWM[TIRE_FR] = 20;
adjAnable = true;
break;
case 'N':
tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
adjAnable = false;
break;
default:
tirePWM[TIRE_FL] = 0;
tirePWM[TIRE_BL] = 0;
tirePWM[TIRE_BR] = 0;
tirePWM[TIRE_FR] = 0;
adjAnable = false;
}
}
motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
}
} else if(lineFase == 15 ) { // ライン 修正
if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
} else if(linePara[LINE_TOW_1] > 0) {
motor[TIRE_FL].dir = BACK;
motor[TIRE_FL].pwm = 16;
motor[TIRE_BL].dir = FOR;
motor[TIRE_BL].pwm = 16;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BR].pwm = 16;
motor[TIRE_FR].dir = BACK;
motor[TIRE_FR].pwm = 16;
} else if(linePara[LINE_TOW_1] < 0) {
motor[TIRE_FL].dir = FOR;
motor[TIRE_FL].pwm = 16;
motor[TIRE_BL].dir = BACK;
motor[TIRE_BL].pwm = 16;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BR].pwm = 16;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FR].pwm = 16;
} else if(linePara[LINE_TOW_1] == 0) {
lineFase = 16;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
} else {
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
}
} else if(lineFase == 16) { // タオル2 解放
Air[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_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 = 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=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
if(LimitSw::IsPressed(LSW_UU)) {
motor[LIFT_U].dir=BRAKE;
motor[LIFT_U].pwm=100;
} 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=20;
motor[TIRE_BL].pwm=0;
motor[TIRE_BR].pwm=20;
} else if(Ult_right<14) { //Ult_rightが近い場合
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=0;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=0;
motor[TIRE_BR].pwm=20;
}
} 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=20;
motor[TIRE_FR].pwm=0;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=0;
} else if(Ult_left<14) { //Ult_leftが近い場合
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=0;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=0;
}
} else { //どっちもあってない場合
if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
if((Ult_left-Ult_right)>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=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
}
} 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=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
}
}
}
} 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) {
static int PuluPulu=0;
if(PuluPulu>100) {
motor[TIRE_FL].dir=BRAKE;
motor[TIRE_FR].dir=BRAKE;
motor[TIRE_BL].dir=BRAKE;
motor[TIRE_BR].dir=BRAKE;
motor[TIRE_FL].pwm=255;
motor[TIRE_FR].pwm=255;
motor[TIRE_BL].pwm=255;
motor[TIRE_BR].pwm=255;
lineFase=104;//system lineFase increasing
}
if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
motor[TIRE_FL].dir=BRAKE;
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=20;
motor[TIRE_BL].pwm=0;
motor[TIRE_BR].pwm=20;
} else if(Ult_right<14) { //Ult_rightが近い場合
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=0;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=0;
motor[TIRE_BR].pwm=20;
}
} 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=20;
motor[TIRE_FR].pwm=0;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=0;
} else if(Ult_left<14) { //Ult_leftが近い場合
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=0;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=0;
}
} else { //どっちもあってない場合
if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
PuluPulu++;
motor[TIRE_FL].dir=FOR;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
PuluPulu++;
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
}
}
} else { //さほど離れてはいないが傾きが大きいとき
if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
PuluPulu++;
motor[TIRE_FL].dir=FOR;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
PuluPulu++;
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
}
}
}
} 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 == 20) { // 前 低速
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=BRAKE;
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] = 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(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 == 1) {
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=BRAKE;
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] == 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 == 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()
{
/* ************************************** //
青ゾーン 青ゾーン 青ゾーン
// ************************************** */
LED_DEBUG0 = LED_ON;
for(int i = 0; i < 8; i++) {
linePara[i] = lineCast(LineHub::GetPara(i));
}
if(lineFase == 0) {
LedOut(1);
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 竿検知
LedOut(4);
static int Towlcount0=0;
Towlcount0++;
if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
if(Towlcount0<=20) {
motor[TIRE_FL].dir = BACK;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BACK;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FR].pwm = 50;
} else {
lineFase = 7;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
}
} else if(LimitSw::IsPressed(TOW_1L)) {
motor[TIRE_FL].dir = FOR;
motor[TIRE_FL].pwm = 20;
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) { // 前
LedOut(2);
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 竿検知
LedOut(4);
static int Towlcount1=0;
Towlcount1++;
if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
if(Towlcount1<=20) {
motor[TIRE_FL].dir = BACK;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BACK;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = FOR;
motor[TIRE_FR].pwm = 50;
} else {
lineFase = 15;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_FL].pwm = 50;
motor[TIRE_BL].dir = BRAKE;
motor[TIRE_BL].pwm = 50;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BR].pwm = 50;
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FR].pwm = 50;
}
} else if(LimitSw::IsPressed(TOW_2L)) {
motor[TIRE_FL].dir = FOR;
motor[TIRE_FL].pwm = 20;
motor[TIRE_BL].dir = 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) {
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=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
if(LimitSw::IsPressed(LSW_UU)) {
motor[LIFT_U].dir=BRAKE;
motor[LIFT_U].pwm=100;
} 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) {
static int PuluPulu=0;
if(PuluPulu>=100) {
motor[TIRE_FL].dir=BRAKE;
motor[TIRE_FR].dir=BRAKE;
motor[TIRE_BL].dir=BRAKE;
motor[TIRE_BR].dir=BRAKE;
motor[TIRE_FL].pwm=255;
motor[TIRE_FR].pwm=255;
motor[TIRE_BL].pwm=255;
motor[TIRE_BR].pwm=255;
lineFase=104;//system lineFase increasing
}
if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
motor[TIRE_FL].dir=BRAKE;
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の方が後ろに来ているとき
PuluPulu++;
motor[TIRE_FL].dir=FOR;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
PuluPulu++;
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
}
}
} else { //さほど離れてはいないが傾きが大きいとき
if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
PuluPulu++;
motor[TIRE_FL].dir=FOR;
motor[TIRE_FR].dir=FOR;
motor[TIRE_BL].dir=FOR;
motor[TIRE_BR].dir=FOR;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
PuluPulu++;
motor[TIRE_FL].dir=BACK;
motor[TIRE_FR].dir=BACK;
motor[TIRE_BL].dir=BACK;
motor[TIRE_BR].dir=BACK;
motor[TIRE_FL].pwm=20;
motor[TIRE_FR].pwm=20;
motor[TIRE_BL].pwm=20;
motor[TIRE_BR].pwm=20;
}
}
}
} 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 == 20) { // 前 低速
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;
}
*/
if(countW == 1) {
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 == 1) {
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()
{
}
#endif
#if USE_PROCESS_NUM>7
static void Process7()
{
static int Processflag=0;
if(Processflag==0) {
if(Limitphase==0) {
//下待機
Limitphase=2;
} else if(Limitphase==1) {
} else if(Limitphase==2) {
//下→上とタオル展開
static int countZYOUGE=0;
countZYOUGE++;
if(countZYOUGE<=20) {
motor[LIFT_LB].dir = FOR;
motor[LIFT_LB].pwm = 180;
motor[LIFT_RB].dir = BACK;
motor[LIFT_RB].pwm = 200;
} else {
if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Limitphase=4;
motor[LIFT_LB].dir = BRAKE;
motor[LIFT_LB].pwm = 200;
motor[LIFT_RB].dir = BRAKE;
motor[LIFT_RB].pwm = 200;
} else if(!(LimitSw::IsPressed(LSW_LB))&&!(LimitSw::IsPressed(LSW_RB))) {
motor[LIFT_LB].dir = FOR;
motor[LIFT_LB].pwm = 180;
motor[LIFT_RB].dir = BACK;
motor[LIFT_RB].pwm = 200;
} else if(!(LimitSw::IsPressed(LSW_LB))&&LimitSw::IsPressed(LSW_RB)) {
motor[LIFT_LB].dir = FOR;
motor[LIFT_LB].pwm = 180;
motor[LIFT_RB].dir = BRAKE;
motor[LIFT_RB].pwm = 200;
} else if(LimitSw::IsPressed(LSW_LB)&&!(LimitSw::IsPressed(LSW_RB))) {
motor[LIFT_LB].dir = BRAKE;
motor[LIFT_LB].pwm = 150;
motor[LIFT_RB].dir = BACK;
motor[LIFT_RB].pwm = 200;
}
}
} else if(Limitphase==3) {
} else if(Limitphase==4) {
//上段待機
Air[TOWEL0]=1;
motor[LIFT_LB].dir = BRAKE;
motor[LIFT_LB].pwm = 200;
motor[LIFT_RB].dir = BRAKE;
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;
}
motor[LIFT_RB].pwm = 200;
if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
motor[LIFT_LB].dir = BRAKE;
motor[LIFT_LB].pwm = 200;
motor[LIFT_RB].dir = BRAKE;
motor[LIFT_RB].pwm = 200;
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
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]);
}
}
}
#endif
#if USE_PROCESS_NUM>9
static void Process9()
{
}
#endif
#endif
#pragma endregion PROCESS
static void AllActuatorReset()
{
#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
}
void BuzzerTimer_func()
{
buzzer = !buzzer;
//LED_DEBUG0 = !LED_DEBUG0;
}
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();
}
}
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