aa

Dependencies:   mbed

System/Process/Process.cpp

Committer:
kishibekairohan
Date:
2019-11-09
Revision:
42:980af34f6fe3
Parent:
41:99a1158f3eca
Child:
43:1bb4fbe7d4c6

File content as of revision 42:980af34f6fe3:

#include "mbed.h"
#include "Process.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 "../../Switch/Switch.h"

#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"

using namespace SWITCH;
using namespace PID_SPACE;

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);

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 pwmVal);
uint8_t SetPWM(int pwmVal);

DigitalIn EMC(PC_13); //非常停止確認用

bool start_flag = true;
bool UP_flag = false;
bool SW_flag = false;
bool Air_flag = false;
bool zyouge;
bool zyougedo;
bool dz2=true;
bool dz2i=false;
bool dz3=true;
bool dz3i=false;
bool dz4=true;
bool dz4i=false;
bool zone;
bool through=false;
bool counts=false;
bool mtc=false;
bool mtc2 = false;
bool emcs=false;
bool kiri;

int g[8];
int Twsh;
int testmode =0;
int mode =0;
int cross=0;//十字数える用
int cros=0;
int countss=100000;//wait代替え

///*********PWM調整用*********///

int fast = 60;
int normal = 40;
int slow = 20;

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

    #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

}

int point1;
void SystemProcess()
{
    SystemProcessInitialize();

    while(1) {
        //pc.printf("4:%d\n\r",g[4]);
        
        if(EMC==0) {
            fast=40;
            normal=30;
            slow=20;
            cross=0;//十字数える用
            cros=0;
            countss=0;//wait代替え
            UP_flag = false;
            SW_flag = false;
            Air_flag = false;
            zyouge=false;
            zyougedo=false;
            dz2=true;
            dz2i=false;
            dz3=true;
            dz3i=false;
            dz4=true;
            dz4i=false;
            through=false;
            counts=false;
            mtc=false;
            mtc2 = false;
            current = 0;
            SystemProcessInitialize();
            if(emcs==false) {
                AllActuatorReset();
                emcs=true;
            }
        } else {
            emcs=false;
        }

        if(mode==0) {
            if(ZoneSW) {
                LED::LED::TapeledMode(1);
            } else {
                LED::LED::TapeledMode(2);
            }
        }
        for(int i = 0; i < 8; i++) {
            g[i] = LineHub::GetPara(i);
        }
        if(ThSW) {
            Twsh=1;
        } else {
            Twsh=2;
        }

        if(StertSW && start_flag == true) {
            start_flag = false;
            lock = false;
            if(ZoneSW) {
                countss=0;
                mode=1;
                current = 3;//10GATU30変更した

            } else {
                countss = 0;
                mode=1;
                current = 2;
            }
        }
        if(DWSW||DOSW) {
            if(DWSW) {
                zyouge=true;
            }
            if(DOSW) {
                if(DOLS) {
                    motor[MOTOR_0].pwm = 100;
                    motor[MOTOR_0].dir = BRAKE;
                } else {
                    motor[MOTOR_0].pwm = 200;
                    motor[MOTOR_0].dir = BACK;
                }
            }
        } else if(zyouge==false) {
            motor[MOTOR_0].pwm = 100;
            motor[MOTOR_0].dir = BRAKE;
        }
        if(zyouge) {
            if(Twsh==1) {
                motor[MOTOR_0].pwm = 255;
                motor[MOTOR_0].dir = FOR;
                if(UPLS) {
                    motor[MOTOR_0].pwm = 100;
                    motor[MOTOR_0].dir = BRAKE;
                    zyouge=false;
                }
            }
            if(Twsh==2) {
                motor[MOTOR_0].pwm = 245;
                motor[MOTOR_0].dir = FOR;
                if(UPLS2) {
                    motor[MOTOR_0].pwm = 100;
                    motor[MOTOR_0].dir = BRAKE;
                    zyouge=false;
                }
            }
        }
        if(AIRSW) {
            if(SW_flag==false) {
                if(Air_flag==false) {
                    solenoid.solenoid3 = SOLENOID_ON;
                    Air_flag=true;
                    SW_flag=true;
                } else {
                    solenoid.solenoid3 = SOLENOID_OFF;
                    Air_flag=false;
                    SW_flag=true;
                }
            }
        } else {
            if(SW_flag==true) {
                countss++;
                if(countss>=2000) {
                    SW_flag=false;
                    countss=0;
                }
            }
        }


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


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

#endif
        {

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

        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()
{
    LED::LED::TapeledMode(6);
    start_flag = true;
    mtc=false;
    mtc2 = false;
    mode=1;
    cross=0;
    Twsh=1;
    motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
    motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
    motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]);
    motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);

    motor[TIRE_FR].pwm = SetPWM(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
    motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
    motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
    motor[TIRE_BL].pwm = SetPWM(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;

    if(controller->Button.UP||controller->Button.DOWN) {

        if(controller->Button.UP) {
            motor[MOTOR_0].pwm =200;
            motor[MOTOR_0].dir = FOR;
            if(UPLS) {
                motor[MOTOR_0].pwm = 0;
                motor[MOTOR_0].dir = BRAKE;
            }
        }
        if(controller->Button.DOWN) {
            motor[MOTOR_0].pwm = 50;
            motor[MOTOR_0].dir = BACK;
        }
    } else {
        motor[MOTOR_0].pwm = 0;
        motor[MOTOR_0].dir = BRAKE;
    }

    if(controller->Button.Y) {
        if(dz2==true) {
            if(dz2i==false) {
                solenoid.solenoid2 = SOLENOID_ON;
                dz2i=true;
            } else {
                solenoid.solenoid2 = SOLENOID_OFF;
                dz2i=false;
            }
            dz2=false;
        }
    } else {
        dz2=true;
    }

    if(controller->Button.A) {
        if(dz3==true) {
            if(dz3i==false) {
                solenoid.solenoid3 = SOLENOID_ON;
                dz3i=true;
            } else {
                solenoid.solenoid3 = SOLENOID_OFF;
                dz3i=false;
            }
            dz3=false;
        }
    } else {
        dz3=true;
    }

    if(controller->Button.B) {
        if(dz4==true) {
            if(dz4i==false) {
                solenoid.solenoid4 = SOLENOID_ON;
                dz4i=true;
            } else {
                solenoid.solenoid4 = SOLENOID_OFF;
                dz4i=false;
            }
            dz4=false;
        }
    } else {
        dz4=true;
    }
}
#endif

#if USE_PROCESS_NUM>2
static void Process2()
{
    //Oh So 青ゾーン
    LED::LED::TapeledMode(2);
    countss++;
    if(mode==1) {//スタートゾーンから白線検知
        fast=40;
        normal=30;
        slow=20;
        motor[TIRE_FR].pwm = normal;
        motor[TIRE_FR].dir = FOR;
        motor[TIRE_FL].pwm = normal;
        motor[TIRE_FL].dir = BACK;
        motor[TIRE_BR].pwm = normal;
        motor[TIRE_BR].dir = FOR;
        motor[TIRE_BL].pwm = normal;
        motor[TIRE_BL].dir = BACK;
        if(g[2]==0&&countss>=100000) {
            countss=100000;
            cross=0;
            mode=3;
        }
    } else if(mode==3) { //横ライントレースから縦ライントレースへ
        motor[TIRE_FR].pwm = 30;
        motor[TIRE_FR].dir = FOR;
        motor[TIRE_FL].pwm = 0;
        motor[TIRE_FL].dir = FREE;
        motor[TIRE_BR].pwm = 0;
        motor[TIRE_BR].dir = FREE;
        motor[TIRE_BL].pwm = 30;
        motor[TIRE_BL].dir = BACK;
        if(g[1]==0) {
            mode=10;
            countss=100000;
        }
    } else if(mode==10) { //縦ライントレース
        switch(g[1]) {
            case 98:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                if(countss>80000) {
                    cross++;
                    countss=0;
                }
                if(cross==3) {
                    mode=11;
                    countss=0;
                    cross=0;
                }
                break;
            case 0:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 255:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 253:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 254:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 1:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 3:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc=true;
                break;
            case 2:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc=true;
                break;
        }
        if(g[0]!=98&&mtc==true) {
            switch(g[0]) {
                case 0:
                    mtc=false;
                    break;
                case 255:
                    motor[TIRE_FL].pwm += 5;
                    mtc=false;
                    break;
                case 253:
                    motor[TIRE_FL].pwm += 10;
                    mtc=false;
                    break;
                case 254:
                    motor[TIRE_FL].pwm += 20;
                    motor[TIRE_FR].pwm = 0;
                    mtc=false;
                    break;
                case 1:
                    motor[TIRE_FR].pwm += 5;
                    mtc=false;
                    break;
                case 3:
                    motor[TIRE_FR].pwm += 10;
                    mtc=false;
                    break;
                case 2:
                    motor[TIRE_FR].pwm += 20;
                    motor[TIRE_FL].pwm = 0;
                    mtc=false;
                    break;
            }
        }
    } else if(mode==11) {
        motor[TIRE_FR].pwm = normal;
        motor[TIRE_FR].dir = FOR;
        motor[TIRE_FL].pwm = 0;
        motor[TIRE_FL].dir = FREE;
        motor[TIRE_BR].pwm = 0;
        motor[TIRE_BR].dir = FREE;
        motor[TIRE_BL].pwm = normal;
        motor[TIRE_BL].dir = BACK;
        if(g[2]==0) {
            countss=0;
            cross=0;
            mode=20;
        }
    } else if(mode==20) {
        solenoid.solenoid4= SOLENOID_ON;
        switch(g[2]) {
            case 98:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = BACK;
                if(countss>30000) {
                    cross++;
                    countss=0;
                }
                mtc2=true;
                if(cross==1) {
                    fast=30;
                    normal=20;
                    slow=20;
                }
                if(YELLOW) {
                    if(cross==2) {
                        mode=21;
                        countss=0;
                        cross=0;
                    }
                } else {
                    if(cross==1) {
                        mode=21;
                        countss=0;
                        cross=0;
                    }
                }
                break;
            case 0:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
            case 255:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
            case 253:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc2=true;
                break;
            case 254:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc2=true;
                break;
            case 1:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
            case 3:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
            case 2:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
        }
        if(g[2]!=98&&mtc2==true) {
            switch(g[3]) {
                case 0:
                    mtc2=false;
                    break;
                case 255:
                    motor[TIRE_BL].pwm += 5;
                    mtc2=false;
                    break;
                case 253:
                    motor[TIRE_BL].pwm += 10;
                    mtc2=false;
                    break;
                case 254:
                    motor[TIRE_BL].pwm += 20;
                    motor[TIRE_FL].pwm = 0;
                    mtc2=false;
                    break;
                case 1:
                    motor[TIRE_FL].pwm += 5;
                    mtc2=false;
                    break;
                case 3:
                    motor[TIRE_FL].pwm += 10;
                    mtc2=false;
                    break;
                case 2:
                    motor[TIRE_FL].pwm += 20;
                    motor[TIRE_BL].pwm = 0;
                    mtc2=false;
                    break;
            }
        }
    }
    if(YELLOW) {
        if(mode==21) {
            motor[TIRE_FR].pwm = 20;
            motor[TIRE_FR].dir = BACK;
            motor[TIRE_FL].pwm = 20;
            motor[TIRE_FL].dir = BACK;
            motor[TIRE_BR].pwm = 20;
            motor[TIRE_BR].dir = FOR;
            motor[TIRE_BL].pwm = 20;
            motor[TIRE_BL].dir = FOR;
            if(g[3]==98&&countss>=10000) {
                mode=22;
                countss=0;
                fast=60;
                normal=40;
                slow=20;
            }
        } else if(mode==22) {
            if(g[4]==3||g[4]==2) {
                motor[TIRE_FR].pwm = 30;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 30;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = 30;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 30;
                motor[TIRE_BL].dir = BACK;
            }
            if(g[4]==253||g[4]==254) {
                motor[TIRE_FR].pwm = 30;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 30;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = 30;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 30;
                motor[TIRE_BL].dir = FOR;
            }
            switch(g[2]) {
                case 0:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = 40;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = FOR;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 40;
                    motor[TIRE_BR].dir = FOR;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = FOR;
                    break;
            }
            switch(g[3]) {
                case 0:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 255:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = BACK;
                case 253:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 40;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 254:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 3:
                    motor[TIRE_FL].pwm = 40;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 2:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = FOR;
                    break;
            }
            if(g[3]==0&&g[2]==0) {
                mode=23;
                countss=0;
            }
        } else if(mode==23) {
            switch(g[4]) {
                case 98:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 99:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 0:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = slow-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = normal-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
            }
            if(DOLS==true||UPLS==true) {
                motor[MOTOR_0].pwm = 200;
                motor[MOTOR_0].dir = BRAKE;
            }
            if(g[0]==98) {
                fast=30;
                normal=20;
                slow=20;
                if(DOLS==true||UPLS==true) {
                    motor[MOTOR_0].pwm = 200;
                    motor[MOTOR_0].dir = BRAKE;
                } else {
                    motor[MOTOR_0].pwm = 100;
                    motor[MOTOR_0].dir = BACK;
                }
            }
            if(g[2]==254&&countss>=100000) {
                mode=24;
                solenoid.solenoid4 = SOLENOID_OFF;
                countss=0;
                fast=60;
                normal=40;
                slow=30;
            }
        } else if(mode==24) {
            if(DOLS==true||UPLS==true) {
                motor[MOTOR_0].pwm = 200;
                motor[MOTOR_0].dir = BRAKE;
            } else {
                motor[MOTOR_0].pwm = 100;
                motor[MOTOR_0].dir = BACK;
            }
            if(g[4]==3||g[4]==2) {
                motor[TIRE_FR].pwm = 30;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 30;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = 30;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 30;
                motor[TIRE_BL].dir = BACK;
            }
            if(g[4]==253||g[4]==254) {
                motor[TIRE_FR].pwm = 30;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 30;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = 30;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 30;
                motor[TIRE_BL].dir = FOR;
            }
            switch(g[2]) {
                case 0:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = 40;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = FOR;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 40;
                    motor[TIRE_BR].dir = FOR;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = FOR;
                    break;
            }
            switch(g[3]) {
                case 0:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 255:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = BACK;
                case 253:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 40;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 254:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 3:
                    motor[TIRE_FL].pwm = 40;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 2:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = FOR;
                    break;
            }
            if(g[3]==0&&g[2]==0) {
                if(UPLS==true||DOLS==true) {
                    motor[MOTOR_0].pwm = 200;
                    motor[MOTOR_0].dir = BRAKE;
                    mode=25;
                } else {
                    motor[MOTOR_0].pwm = 100;
                    motor[MOTOR_0].dir = BACK;
                }
            }
        } else if(mode==25) {
            switch(g[4]) {
                case 98:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 99:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 0:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = slow-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = normal-5;
                    motor[TIRE_BL].dir = FOR;
                    break;
            }
            if(g[1]==98) {
                mode=31;
            }
        }
    } else {
        if(mode==21) {
            motor[TIRE_FR].pwm = 0;
            motor[TIRE_FR].dir = FREE;
            motor[TIRE_FL].pwm = 30;
            motor[TIRE_FL].dir = FOR;
            motor[TIRE_BR].pwm = 30;
            motor[TIRE_BR].dir = BACK;
            motor[TIRE_BL].pwm = 0;
            motor[TIRE_BL].dir = FREE;
            if(g[1]==0) {
                mode=22;
                fast=60;
                normal=40;
                slow=20;
            }
        } else if(mode==22) {
            if(g[3]==0) {
                mode=23;
            }
            switch(g[0]) {
                case 98:
                    solenoid.solenoid4 = SOLENOID_OFF;
                    fast=30;
                    normal=20;
                    slow=20;
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = normal;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 0:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = slow;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = normal;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = slow;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    mtc=true;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    mtc=true;
                    break;
            }
            if(g[0]!=98&&mtc==true) {
                switch(g[0]) {
                    case 0:
                        mtc=false;
                        break;
                    case 255:
                        motor[TIRE_BR].pwm += 5;
                        mtc=false;
                        break;
                    case 253:
                        motor[TIRE_BR].pwm += 10;
                        mtc=false;
                        break;
                    case 254:
                        motor[TIRE_BR].pwm += 20;
                        motor[TIRE_BL].pwm = 0;
                        mtc=false;
                        break;
                    case 1:
                        motor[TIRE_BL].pwm += 5;
                        mtc=false;
                        break;
                    case 3:
                        motor[TIRE_BL].pwm += 10;
                        mtc=false;
                        break;
                    case 2:
                        motor[TIRE_BL].pwm += 20;
                        motor[TIRE_BR].pwm = 0;
                        mtc=false;
                        break;
                }
            }
        } else if(mode==23) {
            if(UPLS==true||DOLS==true) {
                motor[MOTOR_0].pwm = 200;
                motor[MOTOR_0].dir = BRAKE;
                fast=60;
                normal=40;
                slow=20;
                countss=0;
                mode=24;
            } else {
                motor[MOTOR_0].pwm = 100;
                motor[MOTOR_0].dir = BACK;
                motor[TIRE_FR].pwm = 100;
                motor[TIRE_FR].dir = BRAKE;
                motor[TIRE_FL].pwm = 100;
                motor[TIRE_FL].dir = BRAKE;
                motor[TIRE_BR].pwm = 100;
                motor[TIRE_BR].dir = BRAKE;
                motor[TIRE_BL].pwm = 100;
                motor[TIRE_BL].dir = BRAKE;
            }

        } else if(mode==24) {
            if(g[0]==98&&countss>=40000) {
                mode=31;
                countss=0;
            }
            switch(g[0]) {
                case 98:
                    fast=30;
                    normal=20;
                    slow=20;
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = normal;
                    motor[TIRE_BL].dir = FOR;
                    break;
                case 0:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = fast;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = slow;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = normal;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = slow;
                    motor[TIRE_BL].dir = FOR;
                    mtc=true;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    mtc=true;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = BACK;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    mtc=true;
                    break;
            }
            if(g[1]!=98&&mtc==true) {
                switch(g[1]) {
                    case 0:
                        mtc=false;
                        break;
                    case 255:
                        motor[TIRE_BR].pwm += 5;
                        mtc=false;
                        break;
                    case 253:
                        motor[TIRE_BR].pwm += 10;
                        mtc=false;
                        break;
                    case 254:
                        motor[TIRE_BR].pwm += 20;
                        motor[TIRE_BL].pwm = 0;
                        mtc=false;
                        break;
                    case 1:
                        motor[TIRE_BL].pwm += 5;
                        mtc=false;
                        break;
                    case 3:
                        motor[TIRE_BL].pwm += 10;
                        mtc=false;
                        break;
                    case 2:
                        motor[TIRE_BL].pwm += 20;
                        motor[TIRE_BR].pwm = 0;
                        mtc=false;
                        break;
                }
            }
        }
    }
    if(mode==31) {
        motor[TIRE_FR].pwm = 0;
        motor[TIRE_FR].dir = FREE;
        motor[TIRE_FL].pwm = 0;
        motor[TIRE_FL].dir = FREE;
        motor[TIRE_BR].pwm = 0;
        motor[TIRE_BR].dir = FREE;
        motor[TIRE_BL].pwm = 0;
        motor[TIRE_BL].dir = FREE;
        if(countss>=5000) {
            countss=0;
            mode=33;
        }
    } else if(mode==33) {
        motor[TIRE_FR].pwm = 30;
        motor[TIRE_FR].dir = BACK;
        motor[TIRE_FL].pwm = 0;
        motor[TIRE_FL].dir = FREE;
        motor[TIRE_BR].pwm = 0;
        motor[TIRE_BR].dir = FREE;
        motor[TIRE_BL].pwm = 30;
        motor[TIRE_BL].dir = FOR;
        if(g[3]==0) {
            cross=0;
            countss=0;
            mode=40;
        }
    } else if(mode==40) {
        switch(g[3]) {
            case 98:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = FOR;
                if(countss>10000) {
                    cross++;
                    countss=0;
                }
                mtc2=true;
                if(cross==1) {
                    mode=41;
                    countss=0;
                    cross=0;
                }
                break;
            case 0:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
            case 255:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
            case 253:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc2=true;
                break;
            case 254:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc2=true;
                break;
            case 1:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
            case 3:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
            case 2:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
        }
        if(g[2]!=98&&mtc2==true) {
            switch(g[2]) {
                case 0:
                    mtc2=false;
                    break;
                case 255:
                    motor[TIRE_FL].pwm += 5;
                    mtc2=false;
                    break;
                case 253:
                    motor[TIRE_FL].pwm += 10;
                    mtc2=false;
                    break;
                case 254:
                    motor[TIRE_FL].pwm += 20;
                    motor[TIRE_BL].pwm = 0;
                    mtc2=false;
                    break;
                case 1:
                    motor[TIRE_BL].pwm += 5;
                    mtc2=false;
                    break;
                case 3:
                    motor[TIRE_BL].pwm += 10;
                    mtc2=false;
                    break;
                case 2:
                    motor[TIRE_BL].pwm += 20;
                    motor[TIRE_FL].pwm = 0;
                    mtc2=false;
                    break;
            }
        }
    } else if(mode==41) {
        if(LIF||LIB) {
            mode=42;
        }
        motor[TIRE_FR].pwm = 30;
        motor[TIRE_FR].dir = BACK;
        motor[TIRE_FL].pwm = 0;
        motor[TIRE_FL].dir = FREE;
        motor[TIRE_BR].pwm = 0;
        motor[TIRE_BR].dir = FREE;
        motor[TIRE_BL].pwm = 30;
        motor[TIRE_BL].dir = FOR;
        if(g[1]==0) {
            countss=0;
            mode=70;
            normal=60;
            fast=70;
            slow=40;
        }
    } else if(mode==42) {
        motor[TIRE_FR].pwm = 20;
        motor[TIRE_FR].dir = FOR;
        motor[TIRE_FL].pwm = 0;
        motor[TIRE_FL].dir = FREE;
        motor[TIRE_BR].pwm = 0;
        motor[TIRE_BR].dir = FREE;
        motor[TIRE_BL].pwm = 20;
        motor[TIRE_BL].dir = BACK;
        if(g[1]==0) {
            countss=0;
            mode=70;
        }
    } else if(mode==70) {
        switch(g[0]) {
            case 99:
                if(countss>=100000) {
                    mode=100;
                }
                break;
            case 98:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = FOR;
                break;
            case 0:
                motor[TIRE_FR].pwm = 80;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 80;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = 80;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 80;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 255:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 253:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 254:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 1:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 3:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc=true;
                break;
            case 2:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc=true;
                break;
        }
        if(g[1]!=98&&mtc==true) {
            switch(g[1]) {
                case 0:
                    mtc=false;
                    break;
                case 255:
                    motor[TIRE_BR].pwm += 5;
                    mtc=false;
                    break;
                case 253:
                    motor[TIRE_BR].pwm += 10;
                    mtc=false;
                    break;
                case 254:
                    motor[TIRE_BR].pwm += 20;
                    motor[TIRE_BL].pwm = 0;
                    mtc=false;
                    break;
                case 1:
                    motor[TIRE_BL].pwm += 5;
                    mtc=false;
                    break;
                case 3:
                    motor[TIRE_BL].pwm += 10;
                    mtc=false;
                    break;
                case 2:
                    motor[TIRE_BL].pwm += 20;
                    motor[TIRE_BR].pwm = 0;
                    mtc=false;
                    break;
            }
        }
    } else if(mode==100) {
        motor[TIRE_FR].pwm = 40;
        motor[TIRE_FR].dir = BACK;
        motor[TIRE_FL].pwm = 40;
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BR].pwm = 43;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_BL].pwm = 40;
        motor[TIRE_BL].dir = FOR;
    } else {
    }

}
#endif

#if USE_PROCESS_NUM>3
static void Process3()
{
//Clearly 赤ゾーン
    LED::LED::TapeledMode(3);
    countss++;
    if(mode==1) {//スタートゾーンから白線検知
        fast=40;
        normal=30;
        slow=20;
        motor[TIRE_FR].pwm = normal+20;
        motor[TIRE_FR].dir = BACK;
        motor[TIRE_FL].pwm = normal+20;
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BR].pwm = normal+20;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_BL].pwm = normal+20;
        motor[TIRE_BL].dir = FOR;
        if(g[2]==0&&countss>=100000) {
            countss=100000;
            cross=0;
            mode=3;
        }
    } else if(mode==3) { //横ライントレースから縦ライントレースへ
        motor[TIRE_FR].pwm = 0;
        motor[TIRE_FR].dir = FREE;
        motor[TIRE_FL].pwm = 30;
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BR].pwm = 30;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_BL].pwm = 0;
        motor[TIRE_BL].dir = FREE;
        if(g[0]==0) {
            mode=10;
            countss=100000;
        }
    } else if(mode==10) { //縦ライントレース
        switch(g[0]) {
            case 98:
                motor[TIRE_FR].pwm = normal+20;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = normal+20;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = normal+20;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = normal+20;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                if(countss>80000) {
                    cross++;
                    countss=0;
                }
                /*
                if(cross==2){
                    fast = 30;
                    normal = 20;
                    slow = 20;     
                }
                */
                if(cross==3) {
                    mode=11;
                    countss=0;
                    cross=0;
                }
                break;
                //+15した
            case 0:
                motor[TIRE_FR].pwm = fast+20;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = fast+20;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = fast+20;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = fast+20;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 255:
                motor[TIRE_FR].pwm = fast+20;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = slow+20;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = slow+20;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = fast+20;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 253:
                motor[TIRE_FR].pwm = slow+20;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = slow+20;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 254:
                motor[TIRE_FR].pwm = normal+20;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = normal+20;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 1:
                motor[TIRE_FR].pwm = slow+20;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = fast+20;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = fast+20;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = slow+20;
                motor[TIRE_BL].dir = FOR;
                mtc=true;
                break;
            case 3:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = slow+20;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = slow+20;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc=true;
                break;
            case 2:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = normal+20;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = normal+20;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc=true;
                break;
        }
        if(g[0]!=98&&mtc==true) {
            switch(g[1]) {
                case 0:
                    mtc=false;
                    break;
                case 255:
                    motor[TIRE_BR].pwm += 5;
                    mtc=false;
                    break;
                case 253:
                    motor[TIRE_BR].pwm += 10;
                    mtc=false;
                    break;
                case 254:
                    motor[TIRE_BR].pwm += 20;
                    motor[TIRE_BL].pwm = 0;
                    mtc=false;
                    break;
                case 1:
                    motor[TIRE_BL].pwm += 5;
                    mtc=false;
                    break;
                case 3:
                    motor[TIRE_BL].pwm += 10;
                    mtc=false;
                    break;
                case 2:
                    motor[TIRE_BL].pwm += 20;
                    motor[TIRE_BR].pwm = 0;
                    mtc=false;
                    break;
            }
        }
    } else if(mode==11) {
        motor[TIRE_FR].pwm = 0;
        motor[TIRE_FR].dir = FREE;
        motor[TIRE_FL].pwm = normal;
        motor[TIRE_FL].dir = FOR;
        motor[TIRE_BR].pwm = normal;
        motor[TIRE_BR].dir = BACK;
        motor[TIRE_BL].pwm = 0;
        motor[TIRE_BL].dir = FREE;
        if(g[2]==0) {
            countss=0;
            cross=0;
            mode=20;
        }
    } else if(mode==20) {
        solenoid.solenoid4= SOLENOID_ON;
        switch(g[2]) {
            case 98:
                motor[TIRE_FR].pwm = normal+10;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = normal+10;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = normal+10;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = normal+10;
                motor[TIRE_BL].dir = BACK;
                if(countss>30000) {
                    cross++;
                    countss=0;
                }
                mtc2=true;
                //change
                if(cross==1) {
                    fast=30;
                    normal=20;
                    slow=20;
                }
                if(YELLOW) {
                    if(cross==2) {
                        mode=21;
                        countss=0;
                        cross=0;
                    }
                } else {
                    if(cross==3) {
                        mode=21;
                        countss=0;
                        cross=0;
                    }
                }
                break;
            case 0:
                motor[TIRE_FR].pwm = fast+15;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = fast+15;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = fast+15;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = fast+15;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
            case 255:
                motor[TIRE_FR].pwm = normal+15;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = fast+15;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = fast+15;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = normal+15;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
            case 253:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = slow+15;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = slow+15;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc2=true;
                break;
            case 254:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = normal+15;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = normal+15;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc2=true;
                break;
            case 1:
                motor[TIRE_FR].pwm = fast+15;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = slow+15;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = slow+15;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = fast+15;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
            case 3:
                motor[TIRE_FR].pwm = slow+15;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = slow+15;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
            case 2:
                motor[TIRE_FR].pwm = normal+15;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = normal+15;
                motor[TIRE_BL].dir = BACK;
                mtc2=true;
                break;
        }
        if(g[2]!=98&&mtc2==true) {
            switch(g[3]) {
                case 0:
                    mtc2=false;
                    break;
                case 255:
                    motor[TIRE_BL].pwm += 5;
                    mtc2=false;
                    break;
                case 253:
                    motor[TIRE_BL].pwm += 10;
                    mtc2=false;
                    break;
                case 254:
                    motor[TIRE_BL].pwm += 20;
                    motor[TIRE_FL].pwm = 0;
                    mtc2=false;
                    break;
                case 1:
                    motor[TIRE_FL].pwm += 5;
                    mtc2=false;
                    break;
                case 3:
                    motor[TIRE_FL].pwm += 10;
                    mtc2=false;
                    break;
                case 2:
                    motor[TIRE_FL].pwm += 20;
                    motor[TIRE_BL].pwm = 0;
                    mtc2=false;
                    break;
            }
        }
    }
    if(YELLOW) {
        if(mode==21) {
            motor[TIRE_FR].pwm = 20;
            motor[TIRE_FR].dir = BACK;
            motor[TIRE_FL].pwm = 20;
            motor[TIRE_FL].dir = BACK;
            motor[TIRE_BR].pwm = 20;
            motor[TIRE_BR].dir = FOR;
            motor[TIRE_BL].pwm = 20;
            motor[TIRE_BL].dir = FOR;
            if(g[3]==98&&countss>=10000) {
                mode=22;
                countss=0;
                fast=60;
                normal=40;
                slow=20;
            }
        } else if(mode==22) {
            if(g[4]==3||g[4]==2) {
                motor[TIRE_FR].pwm = 30;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 30;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = 30;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 30;
                motor[TIRE_BL].dir = BACK;
            }
            if(g[4]==253||g[4]==254) {
                motor[TIRE_FR].pwm = 30;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 30;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = 30;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 30;
                motor[TIRE_BL].dir = FOR;
            }
            switch(g[2]) {
                case 0:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = 40;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = FOR;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 40;
                    motor[TIRE_BR].dir = FOR;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = FOR;
                    break;
            }
            switch(g[3]) {
                case 0:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 255:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = BACK;
                case 253:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 40;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 254:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 3:
                    motor[TIRE_FL].pwm = 40;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 2:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = FOR;
                    break;
            }
            if(g[3]==0&&g[2]==0) {
                mode=23;
                countss=0;
            }
        } else if(mode==23) {
            switch(g[4]) {
                case 98:
                    motor[TIRE_FR].pwm = fast-5;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = fast-5;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = fast-5;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 99:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 0:
                    motor[TIRE_FR].pwm = fast-5;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = fast-5;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = fast-5;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = slow-5;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = fast-5;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = fast-5;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = slow-5;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = normal-5;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = fast-5;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = fast-5;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = normal-5;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = fast-5;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = slow-5;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = slow-5;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = fast-5;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = normal-5;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = normal-5;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = fast-5;
                    motor[TIRE_BL].dir = BACK;
                    break;
            }
            if(DOLS==true||UPLS==true) {
                motor[MOTOR_0].pwm = 200;
                motor[MOTOR_0].dir = BRAKE;
            }
            if(g[1]==98) {
                //Change
                fast=20;
                normal=15;
                slow=10;
                if(DOLS==true||UPLS==true) {
                    motor[MOTOR_0].pwm = 200;
                    motor[MOTOR_0].dir = BRAKE;
                } else {
                    motor[MOTOR_0].pwm = 100;
                    motor[MOTOR_0].dir = BACK;
                }
            }
            if(g[3]==254&&countss>=100000) {
                mode=24;
                solenoid.solenoid4 = SOLENOID_OFF;
                countss=0;
                fast=60;
                normal=40;
                slow=20;
            }
        } else if(mode==24) {
            if(DOLS==true||UPLS==true) {
                motor[MOTOR_0].pwm = 200;
                motor[MOTOR_0].dir = BRAKE;
            } else {
                motor[MOTOR_0].pwm = 100;
                motor[MOTOR_0].dir = BACK;
            }
            if(g[4]==3||g[4]==2) {
                motor[TIRE_FR].pwm = 30;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 30;
                motor[TIRE_FL].dir = FOR;
                motor[TIRE_BR].pwm = 30;
                motor[TIRE_BR].dir = BACK;
                motor[TIRE_BL].pwm = 30;
                motor[TIRE_BL].dir = BACK;
            }
            if(g[4]==253||g[4]==254) {
                motor[TIRE_FR].pwm = 30;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 30;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = 30;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 30;
                motor[TIRE_BL].dir = FOR;
            }
            switch(g[2]) {
                case 0:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = 40;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FREE;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = BACK;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = FOR;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_BR].pwm = 40;
                    motor[TIRE_BR].dir = FOR;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = 30;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_BR].pwm = 30;
                    motor[TIRE_BR].dir = FOR;
                    break;
            }
            switch(g[3]) {
                case 0:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 255:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = BACK;
                case 253:
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BL].pwm = 40;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 254:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 3:
                    motor[TIRE_FL].pwm = 40;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 2:
                    motor[TIRE_FL].pwm = 30;
                    motor[TIRE_FL].dir = FOR;
                    motor[TIRE_BL].pwm = 30;
                    motor[TIRE_BL].dir = FOR;
                    break;
            }
            if(g[3]==0&&g[2]==0) {
                if(UPLS==true||DOLS==true) {
                    motor[MOTOR_0].pwm = 200;
                    motor[MOTOR_0].dir = BRAKE;
                    mode=25;
                } else {
                    motor[MOTOR_0].pwm = 100;
                    motor[MOTOR_0].dir = BACK;
                }
            }
        } else if(mode==25) {
            switch(g[4]) {
                case 98:
                    motor[TIRE_FR].pwm = 60;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 60;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = 60;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 60;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 99:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 0:
                    motor[TIRE_FR].pwm = 60;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 60;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = 60;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 60;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = 20;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 60;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = 60;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 20;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = 40;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 60;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = 60;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 40;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = 60;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 20;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = 20;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 60;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = 60;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 40;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = 40;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 60;
                    motor[TIRE_BL].dir = BACK;
                    break;
            }
            if(g[1]==98) {
                mode=31;
            }
        }
    } else {
        if(mode==21) {
            motor[TIRE_FR].pwm = 30;
            motor[TIRE_FR].dir = FOR;
            motor[TIRE_FL].pwm = 0;
            motor[TIRE_FL].dir = FREE;
            motor[TIRE_BR].pwm = 0;
            motor[TIRE_BR].dir = FREE;
            motor[TIRE_BL].pwm = 30;
            motor[TIRE_BL].dir = BACK;
            if(g[1]==0) {
                mode=22;
                fast=60;
                normal=40;
                slow=20;
            }
        } else if(mode==22) {
            if(g[1]==98) {
                slow=20;
                normal=20;
                fast=30;
            }
            if(g[3]==0){
                slow=30;
                normal=40;
                fast=60;
                mode=23;
                }
            switch(g[1]) {
                case 98:
                    solenoid.solenoid4 = SOLENOID_OFF;
                    fast=30;
                    normal=20;
                    slow=20;
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = normal;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 0:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = fast;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = fast;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = slow;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = normal;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = slow;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    mtc=true;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    mtc=true;
                    break;
            }
            if(g[1]!=98&&mtc==true) {
                switch(g[0]) {
                    case 0:
                        mtc=false;
                        break;
                    case 255:
                        motor[TIRE_FL].pwm += 5;
                        mtc=false;
                        break;
                    case 253:
                        motor[TIRE_FL].pwm += 10;
                        mtc=false;
                        break;
                    case 254:
                        motor[TIRE_FL].pwm += 20;
                        motor[TIRE_FR].pwm = 0;
                        mtc=false;
                        break;
                    case 1:
                        motor[TIRE_FR].pwm += 5;
                        mtc=false;
                        break;
                    case 3:
                        motor[TIRE_FR].pwm += 10;
                        mtc=false;
                        break;
                    case 2:
                        motor[TIRE_FR].pwm += 20;
                        motor[TIRE_FL].pwm = 0;
                        mtc=false;
                        break;
                }
            }
        }else if(mode==23) {
            if(UPLS==true||DOLS==true) {
                motor[MOTOR_0].pwm = 200;
                motor[MOTOR_0].dir = BRAKE;
                fast=60;
                normal=40;
                slow=20;
                countss=0;
                mode=24;
            } else {
                motor[MOTOR_0].pwm = 100;
                motor[MOTOR_0].dir = BACK;
            }
            motor[TIRE_FR].pwm = 100;
            motor[TIRE_FR].dir = BRAKE;
            motor[TIRE_FL].pwm = 100;
            motor[TIRE_FL].dir = BRAKE;
            motor[TIRE_BR].pwm = 100;
            motor[TIRE_BR].dir = BRAKE;
            motor[TIRE_BL].pwm = 100;
            motor[TIRE_BL].dir = BRAKE;
        } else if(mode==24) {
            if(g[1]==98&&countss>=40000) {
                mode=31;
                countss=0;
            }
            switch(g[1]) {
                case 98:
                    fast=30;
                    normal=20;
                    slow=20;
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = normal;
                    motor[TIRE_BL].dir = BACK;
                    break;
                case 0:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = fast;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 255:
                    motor[TIRE_FR].pwm = fast;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = fast;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 253:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = slow;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 254:
                    motor[TIRE_FR].pwm = normal;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = 0;
                    motor[TIRE_FL].dir = FREE;
                    motor[TIRE_BR].pwm = 0;
                    motor[TIRE_BR].dir = FREE;
                    motor[TIRE_BL].pwm = normal;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 1:
                    motor[TIRE_FR].pwm = slow;
                    motor[TIRE_FR].dir = FOR;
                    motor[TIRE_FL].pwm = fast;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = fast;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = slow;
                    motor[TIRE_BL].dir = BACK;
                    mtc=true;
                    break;
                case 3:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = slow;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = slow;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    mtc=true;
                    break;
                case 2:
                    motor[TIRE_FR].pwm = 0;
                    motor[TIRE_FR].dir = FREE;
                    motor[TIRE_FL].pwm = normal;
                    motor[TIRE_FL].dir = BACK;
                    motor[TIRE_BR].pwm = normal;
                    motor[TIRE_BR].dir = FOR;
                    motor[TIRE_BL].pwm = 0;
                    motor[TIRE_BL].dir = FREE;
                    mtc=true;
                    break;
            }
            if(g[1]!=98&&mtc==true) {
                switch(g[0]) {
                    case 0:
                        mtc=false;
                        break;
                    case 255:
                        motor[TIRE_FL].pwm += 5;
                        mtc=false;
                        break;
                    case 253:
                        motor[TIRE_FL].pwm += 10;
                        mtc=false;
                        break;
                    case 254:
                        motor[TIRE_FL].pwm += 20;
                        motor[TIRE_FR].pwm = 0;
                        mtc=false;
                        break;
                    case 1:
                        motor[TIRE_FR].pwm += 5;
                        mtc=false;
                        break;
                    case 3:
                        motor[TIRE_FR].pwm += 10;
                        mtc=false;
                        break;
                    case 2:
                        motor[TIRE_FR].pwm += 20;
                        motor[TIRE_FL].pwm = 0;
                        mtc=false;
                        break;
                }
            }
        }
    }
    if(mode==31) {
        motor[TIRE_FR].pwm = 0;
        motor[TIRE_FR].dir = FREE;
        motor[TIRE_FL].pwm = 0;
        motor[TIRE_FL].dir = FREE;
        motor[TIRE_BR].pwm = 0;
        motor[TIRE_BR].dir = FREE;
        motor[TIRE_BL].pwm = 0;
        motor[TIRE_BL].dir = FREE;
        if(countss>=5000) {
            countss=0;
            mode=32;
        }
    } else if(mode==32) {
        motor[TIRE_FR].pwm = 10;
        motor[TIRE_FR].dir = BRAKE;
        motor[TIRE_FL].pwm = 10;
        motor[TIRE_FL].dir = BRAKE;
        motor[TIRE_BR].pwm = 10;
        motor[TIRE_BR].dir = BRAKE;
        motor[TIRE_BL].pwm = 10;
        motor[TIRE_BL].dir = BRAKE;
        if(countss>=40000) {
            mode=33;
            countss=0;
        }
    } else if(mode==33) {
        motor[TIRE_FR].pwm = 0;
        motor[TIRE_FR].dir = FREE;
        motor[TIRE_FL].pwm = 40;
        motor[TIRE_FL].dir = BACK;
        motor[TIRE_BR].pwm = 40;
        motor[TIRE_BR].dir = FOR;
        motor[TIRE_BL].pwm = 0;
        motor[TIRE_BL].dir = FREE;
        if(g[3]==0&&g[2]==0) {
            cross=0;
            countss=0;
            mode=40;
        }
    } else if(mode==40) {
        switch(g[3]) {
            case 98:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = FOR;
                if(countss>10000) {
                    cross++;
                    countss=0;
                }
                mtc2=true;
                break;
            case 0:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
            case 255:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
            case 253:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc2=true;
                break;
            case 254:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc2=true;
                break;
            case 1:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
            case 3:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
            case 2:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = BACK;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = FOR;
                mtc2=true;
                break;
        }
        if(g[2]!=98&&mtc2==true) {
            switch(g[2]) {
                case 0:
                    mtc2=false;
                    break;
                case 255:
                    motor[TIRE_FL].pwm += 5;
                    mtc2=false;
                    break;
                case 253:
                    motor[TIRE_FL].pwm += 10;
                    mtc2=false;
                    break;
                case 254:
                    motor[TIRE_FL].pwm += 20;
                    motor[TIRE_BL].pwm = 0;
                    mtc2=false;
                    break;
                case 1:
                    motor[TIRE_BL].pwm += 5;
                    mtc2=false;
                    break;
                case 3:
                    motor[TIRE_BL].pwm += 10;
                    mtc2=false;
                    break;
                case 2:
                    motor[TIRE_BL].pwm += 20;
                    motor[TIRE_FL].pwm = 0;
                    mtc2=false;
                    break;
            }
        }
        
        if(LIF||LIB) {
            mode=41;
            countss=0;
            cross=0;
        }
        if(YELLOW) {
            if(cross==1) {
                mode=41;
                countss=0;
                cross=0;
            }
        } else {
            if(cross==3) {
                mode=41;
                countss=0;
                cross=0;
            }
        }
        
    } else if(mode==41) {
        if(LIF||LIB) {
            mode=42;
        }
        motor[TIRE_FR].pwm = 0;
        motor[TIRE_FR].dir = FREE;
        motor[TIRE_FL].pwm = 40;
        motor[TIRE_FL].dir = BACK;
        motor[TIRE_BR].pwm = 40;
        motor[TIRE_BR].dir = FOR;
        motor[TIRE_BL].pwm = 0;
        motor[TIRE_BL].dir = FREE;
        if(g[1]==0) {
            countss=0;
            mode=70;
            normal=80;
            fast=100;
            slow=60;
        }
    } else if(mode==42) {
        motor[TIRE_FR].pwm = 40;
        motor[TIRE_FR].dir = FOR;
        motor[TIRE_FL].pwm = 0;
        motor[TIRE_FL].dir = FREE;
        motor[TIRE_BR].pwm = 0;
        motor[TIRE_BR].dir = FREE;
        motor[TIRE_BL].pwm = 40;
        motor[TIRE_BL].dir = BACK;
        if(g[1]==0) {
            countss=0;
            mode=70;
        }
    } else if(mode==70) {
        switch(g[1]) {
            case 99:
                if(countss>=100000) {
                    mode=100;
                }
                break;
            case 98:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = BACK;
                break;
            case 0:
                motor[TIRE_FR].pwm = 100;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 100;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = 100;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 100;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 255:
                motor[TIRE_FR].pwm = fast;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = fast;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 253:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 254:
                motor[TIRE_FR].pwm = normal;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = 0;
                motor[TIRE_FL].dir = FREE;
                motor[TIRE_BR].pwm = 0;
                motor[TIRE_BR].dir = FREE;
                motor[TIRE_BL].pwm = normal;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 1:
                motor[TIRE_FR].pwm = slow;
                motor[TIRE_FR].dir = FOR;
                motor[TIRE_FL].pwm = fast;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = fast;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = slow;
                motor[TIRE_BL].dir = BACK;
                mtc=true;
                break;
            case 3:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = slow;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = slow;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc=true;
                break;
            case 2:
                motor[TIRE_FR].pwm = 0;
                motor[TIRE_FR].dir = FREE;
                motor[TIRE_FL].pwm = normal;
                motor[TIRE_FL].dir = BACK;
                motor[TIRE_BR].pwm = normal;
                motor[TIRE_BR].dir = FOR;
                motor[TIRE_BL].pwm = 0;
                motor[TIRE_BL].dir = FREE;
                mtc=true;
                break;
        }
        if(g[1]!=98&&mtc==true) {
            switch(g[0]) {
                case 0:
                    mtc=false;
                    break;
                case 255:
                    motor[TIRE_FL].pwm += 5;
                    mtc=false;
                    break;
                case 253:
                    motor[TIRE_FL].pwm += 10;
                    mtc=false;
                    break;
                case 254:
                    motor[TIRE_FL].pwm += 20;
                    motor[TIRE_FR].pwm = 0;
                    mtc=false;
                    break;
                case 1:
                    motor[TIRE_FR].pwm += 5;
                    mtc=false;
                    break;
                case 3:
                    motor[TIRE_FR].pwm += 10;
                    mtc=false;
                    break;
                case 2:
                    motor[TIRE_FR].pwm += 20;
                    motor[TIRE_FL].pwm = 0;
                    mtc=false;
                    break;
            }
        }
    } else if(mode==100) {
        motor[TIRE_FR].pwm = 50;
        motor[TIRE_FR].dir = FOR;
        motor[TIRE_FL].pwm = 50;
        motor[TIRE_FL].dir = BACK;
        motor[TIRE_BR].pwm = 50;
        motor[TIRE_BR].dir = FOR;
        motor[TIRE_BL].pwm = 50;
        motor[TIRE_BL].dir = BACK;
    } else {
    }

}
#endif

#if USE_PROCESS_NUM>4
static void Process4()
{

}
#endif

#if USE_PROCESS_NUM>5
static void Process5()
{
    
}
#endif

#if USE_PROCESS_NUM>6
static void Process6()
{

}
#endif

#if USE_PROCESS_NUM>7
static void Process7()
{

}
#endif

#if USE_PROCESS_NUM>8
static void Process8()
{

}
#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
}

#pragma region USER-DEFINED-FUNCTIONS

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 pwmVal)
{
    if (pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
    else return abs(pwmVal);
}

#pragma endregion