aa

Dependencies:   mbed

System/Process/Process.cpp

Committer:
Ryosei
Date:
2019-10-03
Revision:
28:479631c2de29
Parent:
27:dd9f27fce7d1
Child:
29:44d5454ce8fa

File content as of revision 28:479631c2de29:

#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=28;////////////////////////////////////////////////温度
    if(num==0) {
        Trig0.write_us(1,10);
        Duration=Echo0.read_high_us(5000);
    } else if(num==1) {
        Trig1.write_us(1,10);
        Duration=Echo1.read_high_us(5000);
    }
    if(Duration>0) {
        Duration=Duration/2;
        double sspead=331.5+0.6*temp;
        Distance=Duration*sspead*100/1000000;
    } else {
        return 0;
    }
    return Distance;
}
//*********Ultra***************

//*********Tape LED**************************
DigitalOut select1(SELECT_1);
DigitalOut select2(SELECT_2);
DigitalOut select3(SELECT_3);
void LedOut(int num)
{
    int selectnum[8][3]= {
        {0,0,0},
        {0,0,1},
        {0,1,0},
        {0,1,1},
        {1,0,0},
        {1,0,1},
        {1,1,0},
        {1,1,1}
    };
    select1=selectnum[num][0];
    select2=selectnum[num][1];
    select3=selectnum[num][2];
}
//*********Tape LED**************************

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

Timer tow_stop;

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

}


double Ult_left=UltraRead(0);///////////////////////////////////////////left sensor
double Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
static int Limitphase=0;
void SystemProcess()
{
    SystemProcessInitialize();
    while(1) {
        AirUpdate();
        int g[8];
        for(int i = 0; i < 8; i++) {
            g[i] = lineCast(LineHub::GetPara(i));
        }
        Ult_left=UltraRead(0);//////////////////////////////////////////left sensor
        Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
        pc.printf("%lf,%lf",UltraRead(0),UltraRead(1));
//        pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);

        pc.printf("%d\n\r",current);
        /*上
        	motor[LIFT_LB].dir = FOR;
        	motor[LIFT_LB].pwm = 180;
        	motor[LIFT_RB].dir = BACK;
        	motor[LIFT_RB].pwm = 200;
        	*/
        if(!(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW))&&startFlag==true) {
            startFlag=false;
            lock=false;
            current=7;
        } else {
            motor[LIFT_LB].dir = BRAKE;
            motor[LIFT_LB].pwm = 180;
            motor[LIFT_RB].dir = BRAKE;
            motor[LIFT_RB].pwm = 200;
        }
        if(LimitSw::IsPressed(START_SW) && startFlag == true) {
            LedOut(6);
            startFlag = false;
            lock = false;
            lineFase = 0;
            lineCount = 0;
            lineCheck = false;
            countW = 0;
            if(LimitSw::IsPressed(REDBLUE_SW)) {
                current = 4;
            } else {
                current = 5;
            }
        }
        buzzer.period(1.0/800);

#ifdef USE_MU
        controller = CONTROLLER::Controller::GetData();
#endif

#ifdef USE_ERRORCHECK
        if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag) {
            CONTROLLER::Controller::DataReset();
            AllActuatorReset();
            lock = true;
        } else
#endif
        {

#ifdef USE_SUBPROCESS
            if(!lock) {
                Process[current]();
            } else
#endif
            {
                //ロック時の処理
            }
        }


        //Emergency!
        /*
        if(!EMG_0 && !EMG_1 && !EMGflag){
        	buzzer = 0;
        	BuzzerTimer.attach(BuzzerTimer_func, 1);
        	EMGflag = true;
        	LED_DEBUG0 = 1;
        }
        if(EMG_0 && EMG_1 && EMGflag){
        	buzzer = 1;
        	BuzzerTimer.detach();
        	EMGflag = false;
        }
        */
        SystemProcessUpdate();
    }
}




#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.ZR) {
        Air[CLOTHESPIN] = SOLENOID_ON;
    }
    if(controller->Button.ZL) {
        Air[CLOTHESPIN] = 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] >= -1 && linePara[4] <= 1) {
            lineFase = 3;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_FR].dir = BRAKE;
        }
        motor[TIRE_FL].pwm = 20;
        motor[TIRE_BL].pwm = 20;
        motor[TIRE_BR].pwm = 20;
        motor[TIRE_FR].pwm = 20;
    } else if(lineFase == 3) {  // 右 ライントレース
        switch(linePara[4]) {
            case -2:
            LedOut(2);
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 0;
                adjAnable = true;
                break;
            case -3:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = -10;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 10;
                adjAnable = true;
                break;
            case -1:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = -20;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 20;
                adjAnable = true;
                break;
            case 0:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 1:
                tirePWM[TIRE_FL] = 20;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = -20;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 3:
                tirePWM[TIRE_FL] = 10;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = -10;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 2:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 'A':
                if(lineCheck == false) {
                    lineCheck = true;
                    lineCount = 0;
                    countW++;
                }
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 'N':
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
                break;
            default:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
        }

        if(adjAnable) {
            if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
        } else {
            adj = 0;
        }

        motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
        motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
        motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
        motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
        motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
        motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
        motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
        motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);

        if(lineCheck == true)  {
            lineCount++;
            if(lineCount > 20) lineCheck = false;
        }

        targetCount = 1;

        if(countW == targetCount) {
            countW = 0;
            lineFase = 4;
            lineCount = 0;
            lineCheck = false;
        }
    } else if(lineFase == 4) { // 右 低速
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BL].dir = BACK;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_FR].dir = FOR;
        if (linePara[LINE_TOW_2] == 0) {  // 1と2逆になります。
            lineFase = 6;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_FR].dir = BRAKE;
        }
        if(linePara[LINE_TOW_1] == 0) {
            if(!LimitSw::IsPressed(SHEETS_SW)) {
                lineFase=100;
            } else {
                lineFase = 6;
            }
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_FR].dir = BRAKE;
        }
        motor[TIRE_FL].pwm = 16;
        motor[TIRE_BL].pwm = 16;
        motor[TIRE_BR].pwm = 16;
        motor[TIRE_FR].pwm = 16;
    } else if (lineFase == 5) {
        lineFase = 6;
        motor[TIRE_FL].dir = BRAKE;
        motor[TIRE_BL].dir = BRAKE;
        motor[TIRE_BR].dir = BRAKE;
        motor[TIRE_FR].dir = BRAKE;
    } else if(lineFase == 6) {  // タオル1 検知
    LedOut(4);
        if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
            lineFase = 7;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_FL].pwm = 50;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FR].pwm = 50;
        } else if(LimitSw::IsPressed(TOW_1L)) {
            motor[TIRE_FL].dir = FOR;
            motor[TIRE_FL].pwm = 16;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = FOR;
            motor[TIRE_FR].pwm = 16;
        } else if(LimitSw::IsPressed(TOW_1R)) {
            motor[TIRE_FL].dir = BACK;
            motor[TIRE_FL].pwm = 16;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BACK;
            motor[TIRE_FR].pwm = 16;
        } else {
           if(LimitSw::IsPressed(QF_SW)) {
				switch(linePara[LINE_TOW_2]) {
					case -2:
						tirePWM[TIRE_FL] = -10;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 10;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case -3:
						tirePWM[TIRE_FL] = -14;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 14;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case -1:
						tirePWM[TIRE_FL] = -17;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 17;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 0:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 1:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -17;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 17;
						adjAnable = true;
						break;
					case 3:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -14;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 14;
						adjAnable = true;
						break;
					case 2:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -10;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 10;
						adjAnable = true;
						break;
					case 'A':
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 'N':
						tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
						tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
						tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
						tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
						adjAnable = false;
						break;
					default:
						tirePWM[TIRE_FL] = 0;
						tirePWM[TIRE_BL] = 0;
						tirePWM[TIRE_BR] = 0;
						tirePWM[TIRE_FR] = 0;
						adjAnable = false;
				}
			} else {
				switch(linePara[LINE_TOW_2]) {
					case 2:
						tirePWM[TIRE_FL] = -10;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 10;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 3:
						tirePWM[TIRE_FL] = -14;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 14;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 1:
						tirePWM[TIRE_FL] = -17;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 17;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 0:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case -1:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -17;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 17;
						adjAnable = true;
						break;
					case -3:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -14;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 14;
						adjAnable = true;
						break;
					case -2:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -10;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 10;
						adjAnable = true;
						break;
					case 'A':
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 'N':
						tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
						tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
						tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
						tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
						adjAnable = false;
						break;
					default:
						tirePWM[TIRE_FL] = 0;
						tirePWM[TIRE_BL] = 0;
						tirePWM[TIRE_BR] = 0;
						tirePWM[TIRE_FR] = 0;
						adjAnable = false;
				}
			}
				
			motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
			motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
			motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
			motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
			motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
			motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
			motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
			motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
		}
	} else if(lineFase == 7) {  // ライン 修正
		if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else if(linePara[LINE_TOW_2] > 0) {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = FOR;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_2] < 0) {
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = BACK;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_2] == 0) {
			lineFase = 8;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		}
	} else if(lineFase == 8) { // タオル1 解放
		Air[0] = SOLENOID_ON;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
		lineFase = 9;
	} else if(lineFase == 9) {  // 前 ライントレース
	LedOut(2);
		if(LimitSw::IsPressed(QF_SW)) {
			switch(linePara[LINE_TOW_2]) {
				case -2:
					tirePWM[TIRE_FL] = 10;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -10;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case -3:
					tirePWM[TIRE_FL] = 14;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -14;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case -1:
					tirePWM[TIRE_FL] = 17;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -17;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case 0:
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case 1:
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 17;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -17;
					adjAnable = true;
					break;
				case 3:
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 14;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -14;
					adjAnable = true;
					break;
				case 2:
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 10;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -10;
					adjAnable = true;
					break;
				case 'A':
					if(lineCheck == false) {
						lineCheck = true;
						lineCount = 0;
						countW++;
					}
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case 'N':
					tirePWM[TIRE_FL] = 0;
					tirePWM[TIRE_BL] = 0;
					tirePWM[TIRE_BR] = 0;
					tirePWM[TIRE_FR] = 0;
					adjAnable = false;
					break;
				default:
					tirePWM[TIRE_FL] = 0;
					tirePWM[TIRE_BL] = 0;
					tirePWM[TIRE_BR] = 0;
					tirePWM[TIRE_FR] = 0;
					adjAnable = false;
			}
		} else {
			switch(linePara[LINE_TOW_2]) {
				case 2:
					tirePWM[TIRE_FL] = 10;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -10;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case 3:
					tirePWM[TIRE_FL] = 14;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -14;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case 1:
					tirePWM[TIRE_FL] = 17;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -17;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case 0:
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case -1:
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 17;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -17;
					adjAnable = true;
					break;
				case -3:
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 14;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -14;
					adjAnable = true;
					break;
				case -2:
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 10;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -10;
					adjAnable = true;
					break;
				case 'A':
					if(lineCheck == false) {
						lineCheck = true;
						lineCount = 0;
						countW++;
					}
					tirePWM[TIRE_FL] = 20;
					tirePWM[TIRE_BL] = 20;
					tirePWM[TIRE_BR] = -20;
					tirePWM[TIRE_FR] = -20;
					adjAnable = true;
					break;
				case 'N':
					tirePWM[TIRE_FL] = 0;
					tirePWM[TIRE_BL] = 0;
					tirePWM[TIRE_BR] = 0;
					tirePWM[TIRE_FR] = 0;
					adjAnable = false;
					break;
				default:
					tirePWM[TIRE_FL] = 0;
					tirePWM[TIRE_BL] = 0;
					tirePWM[TIRE_BR] = 0;
					tirePWM[TIRE_FR] = 0;
					adjAnable = false;
			}
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(countW == 1) {
			countW = 0;
			lineFase = 10;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 10) {  // 前 低速
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = FOR;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = BACK;
		if(linePara[4] == 0) {
			lineFase = 11;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 16;
		motor[TIRE_BL].pwm = 16;
		motor[TIRE_BR].pwm = 16;
		motor[TIRE_FR].pwm = 16; 
	} else if(lineFase == 11) {  
		switch(linePara[4]) {  // 右 ライントレース
			case -2:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 0;
				adjAnable = true;
				break;
			case -3:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -10;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 10;
				adjAnable = true;
				break;
			case -1:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -20;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 20;
				adjAnable = true;
				break;
			case 0:
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 1:
				tirePWM[TIRE_FL] = 20;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -20;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 3:
				tirePWM[TIRE_FL] = 10;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -10;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 2:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 'A':
				if(lineCheck == false) {
					lineCheck = true;
					lineCount = 0;
					countW++;
				}
				tirePWM[TIRE_FL] = 30;
				tirePWM[TIRE_BL] = -30;
				tirePWM[TIRE_BR] = -30;
				tirePWM[TIRE_FR] = 30;
				adjAnable = true;
				break;
			case 'N':
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
				break;
			default:
				tirePWM[TIRE_FL] = 0;
				tirePWM[TIRE_BL] = 0;
				tirePWM[TIRE_BR] = 0;
				tirePWM[TIRE_FR] = 0;
				adjAnable = false;
		}
		
		if(adjAnable){
			if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
		} else {
			adj = 0;
		}
		
		motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
		motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
		motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
		motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
		motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
		motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
		motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);	
		
		if(lineCheck == true)  {
			lineCount++;
			if(lineCount > 20) lineCheck = false;
		}
		if(countW == 2) {
			countW = 0;
			lineFase = 12;
			lineCount = 0;
			lineCheck = false;
		}
	} else if(lineFase == 12) { // 右 低速
		motor[TIRE_FL].dir = FOR;
		motor[TIRE_BL].dir = BACK;
		motor[TIRE_BR].dir = BACK;
		motor[TIRE_FR].dir = FOR;
		if(linePara[LINE_TOW_1] == 0) {
			lineFase = 13;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_FR].dir = BRAKE;
		}
		motor[TIRE_FL].pwm = 16;
		motor[TIRE_BL].pwm = 16;
		motor[TIRE_BR].pwm = 16;
		motor[TIRE_FR].pwm = 16;
	} else if (lineFase == 13) {
		lineFase = 14;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
	} else if(lineFase == 14) {  // タオル2 竿検知
	LedOut(4);
		if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
			lineFase = 15;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else if(LimitSw::IsPressed(TOW_2L)) { 
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 20;
			motor[TIRE_BL].dir = FOR;
			motor[TIRE_BL].pwm = 20;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 20;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 20;
		} else if(LimitSw::IsPressed(TOW_2R)) { 
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 20;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 20;
			motor[TIRE_BR].dir = BACK;
			motor[TIRE_BR].pwm = 20;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 20;
		} else {
			if(LimitSw::IsPressed(QF_SW)) {
				switch(linePara[LINE_TOW_1]) {
					case -2:
						tirePWM[TIRE_FL] = -10;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 10;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case -3:
						tirePWM[TIRE_FL] = -14;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 14;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case -1:
						tirePWM[TIRE_FL] = -17;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 17;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 0:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 1:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -17;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 17;
						adjAnable = true;
						break;
					case 3:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -14;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 14;
						adjAnable = true;
						break;
					case 2:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -10;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 10;
						adjAnable = true;
						break;
					case 'A':
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 'N':
						tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
						tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
						tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
						tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
						adjAnable = false;
						break;
					default:
						tirePWM[TIRE_FL] = 0;
						tirePWM[TIRE_BL] = 0;
						tirePWM[TIRE_BR] = 0;
						tirePWM[TIRE_FR] = 0;
						adjAnable = false;
				}
			} else {
				switch(linePara[LINE_TOW_1]) {
					case 2:
						tirePWM[TIRE_FL] = -10;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 10;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 3:
						tirePWM[TIRE_FL] = -14;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 14;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 1:
						tirePWM[TIRE_FL] = -17;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 17;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 0:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case -1:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -17;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 17;
						adjAnable = true;
						break;
					case -3:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -14;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 14;
						adjAnable = true;
						break;
					case -2:
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -10;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 10;
						adjAnable = true;
						break;
					case 'A':
						tirePWM[TIRE_FL] = -20;
						tirePWM[TIRE_BL] = -20;
						tirePWM[TIRE_BR] = 20;
						tirePWM[TIRE_FR] = 20;
						adjAnable = true;
						break;
					case 'N':
						tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
						tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
						tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
						tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
						adjAnable = false;
						break;
					default:
						tirePWM[TIRE_FL] = 0;
						tirePWM[TIRE_BL] = 0;
						tirePWM[TIRE_BR] = 0;
						tirePWM[TIRE_FR] = 0;
						adjAnable = false;
				}
			}
				
			motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
			motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
			motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
			motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
			motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
			motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
			motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
			motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
		}
	} else if(lineFase == 15 ){  // ライン 修正
		if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else if(linePara[LINE_TOW_1] > 0) {
			motor[TIRE_FL].dir = BACK;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = FOR;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = FOR;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = BACK;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_1] < 0) {
			motor[TIRE_FL].dir = FOR;
			motor[TIRE_FL].pwm = 16;
			motor[TIRE_BL].dir = BACK;
			motor[TIRE_BL].pwm = 16;
			motor[TIRE_BR].dir = BACK;
			motor[TIRE_BR].pwm = 16;
			motor[TIRE_FR].dir = FOR;
			motor[TIRE_FR].pwm = 16;
		} else if(linePara[LINE_TOW_1] == 0) {
			lineFase = 16;
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		} else {
			motor[TIRE_FL].dir = BRAKE;
			motor[TIRE_FL].pwm = 50;
			motor[TIRE_BL].dir = BRAKE;
			motor[TIRE_BL].pwm = 50;
			motor[TIRE_BR].dir = BRAKE;
			motor[TIRE_BR].pwm = 50;
			motor[TIRE_FR].dir = BRAKE;
			motor[TIRE_FR].pwm = 50;
		}
	} else if(lineFase == 16) { // タオル2 解放
		Air[1] = SOLENOID_ON;
		motor[TIRE_FL].dir = BRAKE;
		motor[TIRE_BL].dir = BRAKE;
		motor[TIRE_BR].dir = BRAKE;
		motor[TIRE_FR].dir = BRAKE;
		lineFase = 17;
	} else if(lineFase == 17) { // 前
        if(LimitSw::IsPressed(LSW_UU)) {
            motor[LIFT_U].dir=BRAKE;
            motor[LIFT_U].pwm=100;
        } else {
            motor[LIFT_U].dir=BACK;
            motor[LIFT_U].pwm=180;
        }
        switch(linePara[LINE_TOW_2]) {
            case 2:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 30;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case 3:
                tirePWM[TIRE_FL] = 10;
                tirePWM[TIRE_BL] = 30;
                tirePWM[TIRE_BR] = -10;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case 1:
                tirePWM[TIRE_FL] = 20;
                tirePWM[TIRE_BL] = 30;
                tirePWM[TIRE_BR] = -20;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case 0:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = 30;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case -1:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = 20;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = -20;
                adjAnable = true;
                break;
            case -3:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = 10;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = -10;
                adjAnable = true;
                break;
            case -2:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 0;
                adjAnable = true;
                break;
            case 'A':
                if(lineCheck == false) {
                    lineCheck = true;
                    lineCount = 0;
                    countW++;
                }
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = 30;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case 'N':
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
                break;
            default:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
        }

        if(adjAnable) {
            adj = 0;
        } else {
            adj = 0;
        }

        motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
        motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
        motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
        motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
        motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
        motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
        motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
        motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);

        if(lineCheck == true)  {
            lineCount++;
            if(lineCount > 20) lineCheck = false;
        }
        if(countW == 1) {
            countW = 0;
            lineFase = 100;
            lineCount = 0;
            lineCheck = false;
        }
    } else if(lineFase==100) {
        LedOut(6);
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BL].dir = FOR;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_FR].dir = BACK;
        motor[TIRE_FL].pwm=15;
        motor[TIRE_FR].pwm=15;
        motor[TIRE_BL].pwm=15;
        motor[TIRE_BR].pwm=15;
        if(LimitSw::IsPressed(LSW_UU)) {
            motor[LIFT_U].dir=BRAKE;
            motor[LIFT_U].pwm=100;
        } else {
            motor[LIFT_U].dir=BACK;
            motor[LIFT_U].pwm=150;
        }
        if((Ult_left>0)&&(Ult_right>0)&&(Ult_left<30)&&(Ult_right<30)) {
            lineFase=101;
        }
    } else if(lineFase==101) {
        //位置調整
        //(P制御)
        if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
            if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
                motor[TIRE_FL].dir=BRAKE;
                motor[TIRE_FR].dir=BRAKE;
                motor[TIRE_BL].dir=BRAKE;
                motor[TIRE_BR].dir=BRAKE;
                motor[TIRE_FL].pwm=100;
                motor[TIRE_FR].pwm=100;
                motor[TIRE_BL].pwm=100;
                motor[TIRE_BR].pwm=100;
                lineFase=102;//system lineFase increasing
            } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
                if(Ult_right>16) { //Ult_rightが遠い場合
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=0;
                    motor[TIRE_FR].pwm=15;
                    motor[TIRE_BL].pwm=0;
                    motor[TIRE_BR].pwm=15;
                } else if(Ult_right<14) { //Ult_rightが近い場合
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=0;
                    motor[TIRE_FR].pwm=15;
                    motor[TIRE_BL].pwm=0;
                    motor[TIRE_BR].pwm=15;
                }
            } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
                if(Ult_left>16) { //Ult_leftが遠い場合
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=15;
                    motor[TIRE_FR].pwm=0;
                    motor[TIRE_BL].pwm=15;
                    motor[TIRE_BR].pwm=0;
                } else if(Ult_left<14) { //Ult_leftが近い場合
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=15;
                    motor[TIRE_FR].pwm=0;
                    motor[TIRE_BL].pwm=15;
                    motor[TIRE_BR].pwm=0;
                }
            } else { //どっちもあってない場合
                if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
                    if((Ult_left-Ult_right)>10||((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき
                        if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
                            motor[TIRE_FL].dir=FOR;
                            motor[TIRE_FR].dir=FOR;
                            motor[TIRE_BL].dir=FOR;
                            motor[TIRE_BR].dir=FOR;
                            motor[TIRE_FL].pwm=15;
                            motor[TIRE_FR].pwm=15;
                            motor[TIRE_BL].pwm=15;
                            motor[TIRE_BR].pwm=15;
                        } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
                            motor[TIRE_FL].dir=BACK;
                            motor[TIRE_FR].dir=BACK;
                            motor[TIRE_BL].dir=BACK;
                            motor[TIRE_BR].dir=BACK;
                            motor[TIRE_FL].pwm=15;
                            motor[TIRE_FR].pwm=15;
                            motor[TIRE_BL].pwm=15;
                            motor[TIRE_BR].pwm=15;
                        }
                    } else { //傾きが大きくなくて離れているとき
                        if((Ult_right+Ult_left)<=25) { //近すぎるとき
                            motor[TIRE_FL].dir=BACK;
                            motor[TIRE_FR].dir=FOR;
                            motor[TIRE_BL].dir=BACK;
                            motor[TIRE_BR].dir=FOR;
                            motor[TIRE_FL].pwm=20;
                            motor[TIRE_FR].pwm=20;
                            motor[TIRE_BL].pwm=20;
                            motor[TIRE_BR].pwm=20;
                        } else if((Ult_right+Ult_left)>=35) { //離れているとき
                            motor[TIRE_FL].dir=FOR;
                            motor[TIRE_FR].dir=BACK;
                            motor[TIRE_BL].dir=FOR;
                            motor[TIRE_BR].dir=BACK;
                            motor[TIRE_FL].pwm=20;
                            motor[TIRE_FR].pwm=20;
                            motor[TIRE_BL].pwm=20;
                            motor[TIRE_BR].pwm=20;
                        }
                    }

                } else { //さほど離れてはいないが傾きが大きいとき
                    if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
                        motor[TIRE_FL].dir=FOR;
                        motor[TIRE_FR].dir=FOR;
                        motor[TIRE_BL].dir=FOR;
                        motor[TIRE_BR].dir=FOR;
                        motor[TIRE_FL].pwm=15;
                        motor[TIRE_FR].pwm=15;
                        motor[TIRE_BL].pwm=15;
                        motor[TIRE_BR].pwm=15;
                    } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
                        motor[TIRE_FL].dir=BACK;
                        motor[TIRE_FR].dir=BACK;
                        motor[TIRE_BL].dir=BACK;
                        motor[TIRE_BR].dir=BACK;
                        motor[TIRE_FL].pwm=15;
                        motor[TIRE_FR].pwm=15;
                        motor[TIRE_BL].pwm=15;
                        motor[TIRE_BR].pwm=15;
                    }
                }
            }

        } else {//データを受け取ってないとき
            motor[TIRE_FL].dir=BRAKE;
            motor[TIRE_FR].dir=BRAKE;
            motor[TIRE_BL].dir=BRAKE;
            motor[TIRE_BR].dir=BRAKE;
            motor[TIRE_FL].pwm=100;
            motor[TIRE_FR].pwm=100;
            motor[TIRE_BL].pwm=100;
            motor[TIRE_BR].pwm=100;
        }

    } else if(lineFase==102) {
        Air[CLOTHESPIN]=SOLENOID_OFF;
        LedOut(1);
        //リミットスイッチに当てる
        static int count2=0;
        if(count2==0) {
            if(LimitSw::IsPressed(RIGHTlim)) {
                count2=1;
            } else if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
                if( ((Ult_left+Ult_right)<=25)||((Ult_left+Ult_right)>=35) ) {
                    if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) {
                        if(Ult_left-Ult_right<=0) {
                            //右
                            motor[TIRE_FL].dir=FOR;
                            motor[TIRE_FR].dir=FOR;
                            motor[TIRE_BL].dir=BACK;
                            motor[TIRE_BR].dir=BACK;

                            motor[TIRE_FL].pwm=25;
                            motor[TIRE_FR].pwm=25;

                            motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right));
                            motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right));
                        } else if(Ult_left-Ult_right>0) {
                            motor[TIRE_FL].dir=FOR;
                            motor[TIRE_FR].dir=FOR;
                            motor[TIRE_BL].dir=BACK;
                            motor[TIRE_BR].dir=BACK;

                            motor[TIRE_BL].pwm=25;
                            motor[TIRE_BR].pwm=25;
                            motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right));
                            motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right));
                        }
                    } else if((Ult_left+Ult_right)<=25) {
                        motor[TIRE_FL].dir=FOR;
                        motor[TIRE_FR].dir=FOR;
                        motor[TIRE_BL].dir=BACK;
                        motor[TIRE_BR].dir=BACK;

                        motor[TIRE_FL].pwm=0;
                        motor[TIRE_FR].pwm=25;
                        motor[TIRE_BL].pwm=25;
                        motor[TIRE_BR].pwm=0;
                    } else if((Ult_left+Ult_right)>=35) {
                        motor[TIRE_FL].dir=FOR;
                        motor[TIRE_FR].dir=FOR;
                        motor[TIRE_BL].dir=BACK;
                        motor[TIRE_BR].dir=BACK;

                        motor[TIRE_FL].pwm=25;
                        motor[TIRE_FR].pwm=0;
                        motor[TIRE_BL].pwm=0;
                        motor[TIRE_BR].pwm=25;
                    }
                } else {
                    if(Ult_left-Ult_right<=0) {
                        motor[TIRE_FL].dir=FOR;
                        motor[TIRE_FR].dir=FOR;
                        motor[TIRE_BL].dir=BACK;
                        motor[TIRE_BR].dir=BACK;

                        motor[TIRE_FL].pwm=25;
                        motor[TIRE_FR].pwm=25;

                        motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right));
                        motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right));
                    } else if(Ult_left-Ult_right>0) {
                        motor[TIRE_FL].dir=FOR;
                        motor[TIRE_FR].dir=FOR;
                        motor[TIRE_BL].dir=BACK;
                        motor[TIRE_BR].dir=BACK;

                        motor[TIRE_BL].pwm=25;
                        motor[TIRE_BR].pwm=25;
                        motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right));
                        motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right));
                    }
                }

            } else {
                motor[TIRE_FL].dir=BRAKE;
                motor[TIRE_FR].dir=BRAKE;
                motor[TIRE_BL].dir=BRAKE;
                motor[TIRE_BR].dir=BRAKE;
                motor[TIRE_FL].pwm=100;
                motor[TIRE_FR].pwm=100;
                motor[TIRE_BL].pwm=100;
                motor[TIRE_BR].pwm=100;

            }
        } else if(count2==1) {
            motor[TIRE_FL].dir=BRAKE;
            motor[TIRE_FR].dir=BRAKE;
            motor[TIRE_BL].dir=BRAKE;
            motor[TIRE_BR].dir=BRAKE;
            motor[TIRE_FL].pwm=255;
            motor[TIRE_FR].pwm=255;
            motor[TIRE_BL].pwm=255;
            motor[TIRE_BR].pwm=255;
            lineFase=103;
        }
    } else if(lineFase==103) {
        if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
            if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
                motor[TIRE_FL].dir=BRAKE;
                motor[TIRE_FR].dir=BRAKE;
                motor[TIRE_BL].dir=BRAKE;
                motor[TIRE_BR].dir=BRAKE;
                motor[TIRE_FL].pwm=255;
                motor[TIRE_FR].pwm=255;
                motor[TIRE_BL].pwm=255;
                motor[TIRE_BR].pwm=255;
                lineFase=104;//system lineFase increasing
            } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
                if(Ult_right>16) { //Ult_rightが遠い場合
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=0;
                    motor[TIRE_FR].pwm=13;
                    motor[TIRE_BL].pwm=0;
                    motor[TIRE_BR].pwm=13;
                } else if(Ult_right<14) { //Ult_rightが近い場合
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=0;
                    motor[TIRE_FR].pwm=13;
                    motor[TIRE_BL].pwm=0;
                    motor[TIRE_BR].pwm=13;
                }
            } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
                if(Ult_left>16) { //Ult_leftが遠い場合
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=13;
                    motor[TIRE_FR].pwm=0;
                    motor[TIRE_BL].pwm=13;
                    motor[TIRE_BR].pwm=0;
                } else if(Ult_left<14) { //Ult_leftが近い場合
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=13;
                    motor[TIRE_FR].pwm=0;
                    motor[TIRE_BL].pwm=13;
                    motor[TIRE_BR].pwm=0;
                }
            } else { //どっちもあってない場合
                if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
                    if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
                        if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
                            motor[TIRE_FL].dir=FOR;
                            motor[TIRE_FR].dir=FOR;
                            motor[TIRE_BL].dir=FOR;
                            motor[TIRE_BR].dir=FOR;
                            motor[TIRE_FL].pwm=15;
                            motor[TIRE_FR].pwm=15;
                            motor[TIRE_BL].pwm=15;
                            motor[TIRE_BR].pwm=15;
                        } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
                            motor[TIRE_FL].dir=BACK;
                            motor[TIRE_FR].dir=BACK;
                            motor[TIRE_BL].dir=BACK;
                            motor[TIRE_BR].dir=BACK;
                            motor[TIRE_FL].pwm=15;
                            motor[TIRE_FR].pwm=15;
                            motor[TIRE_BL].pwm=15;
                            motor[TIRE_BR].pwm=15;
                        }
                    }
                } else { //さほど離れてはいないが傾きが大きいとき
                    if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
                        motor[TIRE_FL].dir=FOR;
                        motor[TIRE_FR].dir=FOR;
                        motor[TIRE_BL].dir=FOR;
                        motor[TIRE_BR].dir=FOR;
                        motor[TIRE_FL].pwm=15;
                        motor[TIRE_FR].pwm=15;
                        motor[TIRE_BL].pwm=15;
                        motor[TIRE_BR].pwm=15;
                    } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
                        motor[TIRE_FL].dir=BACK;
                        motor[TIRE_FR].dir=BACK;
                        motor[TIRE_BL].dir=BACK;
                        motor[TIRE_BR].dir=BACK;
                        motor[TIRE_FL].pwm=15;
                        motor[TIRE_FR].pwm=15;
                        motor[TIRE_BL].pwm=15;
                        motor[TIRE_BR].pwm=15;
                    }
                }
            }

        } else {//データを受け取ってないとき
            motor[TIRE_FL].dir=BRAKE;
            motor[TIRE_FR].dir=BRAKE;
            motor[TIRE_BL].dir=BRAKE;
            motor[TIRE_BR].dir=BRAKE;
            motor[TIRE_FL].pwm=100;
            motor[TIRE_FR].pwm=100;
            motor[TIRE_BL].pwm=100;
            motor[TIRE_BR].pwm=100;
        }
    } else if(lineFase==104) {
        static int count3=0;
        static int loop=0;
        pc.printf("%d\r\n",loop);
        Air[CLOTHESPIN]=1;
        if(count3==0) {
            loop++;
            if(loop==60) {
                count3=1;
            }
        } else if(count3==1) {
            Air[CLOTHESPIN]=0;
            lineFase=105;

        }
    } else if(lineFase==105) {
        LedOut(6);
        Air[CLOTHESPIN]=0;
        if(!(LimitSw::IsPressed(LEFTlim))) {

            //超音波
            if(Ult_right>0) {
                if(Ult_right<15) {
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=25+(15-Ult_left);
                    motor[TIRE_FR].pwm=25;
                    motor[TIRE_BL].pwm=25;
                    motor[TIRE_BR].pwm=25+(15-Ult_left);
                } else if(Ult_right>=15) {
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=25;
                    motor[TIRE_FR].pwm=25+(15-Ult_left);
                    motor[TIRE_BL].pwm=25+(15-Ult_left);
                    motor[TIRE_BR].pwm=25;
                }
            } else {
                motor[TIRE_FL].dir=BACK;
                motor[TIRE_FR].dir=BACK;
                motor[TIRE_BL].dir=FOR;
                motor[TIRE_BR].dir=FOR;
                motor[TIRE_FL].pwm=25;
                motor[TIRE_FR].pwm=25;
                motor[TIRE_BL].pwm=25;
                motor[TIRE_BR].pwm=25;
            }


        } else {
            motor[TIRE_FL].dir=BRAKE;
            motor[TIRE_FR].dir=BRAKE;
            motor[TIRE_BL].dir=BRAKE;
            motor[TIRE_BR].dir=BRAKE;
            motor[TIRE_FL].pwm=100;
            motor[TIRE_FR].pwm=100;
            motor[TIRE_BL].pwm=100;
            motor[TIRE_BR].pwm=100;
            lineFase=106;
        }

    } else if(lineFase==106) {
        motor[TIRE_FL].dir=BACK;
        motor[TIRE_FR].dir=FOR;
        motor[TIRE_BL].dir=BACK;
        motor[TIRE_BR].dir=FOR;
        motor[TIRE_FL].pwm=15;
        motor[TIRE_FR].pwm=15;
        motor[TIRE_BL].pwm=15;
        motor[TIRE_BR].pwm=15;
        if(linePara[4]!='N') {
            lineFase=19;
        }
    } else if(lineFase == 18) {  // 前 低速
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BL].dir = FOR;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_FR].dir = BACK;
        if(linePara[4] != 'N') {
            lineFase = 19;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_FR].dir = BRAKE;
        }
        motor[TIRE_FL].pwm = 16;
        motor[TIRE_BL].pwm = 16;
        motor[TIRE_BR].pwm = 16;
        motor[TIRE_FR].pwm = 16;
    } else if(lineFase == 19) {  // 左
        if(!(LimitSw::IsPressed(LSW_UB))) {
            motor[LIFT_U].dir=FOR;
            motor[LIFT_U].pwm=150;
        } else {
            motor[LIFT_U].dir=FOR;
            motor[LIFT_U].pwm=150;
        }
        switch(linePara[3]) {
            case -2:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 0;
                adjAnable = true;
                break;
            case -3:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = 10;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = -10;
                adjAnable = true;
                break;
            case -1:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = 20;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = -20;
                adjAnable = true;
                break;
            case 0:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = 30;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case 1:
                tirePWM[TIRE_FL] = -20;
                tirePWM[TIRE_BL] = 40;
                tirePWM[TIRE_BR] = 20;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case 3:
                tirePWM[TIRE_FL] = -10;
                tirePWM[TIRE_BL] = 30;
                tirePWM[TIRE_BR] = 10;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case 2:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 30;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case 'A':
                if(lineCheck == false) {
                    lineCheck = true;
                    lineCount = 0;
                    countW++;
                }
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = 30;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = -30;
                adjAnable = true;
                break;
            case 'N':
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
                break;
            default:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
        }

        if(adjAnable) {
            if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
        } else {
            adj = 0;
        }

        motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
        motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
        motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
        motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
        motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
        motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
        motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
        motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);

        if(lineCheck == true)  {
            lineCount++;
            if(lineCount > 20) lineCheck = false;
        }

        /*
        if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
        	targetCount = 3;
        } else if(LimitSw::IsPressed(TOWEL1_SW)) {
        	targetCount = 3;
        } else {
        	targetCount = 2;
        }
        */

        targetCount = 3;

        if(countW == targetCount) {
            countW = 0;
            lineFase = 20;
            lineCount = 0;
            lineCheck = false;
        }
    } else if(lineFase == 20) { // 左 低速
        if(!(LimitSw::IsPressed(LSW_UB))) {
            motor[LIFT_U].dir=FOR;
            motor[LIFT_U].pwm=150;
        } else {
            motor[LIFT_U].dir=FOR;
            motor[LIFT_U].pwm=150;
        }
        motor[TIRE_FL].dir = BACK;
        motor[TIRE_BL].dir = FOR;
        motor[TIRE_BR].dir = FOR;
        motor[TIRE_FR].dir = BACK;
        if (linePara[2] == 'N') {
            //(!!LimitSw::Ispressed(SHEETS_SW)) {
            //ineFase = 20;
            //else if(LimitSw::IsPressed(TOWEL2_SW) {
            //ineFase = 14;
            //else {
            lineFase = 21;
            //
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_FR].dir = BRAKE;
        }
        motor[TIRE_FL].pwm = 16;
        motor[TIRE_BL].pwm = 16;
        motor[TIRE_BR].pwm = 16;
        motor[TIRE_FR].pwm = 16;
    } else if(lineFase == 21) {
        if(!(LimitSw::IsPressed(LSW_UB))) {
            motor[LIFT_U].dir=FOR;
            motor[LIFT_U].pwm=150;
        } else {
            motor[LIFT_U].dir=FOR;
            motor[LIFT_U].pwm=150;
        }
        switch(linePara[2]) {
            case -2:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case -3:
                tirePWM[TIRE_FL] = -10;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 10;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case -1:
                tirePWM[TIRE_FL] = -20;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 20;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 0:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 1:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = -20;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 20;
                adjAnable = true;
                break;
            case 3:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = -10;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 10;
                adjAnable = true;
                break;
            case 2:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 0;
                adjAnable = true;
                break;
            case 'A':
                if(lineCheck == false) {
                    lineCheck = true;
                    lineCount = 0;
                    countW++;
                }
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 'N':
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
                break;
            default:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
        }

        if(adjAnable) {
            if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
        } else {
            adj = 0;
        }

        motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
        motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
        motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
        motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
        motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
        motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
        motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
        motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);

        if(lineCheck == true)  {
            lineCount++;
            if(lineCount > 20) lineCheck = false;
        }
        if(countW == 3) {
            countW = 0;
            lineFase = 22;
            lineCount = 0;
            lineCheck = false;
        }
    } else if(lineFase == 22) {
        motor[TIRE_FL].dir = BACK;
        motor[TIRE_BL].dir = FOR;
        motor[TIRE_BR].dir = FOR;
        motor[TIRE_FR].dir = BACK;
        if (linePara[2] == 'N') {
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_FR].dir = BRAKE;
        }
        motor[TIRE_FL].pwm = 30;
        motor[TIRE_BL].pwm = 30;
        motor[TIRE_BR].pwm = 30;
        motor[TIRE_FR].pwm = 30;
    } else {
        motor[TIRE_FL].dir = BRAKE;
        motor[TIRE_BL].dir = BRAKE;
        motor[TIRE_BR].dir = BRAKE;
        motor[TIRE_FR].dir = BRAKE;
    }

}
#endif

#if USE_PROCESS_NUM>5
static void Process5()
{

    /* ************************************** //

    	  青ゾーン	  青ゾーン	  青ゾーン

    // ************************************** */

    for(int i = 0; i < 8; i++) {
        linePara[i] = lineCast(LineHub::GetPara(i));
    }

    if(lineFase == 0) {
        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);
        if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
            lineFase = 7;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_FL].pwm = 50;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FR].pwm = 50;
        } else if(LimitSw::IsPressed(TOW_1L)) {
            motor[TIRE_FL].dir = FOR;
            motor[TIRE_FL].pwm = 20;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = FOR;
            motor[TIRE_FR].pwm = 20;
        } else if(LimitSw::IsPressed(TOW_1R)) {
            motor[TIRE_FL].dir = BACK;
            motor[TIRE_FL].pwm = 20;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BACK;
            motor[TIRE_FR].pwm = 20;
        } else {
            switch(linePara[LINE_TOW_1]) {
                case -2:
                    tirePWM[TIRE_FL] = -10;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 10;
                    tirePWM[TIRE_FR] = 20;
                    adjAnable = true;
                    break;
                case -3:
                    tirePWM[TIRE_FL] = -14;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 14;
                    tirePWM[TIRE_FR] = 20;
                    adjAnable = true;
                    break;
                case -1:
                    tirePWM[TIRE_FL] = -17;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 17;
                    tirePWM[TIRE_FR] = 20;
                    adjAnable = true;
                    break;
                case 0:
                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 20;
                    adjAnable = true;
                    break;
                case 1:
                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -17;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 17;
                    adjAnable = true;
                    break;
                case 3:
                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -14;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 14;
                    adjAnable = true;
                    break;
                case 2:
                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -10;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 10;
                    adjAnable = true;
                    break;
                case 'A':
                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 20;
                    adjAnable = true;
                    break;
                case 'N':
                    tirePWM[TIRE_FL] = 0;
                    tirePWM[TIRE_BL] = 0;
                    tirePWM[TIRE_BR] = 0;
                    tirePWM[TIRE_FR] = 0;
                    adjAnable = false;
                    break;
                default:
                    tirePWM[TIRE_FL] = 0;
                    tirePWM[TIRE_BL] = 0;
                    tirePWM[TIRE_BR] = 0;
                    tirePWM[TIRE_FR] = 0;
                    adjAnable = false;
            }

            motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
            motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
            motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
            motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
            motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
            motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
            motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
            motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
        }
    } else if(lineFase == 7) {  // ライン 修正
        if(linePara[LINE_TOW_1] == 'A' || linePara[LINE_TOW_1] == 'N') {
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_FL].pwm = 50;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FR].pwm = 50;
        } else if(linePara[LINE_TOW_1] > 0) {
            motor[TIRE_FL].dir = BACK;
            motor[TIRE_FL].pwm = 16;
            motor[TIRE_BL].dir = FOR;
            motor[TIRE_BL].pwm = 16;
            motor[TIRE_BR].dir = FOR;
            motor[TIRE_BR].pwm = 16;
            motor[TIRE_FR].dir = BACK;
            motor[TIRE_FR].pwm = 16;
        } else if(linePara[LINE_TOW_1] < 0) {
            motor[TIRE_FL].dir = FOR;
            motor[TIRE_FL].pwm = 16;
            motor[TIRE_BL].dir = BACK;
            motor[TIRE_BL].pwm = 16;
            motor[TIRE_BR].dir = BACK;
            motor[TIRE_BR].pwm = 16;
            motor[TIRE_FR].dir = FOR;
            motor[TIRE_FR].pwm = 16;
        } else if(linePara[LINE_TOW_1] == 0) {
            //tow_stop.reset();
            //tow_stop.start();
            lineFase = 8;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_FL].pwm = 50;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FR].pwm = 50;
        } else {
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_FL].pwm = 50;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FR].pwm = 50;
        }
    } else if(lineFase == 8) { // タオル1 解放
        Air[TOWEL1] = SOLENOID_ON;
        motor[TIRE_FL].dir = BRAKE;
        motor[TIRE_BL].dir = BRAKE;
        motor[TIRE_BR].dir = BRAKE;
        motor[TIRE_FR].dir = BRAKE;
        lineFase = 9;
    } else if(lineFase == 9) {  // 前
    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);
        if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
            lineFase = 15;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_FL].pwm = 50;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FR].pwm = 50;
        } else if(LimitSw::IsPressed(TOW_2L)) {
            motor[TIRE_FL].dir = FOR;
            motor[TIRE_FL].pwm = 20;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = FOR;
            motor[TIRE_FR].pwm = 20;
        } else if(LimitSw::IsPressed(TOW_2R)) {
            motor[TIRE_FL].dir = BACK;
            motor[TIRE_FL].pwm = 20;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BACK;
            motor[TIRE_FR].pwm = 20;
        } else {
            switch(linePara[LINE_TOW_2]) {
                case -2:
                    tirePWM[TIRE_FL] = -10;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 10;
                    tirePWM[TIRE_FR] = 20;
                    break;
                case -3:
                    tirePWM[TIRE_FL] = -14;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 14;
                    tirePWM[TIRE_FR] = 20;
                    break;
                case -1:
                    tirePWM[TIRE_FL] = -17;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 17;
                    tirePWM[TIRE_FR] = 20;
                    break;
                case 0:
                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 20;
                    break;
                case 1:
                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -17;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 17;
                    break;
                case 3:
                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -14;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 14;
                    break;
                case 2:
                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -10;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 10;
                    break;
                case 'A':

                    tirePWM[TIRE_FL] = -20;
                    tirePWM[TIRE_BL] = -20;
                    tirePWM[TIRE_BR] = 20;
                    tirePWM[TIRE_FR] = 20;
                    break;
                case 'N':
                    tirePWM[TIRE_FL] = 0;
                    tirePWM[TIRE_BL] = 0;
                    tirePWM[TIRE_BR] = 0;
                    tirePWM[TIRE_FR] = 0;
                    break;
                default:
                    tirePWM[TIRE_FL] = 0;
                    tirePWM[TIRE_BL] = 0;
                    tirePWM[TIRE_BR] = 0;
                    tirePWM[TIRE_FR] = 0;
            }

            motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
            motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
            motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
            motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
            motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
            motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
            motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
            motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
        }
    } else if(lineFase == 15 ) { // ライン 修正
        if(linePara[LINE_TOW_2] == 'A' || linePara[LINE_TOW_2] == 'N') {
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_FL].pwm = 50;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FR].pwm = 50;
        } else if(linePara[LINE_TOW_2] > 0) {
            motor[TIRE_FL].dir = BACK;
            motor[TIRE_FL].pwm = 16;
            motor[TIRE_BL].dir = FOR;
            motor[TIRE_BL].pwm = 16;
            motor[TIRE_BR].dir = FOR;
            motor[TIRE_BR].pwm = 16;
            motor[TIRE_FR].dir = BACK;
            motor[TIRE_FR].pwm = 16;
        } else if(linePara[LINE_TOW_2] < 0) {
            motor[TIRE_FL].dir = FOR;
            motor[TIRE_FL].pwm = 16;
            motor[TIRE_BL].dir = BACK;
            motor[TIRE_BL].pwm = 16;
            motor[TIRE_BR].dir = BACK;
            motor[TIRE_BR].pwm = 16;
            motor[TIRE_FR].dir = FOR;
            motor[TIRE_FR].pwm = 16;
        } else if(linePara[LINE_TOW_2] == 0) {
            lineFase = 16;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_FL].pwm = 50;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FR].pwm = 50;
        } else {
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_FL].pwm = 50;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BL].pwm = 50;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BR].pwm = 50;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FR].pwm = 50;
        }
    } else if(lineFase == 16) { // タオル2 解放
        Air[TOWEL2] = SOLENOID_ON;
        motor[TIRE_FL].dir = BRAKE;
        motor[TIRE_BL].dir = BRAKE;
        motor[TIRE_BR].dir = BRAKE;
        motor[TIRE_FR].dir = BRAKE;
        lineFase = 17;
    } else if(lineFase == 17) { // 前
        switch(linePara[LINE_TOW_2]) {
            case 2:
                tirePWM[TIRE_FL] = 10;
                tirePWM[TIRE_BL] = 20;
                tirePWM[TIRE_BR] = -10;
                tirePWM[TIRE_FR] = -20;
                adjAnable = true;
                break;
            case 3:
                tirePWM[TIRE_FL] = 14;
                tirePWM[TIRE_BL] = 20;
                tirePWM[TIRE_BR] = -14;
                tirePWM[TIRE_FR] = -20;
                adjAnable = true;
                break;
            case 1:
                tirePWM[TIRE_FL] = 17;
                tirePWM[TIRE_BL] = 20;
                tirePWM[TIRE_BR] = -17;
                tirePWM[TIRE_FR] = -20;
                adjAnable = true;
                break;
            case 0:
                tirePWM[TIRE_FL] = 20;
                tirePWM[TIRE_BL] = 20;
                tirePWM[TIRE_BR] = -20;
                tirePWM[TIRE_FR] = -20;
                adjAnable = true;
                break;
            case -1:
                tirePWM[TIRE_FL] = 20;
                tirePWM[TIRE_BL] = 17;
                tirePWM[TIRE_BR] = -20;
                tirePWM[TIRE_FR] = -17;
                adjAnable = true;
                break;
            case -3:
                tirePWM[TIRE_FL] = 20;
                tirePWM[TIRE_BL] = 14;
                tirePWM[TIRE_BR] = -20;
                tirePWM[TIRE_FR] = -14;
                adjAnable = true;
                break;
            case -2:
                tirePWM[TIRE_FL] = 20;
                tirePWM[TIRE_BL] = 10;
                tirePWM[TIRE_BR] = -20;
                tirePWM[TIRE_FR] = 10;
                adjAnable = true;
                break;
            case 'A':
                if(lineCheck == false) {
                    lineCheck = true;
                    lineCount = 0;
                    countW++;
                }
                tirePWM[TIRE_FL] = 20;
                tirePWM[TIRE_BL] = 20;
                tirePWM[TIRE_BR] = -20;
                tirePWM[TIRE_FR] = -20;
                adjAnable = true;
                break;
            case 'N':
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
                break;
            default:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
        }

        if(adjAnable) {
            adj = 0;
        } else {
            adj = 0;
        }

        motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
        motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
        motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
        motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
        motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
        motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
        motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
        motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);

        if(lineCheck == true)  {
            lineCount++;
            if(lineCount > 20) lineCheck = false;
        }
        if(countW == 1) {
            countW = 0;
            lineFase = 100;
            lineCount = 0;
            lineCheck = false;
        }
    } else if(lineFase==100) {
        LedOut(6);
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BL].dir = FOR;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_FR].dir = BACK;
        if(LimitSw::IsPressed(LSW_UU)) {
            motor[LIFT_U].dir=BRAKE;
            motor[LIFT_U].pwm=100;
        } else {
            motor[LIFT_U].dir=BACK;
            motor[LIFT_U].pwm=150;
        }
        if((Ult_left>0)&&(Ult_right>0)&&(Ult_left<30)&&(Ult_right<30)) {
            lineFase=101;
        }
    } else if(lineFase==101) {
        //位置調整
        //(P制御)
        if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
            if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
                motor[TIRE_FL].dir=BRAKE;
                motor[TIRE_FR].dir=BRAKE;
                motor[TIRE_BL].dir=BRAKE;
                motor[TIRE_BR].dir=BRAKE;
                motor[TIRE_FL].pwm=100;
                motor[TIRE_FR].pwm=100;
                motor[TIRE_BL].pwm=100;
                motor[TIRE_BR].pwm=100;
                lineFase=102;//system lineFase increasing
            } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
                if(Ult_right>16) { //Ult_rightが遠い場合
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=0;
                    motor[TIRE_FR].pwm=15;
                    motor[TIRE_BL].pwm=0;
                    motor[TIRE_BR].pwm=15;
                } else if(Ult_right<14) { //Ult_rightが近い場合
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=0;
                    motor[TIRE_FR].pwm=15;
                    motor[TIRE_BL].pwm=0;
                    motor[TIRE_BR].pwm=15;
                }
            } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
                if(Ult_left>16) { //Ult_leftが遠い場合
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=15;
                    motor[TIRE_FR].pwm=0;
                    motor[TIRE_BL].pwm=15;
                    motor[TIRE_BR].pwm=0;
                } else if(Ult_left<14) { //Ult_leftが近い場合
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=15;
                    motor[TIRE_FR].pwm=0;
                    motor[TIRE_BL].pwm=15;
                    motor[TIRE_BR].pwm=0;
                }
            } else { //どっちもあってない場合
                if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
                    if((Ult_left-Ult_right)>10||((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき
                        if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
                            motor[TIRE_FL].dir=FOR;
                            motor[TIRE_FR].dir=FOR;
                            motor[TIRE_BL].dir=FOR;
                            motor[TIRE_BR].dir=FOR;
                            motor[TIRE_FL].pwm=15;
                            motor[TIRE_FR].pwm=15;
                            motor[TIRE_BL].pwm=15;
                            motor[TIRE_BR].pwm=15;
                        } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
                            motor[TIRE_FL].dir=BACK;
                            motor[TIRE_FR].dir=BACK;
                            motor[TIRE_BL].dir=BACK;
                            motor[TIRE_BR].dir=BACK;
                            motor[TIRE_FL].pwm=15;
                            motor[TIRE_FR].pwm=15;
                            motor[TIRE_BL].pwm=15;
                            motor[TIRE_BR].pwm=15;
                        }
                    } else { //傾きが大きくなくて離れているとき
                        if((Ult_right+Ult_left)<=25) { //近すぎるとき
                            motor[TIRE_FL].dir=BACK;
                            motor[TIRE_FR].dir=FOR;
                            motor[TIRE_BL].dir=BACK;
                            motor[TIRE_BR].dir=FOR;
                            motor[TIRE_FL].pwm=20;
                            motor[TIRE_FR].pwm=20;
                            motor[TIRE_BL].pwm=20;
                            motor[TIRE_BR].pwm=20;
                        } else if((Ult_right+Ult_left)>=35) { //離れているとき
                            motor[TIRE_FL].dir=FOR;
                            motor[TIRE_FR].dir=BACK;
                            motor[TIRE_BL].dir=FOR;
                            motor[TIRE_BR].dir=BACK;
                            motor[TIRE_FL].pwm=20;
                            motor[TIRE_FR].pwm=20;
                            motor[TIRE_BL].pwm=20;
                            motor[TIRE_BR].pwm=20;
                        }
                    }

                } else { //さほど離れてはいないが傾きが大きいとき
                    if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
                        motor[TIRE_FL].dir=FOR;
                        motor[TIRE_FR].dir=FOR;
                        motor[TIRE_BL].dir=FOR;
                        motor[TIRE_BR].dir=FOR;
                        motor[TIRE_FL].pwm=15;
                        motor[TIRE_FR].pwm=15;
                        motor[TIRE_BL].pwm=15;
                        motor[TIRE_BR].pwm=15;
                    } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
                        motor[TIRE_FL].dir=BACK;
                        motor[TIRE_FR].dir=BACK;
                        motor[TIRE_BL].dir=BACK;
                        motor[TIRE_BR].dir=BACK;
                        motor[TIRE_FL].pwm=15;
                        motor[TIRE_FR].pwm=15;
                        motor[TIRE_BL].pwm=15;
                        motor[TIRE_BR].pwm=15;
                    }
                }
            }

        } else {//データを受け取ってないとき
            motor[TIRE_FL].dir=BRAKE;
            motor[TIRE_FR].dir=BRAKE;
            motor[TIRE_BL].dir=BRAKE;
            motor[TIRE_BR].dir=BRAKE;
            motor[TIRE_FL].pwm=100;
            motor[TIRE_FR].pwm=100;
            motor[TIRE_BL].pwm=100;
            motor[TIRE_BR].pwm=100;
        }

    } else if(lineFase==102) {
        Air[CLOTHESPIN]=SOLENOID_OFF;
        LedOut(1);
        //リミットスイッチに当てる
        static int count2=0;
        if(count2==0) {
            if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
                if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {
                    if(((Ult_left-Ult_right)>=5)||((Ult_left-Ult_right)<=-5)) {
                        if(Ult_left-Ult_right<=0) {
                            motor[TIRE_FL].dir=BACK;
                            motor[TIRE_FR].dir=BACK;
                            motor[TIRE_BL].dir=FOR;
                            motor[TIRE_BR].dir=FOR;

                            motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
                            motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));

                            motor[TIRE_BL].pwm=25;
                            motor[TIRE_BR].pwm=25;
                        } else if(Ult_left-Ult_right>0) {
                            motor[TIRE_FL].dir=BACK;
                            motor[TIRE_FR].dir=BACK;
                            motor[TIRE_BL].dir=FOR;
                            motor[TIRE_BR].dir=FOR;
                            motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
                            motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
                            motor[TIRE_FL].pwm=25;
                            motor[TIRE_FR].pwm=25;
                        }
                    } else if((Ult_left+Ult_right)<=25) {
                        motor[TIRE_FL].dir=BACK;
                        motor[TIRE_FR].dir=BACK;
                        motor[TIRE_BL].dir=FOR;
                        motor[TIRE_BR].dir=FOR;
                        motor[TIRE_FL].pwm=25;
                        motor[TIRE_FR].pwm=0;
                        motor[TIRE_BL].pwm=0;
                        motor[TIRE_BR].pwm=25;
                    } else if((Ult_left+Ult_right)>=35) {
                        motor[TIRE_FL].dir=BACK;
                        motor[TIRE_FR].dir=BACK;
                        motor[TIRE_BL].dir=FOR;
                        motor[TIRE_BR].dir=FOR;
                        motor[TIRE_FL].pwm=0;
                        motor[TIRE_FR].pwm=25;
                        motor[TIRE_BL].pwm=25;
                        motor[TIRE_BR].pwm=0;
                    }
                } else {
                    if(Ult_left-Ult_right<=0) {
                        motor[TIRE_FL].dir=BACK;
                        motor[TIRE_FR].dir=BACK;
                        motor[TIRE_BL].dir=FOR;
                        motor[TIRE_BR].dir=FOR;

                        motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
                        motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));

                        motor[TIRE_BL].pwm=25;
                        motor[TIRE_BR].pwm=25;
                    } else if(Ult_left-Ult_right>0) {
                        motor[TIRE_FL].dir=BACK;
                        motor[TIRE_FR].dir=BACK;
                        motor[TIRE_BL].dir=FOR;
                        motor[TIRE_BR].dir=FOR;
                        motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
                        motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
                        motor[TIRE_FL].pwm=25;
                        motor[TIRE_FR].pwm=25;
                    }
                }

            } else {
                motor[TIRE_FL].dir=BRAKE;
                motor[TIRE_FR].dir=BRAKE;
                motor[TIRE_BL].dir=BRAKE;
                motor[TIRE_BR].dir=BRAKE;
                motor[TIRE_FL].pwm=100;
                motor[TIRE_FR].pwm=100;
                motor[TIRE_BL].pwm=100;
                motor[TIRE_BR].pwm=100;

            }
            if(LimitSw::IsPressed(LEFTlim)) {
                count2=1;
            }

        } else if(count2==1) {
            motor[TIRE_FL].dir=BRAKE;
            motor[TIRE_FR].dir=BRAKE;
            motor[TIRE_BL].dir=BRAKE;
            motor[TIRE_BR].dir=BRAKE;
            motor[TIRE_FL].pwm=255;
            motor[TIRE_FR].pwm=255;
            motor[TIRE_BL].pwm=255;
            motor[TIRE_BR].pwm=255;
            lineFase=103;
        }
    } else if(lineFase==103) {
        if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
            if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
                motor[TIRE_FL].dir=BRAKE;
                motor[TIRE_FR].dir=BRAKE;
                motor[TIRE_BL].dir=BRAKE;
                motor[TIRE_BR].dir=BRAKE;
                motor[TIRE_FL].pwm=255;
                motor[TIRE_FR].pwm=255;
                motor[TIRE_BL].pwm=255;
                motor[TIRE_BR].pwm=255;
                lineFase=104;//system lineFase increasing
            } else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
                if(Ult_right>16) { //Ult_rightが遠い場合
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=0;
                    motor[TIRE_FR].pwm=13;
                    motor[TIRE_BL].pwm=0;
                    motor[TIRE_BR].pwm=13;
                } else if(Ult_right<14) { //Ult_rightが近い場合
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=0;
                    motor[TIRE_FR].pwm=13;
                    motor[TIRE_BL].pwm=0;
                    motor[TIRE_BR].pwm=13;
                }
            } else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
                if(Ult_left>16) { //Ult_leftが遠い場合
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=BACK;
                    motor[TIRE_BL].dir=FOR;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=13;
                    motor[TIRE_FR].pwm=0;
                    motor[TIRE_BL].pwm=13;
                    motor[TIRE_BR].pwm=0;
                } else if(Ult_left<14) { //Ult_leftが近い場合
                    motor[TIRE_FL].dir=BACK;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=FOR;
                    motor[TIRE_FL].pwm=13;
                    motor[TIRE_FR].pwm=0;
                    motor[TIRE_BL].pwm=13;
                    motor[TIRE_BR].pwm=0;
                }
            } else { //どっちもあってない場合
                if( (Ult_left+Ult_right)<=25||(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
                    if((Ult_left-Ult_right)>0||((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
                        if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
                            motor[TIRE_FL].dir=FOR;
                            motor[TIRE_FR].dir=FOR;
                            motor[TIRE_BL].dir=FOR;
                            motor[TIRE_BR].dir=FOR;
                            motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
                            motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
                            motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
                            motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
                        } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
                            motor[TIRE_FL].dir=BACK;
                            motor[TIRE_FR].dir=BACK;
                            motor[TIRE_BL].dir=BACK;
                            motor[TIRE_BR].dir=BACK;
                            motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
                            motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
                            motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
                            motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
                        }
                    }
                } else { //さほど離れてはいないが傾きが大きいとき
                    if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
                        motor[TIRE_FL].dir=FOR;
                        motor[TIRE_FR].dir=FOR;
                        motor[TIRE_BL].dir=FOR;
                        motor[TIRE_BR].dir=FOR;
                        motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
                        motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
                        motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
                        motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
                    } else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
                        motor[TIRE_FL].dir=BACK;
                        motor[TIRE_FR].dir=BACK;
                        motor[TIRE_BL].dir=BACK;
                        motor[TIRE_BR].dir=BACK;
                        motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
                        motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
                        motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
                        motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
                    }
                }
            }

        } else {//データを受け取ってないとき
            motor[TIRE_FL].dir=BRAKE;
            motor[TIRE_FR].dir=BRAKE;
            motor[TIRE_BL].dir=BRAKE;
            motor[TIRE_BR].dir=BRAKE;
            motor[TIRE_FL].pwm=100;
            motor[TIRE_FR].pwm=100;
            motor[TIRE_BL].pwm=100;
            motor[TIRE_BR].pwm=100;
        }
    } else if(lineFase==104) {
        static int count3=0;
        static int loop=0;
        pc.printf("%d\r\n",loop);
        Air[CLOTHESPIN]=1;
        if(count3==0) {
            loop++;
            if(loop==60) {
                count3=1;
            }
        } else if(count3==1) {
            Air[CLOTHESPIN]=0;
            lineFase=105;

        }
    } else if(lineFase==105) {
        LedOut(6);
        Air[CLOTHESPIN]=0;
        if(!(LimitSw::IsPressed(RIGHTlim))) {

            //超音波
            if(Ult_right>0) {
                if(Ult_right<15) {
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=25;
                    motor[TIRE_FR].pwm=25+(15-Ult_right);
                    motor[TIRE_BL].pwm=25+(15-Ult_right);
                    motor[TIRE_BR].pwm=25;
                } else if(Ult_right>=15) {
                    motor[TIRE_FL].dir=FOR;
                    motor[TIRE_FR].dir=FOR;
                    motor[TIRE_BL].dir=BACK;
                    motor[TIRE_BR].dir=BACK;
                    motor[TIRE_FL].pwm=25+(Ult_right-15);
                    motor[TIRE_FR].pwm=25;
                    motor[TIRE_BL].pwm=25;
                    motor[TIRE_BR].pwm=25+(Ult_right-15);
                }
            } else {
                motor[TIRE_FL].dir=FOR;
                motor[TIRE_FR].dir=FOR;
                motor[TIRE_BL].dir=BACK;
                motor[TIRE_BR].dir=BACK;
                motor[TIRE_FL].pwm=25;
                motor[TIRE_FR].pwm=25;
                motor[TIRE_BL].pwm=25;
                motor[TIRE_BR].pwm=25;
            }


        } else {
            motor[TIRE_FL].dir=BRAKE;
            motor[TIRE_FR].dir=BRAKE;
            motor[TIRE_BL].dir=BRAKE;
            motor[TIRE_BR].dir=BRAKE;
            motor[TIRE_FL].pwm=100;
            motor[TIRE_FR].pwm=100;
            motor[TIRE_BL].pwm=100;
            motor[TIRE_BR].pwm=100;
            lineFase=106;
        }

    } else if(lineFase==106) {
        motor[TIRE_FL].dir=BACK;
        motor[TIRE_FR].dir=FOR;
        motor[TIRE_BL].dir=BACK;
        motor[TIRE_BR].dir=FOR;
        motor[TIRE_FL].pwm=15;
        motor[TIRE_FR].pwm=15;
        motor[TIRE_BL].pwm=15;
        motor[TIRE_BR].pwm=15;
        if(linePara[4]!='N') {
            lineFase=19;
        }
    } else if(lineFase == 18) {  // 前 低速
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BL].dir = FOR;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_FR].dir = BACK;
        if(linePara[4] == 0) {
            lineFase = 19;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_FR].dir = BRAKE;
        }
        motor[TIRE_FL].pwm = 16;
        motor[TIRE_BL].pwm = 16;
        motor[TIRE_BR].pwm = 16;
        motor[TIRE_FR].pwm = 16;
    } else if(lineFase == 19) {  // 右
        switch(linePara[4]) {
            case -2:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 0;
                adjAnable = true;
                break;
            case -3:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = -10;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 10;
                adjAnable = true;
                break;
            case -1:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = -20;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 20;
                adjAnable = true;
                break;
            case 0:
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 1:
                tirePWM[TIRE_FL] = 20;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = -20;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 3:
                tirePWM[TIRE_FL] = 10;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = -10;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 2:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 'A':
                if(lineCheck == false) {
                    lineCheck = true;
                    lineCount = 0;
                    countW++;
                }
                tirePWM[TIRE_FL] = 30;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = -30;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 'N':
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
                break;
            default:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
        }

        if(adjAnable) {
            if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
        } else {
            adj = 0;
        }

        motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
        motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
        motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
        motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
        motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
        motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
        motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
        motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);

        if(lineCheck == true)  {
            lineCount++;
            if(lineCount > 20) lineCheck = false;
        }

        /*
        if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
        	targetCount = 3;
        } else if(LimitSw::IsPressed(TOWEL1_SW)) {
        	targetCount = 3;
        } else {
        	targetCount = 2;
        }
        */

        targetCount = 3;

        if(countW == targetCount) {
            countW = 0;
            lineFase = 20;
            lineCount = 0;
            lineCheck = false;
        }
    } else if(lineFase == 20) { // 右 低速
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BL].dir = BACK;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_FR].dir = FOR;
        if (linePara[2] == 0) {
            //(!!LimitSw::Ispressed(SHEETS_SW)) {
            //ineFase = 20;
            //else if(LimitSw::IsPressed(TOWEL2_SW) {
            //ineFase = 14;
            //else {
            lineFase = 21;
            //
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_FR].dir = BRAKE;
        }
        motor[TIRE_FL].pwm = 16;
        motor[TIRE_BL].pwm = 16;
        motor[TIRE_BR].pwm = 16;
        motor[TIRE_FR].pwm = 16;
    } else if(lineFase == 21) {
        switch(linePara[2]) {
            case -2:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case -3:
                tirePWM[TIRE_FL] = -10;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 10;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case -1:
                tirePWM[TIRE_FL] = -20;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 20;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 0:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 1:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = -20;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 20;
                adjAnable = true;
                break;
            case 3:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = -10;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 10;
                adjAnable = true;
                break;
            case 2:
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 0;
                adjAnable = true;
                break;
            case 'A':
                if(lineCheck == false) {
                    lineCheck = true;
                    lineCount = 0;
                    countW++;
                }
                tirePWM[TIRE_FL] = -30;
                tirePWM[TIRE_BL] = -30;
                tirePWM[TIRE_BR] = 30;
                tirePWM[TIRE_FR] = 30;
                adjAnable = true;
                break;
            case 'N':
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
                break;
            default:
                tirePWM[TIRE_FL] = 0;
                tirePWM[TIRE_BL] = 0;
                tirePWM[TIRE_BR] = 0;
                tirePWM[TIRE_FR] = 0;
                adjAnable = false;
        }

        if(adjAnable) {
            if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
        } else {
            adj = 0;
        }

        motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
        motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
        motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
        motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
        motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
        motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
        motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
        motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);

        if(lineCheck == true)  {
            lineCount++;
            if(lineCount > 20) lineCheck = false;
        }
        if(countW == 2) {
            countW = 0;
            lineFase = 22;
            lineCount = 0;
            lineCheck = false;
        }
    } else if(lineFase == 22) {
        motor[TIRE_FL].dir = BACK;
        motor[TIRE_BL].dir = BACK;
        motor[TIRE_BR].dir = FOR;
        motor[TIRE_FR].dir = FOR;
        if (linePara[0] == 'N') {
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BL].dir = BRAKE;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_FR].dir = BRAKE;
        }
        motor[TIRE_FL].pwm = 30;
        motor[TIRE_BL].pwm = 30;
        motor[TIRE_BR].pwm = 30;
        motor[TIRE_FR].pwm = 30;
    } else {
        motor[TIRE_FL].dir = BRAKE;
        motor[TIRE_BL].dir = BRAKE;
        motor[TIRE_BR].dir = BRAKE;
        motor[TIRE_FR].dir = BRAKE;
    }

}
#endif

#if USE_PROCESS_NUM>6
static void Process6()
{

}
#endif

#if USE_PROCESS_NUM>7
static void Process7()
{
    static int SW_flag=0;
    static int Processflag=0;
    if(Processflag==0) {
        if(Limitphase==0) {
            //下待機
            Limitphase=2;
        } else if(Limitphase==1) {
        } else if(Limitphase==2) {
            //下→上とタオル展開
            pc.printf("%d\r\n",SW_flag);

            motor[LIFT_LB].dir = FOR;
            motor[LIFT_LB].pwm = 200;
            motor[LIFT_RB].dir = BACK;
            motor[LIFT_RB].pwm = 180;

            if(SW_flag==0) {
                motor[LIFT_LB].dir = FOR;
                motor[LIFT_LB].pwm = 200;
                motor[LIFT_RB].dir = BACK;
                motor[LIFT_RB].pwm = 180;
                if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
                    SW_flag=1;
                }
            } else if(SW_flag==1) {
                motor[LIFT_LB].dir = FOR;
                motor[LIFT_LB].pwm = 200;
                motor[LIFT_RB].dir = BACK;
                motor[LIFT_RB].pwm = 180;
                if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
                    SW_flag=2;
                }
            } else if(SW_flag==2) {
                motor[LIFT_LB].dir = FOR;
                motor[LIFT_LB].pwm = 200;
                motor[LIFT_RB].dir = BACK;
                motor[LIFT_RB].pwm = 180;
                if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
                    SW_flag=3;
                }
            } else if(SW_flag==3) {
                motor[LIFT_LB].dir = FOR;
                motor[LIFT_LB].pwm = 200;
                motor[LIFT_RB].dir = BACK;
                motor[LIFT_RB].pwm = 180;
                if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
                    motor[LIFT_LB].dir = BRAKE;
                    motor[LIFT_LB].pwm = 200;
                    motor[LIFT_RB].dir = BRAKE;
                    motor[LIFT_RB].pwm = 180;
                    Limitphase=4;
                }
            }
        } else if(Limitphase==3) {
        } else if(Limitphase==4) {
            //上段待機
            motor[LIFT_LB].dir = BRAKE;
            motor[LIFT_LB].pwm = 200;
            motor[LIFT_RB].dir = BRAKE;
            motor[LIFT_RB].pwm = 200;
            current=0;
            startFlag=true;
        } else {
            motor[LIFT_LB].dir = BRAKE;
            motor[LIFT_LB].pwm = 200;
            motor[LIFT_RB].dir = BRAKE;
            motor[LIFT_RB].pwm = 200;
            startFlag=true;
            current=0;
        }
        if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)) {
            motor[LIFT_LB].dir = BRAKE;
            motor[LIFT_LB].pwm = 200;
            motor[LIFT_RB].dir = BRAKE;
            motor[LIFT_RB].pwm = 200;
            Air[TOWEL0]=1;
            SW_flag=0;
            Limitphase=0;
            current=0;
            startFlag=true;
            Processflag=1;
        }
    } else if(Processflag==1) {
        motor[LIFT_LB].dir = FOR;
        motor[LIFT_LB].pwm = 200;
        motor[LIFT_RB].dir = BACK;
        motor[LIFT_RB].pwm = 180;
        if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
            motor[LIFT_LB].dir = BRAKE;
            motor[LIFT_LB].pwm = 200;
            motor[LIFT_RB].dir = BRAKE;
            motor[LIFT_RB].pwm = 180;
            current=0;
            startFlag=true;
            Processflag=1;
        }
        if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)){
        	motor[LIFT_LB].dir = BRAKE;
            motor[LIFT_LB].pwm = 200;
            motor[LIFT_RB].dir = BRAKE;
            motor[LIFT_RB].pwm = 200;
            Air[TOWEL0]=1;
            SW_flag=0;
            Limitphase=0;
            current=0;
            startFlag=true;
            Processflag=1;
		}
    }
}



#endif

#if USE_PROCESS_NUM>8
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