File content as of revision 25:b3a9f34b201d:

#include "mbed.h"
#include "Process.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 "../../Input/Ultrasonic/Ultrasonic.h"
#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"

using namespace SWITCH;
using namespace PID_SPACE;
using namespace ENCODER;
using namespace ULTRASONIC;
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.*/

USS ultrasonic[] = {
        USS(ECHO_0,TRIG_0,TEMP),
        USS(ECHO_1,TRIG_1,TEMP),
    };

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 ***************

//*************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**************//

// ************* Line ************** //

float pw = 0;
int lineFase = 0;
bool lineCheck = false;
bool lineFlag  = false;
bool adjAnable = false;
int adj = 0;

int count = 0;
int lineCount = 0;
int targetCount = 0;

bool startFlag = true;

int linePWM;
int adj_F;
int adj_B;

int mode = 0;

// ************* Line ************** //

const int omni[15][15] =
{
{    0,     5,    21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255 },
{   -5,     0,     5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255 },
{  -21,    -5,     0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255 },
{  -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255 },
{  -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255 },
{ -130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255 },
{ -187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255 },
{ -255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255 },
{ -255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187 },
{ -255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130 },
{ -255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83 },
{ -255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47 },
{ -255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21 },
{ -255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5 },
{ -255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0 }
};

const int 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
	
}



void SystemProcess()
{
	SystemProcessInitialize();

	while(1)
	{
		int g[8];
		for(int i = 0; i < 8; i++){
			g[i] = lineCast(LineHub::GetPara(i));
		}
		
		pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);


		if(LimitSw::IsPressed(START_SW) && startFlag == true) {
			startFlag == false;
			lock = false;
			lineFase = 0;
			lineCount = 0;
			lineCheck = false;
			count = 0;
			/*
			if(LimitSw::IsPressed(ISREDBLUE_SW) {
				current = 4;
			} else {
				current = 5;
			}
			*/
			current = 4;
		}
		
		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;
    
    if(controller->Button.UP) {
    	motor[LIFT_LB].dir = FOR;
    	motor[LIFT_LB].pwm = 190;
    	motor[LIFT_RB].dir = BACK;
    	motor[LIFT_RB].pwm = 180;
    } else if(controller->Button.DOWN) {
    	motor[LIFT_LB].dir = BACK;
    	motor[LIFT_LB].pwm = 180;
    	motor[LIFT_RB].dir = FOR;
    	motor[LIFT_RB].pwm = 190;
    } 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 = BACK;
    	motor[LIFT_RB].pwm = 20;
    }
    
    
    if(controller->Button.X) {
    	motor[LIFT_U].dir = FOR;
    	motor[LIFT_U].pwm = 180;
    } else if(controller->Button.Y) {
    	motor[LIFT_U].dir = BACK;
    	motor[LIFT_U].pwm = 230;
    } else {
    	motor[LIFT_U].dir = BRAKE;
    	motor[LIFT_U].pwm = 255;
    }

	if(!(controller->AnalogL.Y == 7) || !(controller->AnalogL.X == 7)) {
	    motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     );
		motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         );
		motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]  );
		motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      );
	   
		motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
		motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2)    ;
		motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
		motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2)   ;	
	} else {
		motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
		motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
		motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
		motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
	   
		motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
		motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
		motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
		motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
	}
	
	if(controller->Button.ZR) {
		solenoid.solenoid4 = SOLENOID_ON;
	} else {
		solenoid.solenoid4 = SOLENOID_OFF;
	}
}
#endif

#if USE_PROCESS_NUM>2
static void Process2()
{	

	


/*
	if(moving) {
		if(LimiSw::IsPressed(LSW_LB)) {
			if(switchFlag_LB) {
				switchFlag_LB = false;
				motor[LIFT_LB].dir = BRAKE;  
	    		motor[LIFT_LB].pwm = 200;
	    	} else {
	    		seitchFlag_LB = true;
	    	}
		}
		if(LimiSw::IsPressed(LSW_RB)) {
			if(switchFlag_RB) {
				switchFlag_RB = false;
				motor[LIFT_RB].dir = BRAKE;
	    		motor[LIFT_RB].pwm = 200;
	    	} else {
	    		seitchFlag_RB = true;
	    	}
		}
		if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false;
	} else {
		if(controller->Button.UP) {
			if(!(state == UPPER)) {
				state++;
				motor[LIFT_LB].dir = BACK;
				motor[LIFT_RB].dir = FOR;
    			motor[LIFT_LB].pwm = 200;
    			motor[LIFT_RB].pwm = 200;
			}
		} else if(controller->Button.DOWN) {
			if(!(state == LOWER)) {
				state--;
				moving = true;
				motor[LIFT_LB].dir = FOR;
				motor[LIFT_RB].dir = BACK;
    			motor[LIFT_LB].pwm = 200;
    			motor[LIFT_RB].pwm = 200;
			}
		} else {
			motor[LIFT_LB].dir = BRAKE;
			motor[LIFT_RB].dir = BRAKE;
    		motor[LIFT_LB].pwm = 200;
    		motor[LIFT_RB].pwm = 200;
		}
	}
*/
}
#endif

#if USE_PROCESS_NUM>3
static void Process3() 
{
	startFlag = true;
	AllActuatorReset();
	lineFase = 0;
	lineCheck = false;
	lineCount = 0;
	count = 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) {
		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;
					count++;
				}
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
				tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
				tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
				tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(count == 3) {
			count = 0;
			lineFase = 2;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 2) {  // 前 低速
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = FOR;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = BACK;
		if(linePara[4] == 0) {
			lineFase = 3;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 16;
		motor[TIRE_BL].pwm = 16;
		motor[TIRE_BR].pwm = 16;
		motor[TIRE_FR].pwm = 16;
	} else if(lineFase == 3) {  // 右 ライントレース
		switch(linePara[4]) {
			case -2:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -10;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 10;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -20;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 20;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = 20;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -20;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = 10;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -10;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		
		if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
			targetCount = 3;
		} else if(LimitSw::IsPressed(TOWEL1_SW)) {
			targetCount = 3;
		} else {
			targetCount = 2;
		}
		
		if(count == targetCount) {
			count = 0;
			lineFase = 4;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 4) { // 右 低速
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = BACK;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = FOR;
		if (linePara[LINE_TOW_1] == 0) {
			lineFase = 6;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 14;
		motor[TIRE_BL].pwm = 14;
		motor[TIRE_BR].pwm = 14;
		motor[TIRE_FR].pwm = 14;
	} else if (lineFase == 5) {
		lineFase = 6;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
	} else if(lineFase == 6) {  // タオル1 検知
		if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
			lineFase = 7;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
	} else if(LimitSw::IsPressed(TOW_1L)) { 
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_FL].pwm = 16;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BL].pwm = 50;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		} else if(LimitSw::IsPressed(TOW_1R)) { 
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 16;
		} else {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		}
	} else if(lineFase == 7) {  // ライン 修正
		if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else if(linePara[LINE_TOW_1] > 0) {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = FOR;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_1] < 0) {
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = BACK;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_1] == 0) {
			lineFase = 8;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		}
	} else if(lineFase == 8) { // タオル1 解放
		//solenoid.TOWEL1 = SOLENOID_ON;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
		lineFase = 9;
	} else if(lineFase == 9) {  // 前
		switch(linePara[LINE_TOW_1]) {
			case 2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = 10;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = -10;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = 20;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = -20;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 20;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -20;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 10;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -10;
				adjAnable = true;
				break;
			case -2:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			adj = 0;
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(count == 1) {
			count = 0;
			lineFase = 10;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 10) {
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = FOR;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = BACK;
		if(linePara[4] == 0) {
			lineFase = 11;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 16;
		motor[TIRE_BL].pwm = 16;
		motor[TIRE_BR].pwm = 16;
		motor[TIRE_FR].pwm = 16; 
	} else if(lineFase == 11) {
		switch(linePara[4]) {
			case -2:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -10;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 10;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -20;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 20;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = 20;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -20;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = 10;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -10;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			adj = 0;//linePara[3];
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(count == 1) {
			count = 0;
			lineFase = 12;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 12) { // 右 低速
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = BACK;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = FOR;
		if(linePara[LINE_TOW_2] == 0) {
			lineFase = 13;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 16;
		motor[TIRE_BL].pwm = 16;
		motor[TIRE_BR].pwm = 16;
		motor[TIRE_FR].pwm = 16;
	} else if (lineFase == 13) {
		lineFase = 14;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
	} else if(lineFase == 14) {  // タオル2 竿検知
		if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
			lineFase = 15;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else if(LimitSw::IsPressed(TOW_2L)) { 
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		} else if(LimitSw::IsPressed(TOW_2R)) { 
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 16;
		} else {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		}
	} else if(lineFase == 15 ){  // ライン 修正
		if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else if(linePara[LINE_TOW_2] > 0) {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = FOR;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_2] < 0) {
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = BACK;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_2] == 0) {
			lineFase = 16;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		}
	} else if(lineFase == 16) { // タオル2 解放
		//solenoid.TOWEL2 = SOLENOID_ON;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
		lineFase = 17;
	} else if(lineFase == 17) { // 前
		switch(linePara[LINE_TOW_2]) {
			case 2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = 10;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = -10;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = 20;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = -20;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 20;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -20;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 10;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -10;
				adjAnable = true;
				break;
			case -2:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			adj = 0;
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(count == 1) {
			count = 0;
			lineFase = 18;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 18) {  // 前 低速
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = FOR;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = BACK;
		if(linePara[4] != 'N') {
			lineFase = 19;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 16;
		motor[TIRE_BL].pwm = 16;
		motor[TIRE_BR].pwm = 16;
		motor[TIRE_FR].pwm = 16;
	} else if(lineFase == 19) {  // 左
		switch(linePara[3]) {
			case -2:
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 30;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = 10;
				tirePWM[TIRE_BR] = 30;
				tirePWM[TIRE_FR] = -10;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = 20;
				tirePWM[TIRE_BR] = 30
				tirePWM[TIRE_FR] = -2;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = 30;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = -20;
				tirePWM[TIRE_BL] = 40;
				tirePWM[TIRE_BR] = 20;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = -10;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = 10;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = 30;
				tirePWM[TIRE_BR] = 30;
				tirePWM[TIRE_FR] = -30;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		
		/*
		if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
			targetCount = 3;
		} else if(LimitSw::IsPressed(TOWEL1_SW)) {
			targetCount = 3;
		} else {
			targetCount = 2;
		}
		*/
		
		targetCount = 3;
		
		if(count == targetCount) {
			count = 0;
			lineFase = 20;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 20) { // 左 低速
		motor[TIRE_FL].dir = BACK;
		motor[TIRE_BL].dir = FOR;
		motor[TIRE_BR].dir = FOR;
		motor[TIRE_FR].dir = BACK;
		if (linePara[2] == 'N') {
			//(LimitSw::IsPressed(SHEETS_SW)) {
			//ineFase = 20; 
			//else if(LimitSw::IsPressed(TOWEL2_SW) {
			//ineFase = 14;
			//else {
			lineFase = 21;
			//
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 16;
		motor[TIRE_BL].pwm = 16;
		motor[TIRE_BR].pwm = 16;
		motor[TIRE_FR].pwm = 16;
	} else if(lineFase == 21) {
		switch(linePara[2]) {
			case -2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = -10;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = 10;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = -20;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = 20;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = 30;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = -20;
				tirePWM[TIRE_BR] = 30;
				tirePWM[TIRE_FR] = 20;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = -10;
				tirePWM[TIRE_BR] = 30;
				tirePWM[TIRE_FR] = 10;
				adjAnable = true;
				break;
			case 2:
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 30;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = -30;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = 30;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(count == 3) {
			count = 0;
			lineFase = 22;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 22) {
		motor[TIRE_FL].dir = BACK;
		motor[TIRE_BL].dir = FOR;
		motor[TIRE_BR].dir = FOR;
		motor[TIRE_FR].dir = BACK;
		if (linePara[2] == 'N') {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 30;
		motor[TIRE_BL].pwm = 30;
		motor[TIRE_BR].pwm = 30;
		motor[TIRE_FR].pwm = 30;
	} else{
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
	}
	
}
#endif

#if USE_PROCESS_NUM>5
static void Process5() 
{	
	
	/* ************************************** //
	
		  青ゾーン	  青ゾーン	  青ゾーン
	
	// ************************************** */
	
	for(int i = 0; i < 8; i++) {
		linePara[i] = lineCast(LineHub::GetPara(i));
	}
	
	if(lineFase == 0) {
 		motor[TIRE_FL].dir = BACK;
		motor[TIRE_BL].dir = BACK;
		motor[TIRE_BR].dir = FOR;
		motor[TIRE_FR].dir = FOR;
		if(linePara[0] != 'N' && linePara[0] != 'A') {
			lineFase = 1;
		}
		motor[TIRE_FL].pwm = 20;
		motor[TIRE_BL].pwm = 20;
		motor[TIRE_BR].pwm = 20;
		motor[TIRE_FR].pwm = 20;
	}else if(lineFase == 1) {  // 前 ライントレース
		switch(linePara[0]) {
			case -2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = 20;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = 20;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = -20;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 40;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = -40;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 20;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = -20;
				adjAnable = true;
				break;
			case 2:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]) * 0.8;
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj) * 0.8;
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj) * 0.8;
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]) * 0.8;	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(count == 3) {
			count = 0;
			lineFase = 2;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 2) {  // 前 低速
		motor[TIRE_FL].dir = BACK;
		motor[TIRE_BL].dir = BACK;
		motor[TIRE_BR].dir = FOR;
		motor[TIRE_FR].dir = FOR;
		if(linePara[3] != 'N') {
			lineFase = 3;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 20;
		motor[TIRE_BL].pwm = 20;
		motor[TIRE_BR].pwm = 20;
		motor[TIRE_FR].pwm = 20;
	} else if(lineFase == 3) {  // 左 ライントレース
		switch(linePara[4]) {
			case -2:
				tirePWM[TIRE_FL] = -50;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 50;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = -50;
				tirePWM[TIRE_BL] = 20;
				tirePWM[TIRE_BR] = 50;
				tirePWM[TIRE_FR] = -20;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = -50;
				tirePWM[TIRE_BL] = 40;
				tirePWM[TIRE_BR] = 50;
				tirePWM[TIRE_FR] = -40;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = -50;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = 50;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = -40;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = 40;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = -20;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = 20;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = -50;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = 50;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(count == 2) {
			count = 0;
			lineFase = 4;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 4) { // 右 低速
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = BACK;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = FOR;
		if(linePara[LINE_TOW_1] == 0) {
			lineFase = 5;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 20;
		motor[TIRE_BL].pwm = 20;
		motor[TIRE_BR].pwm = 20;
		motor[TIRE_FR].pwm = 20;
	} else if (lineFase == 5) {
		lineFase = 6;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
	} else if(lineFase == 6) {  // タオル 竿検知
		if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
			lineFase = 7;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else if(LimitSw::IsPressed(TOW_1L)) { 
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		} else if(LimitSw::IsPressed(TOW_1R)) { 
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 16;
		} else {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 20;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 20;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 20;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 20;
		}
	} else if(lineFase == 7) {  // ライン 修正
		if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else if(linePara[LINE_TOW_1] > 0) {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = FOR;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_1] < 0) {
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = BACK;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_1] == 0) {
			lineFase = 8;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 30;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 30;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 30;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 30;
		}
	} else if(lineFase == 8) { // タオル1 解放
		//solenoid.TOWEL1 = SOLENOID_ON;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
		lineFase = 9;
	} else if(lineFase == 9) {  // 前
		switch(linePara[LINE_TOW_1]) {
			case 2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = 20;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = -20;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = 40;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = -40;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 40;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = -40;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 20;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = -20;
				adjAnable = true;
				break;
			case -2:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 50;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = -50;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			adj = 0;
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(count == 1) {
			count = 0;
			lineFase = 10;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 10) {
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = FOR;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = BACK;
		if(linePara[4] == 0) {
			lineFase = 11;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 20;
		motor[TIRE_BL].pwm = 20;
		motor[TIRE_BR].pwm = 20;
		motor[TIRE_FR].pwm = 20; 
	} else if(lineFase == 11) {
		switch(linePara[4]) {
			case -2:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = -40;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = 40;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = -50;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = 50;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = 40;
				tirePWM[TIRE_BL] = -50;
				tirePWM[TIRE_BR] = -40;
				tirePWM[TIRE_FR] = 50;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = 20;
				tirePWM[TIRE_BL] = -50;
				tirePWM[TIRE_BR] = -20;
				tirePWM[TIRE_FR] = 50;
				adjAnable = true;
				break;
			case 2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = -50;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 50;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					count++;
				}
				tirePWM[TIRE_FL] = 50;
				tirePWM[TIRE_BL] = -50;
				tirePWM[TIRE_BR] = -50;
				tirePWM[TIRE_FR] = 50;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0.;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			adj = 0;//linePara[3];
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(count == 3) {
			count = 0;
			lineFase = 12;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 12) { // 右 低速
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = BACK;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = FOR;
		if(linePara[LINE_TOW_2] == 0) {
			lineFase = 13;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 16;
		motor[TIRE_BL].pwm = 16;
		motor[TIRE_BR].pwm = 16;
		motor[TIRE_FR].pwm = 16;
	} else if (lineFase == 13) {
		lineFase = 14;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
	} else {
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
	}
	
}
#endif

#if USE_PROCESS_NUM>6
static void Process6() 
{
	for(int i = 0; i < 8; i++) {
		linePara[i] = lineCast(LineHub::GetPara(i));
	}
	
	if(lineFase == 0) {
		if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
			lineFase = 1;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 30;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 30;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 30;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 30;
		} else if(LimitSw::IsPressed(TOW_1L)) { 
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 12;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 12;
		} else if(LimitSw::IsPressed(TOW_1R)) { 
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 12;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 12;
		} else {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 18;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 18;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 18;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 18;
		}
	} else if(lineFase == 1) {
		if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 30;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 30;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 30;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 30;
		} else if(linePara[LINE_TOW_1] > 0) {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 15;
			motor[TIRE_BL].dir = FOR;
			motor[TIRE_BL].pwm = 15;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 15;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 15;
		} else if(linePara[LINE_TOW_1] < 0) {
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 15;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 15;
			motor[TIRE_BR].dir = BACK;
			motor[TIRE_BR].pwm = 15;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 15;
		} else if(linePara[LINE_TOW_1] == 0) {
			lineFase = 2;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 30;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 30;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 30;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 30;
		} else {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 30;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 30;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 30;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 30;
		}
	} else if(lineFase == 2) {
		//solenoid::solenoid1 = SOLENOID_ON
		
	}
}
#endif

#if USE_PROCESS_NUM>7
static void Process7()
{
	
}
#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