aa

Dependencies:   mbed TrapezoidControl QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Process.cpp Source File

Process.cpp

00001 
00002 #include "mbed.h"
00003 #include "Process.h"
00004 #include "QEI.h"
00005 
00006 #include "../../CommonLibraries/PID/PID.h"
00007 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
00008 #include "../../Communication/RS485/LineHub/LineHub.h"
00009 #include "../../Communication/Controller/Controller.h"
00010 #include "../../Input/ExternalInt/ExternalInt.h"
00011 #include "../../Input/Switch/Switch.h"
00012 #include "../../Input/Potentiometer/Potentiometer.h"
00013 #include "../../Input/Encoder/Encoder.h"
00014 #include "../../LED/LED.h"
00015 #include "../../Safty/Safty.h"
00016 #include "../Using.h"
00017 
00018 using namespace SWITCH;
00019 using namespace PID_SPACE;
00020 using namespace ENCODER;
00021 using namespace LINEHUB;
00022 
00023 static CONTROLLER::ControllerData *controller;
00024 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
00025 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
00026 
00027 static bool lock;
00028 static bool processChangeComp;
00029 static int current;
00030 
00031 static void AllActuatorReset();
00032 
00033 #ifdef USE_SUBPROCESS
00034 static void (*Process[USE_PROCESS_NUM])(void);
00035 #endif
00036 
00037 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
00038 
00039 /*Replace here with the definition code of your variables.*/
00040 
00041 Serial pc(USBTX, USBRX);
00042 
00043 //**************Encoder***************
00044 const int PerRev = 256;
00045 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
00046 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
00047 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
00048 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
00049 //**************Encoder***************
00050 
00051 //**************Buzzer****************
00052 //DigitalOut buzzer(BUZZER_PIN);
00053 void BuzzerTimer_func();
00054 Ticker BuzzerTimer;
00055 bool EMGflag = false;
00056 //PWMOut buzzer(BUZZER_PIN);
00057 //**************Buzzer****************
00058 
00059 //************TapeLed*****************
00060 void TapeLedEms_func();
00061 TapeLedData tapeLED;
00062 TapeLedData sendLedData;
00063 TapeLED_Mode ledMode = Normal;
00064 Ticker tapeLedTimer;
00065 //************TapaLed*****************
00066 
00067 float tireProRPM[4];
00068 float tireTarRPM[4];
00069 float tirepwm[4];
00070 
00071 const int omni[15][15] = {
00072     {    0,     5,    21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255 },
00073     {   -5,     0,     5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255 },
00074     {  -21,    -5,     0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255 },
00075     {  -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255 },
00076     {  -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255 },
00077     { -130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255 },
00078     { -187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255 },
00079     { -255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255 },
00080     { -255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187 },
00081     { -255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130 },
00082     { -255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83 },
00083     { -255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47 },
00084     { -255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21 },
00085     { -255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5 },
00086     { -255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0 }
00087 };
00088 
00089 const int curve[15] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 };
00090 //{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200}
00091 
00092 uint8_t SetStatus(int);
00093 uint8_t SetStatus(int pwmVal)
00094 {
00095     if (pwmVal < 0) return BACK;
00096     else if (pwmVal > 0) return FOR;
00097     else if (pwmVal == 0) return BRAKE;
00098     else return BRAKE;
00099 }
00100 uint8_t Setpwm(int);
00101 uint8_t Setpwm(int pwmVal)
00102 {
00103     if (pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
00104     else return abs(pwmVal);
00105 }
00106 
00107 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
00108 
00109 #ifdef USE_SUBPROCESS
00110 #if USE_PROCESS_NUM>0
00111 static void Process0(void);
00112 #endif
00113 #if USE_PROCESS_NUM>1
00114 static void Process1(void);
00115 #endif
00116 #if USE_PROCESS_NUM>2
00117 static void Process2(void);
00118 #endif
00119 #if USE_PROCESS_NUM>3
00120 static void Process3(void);
00121 #endif
00122 #if USE_PROCESS_NUM>4
00123 static void Process4(void);
00124 #endif
00125 #if USE_PROCESS_NUM>5
00126 static void Process5(void);
00127 #endif
00128 #if USE_PROCESS_NUM>6
00129 static void Process6(void);
00130 #endif
00131 #if USE_PROCESS_NUM>7
00132 static void Process7(void);
00133 #endif
00134 #if USE_PROCESS_NUM>8
00135 static void Process8(void);
00136 #endif
00137 #if USE_PROCESS_NUM>9
00138 static void Process9(void);
00139 #endif
00140 #endif
00141 
00142 void SystemProcessInitialize()
00143 {
00144     #pragma region USER-DEFINED_VARIABLE_INIT
00145     /*Replace here with the initialization code of your variables.*/
00146     #pragma endregion USER-DEFINED_VARIABLE_INIT
00147 
00148     lock = true;
00149     processChangeComp = true;
00150     current = DEFAULT_PROCESS;
00151 
00152 #ifdef USE_SUBPROCESS
00153 #if USE_PROCESS_NUM>0
00154     Process[0] = Process0;
00155 #endif
00156 #if USE_PROCESS_NUM>1
00157     Process[1] = Process1;
00158 #endif
00159 #if USE_PROCESS_NUM>2
00160     Process[2] = Process2;
00161 #endif
00162 #if USE_PROCESS_NUM>3
00163     Process[3] = Process3;
00164 #endif
00165 #if USE_PROCESS_NUM>4
00166     Process[4] = Process4;
00167 #endif
00168 #if USE_PROCESS_NUM>5
00169     Process[5] = Process5;
00170 #endif
00171 #if USE_PROCESS_NUM>6
00172     Process[6] = Process6;
00173 #endif
00174 #if USE_PROCESS_NUM>7
00175     Process[7] = Process7;
00176 #endif
00177 #if USE_PROCESS_NUM>8
00178     Process[8] = Process8;
00179 #endif
00180 #if USE_PROCESS_NUM>9
00181     Process[9] = Process9;
00182 #endif
00183 #endif
00184 }
00185 
00186 static void SystemProcessUpdate()
00187 {
00188 #ifdef USE_SUBPROCESS
00189     if(controller->Button.HOME) lock = false;
00190 
00191     if(controller->Button.START && processChangeComp) {
00192         current++;
00193         if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
00194         processChangeComp = false;
00195     } else if(controller->Button.SELECT && processChangeComp) {
00196         current--;
00197         if (current < 0) current = 0;
00198         processChangeComp = false;
00199     } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
00200 #endif
00201 
00202 #ifdef USE_MOTOR
00203     ACTUATORHUB::MOTOR::Motor::Update(motor);
00204 #endif
00205 
00206 #ifdef USE_SOLENOID
00207     ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
00208 #endif
00209 
00210 #ifdef USE_RS485
00211     ACTUATORHUB::ActuatorHub::Update();
00212 #endif
00213 
00214 }
00215 
00216 int g[8];
00217 
00218 void SystemProcess()
00219 {
00220     SystemProcessInitialize();
00221 
00222     while(1) {
00223         for(int i = 0; i < 8; i++) {
00224             g[i] = LineHub::GetPara(i);
00225         }
00226 
00227 #ifdef USE_MU
00228         controller = CONTROLLER::Controller::GetData();
00229 #endif
00230 
00231 #ifdef USE_ERRORCHECK
00232         if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
00233             CONTROLLER::Controller::DataReset();
00234             AllActuatorReset();
00235             lock = true;
00236         } else
00237 #endif
00238         {
00239 
00240 #ifdef USE_SUBPROCESS
00241             if(!lock) {
00242                 Process[current]();
00243             } else
00244 #endif
00245             {
00246                 //ロック時の処理
00247             }
00248         }
00249         /*
00250         //Emergency!
00251         if(!EMG_0 && !EMG_1 && !EMGflag) {
00252             buzzer = 0;
00253             BuzzerTimer.attach(BuzzerTimer_func, 1);
00254             EMGflag = true;
00255             LED_DEBUG0 = 1;
00256         }
00257         if(EMG_0 && EMG_1 && EMGflag) {
00258             buzzer = 1;
00259             BuzzerTimer.detach();
00260             EMGflag = false;
00261         }
00262         */
00263         SystemProcessUpdate();
00264     }
00265 }
00266 
00267 
00268 int Twsh;
00269 
00270 #pragma region PROCESS
00271 #ifdef USE_SUBPROCESS
00272 #if USE_PROCESS_NUM>0
00273 
00274 static void Process0()
00275 {
00276     if(StertSW) {
00277         current = 2;
00278         Twsh=3;
00279     }
00280 }
00281 #endif
00282 
00283 #if USE_PROCESS_NUM>1
00284 
00285 bool dz1=true;
00286 bool dz1i=false;
00287 bool dz2=true;
00288 bool dz2i=false;
00289 bool dz3=true;
00290 bool dz3i=false;
00291 bool dz4=true;
00292 bool dz4i=false;
00293 
00294 bool zone;
00295 
00296 int mode =1;
00297 int cross=0;//十字数える用
00298 int count=100000;//wait代替え
00299 
00300 
00301 static void Process1()
00302 {
00303     count=100000;
00304     mode=0;
00305     cross=0;
00306     motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
00307     motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
00308     motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]);
00309     motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);
00310 
00311     motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
00312     motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
00313     motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
00314     motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
00315 
00316     if(controller->Button.UP||controller->Button.DOWN) {
00317 
00318         if(controller->Button.UP) {
00319             motor[MOTOR_0].pwm = 110;
00320             motor[MOTOR_0].dir = FOR;
00321         }
00322         if(controller->Button.DOWN) {
00323             motor[MOTOR_0].pwm = 50;
00324             motor[MOTOR_0].dir = BACK;
00325         }
00326     } else {
00327         motor[MOTOR_0].pwm = 0;
00328         motor[MOTOR_0].dir = BRAKE;
00329     }
00330     if(controller->Button.X) {
00331         if(dz1==true) {
00332             if(dz1i==false) {
00333                 solenoid.solenoid1 = SOLENOID_ON;
00334                 dz1i=true;
00335             } else {
00336                 solenoid.solenoid1 = SOLENOID_OFF;
00337                 dz1i=false;
00338             }
00339             dz1=false;
00340         }
00341     } else {
00342         dz1=true;
00343     }
00344 
00345     if(controller->Button.Y) {
00346         if(dz2==true) {
00347             if(dz2i==false) {
00348                 solenoid.solenoid2 = SOLENOID_ON;
00349                 dz2i=true;
00350             } else {
00351                 solenoid.solenoid2 = SOLENOID_OFF;
00352                 dz2i=false;
00353             }
00354             dz2=false;
00355         }
00356     } else {
00357         dz2=true;
00358     }
00359 
00360     if(controller->Button.A) {
00361         if(dz3==true) {
00362             if(dz3i==false) {
00363                 solenoid.solenoid3 = SOLENOID_ON;
00364                 dz3i=true;
00365             } else {
00366                 solenoid.solenoid3 = SOLENOID_OFF;
00367                 dz3i=false;
00368             }
00369             dz3=false;
00370         }
00371     } else {
00372         dz3=true;
00373     }
00374 
00375     if(controller->Button.B) {
00376         if(dz4==true) {
00377             if(dz4i==false) {
00378                 solenoid.solenoid4 = SOLENOID_ON;
00379                 dz4i=true;
00380             } else {
00381                 solenoid.solenoid4 = SOLENOID_OFF;
00382                 dz4i=false;
00383             }
00384             dz4=false;
00385         }
00386     } else {
00387         dz4=true;
00388     }
00389     /*
00390     if(controller->Button.RIGHT){
00391      motor[MOTOR_1].dir = FOR;
00392      motor[MOTOR_1].pwm = 60;
00393      if (ARM_1){
00394       motor[MOTOR_1].dir = BRAKE;
00395      }
00396     }
00397     else if(controller->Button.LEFT){
00398      motor[MOTOR_1].dir = BACK;
00399      motor[MOTOR_1].pwm = 60;
00400      if (ARM_0){
00401       motor[MOTOR_1].dir = BRAKE;
00402      }
00403     }
00404     */
00405 }
00406 #endif
00407 
00408 #if USE_PROCESS_NUM>2
00409 bool mtc=false;
00410 //追加
00411 bool mtc2 = false;
00412 
00413 static void Process2()
00414 {
00415 //    printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
00416     count++;
00417     if(mode==0&&StertSW) {
00418         if(ThSW) {
00419             zone=ZoneSW;
00420             Twsh=1;
00421             mode=1;
00422         } else {
00423             zone=ZoneSW;
00424             Twsh=2;
00425             mode=1;
00426         }
00427     }
00428     if(ZoneSW==false) {
00429         if(mode==1) {//スタートゾーンから白線検知
00430             motor[TIRE_FR].pwm = 30;
00431             motor[TIRE_FR].dir = BACK;
00432             motor[TIRE_FL].pwm = 30;
00433             motor[TIRE_FL].dir = FOR;
00434             motor[TIRE_BR].pwm = 30;
00435             motor[TIRE_BR].dir = BACK;
00436             motor[TIRE_BL].pwm = 30;
00437             motor[TIRE_BL].dir = FOR;
00438             if(g[2]==0) {
00439                 count=100000;
00440                 cross=0;
00441                 mode=2;
00442             }
00443         } else if(mode==2) {//横移動
00444             if(g[2]==0) {
00445                 count=100000;
00446                 cross=0;
00447                 mode=3;
00448                 mtc2=true;
00449             } else {
00450                 motor[TIRE_FR].pwm = 30;
00451                 motor[TIRE_FR].dir = FOR;
00452                 motor[TIRE_FL].pwm = 30;
00453                 motor[TIRE_FL].dir = FOR;
00454                 motor[TIRE_BR].pwm = 30;
00455                 motor[TIRE_BR].dir = BACK;
00456                 motor[TIRE_BL].pwm = 30;
00457                 motor[TIRE_BL].dir = BACK;
00458             }
00459         } else if(mode==3) { //横ライントレースから縦ライントレースへ
00460             motor[TIRE_FR].pwm = 0;
00461             motor[TIRE_FR].dir = FREE;
00462             motor[TIRE_FL].pwm = 20;
00463             motor[TIRE_FL].dir = FOR;
00464             motor[TIRE_BR].pwm = 20;
00465             motor[TIRE_BR].dir = BACK;
00466             motor[TIRE_BL].pwm = 0;
00467             motor[TIRE_BL].dir = FREE;
00468             if(count>100000&&g[0]==0) {
00469                 count=0;
00470                 mode=10;
00471             }
00472         } else if(mode==4) { //横ライントレースから縦ライントレースへ
00473             motor[TIRE_FR].pwm = 15;
00474             motor[TIRE_FR].dir = BACK;
00475             motor[TIRE_FL].pwm = 15;
00476             motor[TIRE_FL].dir = BACK;
00477             motor[TIRE_BR].pwm = 15;
00478             motor[TIRE_BR].dir = FOR;
00479             motor[TIRE_BL].pwm = 15;
00480             motor[TIRE_BL].dir = FOR;
00481             if(count>30000&&g[0]==0) {
00482                 count=0;
00483                 cross=0;
00484                 mode=10;
00485             }
00486         } else if(g[0]!=99&&mode==10) { //縦ライントレース
00487             switch(g[0]) {
00488                 case 98:
00489                     motor[TIRE_FR].pwm = 30;
00490                     motor[TIRE_FR].dir = BACK;
00491                     motor[TIRE_FL].pwm = 30;
00492                     motor[TIRE_FL].dir = FOR;
00493                     motor[TIRE_BR].pwm = 30;
00494                     motor[TIRE_BR].dir = BACK;
00495                     motor[TIRE_BL].pwm = 30;
00496                     motor[TIRE_BL].dir = FOR;
00497                     if(count>100000) {
00498                         cross++;
00499                         count=0;
00500                     }
00501                     mtc=true;
00502                     if(cross==Twsh) {
00503                         mode=11;
00504                         count=0;
00505                     }
00506                     break;
00507                 case 0:
00508                     motor[TIRE_FR].pwm = 60;
00509                     motor[TIRE_FR].dir = BACK;
00510                     motor[TIRE_FL].pwm = 60;
00511                     motor[TIRE_FL].dir = FOR;
00512                     motor[TIRE_BR].pwm = 60;
00513                     motor[TIRE_BR].dir = BACK;
00514                     motor[TIRE_BL].pwm = 60;
00515                     motor[TIRE_BL].dir = FOR;
00516                     mtc=true;
00517                     break;
00518                 case 255:
00519                     motor[TIRE_FR].pwm = 60;
00520                     motor[TIRE_FR].dir = BACK;
00521                     motor[TIRE_FL].pwm = 40;
00522                     motor[TIRE_FL].dir = FOR;
00523                     motor[TIRE_BR].pwm = 40;
00524                     motor[TIRE_BR].dir = BACK;
00525                     motor[TIRE_BL].pwm = 60;
00526                     motor[TIRE_BL].dir = FOR;
00527                     mtc=true;
00528                     break;
00529                 case 253:
00530                     motor[TIRE_FR].pwm = 20;
00531                     motor[TIRE_FR].dir = BACK;
00532                     motor[TIRE_FL].pwm = 0;
00533                     motor[TIRE_FL].dir = FREE;
00534                     motor[TIRE_BR].pwm = 0;
00535                     motor[TIRE_BR].dir = FREE;
00536                     motor[TIRE_BL].pwm = 20;
00537                     motor[TIRE_BL].dir = FOR;
00538                     mtc=true;
00539                     break;
00540                 case 254:
00541                     motor[TIRE_FR].pwm = 30;
00542                     motor[TIRE_FR].dir = BACK;
00543                     motor[TIRE_FL].pwm = 0;
00544                     motor[TIRE_FL].dir = FREE;
00545                     motor[TIRE_BR].pwm = 0;
00546                     motor[TIRE_BR].dir = FREE;
00547                     motor[TIRE_BL].pwm = 30;
00548                     motor[TIRE_BL].dir = FOR;
00549                     mtc=true;
00550                     break;
00551                 case 1:
00552                     motor[TIRE_FR].pwm = 40;
00553                     motor[TIRE_FR].dir = BACK;
00554                     motor[TIRE_FL].pwm = 60;
00555                     motor[TIRE_FL].dir = FOR;
00556                     motor[TIRE_BR].pwm = 60;
00557                     motor[TIRE_BR].dir = BACK;
00558                     motor[TIRE_BL].pwm = 40;
00559                     motor[TIRE_BL].dir = FOR;
00560                     mtc=true;
00561                     break;
00562                 case 3:
00563                     motor[TIRE_FR].pwm = 0;
00564                     motor[TIRE_FR].dir = FREE;
00565                     motor[TIRE_FL].pwm = 20;
00566                     motor[TIRE_FL].dir = FOR;
00567                     motor[TIRE_BR].pwm = 20;
00568                     motor[TIRE_BR].dir = BACK;
00569                     motor[TIRE_BL].pwm = 0;
00570                     motor[TIRE_BL].dir = FREE;
00571                     mtc=true;
00572                     break;
00573                 case 2:
00574                     motor[TIRE_FR].pwm = 0;
00575                     motor[TIRE_FR].dir = FREE;
00576                     motor[TIRE_FL].pwm = 30;
00577                     motor[TIRE_FL].dir = FOR;
00578                     motor[TIRE_BR].pwm = 30;
00579                     motor[TIRE_BR].dir = BACK;
00580                     motor[TIRE_BL].pwm = 0;
00581                     motor[TIRE_BL].dir = FREE;
00582                     mtc=true;
00583                     break;
00584             }
00585             if(g[0]!=98&&mtc==true) {
00586                 switch(g[1]) {
00587                     case 0:
00588                         mtc=false;
00589                         break;
00590                     case 255:
00591                         motor[TIRE_BR].pwm += 5;
00592                         mtc=false;
00593                         break;
00594                     case 253:
00595                         motor[TIRE_BR].pwm += 10;
00596                         mtc=false;
00597                         break;
00598                     case 254:
00599                         motor[TIRE_BR].pwm += 20;
00600                         motor[TIRE_BL].pwm = 0;
00601                         mtc=false;
00602                         break;
00603                     case 1:
00604                         motor[TIRE_BL].pwm += 5;
00605                         mtc=false;
00606                         break;
00607                     case 3:
00608                         motor[TIRE_BL].pwm += 10;
00609                         mtc=false;
00610                         break;
00611                     case 2:
00612                         motor[TIRE_BL].pwm += 20;
00613                         motor[TIRE_BR].pwm = 0;
00614                         mtc=false;
00615                         break;
00616                 }
00617             }
00618         } else if(mode==11) {
00619             motor[TIRE_FR].pwm = 0;
00620             motor[TIRE_FR].dir = FREE;
00621             motor[TIRE_FL].pwm = 15;
00622             motor[TIRE_FL].dir = FOR;
00623             motor[TIRE_BR].pwm = 15;
00624             motor[TIRE_BR].dir = BACK;
00625             motor[TIRE_BL].pwm = 0;
00626             motor[TIRE_BL].dir = FREE;
00627             if(count>5000&&g[2]==0) {
00628                 count=0;
00629                 mode=12;
00630             }
00631         } else if(mode==12) {
00632             motor[TIRE_FR].pwm = 15;
00633             motor[TIRE_FR].dir = FOR;
00634             motor[TIRE_FL].pwm = 15;
00635             motor[TIRE_FL].dir = BACK;
00636             motor[TIRE_BR].pwm = 15;
00637             motor[TIRE_BR].dir = FOR;
00638             motor[TIRE_BL].pwm = 15;
00639             motor[TIRE_BL].dir = BACK;
00640             if(count>10000&&g[2]==0) {
00641                 count=0;
00642                 cross=0;
00643                 mode=20;
00644             }
00645         } else if(mode==20) {
00646             switch(g[2]) {
00647                 case 98:
00648                     motor[TIRE_FR].pwm = 30;
00649                     motor[TIRE_FR].dir = FOR;
00650                     motor[TIRE_FL].pwm = 30;
00651                     motor[TIRE_FL].dir = FOR;
00652                     motor[TIRE_BR].pwm = 30;
00653                     motor[TIRE_BR].dir = BACK;
00654                     motor[TIRE_BL].pwm = 30;
00655                     motor[TIRE_BL].dir = BACK;
00656                     if(count>70000) {
00657                         cross++;
00658                         count=0;
00659                     }
00660                     mtc2=true;
00661                     if(cross==3) {
00662                         mode=21;
00663                         count=0;
00664                         cross=0;
00665                     }
00666                     break;
00667                 case 0:
00668                     motor[TIRE_FR].pwm = 60;
00669                     motor[TIRE_FR].dir = FOR;
00670                     motor[TIRE_FL].pwm = 60;
00671                     motor[TIRE_FL].dir = FOR;
00672                     motor[TIRE_BR].pwm = 60;
00673                     motor[TIRE_BR].dir = BACK;
00674                     motor[TIRE_BL].pwm = 60;
00675                     motor[TIRE_BL].dir = BACK;
00676                     mtc2=true;
00677                     break;
00678                 case 255:
00679                     motor[TIRE_FR].pwm = 40;
00680                     motor[TIRE_FR].dir = FOR;
00681                     motor[TIRE_FL].pwm = 60;
00682                     motor[TIRE_FL].dir = FOR;
00683                     motor[TIRE_BR].pwm = 60;
00684                     motor[TIRE_BR].dir = BACK;
00685                     motor[TIRE_BL].pwm = 40;
00686                     motor[TIRE_BL].dir = BACK;
00687                     mtc2=true;
00688                     break;
00689                 case 253:
00690                     motor[TIRE_FR].pwm = 0;
00691                     motor[TIRE_FR].dir = FREE;
00692                     motor[TIRE_FL].pwm = 20;
00693                     motor[TIRE_FL].dir = FOR;
00694                     motor[TIRE_BR].pwm = 20;
00695                     motor[TIRE_BR].dir = BACK;
00696                     motor[TIRE_BL].pwm = 0;
00697                     motor[TIRE_BL].dir = FREE;
00698                     mtc2=true;
00699                     break;
00700                 case 254:
00701                     motor[TIRE_FR].pwm = 0;
00702                     motor[TIRE_FR].dir = FREE;
00703                     motor[TIRE_FL].pwm = 30;
00704                     motor[TIRE_FL].dir = FOR;
00705                     motor[TIRE_BR].pwm = 30;
00706                     motor[TIRE_BR].dir = BACK;
00707                     motor[TIRE_BL].pwm = 0;
00708                     motor[TIRE_BL].dir = FREE;
00709                     mtc2=true;
00710                     break;
00711                 case 1:
00712                     motor[TIRE_FR].pwm = 60;
00713                     motor[TIRE_FR].dir = FOR;
00714                     motor[TIRE_FL].pwm = 40;
00715                     motor[TIRE_FL].dir = FOR;
00716                     motor[TIRE_BR].pwm = 40;
00717                     motor[TIRE_BR].dir = BACK;
00718                     motor[TIRE_BL].pwm = 60;
00719                     motor[TIRE_BL].dir = BACK;
00720                     mtc2=true;
00721                     break;
00722                 case 3:
00723                     motor[TIRE_FR].pwm = 20;
00724                     motor[TIRE_FR].dir = FOR;
00725                     motor[TIRE_FL].pwm = 0;
00726                     motor[TIRE_FL].dir = FREE;
00727                     motor[TIRE_BR].pwm = 0;
00728                     motor[TIRE_BR].dir = FREE;
00729                     motor[TIRE_BL].pwm = 20;
00730                     motor[TIRE_BL].dir = BACK;
00731                     mtc2=true;
00732                     break;
00733                 case 2:
00734                     motor[TIRE_FR].pwm = 30;
00735                     motor[TIRE_FR].dir = FOR;
00736                     motor[TIRE_FL].pwm = 0;
00737                     motor[TIRE_FL].dir = FREE;
00738                     motor[TIRE_BR].pwm = 0;
00739                     motor[TIRE_BR].dir = FREE;
00740                     motor[TIRE_BL].pwm = 30;
00741                     motor[TIRE_BL].dir = BACK;
00742                     mtc2=true;
00743                     break;
00744             }
00745             if(g[2]!=98&&mtc2==true) {
00746                 switch(g[3]) {
00747                     case 0:
00748                         mtc2=false;
00749                         break;
00750                     case 255:
00751                         motor[TIRE_BL].pwm += 5;
00752                         mtc2=false;
00753                         break;
00754                     case 253:
00755                         motor[TIRE_BL].pwm += 10;
00756                         mtc2=false;
00757                         break;
00758                     case 254:
00759                         motor[TIRE_BL].pwm += 20;
00760                         motor[TIRE_FL].pwm = 0;
00761                         mtc2=false;
00762                         break;
00763                     case 1:
00764                         motor[TIRE_FL].pwm += 5;
00765                         mtc2=false;
00766                         break;
00767                     case 3:
00768                         motor[TIRE_FL].pwm += 10;
00769                         mtc2=false;
00770                         break;
00771                     case 2:
00772                         motor[TIRE_FL].pwm += 20;
00773                         motor[TIRE_BL].pwm = 0;
00774                         mtc2=false;
00775                         break;
00776                 }
00777             }
00778         } else if(mode==21) {
00779             motor[TIRE_FR].pwm = 0;
00780             motor[TIRE_FR].dir = FREE;
00781             motor[TIRE_FL].pwm = 15;
00782             motor[TIRE_FL].dir = FOR;
00783             motor[TIRE_BR].pwm = 15;
00784             motor[TIRE_BR].dir = BACK;
00785             motor[TIRE_BL].pwm = 0;
00786             motor[TIRE_BL].dir = FREE;
00787             if(count>2000&&g[0]==0) {
00788                 count=0;
00789                 mode=22;
00790             }
00791         } else if(mode==22) {
00792             motor[TIRE_FR].pwm = 15;
00793             motor[TIRE_FR].dir = BACK;
00794             motor[TIRE_FL].pwm = 15;
00795             motor[TIRE_FL].dir = BACK;
00796             motor[TIRE_BR].pwm = 15;
00797             motor[TIRE_BR].dir = FOR;
00798             motor[TIRE_BL].pwm = 15;
00799             motor[TIRE_BL].dir = FOR;
00800             if(count>5000&&g[0]==0) {
00801                 count=0;
00802                 cross=0;
00803                 mode=30;
00804             }
00805         } else if(mode==30) {
00806             switch(g[0]) {
00807                 case 98:
00808                     motor[TIRE_FR].pwm = 30;
00809                     motor[TIRE_FR].dir = BACK;
00810                     motor[TIRE_FL].pwm = 30;
00811                     motor[TIRE_FL].dir = FOR;
00812                     motor[TIRE_BR].pwm = 30;
00813                     motor[TIRE_BR].dir = BACK;
00814                     motor[TIRE_BL].pwm = 30;
00815                     motor[TIRE_BL].dir = FOR;
00816                     if(count>100000) {
00817                         cross++;
00818                         count=0;
00819                     }
00820                     mtc=true;
00821                     if(cross==1) {
00822                         mode=31;
00823                         count=0;
00824                     }
00825                     break;
00826                 case 0:
00827                     motor[TIRE_FR].pwm = 60;
00828                     motor[TIRE_FR].dir = BACK;
00829                     motor[TIRE_FL].pwm = 60;
00830                     motor[TIRE_FL].dir = FOR;
00831                     motor[TIRE_BR].pwm = 60;
00832                     motor[TIRE_BR].dir = BACK;
00833                     motor[TIRE_BL].pwm = 60;
00834                     motor[TIRE_BL].dir = FOR;
00835                     mtc=true;
00836                     break;
00837                 case 255:
00838                     motor[TIRE_FR].pwm = 60;
00839                     motor[TIRE_FR].dir = BACK;
00840                     motor[TIRE_FL].pwm = 40;
00841                     motor[TIRE_FL].dir = FOR;
00842                     motor[TIRE_BR].pwm = 40;
00843                     motor[TIRE_BR].dir = BACK;
00844                     motor[TIRE_BL].pwm = 60;
00845                     motor[TIRE_BL].dir = FOR;
00846                     mtc=true;
00847                     break;
00848                 case 253:
00849                     motor[TIRE_FR].pwm = 20;
00850                     motor[TIRE_FR].dir = BACK;
00851                     motor[TIRE_FL].pwm = 0;
00852                     motor[TIRE_FL].dir = FREE;
00853                     motor[TIRE_BR].pwm = 0;
00854                     motor[TIRE_BR].dir = FREE;
00855                     motor[TIRE_BL].pwm = 20;
00856                     motor[TIRE_BL].dir = FOR;
00857                     mtc=true;
00858                     break;
00859                 case 254:
00860                     motor[TIRE_FR].pwm = 30;
00861                     motor[TIRE_FR].dir = BACK;
00862                     motor[TIRE_FL].pwm = 0;
00863                     motor[TIRE_FL].dir = FREE;
00864                     motor[TIRE_BR].pwm = 0;
00865                     motor[TIRE_BR].dir = FREE;
00866                     motor[TIRE_BL].pwm = 30;
00867                     motor[TIRE_BL].dir = FOR;
00868                     mtc=true;
00869                     break;
00870                 case 1:
00871                     motor[TIRE_FR].pwm = 40;
00872                     motor[TIRE_FR].dir = BACK;
00873                     motor[TIRE_FL].pwm = 60;
00874                     motor[TIRE_FL].dir = FOR;
00875                     motor[TIRE_BR].pwm = 60;
00876                     motor[TIRE_BR].dir = BACK;
00877                     motor[TIRE_BL].pwm = 40;
00878                     motor[TIRE_BL].dir = FOR;
00879                     mtc=true;
00880                     break;
00881                 case 3:
00882                     motor[TIRE_FR].pwm = 0;
00883                     motor[TIRE_FR].dir = FREE;
00884                     motor[TIRE_FL].pwm = 20;
00885                     motor[TIRE_FL].dir = FOR;
00886                     motor[TIRE_BR].pwm = 20;
00887                     motor[TIRE_BR].dir = BACK;
00888                     motor[TIRE_BL].pwm = 0;
00889                     motor[TIRE_BL].dir = FREE;
00890                     mtc=true;
00891                     break;
00892                 case 2:
00893                     motor[TIRE_FR].pwm = 0;
00894                     motor[TIRE_FR].dir = FREE;
00895                     motor[TIRE_FL].pwm = 30;
00896                     motor[TIRE_FL].dir = FOR;
00897                     motor[TIRE_BR].pwm = 30;
00898                     motor[TIRE_BR].dir = BACK;
00899                     motor[TIRE_BL].pwm = 0;
00900                     motor[TIRE_BL].dir = FREE;
00901                     mtc=true;
00902                     break;
00903             }
00904             if(g[0]!=98&&mtc==true) {
00905                 switch(g[1]) {
00906                     case 0:
00907                         mtc=false;
00908                         break;
00909                     case 255:
00910                         motor[TIRE_BR].pwm += 5;
00911                         mtc=false;
00912                         break;
00913                     case 253:
00914                         motor[TIRE_BR].pwm += 10;
00915                         mtc=false;
00916                         break;
00917                     case 254:
00918                         motor[TIRE_BR].pwm += 20;
00919                         motor[TIRE_BL].pwm = 0;
00920                         mtc=false;
00921                         break;
00922                     case 1:
00923                         motor[TIRE_BL].pwm += 5;
00924                         mtc=false;
00925                         break;
00926                     case 3:
00927                         motor[TIRE_BL].pwm += 10;
00928                         mtc=false;
00929                         break;
00930                     case 2:
00931                         motor[TIRE_BL].pwm += 20;
00932                         motor[TIRE_BR].pwm = 0;
00933                         mtc=false;
00934                         break;
00935                 }
00936             }
00937         } else if(mode==31) {
00938             motor[TIRE_FR].pwm = 15;
00939             motor[TIRE_FR].dir = BACK;
00940             motor[TIRE_FL].pwm = 0;
00941             motor[TIRE_FL].dir = FREE;
00942             motor[TIRE_BR].pwm = 0;
00943             motor[TIRE_BR].dir = FREE;
00944             motor[TIRE_BL].pwm = 15;
00945             motor[TIRE_BL].dir = FOR;
00946             if(count>5000&&g[3]==0) {
00947                 count=0;
00948                 mode=32;
00949             }
00950         } else if(mode==32) {
00951             motor[TIRE_FR].pwm = 15;
00952             motor[TIRE_FR].dir = FOR;
00953             motor[TIRE_FL].pwm = 15;
00954             motor[TIRE_FL].dir = BACK;
00955             motor[TIRE_BR].pwm = 15;
00956             motor[TIRE_BR].dir = FOR;
00957             motor[TIRE_BL].pwm = 15;
00958             motor[TIRE_BL].dir = BACK;
00959             if(count>5000&&g[3]==0) {
00960                 count=0;
00961                 cross=0;
00962                 mode=40;
00963             }
00964         } else if(mode==40) {
00965             switch(g[3]) {
00966                 case 98:
00967                     motor[TIRE_FR].pwm = 30;
00968                     motor[TIRE_FR].dir = BACK;
00969                     motor[TIRE_FL].pwm = 30;
00970                     motor[TIRE_FL].dir = BACK;
00971                     motor[TIRE_BR].pwm = 30;
00972                     motor[TIRE_BR].dir = FOR;
00973                     motor[TIRE_BL].pwm = 30;
00974                     motor[TIRE_BL].dir = FOR;
00975                     if(count>30000) {
00976                         cross++;
00977                         count=0;
00978                     }
00979                     mtc2=true;
00980                     if(cross==1) {
00981                         mode=41;
00982                         count=0;
00983                         cross=0;
00984                     }
00985                     break;
00986                 case 0:
00987                     motor[TIRE_FR].pwm = 60;
00988                     motor[TIRE_FR].dir = BACK;
00989                     motor[TIRE_FL].pwm = 60;
00990                     motor[TIRE_FL].dir = BACK;
00991                     motor[TIRE_BR].pwm = 60;
00992                     motor[TIRE_BR].dir = FOR;
00993                     motor[TIRE_BL].pwm = 60;
00994                     motor[TIRE_BL].dir = FOR;
00995                     mtc2=true;
00996                     break;
00997                 case 255:
00998                     motor[TIRE_FR].pwm = 40;
00999                     motor[TIRE_FR].dir = BACK;
01000                     motor[TIRE_FL].pwm = 60;
01001                     motor[TIRE_FL].dir = BACK;
01002                     motor[TIRE_BR].pwm = 60;
01003                     motor[TIRE_BR].dir = FOR;
01004                     motor[TIRE_BL].pwm = 40;
01005                     motor[TIRE_BL].dir = FOR;
01006                     mtc2=true;
01007                     break;
01008                 case 253:
01009                     motor[TIRE_FR].pwm = 0;
01010                     motor[TIRE_FR].dir = FREE;
01011                     motor[TIRE_FL].pwm = 20;
01012                     motor[TIRE_FL].dir = BACK;
01013                     motor[TIRE_BR].pwm = 20;
01014                     motor[TIRE_BR].dir = FOR;
01015                     motor[TIRE_BL].pwm = 0;
01016                     motor[TIRE_BL].dir = FREE;
01017                     mtc2=true;
01018                     break;
01019                 case 254:
01020                     motor[TIRE_FR].pwm = 0;
01021                     motor[TIRE_FR].dir = FREE;
01022                     motor[TIRE_FL].pwm = 30;
01023                     motor[TIRE_FL].dir = BACK;
01024                     motor[TIRE_BR].pwm = 30;
01025                     motor[TIRE_BR].dir = FOR;
01026                     motor[TIRE_BL].pwm = 0;
01027                     motor[TIRE_BL].dir = FREE;
01028                     mtc2=true;
01029                     break;
01030                 case 1:
01031                     motor[TIRE_FR].pwm = 60;
01032                     motor[TIRE_FR].dir = BACK;
01033                     motor[TIRE_FL].pwm = 40;
01034                     motor[TIRE_FL].dir = BACK;
01035                     motor[TIRE_BR].pwm = 40;
01036                     motor[TIRE_BR].dir = FOR;
01037                     motor[TIRE_BL].pwm = 60;
01038                     motor[TIRE_BL].dir = FOR;
01039                     mtc2=true;
01040                     break;
01041                 case 3:
01042                     motor[TIRE_FR].pwm = 20;
01043                     motor[TIRE_FR].dir = BACK;
01044                     motor[TIRE_FL].pwm = 0;
01045                     motor[TIRE_FL].dir = FREE;
01046                     motor[TIRE_BR].pwm = 0;
01047                     motor[TIRE_BR].dir = FREE;
01048                     motor[TIRE_BL].pwm = 20;
01049                     motor[TIRE_BL].dir = FOR;
01050                     mtc2=true;
01051                     break;
01052                 case 2:
01053                     motor[TIRE_FR].pwm = 30;
01054                     motor[TIRE_FR].dir = BACK;
01055                     motor[TIRE_FL].pwm = 0;
01056                     motor[TIRE_FL].dir = FREE;
01057                     motor[TIRE_BR].pwm = 0;
01058                     motor[TIRE_BR].dir = FREE;
01059                     motor[TIRE_BL].pwm = 30;
01060                     motor[TIRE_BL].dir = FOR;
01061                     mtc2=true;
01062                     break;
01063             }
01064             if(g[2]!=98&&mtc2==true) {
01065                 switch(g[2]) {
01066                     case 0:
01067                         mtc2=false;
01068                         break;
01069                     case 255:
01070                         motor[TIRE_FL].pwm += 5;
01071                         mtc2=false;
01072                         break;
01073                     case 253:
01074                         motor[TIRE_FL].pwm += 10;
01075                         mtc2=false;
01076                         break;
01077                     case 254:
01078                         motor[TIRE_FL].pwm += 20;
01079                         motor[TIRE_BL].pwm = 0;
01080                         mtc2=false;
01081                         break;
01082                     case 1:
01083                         motor[TIRE_BL].pwm += 5;
01084                         mtc2=false;
01085                         break;
01086                     case 3:
01087                         motor[TIRE_BL].pwm += 10;
01088                         mtc2=false;
01089                         break;
01090                     case 2:
01091                         motor[TIRE_BL].pwm += 20;
01092                         motor[TIRE_FL].pwm = 0;
01093                         mtc2=false;
01094                         break;
01095                 }
01096             }
01097         } else if(mode==41) {
01098             motor[TIRE_FR].pwm = 0;
01099             motor[TIRE_FR].dir = FREE;
01100             motor[TIRE_FL].pwm = 15;
01101             motor[TIRE_FL].dir = BACK;
01102             motor[TIRE_BR].pwm = 15;
01103             motor[TIRE_BR].dir = FOR;
01104             motor[TIRE_BL].pwm = 0;
01105             motor[TIRE_BL].dir = FREE;
01106             if(count>5000&&g[1]==0) {
01107                 count=0;
01108                 mode=42;
01109             }
01110         } else if(mode==42) {
01111             motor[TIRE_FR].pwm = 15;
01112             motor[TIRE_FR].dir = FOR;
01113             motor[TIRE_FL].pwm = 15;
01114             motor[TIRE_FL].dir = FOR;
01115             motor[TIRE_BR].pwm = 15;
01116             motor[TIRE_BR].dir = BACK;
01117             motor[TIRE_BL].pwm = 15;
01118             motor[TIRE_BL].dir = BACK;
01119             if(count>5000&&g[1]==0) {
01120                 count=0;
01121                 mode=50;
01122             }
01123         } else if(mode==50) {
01124             switch(g[1]) {
01125                 case 98:
01126                     motor[TIRE_FR].pwm = 30;
01127                     motor[TIRE_FR].dir = FOR;
01128                     motor[TIRE_FL].pwm = 30;
01129                     motor[TIRE_FL].dir = BACK;
01130                     motor[TIRE_BR].pwm = 30;
01131                     motor[TIRE_BR].dir = FOR;
01132                     motor[TIRE_BL].pwm = 30;
01133                     motor[TIRE_BL].dir = BACK;
01134                     if(count>100000) {
01135                         cross++;
01136                         count=0;
01137                     }
01138                     mtc=true;
01139                     if(cross==1) {
01140                         mode=51;
01141                         count=0;
01142                     }
01143                     break;
01144                 case 0:
01145                     motor[TIRE_FR].pwm = 60;
01146                     motor[TIRE_FR].dir = FOR;
01147                     motor[TIRE_FL].pwm = 60;
01148                     motor[TIRE_FL].dir = BACK;
01149                     motor[TIRE_BR].pwm = 60;
01150                     motor[TIRE_BR].dir = FOR;
01151                     motor[TIRE_BL].pwm = 60;
01152                     motor[TIRE_BL].dir = BACK;
01153                     mtc=true;
01154                     break;
01155                 case 255:
01156                     motor[TIRE_FR].pwm = 60;
01157                     motor[TIRE_FR].dir = FOR;
01158                     motor[TIRE_FL].pwm = 40;
01159                     motor[TIRE_FL].dir = BACK;
01160                     motor[TIRE_BR].pwm = 40;
01161                     motor[TIRE_BR].dir = FOR;
01162                     motor[TIRE_BL].pwm = 60;
01163                     motor[TIRE_BL].dir = BACK;
01164                     mtc=true;
01165                     break;
01166                 case 253:
01167                     motor[TIRE_FR].pwm = 20;
01168                     motor[TIRE_FR].dir = FOR;
01169                     motor[TIRE_FL].pwm = 0;
01170                     motor[TIRE_FL].dir = FREE;
01171                     motor[TIRE_BR].pwm = 0;
01172                     motor[TIRE_BR].dir = FREE;
01173                     motor[TIRE_BL].pwm = 20;
01174                     motor[TIRE_BL].dir = BACK;
01175                     mtc=true;
01176                     break;
01177                 case 254:
01178                     motor[TIRE_FR].pwm = 30;
01179                     motor[TIRE_FR].dir = FOR;
01180                     motor[TIRE_FL].pwm = 0;
01181                     motor[TIRE_FL].dir = FREE;
01182                     motor[TIRE_BR].pwm = 0;
01183                     motor[TIRE_BR].dir = FREE;
01184                     motor[TIRE_BL].pwm = 30;
01185                     motor[TIRE_BL].dir = BACK;
01186                     mtc=true;
01187                     break;
01188                 case 1:
01189                     motor[TIRE_FR].pwm = 40;
01190                     motor[TIRE_FR].dir = FOR;
01191                     motor[TIRE_FL].pwm = 60;
01192                     motor[TIRE_FL].dir = BACK;
01193                     motor[TIRE_BR].pwm = 60;
01194                     motor[TIRE_BR].dir = FOR;
01195                     motor[TIRE_BL].pwm = 40;
01196                     motor[TIRE_BL].dir = BACK;
01197                     mtc=true;
01198                     break;
01199                 case 3:
01200                     motor[TIRE_FR].pwm = 0;
01201                     motor[TIRE_FR].dir = FREE;
01202                     motor[TIRE_FL].pwm = 20;
01203                     motor[TIRE_FL].dir = BACK;
01204                     motor[TIRE_BR].pwm = 20;
01205                     motor[TIRE_BR].dir = FOR;
01206                     motor[TIRE_BL].pwm = 0;
01207                     motor[TIRE_BL].dir = FREE;
01208                     mtc=true;
01209                     break;
01210                 case 2:
01211                     motor[TIRE_FR].pwm = 0;
01212                     motor[TIRE_FR].dir = FREE;
01213                     motor[TIRE_FL].pwm = 30;
01214                     motor[TIRE_FL].dir = BACK;
01215                     motor[TIRE_BR].pwm = 30;
01216                     motor[TIRE_BR].dir = FOR;
01217                     motor[TIRE_BL].pwm = 0;
01218                     motor[TIRE_BL].dir = FREE;
01219                     mtc=true;
01220                     break;
01221             }
01222             if(g[1]!=98&&mtc==true) {
01223                 switch(g[0]) {
01224                     case 0:
01225                         mtc=false;
01226                         break;
01227                     case 255:
01228                         motor[TIRE_FL].pwm += 5;
01229                         mtc=false;
01230                         break;
01231                     case 253:
01232                         motor[TIRE_FL].pwm += 10;
01233                         mtc=false;
01234                         break;
01235                     case 254:
01236                         motor[TIRE_FL].pwm += 20;
01237                         motor[TIRE_FR].pwm = 0;
01238                         mtc=false;
01239                         break;
01240                     case 1:
01241                         motor[TIRE_FR].pwm += 5;
01242                         mtc=false;
01243                         break;
01244                     case 3:
01245                         motor[TIRE_FR].pwm += 10;
01246                         mtc=false;
01247                         break;
01248                     case 2:
01249                         motor[TIRE_FR].pwm += 20;
01250                         motor[TIRE_FL].pwm = 0;
01251                         mtc=false;
01252                         break;
01253                 }
01254             }
01255         } else if(mode==51) {
01256             motor[TIRE_FR].pwm = 0;
01257             motor[TIRE_FR].dir = FREE;
01258             motor[TIRE_FL].pwm = 15;
01259             motor[TIRE_FL].dir = BACK;
01260             motor[TIRE_BR].pwm = 15;
01261             motor[TIRE_BR].dir = FOR;
01262             motor[TIRE_BL].pwm = 0;
01263             motor[TIRE_BL].dir = FREE;
01264             if(count>5000&&g[3]==0) {
01265                 count=0;
01266                 mode=52;
01267             }
01268         } else if(mode==52) {
01269             motor[TIRE_FR].pwm = 15;
01270             motor[TIRE_FR].dir = BACK;
01271             motor[TIRE_FL].pwm = 15;
01272             motor[TIRE_FL].dir = FOR;
01273             motor[TIRE_BR].pwm = 15;
01274             motor[TIRE_BR].dir = BACK;
01275             motor[TIRE_BL].pwm = 15;
01276             motor[TIRE_BL].dir = FOR;
01277             if(count>5000&&g[3]==0) {
01278                 count=0;
01279                 cross=0;
01280                 mode=60;
01281             }
01282         } else if(mode==60) {
01283             switch(g[3]) {
01284                 case 98:
01285                     motor[TIRE_FR].pwm = 30;
01286                     motor[TIRE_FR].dir = BACK;
01287                     motor[TIRE_FL].pwm = 30;
01288                     motor[TIRE_FL].dir = BACK;
01289                     motor[TIRE_BR].pwm = 30;
01290                     motor[TIRE_BR].dir = FOR;
01291                     motor[TIRE_BL].pwm = 30;
01292                     motor[TIRE_BL].dir = FOR;
01293                     if(count>50000) {
01294                         cross++;
01295                         count=0;
01296                     }
01297                     mtc2=true;
01298                     if(cross==2) {
01299                         mode=61;
01300                         count=0;
01301                         cross=0;
01302                     }
01303                     break;
01304                 case 0:
01305                     motor[TIRE_FR].pwm = 60;
01306                     motor[TIRE_FR].dir = BACK;
01307                     motor[TIRE_FL].pwm = 60;
01308                     motor[TIRE_FL].dir = BACK;
01309                     motor[TIRE_BR].pwm = 60;
01310                     motor[TIRE_BR].dir = FOR;
01311                     motor[TIRE_BL].pwm = 60;
01312                     motor[TIRE_BL].dir = FOR;
01313                     mtc2=true;
01314                     break;
01315                 case 255:
01316                     motor[TIRE_FR].pwm = 40;
01317                     motor[TIRE_FR].dir = BACK;
01318                     motor[TIRE_FL].pwm = 60;
01319                     motor[TIRE_FL].dir = BACK;
01320                     motor[TIRE_BR].pwm = 60;
01321                     motor[TIRE_BR].dir = FOR;
01322                     motor[TIRE_BL].pwm = 40;
01323                     motor[TIRE_BL].dir = FOR;
01324                     mtc2=true;
01325                     break;
01326                 case 253:
01327                     motor[TIRE_FR].pwm = 0;
01328                     motor[TIRE_FR].dir = FREE;
01329                     motor[TIRE_FL].pwm = 20;
01330                     motor[TIRE_FL].dir = BACK;
01331                     motor[TIRE_BR].pwm = 20;
01332                     motor[TIRE_BR].dir = FOR;
01333                     motor[TIRE_BL].pwm = 0;
01334                     motor[TIRE_BL].dir = FREE;
01335                     mtc2=true;
01336                     break;
01337                 case 254:
01338                     motor[TIRE_FR].pwm = 0;
01339                     motor[TIRE_FR].dir = FREE;
01340                     motor[TIRE_FL].pwm = 30;
01341                     motor[TIRE_FL].dir = BACK;
01342                     motor[TIRE_BR].pwm = 30;
01343                     motor[TIRE_BR].dir = FOR;
01344                     motor[TIRE_BL].pwm = 0;
01345                     motor[TIRE_BL].dir = FREE;
01346                     mtc2=true;
01347                     break;
01348                 case 1:
01349                     motor[TIRE_FR].pwm = 60;
01350                     motor[TIRE_FR].dir = BACK;
01351                     motor[TIRE_FL].pwm = 40;
01352                     motor[TIRE_FL].dir = BACK;
01353                     motor[TIRE_BR].pwm = 40;
01354                     motor[TIRE_BR].dir = FOR;
01355                     motor[TIRE_BL].pwm = 60;
01356                     motor[TIRE_BL].dir = FOR;
01357                     mtc2=true;
01358                     break;
01359                 case 3:
01360                     motor[TIRE_FR].pwm = 20;
01361                     motor[TIRE_FR].dir = BACK;
01362                     motor[TIRE_FL].pwm = 0;
01363                     motor[TIRE_FL].dir = FREE;
01364                     motor[TIRE_BR].pwm = 0;
01365                     motor[TIRE_BR].dir = FREE;
01366                     motor[TIRE_BL].pwm = 20;
01367                     motor[TIRE_BL].dir = FOR;
01368                     mtc2=true;
01369                     break;
01370                 case 2:
01371                     motor[TIRE_FR].pwm = 30;
01372                     motor[TIRE_FR].dir = BACK;
01373                     motor[TIRE_FL].pwm = 0;
01374                     motor[TIRE_FL].dir = FREE;
01375                     motor[TIRE_BR].pwm = 0;
01376                     motor[TIRE_BR].dir = FREE;
01377                     motor[TIRE_BL].pwm = 30;
01378                     motor[TIRE_BL].dir = FOR;
01379                     mtc2=true;
01380                     break;
01381             }
01382             if(g[2]!=98&&mtc2==true) {
01383                 switch(g[2]) {
01384                     case 0:
01385                         mtc2=false;
01386                         break;
01387                     case 255:
01388                         motor[TIRE_FL].pwm += 5;
01389                         mtc2=false;
01390                         break;
01391                     case 253:
01392                         motor[TIRE_FL].pwm += 10;
01393                         mtc2=false;
01394                         break;
01395                     case 254:
01396                         motor[TIRE_FL].pwm += 20;
01397                         motor[TIRE_BL].pwm = 0;
01398                         mtc2=false;
01399                         break;
01400                     case 1:
01401                         motor[TIRE_BL].pwm += 5;
01402                         mtc2=false;
01403                         break;
01404                     case 3:
01405                         motor[TIRE_BL].pwm += 10;
01406                         mtc2=false;
01407                         break;
01408                     case 2:
01409                         motor[TIRE_BL].pwm += 20;
01410                         motor[TIRE_FL].pwm = 0;
01411                         mtc2=false;
01412                         break;
01413                 }
01414             }
01415         } else if(mode==61) {
01416             motor[TIRE_FR].pwm = 0;
01417             motor[TIRE_FR].dir = FREE;
01418             motor[TIRE_FL].pwm = 15;
01419             motor[TIRE_FL].dir = BACK;
01420             motor[TIRE_BR].pwm = 15;
01421             motor[TIRE_BR].dir = FOR;
01422             motor[TIRE_BL].pwm = 0;
01423             motor[TIRE_BL].dir = FREE;
01424             if(count>5000&&g[1]==0) {
01425                 count=0;
01426                 mode=62;
01427             }
01428         } else if(mode==62) {
01429             motor[TIRE_FR].pwm = 15;
01430             motor[TIRE_FR].dir = FOR;
01431             motor[TIRE_FL].pwm = 15;
01432             motor[TIRE_FL].dir = FOR;
01433             motor[TIRE_BR].pwm = 15;
01434             motor[TIRE_BR].dir = BACK;
01435             motor[TIRE_BL].pwm = 15;
01436             motor[TIRE_BL].dir = BACK;
01437             if(count>5000&&g[1]==0) {
01438                 count=0;
01439                 mode=70;
01440             }
01441         } else if(mode==70) {
01442             switch(g[1]) {
01443                 case 99:
01444                     if(count>=100000) {
01445                         mode=100;
01446                     }
01447                     break;
01448                 case 98:
01449                     motor[TIRE_FR].pwm = 30;
01450                     motor[TIRE_FR].dir = FOR;
01451                     motor[TIRE_FL].pwm = 30;
01452                     motor[TIRE_FL].dir = BACK;
01453                     motor[TIRE_BR].pwm = 30;
01454                     motor[TIRE_BR].dir = FOR;
01455                     motor[TIRE_BL].pwm = 30;
01456                     motor[TIRE_BL].dir = BACK;
01457                     break;
01458                 case 0:
01459                     motor[TIRE_FR].pwm = 60;
01460                     motor[TIRE_FR].dir = FOR;
01461                     motor[TIRE_FL].pwm = 60;
01462                     motor[TIRE_FL].dir = BACK;
01463                     motor[TIRE_BR].pwm = 60;
01464                     motor[TIRE_BR].dir = FOR;
01465                     motor[TIRE_BL].pwm = 60;
01466                     motor[TIRE_BL].dir = BACK;
01467                     mtc=true;
01468                     break;
01469                 case 255:
01470                     motor[TIRE_FR].pwm = 60;
01471                     motor[TIRE_FR].dir = FOR;
01472                     motor[TIRE_FL].pwm = 40;
01473                     motor[TIRE_FL].dir = BACK;
01474                     motor[TIRE_BR].pwm = 40;
01475                     motor[TIRE_BR].dir = FOR;
01476                     motor[TIRE_BL].pwm = 60;
01477                     motor[TIRE_BL].dir = BACK;
01478                     mtc=true;
01479                     break;
01480                 case 253:
01481                     motor[TIRE_FR].pwm = 20;
01482                     motor[TIRE_FR].dir = FOR;
01483                     motor[TIRE_FL].pwm = 0;
01484                     motor[TIRE_FL].dir = FREE;
01485                     motor[TIRE_BR].pwm = 0;
01486                     motor[TIRE_BR].dir = FREE;
01487                     motor[TIRE_BL].pwm = 20;
01488                     motor[TIRE_BL].dir = BACK;
01489                     mtc=true;
01490                     break;
01491                 case 254:
01492                     motor[TIRE_FR].pwm = 30;
01493                     motor[TIRE_FR].dir = FOR;
01494                     motor[TIRE_FL].pwm = 0;
01495                     motor[TIRE_FL].dir = FREE;
01496                     motor[TIRE_BR].pwm = 0;
01497                     motor[TIRE_BR].dir = FREE;
01498                     motor[TIRE_BL].pwm = 30;
01499                     motor[TIRE_BL].dir = BACK;
01500                     mtc=true;
01501                     break;
01502                 case 1:
01503                     motor[TIRE_FR].pwm = 40;
01504                     motor[TIRE_FR].dir = FOR;
01505                     motor[TIRE_FL].pwm = 60;
01506                     motor[TIRE_FL].dir = BACK;
01507                     motor[TIRE_BR].pwm = 60;
01508                     motor[TIRE_BR].dir = FOR;
01509                     motor[TIRE_BL].pwm = 40;
01510                     motor[TIRE_BL].dir = BACK;
01511                     mtc=true;
01512                     break;
01513                 case 3:
01514                     motor[TIRE_FR].pwm = 0;
01515                     motor[TIRE_FR].dir = FREE;
01516                     motor[TIRE_FL].pwm = 20;
01517                     motor[TIRE_FL].dir = BACK;
01518                     motor[TIRE_BR].pwm = 20;
01519                     motor[TIRE_BR].dir = FOR;
01520                     motor[TIRE_BL].pwm = 0;
01521                     motor[TIRE_BL].dir = FREE;
01522                     mtc=true;
01523                     break;
01524                 case 2:
01525                     motor[TIRE_FR].pwm = 0;
01526                     motor[TIRE_FR].dir = FREE;
01527                     motor[TIRE_FL].pwm = 30;
01528                     motor[TIRE_FL].dir = BACK;
01529                     motor[TIRE_BR].pwm = 30;
01530                     motor[TIRE_BR].dir = FOR;
01531                     motor[TIRE_BL].pwm = 0;
01532                     motor[TIRE_BL].dir = FREE;
01533                     mtc=true;
01534                     break;
01535             }
01536             if(g[1]!=98&&mtc==true) {
01537                 switch(g[0]) {
01538                     case 0:
01539                         mtc=false;
01540                         break;
01541                     case 255:
01542                         motor[TIRE_FL].pwm += 5;
01543                         mtc=false;
01544                         break;
01545                     case 253:
01546                         motor[TIRE_FL].pwm += 10;
01547                         mtc=false;
01548                         break;
01549                     case 254:
01550                         motor[TIRE_FL].pwm += 20;
01551                         motor[TIRE_FR].pwm = 0;
01552                         mtc=false;
01553                         break;
01554                     case 1:
01555                         motor[TIRE_FR].pwm += 5;
01556                         mtc=false;
01557                         break;
01558                     case 3:
01559                         motor[TIRE_FR].pwm += 10;
01560                         mtc=false;
01561                         break;
01562                     case 2:
01563                         motor[TIRE_FR].pwm += 20;
01564                         motor[TIRE_FL].pwm = 0;
01565                         mtc=false;
01566                         break;
01567                 }
01568             }
01569         } else if(mode==100) {
01570             motor[TIRE_FR].pwm = 20;
01571             motor[TIRE_FR].dir = FOR;
01572             motor[TIRE_FL].pwm = 20;
01573             motor[TIRE_FL].dir = BACK;
01574             motor[TIRE_BR].pwm = 23;
01575             motor[TIRE_BR].dir = FOR;
01576             motor[TIRE_BL].pwm = 20;
01577             motor[TIRE_BL].dir = BACK;
01578         } else {
01579             motor[TIRE_FR].pwm = 0;
01580             motor[TIRE_FR].dir = BRAKE;
01581             motor[TIRE_FL].pwm = 0;
01582             motor[TIRE_FL].dir = BRAKE;
01583             motor[TIRE_BR].pwm = 0;
01584             motor[TIRE_BR].dir = BRAKE;
01585             motor[TIRE_BL].pwm = 0;
01586             motor[TIRE_BL].dir = BRAKE;
01587         }
01588     } else {//////////////////////////////////////////////////これより別ゾーン用/////////////////////////////////////////////////////////////////////////////////
01589         if(mode==1) {//スタートゾーンから白線検知
01590             motor[TIRE_FR].pwm = 30;
01591             motor[TIRE_FR].dir = FOR;
01592             motor[TIRE_FL].pwm = 30;
01593             motor[TIRE_FL].dir = BACK;
01594             motor[TIRE_BR].pwm = 30;
01595             motor[TIRE_BR].dir = FOR;
01596             motor[TIRE_BL].pwm = 30;
01597             motor[TIRE_BL].dir = BACK;
01598             if(g[2]==0) {
01599                 count=100000;
01600                 cross=0;
01601                 mode=2;
01602             }
01603         } else if(mode==2) {//横移動
01604             if(g[2]==0) {
01605                 count=100000;
01606                 cross=0;
01607                 mode=3;
01608                 mtc2=true;
01609             } else {
01610                 motor[TIRE_FR].pwm = 30;
01611                 motor[TIRE_FR].dir = FOR;
01612                 motor[TIRE_FL].pwm = 30;
01613                 motor[TIRE_FL].dir = FOR;
01614                 motor[TIRE_BR].pwm = 30;
01615                 motor[TIRE_BR].dir = BACK;
01616                 motor[TIRE_BL].pwm = 30;
01617                 motor[TIRE_BL].dir = BACK;
01618             }
01619         } else if(mode==3) { //横ライントレースから縦ライントレースへ
01620             motor[TIRE_FR].pwm = 0;
01621             motor[TIRE_FR].dir = FREE;
01622             motor[TIRE_FL].pwm = 20;
01623             motor[TIRE_FL].dir = FOR;
01624             motor[TIRE_BR].pwm = 20;
01625             motor[TIRE_BR].dir = BACK;
01626             motor[TIRE_BL].pwm = 0;
01627             motor[TIRE_BL].dir = FREE;
01628             if(count>100000&&g[0]==0) {
01629                 count=0;
01630                 mode=10;
01631             }
01632         } else if(mode==4) { //横ライントレースから縦ライントレースへ
01633             motor[TIRE_FR].pwm = 15;
01634             motor[TIRE_FR].dir = BACK;
01635             motor[TIRE_FL].pwm = 15;
01636             motor[TIRE_FL].dir = BACK;
01637             motor[TIRE_BR].pwm = 15;
01638             motor[TIRE_BR].dir = FOR;
01639             motor[TIRE_BL].pwm = 15;
01640             motor[TIRE_BL].dir = FOR;
01641             if(count>30000&&g[0]==0) {
01642                 count=0;
01643                 cross=0;
01644                 mode=10;
01645             }
01646         } else if(g[0]!=99&&mode==10) { //縦ライントレース
01647             switch(g[0]) {
01648                 case 98:
01649                     motor[TIRE_FR].pwm = 30;
01650                     motor[TIRE_FR].dir = FOR;
01651                     motor[TIRE_FL].pwm = 30;
01652                     motor[TIRE_FL].dir = BACK;
01653                     motor[TIRE_BR].pwm = 30;
01654                     motor[TIRE_BR].dir = FOR;
01655                     motor[TIRE_BL].pwm = 30;
01656                     motor[TIRE_BL].dir = BACK;
01657                     if(count>100000) {
01658                         cross++;
01659                         count=0;
01660                     }
01661                     mtc=true;
01662                     if(cross==Twsh) {
01663                         mode=11;
01664                         count=0;
01665                     }
01666                     break;
01667                 case 0:
01668                     motor[TIRE_FR].pwm = 60;
01669                     motor[TIRE_FR].dir = FOR;
01670                     motor[TIRE_FL].pwm = 60;
01671                     motor[TIRE_FL].dir = BACK;
01672                     motor[TIRE_BR].pwm = 60;
01673                     motor[TIRE_BR].dir = FOR;
01674                     motor[TIRE_BL].pwm = 60;
01675                     motor[TIRE_BL].dir = BACK;
01676                     mtc=true;
01677                     break;
01678                 case 255:
01679                     motor[TIRE_FR].pwm = 60;
01680                     motor[TIRE_FR].dir = FOR;
01681                     motor[TIRE_FL].pwm = 40;
01682                     motor[TIRE_FL].dir = BACK;
01683                     motor[TIRE_BR].pwm = 40;
01684                     motor[TIRE_BR].dir = FOR;
01685                     motor[TIRE_BL].pwm = 60;
01686                     motor[TIRE_BL].dir = BACK;
01687                     mtc=true;
01688                     break;
01689                 case 253:
01690                     motor[TIRE_FR].pwm = 20;
01691                     motor[TIRE_FR].dir = FOR;
01692                     motor[TIRE_FL].pwm = 0;
01693                     motor[TIRE_FL].dir = FREE;
01694                     motor[TIRE_BR].pwm = 0;
01695                     motor[TIRE_BR].dir = FREE;
01696                     motor[TIRE_BL].pwm = 20;
01697                     motor[TIRE_BL].dir = BACK;
01698                     mtc=true;
01699                     break;
01700                 case 254:
01701                     motor[TIRE_FR].pwm = 30;
01702                     motor[TIRE_FR].dir = FOR;
01703                     motor[TIRE_FL].pwm = 0;
01704                     motor[TIRE_FL].dir = FREE;
01705                     motor[TIRE_BR].pwm = 0;
01706                     motor[TIRE_BR].dir = FREE;
01707                     motor[TIRE_BL].pwm = 30;
01708                     motor[TIRE_BL].dir = BACK;
01709                     mtc=true;
01710                     break;
01711                 case 1:
01712                     motor[TIRE_FR].pwm = 40;
01713                     motor[TIRE_FR].dir = FOR;
01714                     motor[TIRE_FL].pwm = 60;
01715                     motor[TIRE_FL].dir = BACK;
01716                     motor[TIRE_BR].pwm = 60;
01717                     motor[TIRE_BR].dir = FOR;
01718                     motor[TIRE_BL].pwm = 40;
01719                     motor[TIRE_BL].dir = BACK;
01720                     mtc=true;
01721                     break;
01722                 case 3:
01723                     motor[TIRE_FR].pwm = 0;
01724                     motor[TIRE_FR].dir = FREE;
01725                     motor[TIRE_FL].pwm = 20;
01726                     motor[TIRE_FL].dir = BACK;
01727                     motor[TIRE_BR].pwm = 20;
01728                     motor[TIRE_BR].dir = FOR;
01729                     motor[TIRE_BL].pwm = 0;
01730                     motor[TIRE_BL].dir = FREE;
01731                     mtc=true;
01732                     break;
01733                 case 2:
01734                     motor[TIRE_FR].pwm = 0;
01735                     motor[TIRE_FR].dir = FREE;
01736                     motor[TIRE_FL].pwm = 30;
01737                     motor[TIRE_FL].dir = BACK;
01738                     motor[TIRE_BR].pwm = 30;
01739                     motor[TIRE_BR].dir = FOR;
01740                     motor[TIRE_BL].pwm = 0;
01741                     motor[TIRE_BL].dir = FREE;
01742                     mtc=true;
01743                     break;
01744             }
01745             if(g[0]!=98&&mtc==true) {
01746                 switch(g[1]) {
01747                     case 0:
01748                         mtc=false;
01749                         break;
01750                     case 255:
01751                         motor[TIRE_BL].pwm += 5;
01752                         mtc=false;
01753                         break;
01754                     case 253:
01755                         motor[TIRE_BL].pwm += 10;
01756                         mtc=false;
01757                         break;
01758                     case 254:
01759                         motor[TIRE_BL].pwm += 20;
01760                         motor[TIRE_BR].pwm = 0;
01761                         mtc=false;
01762                         break;
01763                     case 1:
01764                         motor[TIRE_BR].pwm += 5;
01765                         mtc=false;
01766                         break;
01767                     case 3:
01768                         motor[TIRE_BR].pwm += 10;
01769                         mtc=false;
01770                         break;
01771                     case 2:
01772                         motor[TIRE_BR].pwm += 20;
01773                         motor[TIRE_BL].pwm = 0;
01774                         mtc=false;
01775                         break;
01776                 }
01777             }
01778         } else if(mode==11) {
01779             motor[TIRE_FR].pwm = 0;
01780             motor[TIRE_FR].dir = FREE;
01781             motor[TIRE_FL].pwm = 15;
01782             motor[TIRE_FL].dir = FOR;
01783             motor[TIRE_BR].pwm = 15;
01784             motor[TIRE_BR].dir = BACK;
01785             motor[TIRE_BL].pwm = 0;
01786             motor[TIRE_BL].dir = FREE;
01787             if(count>5000&&g[2]==0) {
01788                 count=0;
01789                 mode=12;
01790             }
01791         } else if(mode==12) {
01792             motor[TIRE_FR].pwm = 15;
01793             motor[TIRE_FR].dir = FOR;
01794             motor[TIRE_FL].pwm = 15;
01795             motor[TIRE_FL].dir = BACK;
01796             motor[TIRE_BR].pwm = 15;
01797             motor[TIRE_BR].dir = FOR;
01798             motor[TIRE_BL].pwm = 15;
01799             motor[TIRE_BL].dir = BACK;
01800             if(count>10000&&g[2]==0) {
01801                 count=0;
01802                 cross=0;
01803                 mode=20;
01804             }
01805         } else if(mode==20) {
01806             switch(g[2]) {
01807                 case 98:
01808                     motor[TIRE_FR].pwm = 30;
01809                     motor[TIRE_FR].dir = FOR;
01810                     motor[TIRE_FL].pwm = 30;
01811                     motor[TIRE_FL].dir = FOR;
01812                     motor[TIRE_BR].pwm = 30;
01813                     motor[TIRE_BR].dir = BACK;
01814                     motor[TIRE_BL].pwm = 30;
01815                     motor[TIRE_BL].dir = BACK;
01816                     if(count>70000) {
01817                         cross++;
01818                         count=0;
01819                     }
01820                     mtc2=true;
01821                     if(cross==3) {
01822                         mode=21;
01823                         count=0;
01824                         cross=0;
01825                     }
01826                     break;
01827                 case 0:
01828                     motor[TIRE_FR].pwm = 60;
01829                     motor[TIRE_FR].dir = FOR;
01830                     motor[TIRE_FL].pwm = 60;
01831                     motor[TIRE_FL].dir = FOR;
01832                     motor[TIRE_BR].pwm = 60;
01833                     motor[TIRE_BR].dir = BACK;
01834                     motor[TIRE_BL].pwm = 60;
01835                     motor[TIRE_BL].dir = BACK;
01836                     mtc2=true;
01837                     break;
01838                 case 255:
01839                     motor[TIRE_FR].pwm = 40;
01840                     motor[TIRE_FR].dir = FOR;
01841                     motor[TIRE_FL].pwm = 60;
01842                     motor[TIRE_FL].dir = FOR;
01843                     motor[TIRE_BR].pwm = 60;
01844                     motor[TIRE_BR].dir = BACK;
01845                     motor[TIRE_BL].pwm = 40;
01846                     motor[TIRE_BL].dir = BACK;
01847                     mtc2=true;
01848                     break;
01849                 case 253:
01850                     motor[TIRE_FR].pwm = 0;
01851                     motor[TIRE_FR].dir = FREE;
01852                     motor[TIRE_FL].pwm = 20;
01853                     motor[TIRE_FL].dir = FOR;
01854                     motor[TIRE_BR].pwm = 20;
01855                     motor[TIRE_BR].dir = BACK;
01856                     motor[TIRE_BL].pwm = 0;
01857                     motor[TIRE_BL].dir = FREE;
01858                     mtc2=true;
01859                     break;
01860                 case 254:
01861                     motor[TIRE_FR].pwm = 0;
01862                     motor[TIRE_FR].dir = FREE;
01863                     motor[TIRE_FL].pwm = 30;
01864                     motor[TIRE_FL].dir = FOR;
01865                     motor[TIRE_BR].pwm = 30;
01866                     motor[TIRE_BR].dir = BACK;
01867                     motor[TIRE_BL].pwm = 0;
01868                     motor[TIRE_BL].dir = FREE;
01869                     mtc2=true;
01870                     break;
01871                 case 1:
01872                     motor[TIRE_FR].pwm = 60;
01873                     motor[TIRE_FR].dir = FOR;
01874                     motor[TIRE_FL].pwm = 40;
01875                     motor[TIRE_FL].dir = FOR;
01876                     motor[TIRE_BR].pwm = 40;
01877                     motor[TIRE_BR].dir = BACK;
01878                     motor[TIRE_BL].pwm = 60;
01879                     motor[TIRE_BL].dir = BACK;
01880                     mtc2=true;
01881                     break;
01882                 case 3:
01883                     motor[TIRE_FR].pwm = 20;
01884                     motor[TIRE_FR].dir = FOR;
01885                     motor[TIRE_FL].pwm = 0;
01886                     motor[TIRE_FL].dir = FREE;
01887                     motor[TIRE_BR].pwm = 0;
01888                     motor[TIRE_BR].dir = FREE;
01889                     motor[TIRE_BL].pwm = 20;
01890                     motor[TIRE_BL].dir = BACK;
01891                     mtc2=true;
01892                     break;
01893                 case 2:
01894                     motor[TIRE_FR].pwm = 30;
01895                     motor[TIRE_FR].dir = FOR;
01896                     motor[TIRE_FL].pwm = 0;
01897                     motor[TIRE_FL].dir = FREE;
01898                     motor[TIRE_BR].pwm = 0;
01899                     motor[TIRE_BR].dir = FREE;
01900                     motor[TIRE_BL].pwm = 30;
01901                     motor[TIRE_BL].dir = BACK;
01902                     mtc2=true;
01903                     break;
01904             }
01905             if(g[2]!=98&&mtc2==true) {
01906                 switch(g[3]) {
01907                     case 0:
01908                         mtc2=false;
01909                         break;
01910                     case 255:
01911                         motor[TIRE_BL].pwm += 5;
01912                         mtc2=false;
01913                         break;
01914                     case 253:
01915                         motor[TIRE_BL].pwm += 10;
01916                         mtc2=false;
01917                         break;
01918                     case 254:
01919                         motor[TIRE_BL].pwm += 20;
01920                         motor[TIRE_FL].pwm = 0;
01921                         mtc2=false;
01922                         break;
01923                     case 1:
01924                         motor[TIRE_FL].pwm += 5;
01925                         mtc2=false;
01926                         break;
01927                     case 3:
01928                         motor[TIRE_FL].pwm += 10;
01929                         mtc2=false;
01930                         break;
01931                     case 2:
01932                         motor[TIRE_FL].pwm += 20;
01933                         motor[TIRE_BL].pwm = 0;
01934                         mtc2=false;
01935                         break;
01936                 }
01937             }
01938         } else if(mode==21) {
01939             motor[TIRE_FR].pwm = 0;
01940             motor[TIRE_FR].dir = FREE;
01941             motor[TIRE_FL].pwm = 15;
01942             motor[TIRE_FL].dir = FOR;
01943             motor[TIRE_BR].pwm = 15;
01944             motor[TIRE_BR].dir = BACK;
01945             motor[TIRE_BL].pwm = 0;
01946             motor[TIRE_BL].dir = FREE;
01947             if(count>2000&&g[0]==0) {
01948                 count=0;
01949                 mode=22;
01950             }
01951         } else if(mode==22) {
01952             motor[TIRE_FR].pwm = 15;
01953             motor[TIRE_FR].dir = BACK;
01954             motor[TIRE_FL].pwm = 15;
01955             motor[TIRE_FL].dir = BACK;
01956             motor[TIRE_BR].pwm = 15;
01957             motor[TIRE_BR].dir = FOR;
01958             motor[TIRE_BL].pwm = 15;
01959             motor[TIRE_BL].dir = FOR;
01960             if(count>5000&&g[0]==0) {
01961                 count=0;
01962                 cross=0;
01963                 mode=30;
01964             }
01965         } else if(mode==30) {
01966             switch(g[0]) {
01967                 case 98:
01968                     motor[TIRE_FR].pwm = 30;
01969                     motor[TIRE_FR].dir = FOR;
01970                     motor[TIRE_FL].pwm = 30;
01971                     motor[TIRE_FL].dir = BACK;
01972                     motor[TIRE_BR].pwm = 30;
01973                     motor[TIRE_BR].dir = FOR;
01974                     motor[TIRE_BL].pwm = 30;
01975                     motor[TIRE_BL].dir = BACK;
01976                     if(count>100000) {
01977                         cross++;
01978                         count=0;
01979                     }
01980                     mtc=true;
01981                     if(cross==1) {
01982                         mode=31;
01983                         count=0;
01984                     }
01985                     break;
01986                 case 0:
01987                     motor[TIRE_FR].pwm = 60;
01988                     motor[TIRE_FR].dir = FOR;
01989                     motor[TIRE_FL].pwm = 60;
01990                     motor[TIRE_FL].dir = BACK;
01991                     motor[TIRE_BR].pwm = 60;
01992                     motor[TIRE_BR].dir = FOR;
01993                     motor[TIRE_BL].pwm = 60;
01994                     motor[TIRE_BL].dir = BACK;
01995                     mtc=true;
01996                     break;
01997                 case 255:
01998                     motor[TIRE_FR].pwm = 60;
01999                     motor[TIRE_FR].dir = FOR;
02000                     motor[TIRE_FL].pwm = 40;
02001                     motor[TIRE_FL].dir = BACK;
02002                     motor[TIRE_BR].pwm = 40;
02003                     motor[TIRE_BR].dir = FOR;
02004                     motor[TIRE_BL].pwm = 60;
02005                     motor[TIRE_BL].dir = BACK;
02006                     mtc=true;
02007                     break;
02008                 case 253:
02009                     motor[TIRE_FR].pwm = 20;
02010                     motor[TIRE_FR].dir = FOR;
02011                     motor[TIRE_FL].pwm = 0;
02012                     motor[TIRE_FL].dir = BACK;
02013                     motor[TIRE_BR].pwm = 0;
02014                     motor[TIRE_BR].dir = FOR;
02015                     motor[TIRE_BL].pwm = 20;
02016                     motor[TIRE_BL].dir = BACK;
02017                     mtc=true;
02018                     break;
02019                 case 254:
02020                     motor[TIRE_FR].pwm = 30;
02021                     motor[TIRE_FR].dir = FOR;
02022                     motor[TIRE_FL].pwm = 0;
02023                     motor[TIRE_FL].dir = FREE;
02024                     motor[TIRE_BR].pwm = 0;
02025                     motor[TIRE_BR].dir = FREE;
02026                     motor[TIRE_BL].pwm = 30;
02027                     motor[TIRE_BL].dir = BACK;
02028                     mtc=true;
02029                     break;
02030                 case 1:
02031                     motor[TIRE_FR].pwm = 40;
02032                     motor[TIRE_FR].dir = FOR;
02033                     motor[TIRE_FL].pwm = 60;
02034                     motor[TIRE_FL].dir = BACK;
02035                     motor[TIRE_BR].pwm = 60;
02036                     motor[TIRE_BR].dir = FOR;
02037                     motor[TIRE_BL].pwm = 40;
02038                     motor[TIRE_BL].dir = BACK;
02039                     mtc=true;
02040                     break;
02041                 case 3:
02042                     motor[TIRE_FR].pwm = 0;
02043                     motor[TIRE_FR].dir = FREE;
02044                     motor[TIRE_FL].pwm = 20;
02045                     motor[TIRE_FL].dir = BACK;
02046                     motor[TIRE_BR].pwm = 20;
02047                     motor[TIRE_BR].dir = FOR;
02048                     motor[TIRE_BL].pwm = 0;
02049                     motor[TIRE_BL].dir = FREE;
02050                     mtc=true;
02051                     break;
02052                 case 2:
02053                     motor[TIRE_FR].pwm = 0;
02054                     motor[TIRE_FR].dir = FREE;
02055                     motor[TIRE_FL].pwm = 30;
02056                     motor[TIRE_FL].dir = BACK;
02057                     motor[TIRE_BR].pwm = 30;
02058                     motor[TIRE_BR].dir = FOR;
02059                     motor[TIRE_BL].pwm = 0;
02060                     motor[TIRE_BL].dir = FREE;
02061                     mtc=true;
02062                     break;
02063             }
02064             if(g[0]!=98&&mtc==true) {
02065                 switch(g[1]) {
02066                     case 0:
02067                         mtc=false;
02068                         break;
02069                     case 255:
02070                         motor[TIRE_BL].pwm += 5;
02071                         mtc=false;
02072                         break;
02073                     case 253:
02074                         motor[TIRE_BL].pwm += 10;
02075                         mtc=false;
02076                         break;
02077                     case 254:
02078                         motor[TIRE_BL].pwm += 20;
02079                         motor[TIRE_BR].pwm = 0;
02080                         mtc=false;
02081                         break;
02082                     case 1:
02083                         motor[TIRE_BR].pwm += 5;
02084                         mtc=false;
02085                         break;
02086                     case 3:
02087                         motor[TIRE_BR].pwm += 10;
02088                         mtc=false;
02089                         break;
02090                     case 2:
02091                         motor[TIRE_BR].pwm += 20;
02092                         motor[TIRE_BL].pwm = 0;
02093                         mtc=false;
02094                         break;
02095                 }
02096             }
02097         } else if(mode==31) {
02098             motor[TIRE_FR].pwm = 15;
02099             motor[TIRE_FR].dir = BACK;
02100             motor[TIRE_FL].pwm = 0;
02101             motor[TIRE_FL].dir = FREE;
02102             motor[TIRE_BR].pwm = 0;
02103             motor[TIRE_BR].dir = FREE;
02104             motor[TIRE_BL].pwm = 15;
02105             motor[TIRE_BL].dir = FOR;
02106             if(count>5000&&g[3]==0) {
02107                 count=0;
02108                 mode=32;
02109             }
02110         } else if(mode==32) {
02111             motor[TIRE_FR].pwm = 15;
02112             motor[TIRE_FR].dir = FOR;
02113             motor[TIRE_FL].pwm = 15;
02114             motor[TIRE_FL].dir = BACK;
02115             motor[TIRE_BR].pwm = 15;
02116             motor[TIRE_BR].dir = FOR;
02117             motor[TIRE_BL].pwm = 15;
02118             motor[TIRE_BL].dir = BACK;
02119             if(count>5000&&g[3]==0) {
02120                 count=0;
02121                 cross=0;
02122                 mode=40;
02123             }
02124         } else if(mode==40) {
02125             switch(g[3]) {
02126                 case 98:
02127                     motor[TIRE_FR].pwm = 30;
02128                     motor[TIRE_FR].dir = BACK;
02129                     motor[TIRE_FL].pwm = 30;
02130                     motor[TIRE_FL].dir = BACK;
02131                     motor[TIRE_BR].pwm = 30;
02132                     motor[TIRE_BR].dir = FOR;
02133                     motor[TIRE_BL].pwm = 30;
02134                     motor[TIRE_BL].dir = FOR;
02135                     if(count>30000) {
02136                         cross++;
02137                         count=0;
02138                     }
02139                     mtc2=true;
02140                     if(cross==1) {
02141                         mode=41;
02142                         count=0;
02143                         cross=0;
02144                     }
02145                     break;
02146                 case 0:
02147                     motor[TIRE_FR].pwm = 60;
02148                     motor[TIRE_FR].dir = BACK;
02149                     motor[TIRE_FL].pwm = 60;
02150                     motor[TIRE_FL].dir = BACK;
02151                     motor[TIRE_BR].pwm = 60;
02152                     motor[TIRE_BR].dir = FOR;
02153                     motor[TIRE_BL].pwm = 60;
02154                     motor[TIRE_BL].dir = FOR;
02155                     mtc2=true;
02156                     break;
02157                 case 255:
02158                     motor[TIRE_FR].pwm = 40;
02159                     motor[TIRE_FR].dir = BACK;
02160                     motor[TIRE_FL].pwm = 60;
02161                     motor[TIRE_FL].dir = BACK;
02162                     motor[TIRE_BR].pwm = 60;
02163                     motor[TIRE_BR].dir = FOR;
02164                     motor[TIRE_BL].pwm = 40;
02165                     motor[TIRE_BL].dir = FOR;
02166                     mtc2=true;
02167                     break;
02168                 case 253:
02169                     motor[TIRE_FR].pwm = 0;
02170                     motor[TIRE_FR].dir = FREE;
02171                     motor[TIRE_FL].pwm = 20;
02172                     motor[TIRE_FL].dir = BACK;
02173                     motor[TIRE_BR].pwm = 20;
02174                     motor[TIRE_BR].dir = FOR;
02175                     motor[TIRE_BL].pwm = 0;
02176                     motor[TIRE_BL].dir = FREE;
02177                     mtc2=true;
02178                     break;
02179                 case 254:
02180                     motor[TIRE_FR].pwm = 0;
02181                     motor[TIRE_FR].dir = FREE;
02182                     motor[TIRE_FL].pwm = 30;
02183                     motor[TIRE_FL].dir = BACK;
02184                     motor[TIRE_BR].pwm = 30;
02185                     motor[TIRE_BR].dir = FOR;
02186                     motor[TIRE_BL].pwm = 0;
02187                     motor[TIRE_BL].dir = FREE;
02188                     mtc2=true;
02189                     break;
02190                 case 1:
02191                     motor[TIRE_FR].pwm = 60;
02192                     motor[TIRE_FR].dir = BACK;
02193                     motor[TIRE_FL].pwm = 40;
02194                     motor[TIRE_FL].dir = BACK;
02195                     motor[TIRE_BR].pwm = 40;
02196                     motor[TIRE_BR].dir = FOR;
02197                     motor[TIRE_BL].pwm = 60;
02198                     motor[TIRE_BL].dir = FOR;
02199                     mtc2=true;
02200                     break;
02201                 case 3:
02202                     motor[TIRE_FR].pwm = 20;
02203                     motor[TIRE_FR].dir = BACK;
02204                     motor[TIRE_FL].pwm = 0;
02205                     motor[TIRE_FL].dir = FREE;
02206                     motor[TIRE_BR].pwm = 0;
02207                     motor[TIRE_BR].dir = FREE;
02208                     motor[TIRE_BL].pwm = 20;
02209                     motor[TIRE_BL].dir = FOR;
02210                     mtc2=true;
02211                     break;
02212                 case 2:
02213                     motor[TIRE_FR].pwm = 30;
02214                     motor[TIRE_FR].dir = BACK;
02215                     motor[TIRE_FL].pwm = 0;
02216                     motor[TIRE_FL].dir = FREE;
02217                     motor[TIRE_BR].pwm = 0;
02218                     motor[TIRE_BR].dir = FREE;
02219                     motor[TIRE_BL].pwm = 30;
02220                     motor[TIRE_BL].dir = FOR;
02221                     mtc2=true;
02222                     break;
02223             }
02224             if(g[2]!=98&&mtc2==true) {
02225                 switch(g[2]) {
02226                     case 0:
02227                         mtc2=false;
02228                         break;
02229                     case 255:
02230                         motor[TIRE_FL].pwm += 5;
02231                         mtc2=false;
02232                         break;
02233                     case 253:
02234                         motor[TIRE_FL].pwm += 10;
02235                         mtc2=false;
02236                         break;
02237                     case 254:
02238                         motor[TIRE_FL].pwm += 20;
02239                         motor[TIRE_BL].pwm = 0;
02240                         mtc2=false;
02241                         break;
02242                     case 1:
02243                         motor[TIRE_BL].pwm += 5;
02244                         mtc2=false;
02245                         break;
02246                     case 3:
02247                         motor[TIRE_BL].pwm += 10;
02248                         mtc2=false;
02249                         break;
02250                     case 2:
02251                         motor[TIRE_BL].pwm += 20;
02252                         motor[TIRE_FL].pwm = 0;
02253                         mtc2=false;
02254                         break;
02255                 }
02256             }
02257         } else if(mode==41) {
02258             motor[TIRE_FR].pwm = 0;
02259             motor[TIRE_FR].dir = FREE;
02260             motor[TIRE_FL].pwm = 15;
02261             motor[TIRE_FL].dir = BACK;
02262             motor[TIRE_BR].pwm = 15;
02263             motor[TIRE_BR].dir = FOR;
02264             motor[TIRE_BL].pwm = 0;
02265             motor[TIRE_BL].dir = FREE;
02266             if(count>5000&&g[1]==0) {
02267                 count=0;
02268                 mode=42;
02269             }
02270         } else if(mode==42) {
02271             motor[TIRE_FR].pwm = 15;
02272             motor[TIRE_FR].dir = FOR;
02273             motor[TIRE_FL].pwm = 15;
02274             motor[TIRE_FL].dir = FOR;
02275             motor[TIRE_BR].pwm = 15;
02276             motor[TIRE_BR].dir = BACK;
02277             motor[TIRE_BL].pwm = 15;
02278             motor[TIRE_BL].dir = BACK;
02279             if(count>5000&&g[1]==0) {
02280                 count=0;
02281                 mode=50;
02282             }
02283         } else if(mode==50) {
02284             switch(g[1]) {
02285                 case 98:
02286                     motor[TIRE_FR].pwm = 30;
02287                     motor[TIRE_FR].dir = BACK;
02288                     motor[TIRE_FL].pwm = 30;
02289                     motor[TIRE_FL].dir = FOR;
02290                     motor[TIRE_BR].pwm = 30;
02291                     motor[TIRE_BR].dir = BACK;
02292                     motor[TIRE_BL].pwm = 30;
02293                     motor[TIRE_BL].dir = FOR;
02294                     if(count>100000) {
02295                         cross++;
02296                         count=0;
02297                     }
02298                     mtc=true;
02299                     if(cross==1) {
02300                         mode=51;
02301                         count=0;
02302                     }
02303                     break;
02304                 case 0:
02305                     motor[TIRE_FR].pwm = 60;
02306                     motor[TIRE_FR].dir = BACK;
02307                     motor[TIRE_FL].pwm = 60;
02308                     motor[TIRE_FL].dir = FOR;
02309                     motor[TIRE_BR].pwm = 60;
02310                     motor[TIRE_BR].dir = BACK;
02311                     motor[TIRE_BL].pwm = 60;
02312                     motor[TIRE_BL].dir = FOR;
02313                     mtc=true;
02314                     break;
02315                 case 255:
02316                     motor[TIRE_FR].pwm = 60;
02317                     motor[TIRE_FR].dir = BACK;
02318                     motor[TIRE_FL].pwm = 40;
02319                     motor[TIRE_FL].dir = FOR;
02320                     motor[TIRE_BR].pwm = 40;
02321                     motor[TIRE_BR].dir = BACK;
02322                     motor[TIRE_BL].pwm = 60;
02323                     motor[TIRE_BL].dir = FOR;
02324                     mtc=true;
02325                     break;
02326                 case 253:
02327                     motor[TIRE_FR].pwm = 20;
02328                     motor[TIRE_FR].dir = BACK;
02329                     motor[TIRE_FL].pwm = 0;
02330                     motor[TIRE_FL].dir = FREE;
02331                     motor[TIRE_BR].pwm = 0;
02332                     motor[TIRE_BR].dir = FREE;
02333                     motor[TIRE_BL].pwm = 20;
02334                     motor[TIRE_BL].dir = FOR;
02335                     mtc=true;
02336                     break;
02337                 case 254:
02338                     motor[TIRE_FR].pwm = 30;
02339                     motor[TIRE_FR].dir = BACK;
02340                     motor[TIRE_FL].pwm = 0;
02341                     motor[TIRE_FL].dir = FREE;
02342                     motor[TIRE_BR].pwm = 0;
02343                     motor[TIRE_BR].dir = FREE;
02344                     motor[TIRE_BL].pwm = 30;
02345                     motor[TIRE_BL].dir = FOR;
02346                     mtc=true;
02347                     break;
02348                 case 1:
02349                     motor[TIRE_FR].pwm = 40;
02350                     motor[TIRE_FR].dir = BACK;
02351                     motor[TIRE_FL].pwm = 60;
02352                     motor[TIRE_FL].dir = FOR;
02353                     motor[TIRE_BR].pwm = 60;
02354                     motor[TIRE_BR].dir = BACK;
02355                     motor[TIRE_BL].pwm = 40;
02356                     motor[TIRE_BL].dir = FOR;
02357                     mtc=true;
02358                     break;
02359                 case 3:
02360                     motor[TIRE_FR].pwm = 0;
02361                     motor[TIRE_FR].dir = FREE;
02362                     motor[TIRE_FL].pwm = 20;
02363                     motor[TIRE_FL].dir = FOR;
02364                     motor[TIRE_BR].pwm = 20;
02365                     motor[TIRE_BR].dir = BACK;
02366                     motor[TIRE_BL].pwm = 0;
02367                     motor[TIRE_BL].dir = FREE;
02368                     mtc=true;
02369                     break;
02370                 case 2:
02371                     motor[TIRE_FR].pwm = 0;
02372                     motor[TIRE_FR].dir = FREE;
02373                     motor[TIRE_FL].pwm = 30;
02374                     motor[TIRE_FL].dir = FOR;
02375                     motor[TIRE_BR].pwm = 30;
02376                     motor[TIRE_BR].dir = BACK;
02377                     motor[TIRE_BL].pwm = 0;
02378                     motor[TIRE_BL].dir = FREE;
02379                     mtc=true;
02380                     break;
02381             }
02382             if(g[1]!=98&&mtc==true) {
02383                 switch(g[0]) {
02384                     case 0:
02385                         mtc=false;
02386                         break;
02387                     case 255:
02388                         motor[TIRE_FR].pwm += 5;
02389                         mtc=false;
02390                         break;
02391                     case 253:
02392                         motor[TIRE_FR].pwm += 10;
02393                         mtc=false;
02394                         break;
02395                     case 254:
02396                         motor[TIRE_FR].pwm += 20;
02397                         motor[TIRE_FL].pwm = 0;
02398                         mtc=false;
02399                         break;
02400                     case 1:
02401                         motor[TIRE_FL].pwm += 5;
02402                         mtc=false;
02403                         break;
02404                     case 3:
02405                         motor[TIRE_FL].pwm += 10;
02406                         mtc=false;
02407                         break;
02408                     case 2:
02409                         motor[TIRE_FL].pwm += 20;
02410                         motor[TIRE_FR].pwm = 0;
02411                         mtc=false;
02412                         break;
02413                 }
02414             }
02415         } else if(mode==51) {
02416             motor[TIRE_FR].pwm = 0;
02417             motor[TIRE_FR].dir = FREE;
02418             motor[TIRE_FL].pwm = 15;
02419             motor[TIRE_FL].dir = BACK;
02420             motor[TIRE_BR].pwm = 15;
02421             motor[TIRE_BR].dir = FOR;
02422             motor[TIRE_BL].pwm = 0;
02423             motor[TIRE_BL].dir = FREE;
02424             if(count>5000&&g[3]==0) {
02425                 count=0;
02426                 mode=52;
02427             }
02428         } else if(mode==52) {
02429             motor[TIRE_FR].pwm = 15;
02430             motor[TIRE_FR].dir = BACK;
02431             motor[TIRE_FL].pwm = 15;
02432             motor[TIRE_FL].dir = FOR;
02433             motor[TIRE_BR].pwm = 15;
02434             motor[TIRE_BR].dir = BACK;
02435             motor[TIRE_BL].pwm = 15;
02436             motor[TIRE_BL].dir = FOR;
02437             if(count>5000&&g[3]==0) {
02438                 count=0;
02439                 cross=0;
02440                 mode=60;
02441             }
02442         } else if(mode==60) {
02443             switch(g[3]) {
02444                 case 98:
02445                     motor[TIRE_FR].pwm = 30;
02446                     motor[TIRE_FR].dir = BACK;
02447                     motor[TIRE_FL].pwm = 30;
02448                     motor[TIRE_FL].dir = BACK;
02449                     motor[TIRE_BR].pwm = 30;
02450                     motor[TIRE_BR].dir = FOR;
02451                     motor[TIRE_BL].pwm = 30;
02452                     motor[TIRE_BL].dir = FOR;
02453                     if(count>50000) {
02454                         cross++;
02455                         count=0;
02456                     }
02457                     mtc2=true;
02458                     if(cross==2) {
02459                         mode=61;
02460                         count=0;
02461                         cross=0;
02462                     }
02463                     break;
02464                 case 0:
02465                     motor[TIRE_FR].pwm = 60;
02466                     motor[TIRE_FR].dir = BACK;
02467                     motor[TIRE_FL].pwm = 60;
02468                     motor[TIRE_FL].dir = BACK;
02469                     motor[TIRE_BR].pwm = 60;
02470                     motor[TIRE_BR].dir = FOR;
02471                     motor[TIRE_BL].pwm = 60;
02472                     motor[TIRE_BL].dir = FOR;
02473                     mtc2=true;
02474                     break;
02475                 case 255:
02476                     motor[TIRE_FR].pwm = 40;
02477                     motor[TIRE_FR].dir = BACK;
02478                     motor[TIRE_FL].pwm = 60;
02479                     motor[TIRE_FL].dir = BACK;
02480                     motor[TIRE_BR].pwm = 60;
02481                     motor[TIRE_BR].dir = FOR;
02482                     motor[TIRE_BL].pwm = 40;
02483                     motor[TIRE_BL].dir = FOR;
02484                     mtc2=true;
02485                     break;
02486                 case 253:
02487                     motor[TIRE_FR].pwm = 0;
02488                     motor[TIRE_FR].dir = FREE;
02489                     motor[TIRE_FL].pwm = 20;
02490                     motor[TIRE_FL].dir = BACK;
02491                     motor[TIRE_BR].pwm = 20;
02492                     motor[TIRE_BR].dir = FOR;
02493                     motor[TIRE_BL].pwm = 0;
02494                     motor[TIRE_BL].dir = FREE;
02495                     mtc2=true;
02496                     break;
02497                 case 254:
02498                     motor[TIRE_FR].pwm = 0;
02499                     motor[TIRE_FR].dir = FREE;
02500                     motor[TIRE_FL].pwm = 30;
02501                     motor[TIRE_FL].dir = BACK;
02502                     motor[TIRE_BR].pwm = 30;
02503                     motor[TIRE_BR].dir = FOR;
02504                     motor[TIRE_BL].pwm = 0;
02505                     motor[TIRE_BL].dir = FREE;
02506                     mtc2=true;
02507                     break;
02508                 case 1:
02509                     motor[TIRE_FR].pwm = 60;
02510                     motor[TIRE_FR].dir = BACK;
02511                     motor[TIRE_FL].pwm = 40;
02512                     motor[TIRE_FL].dir = BACK;
02513                     motor[TIRE_BR].pwm = 40;
02514                     motor[TIRE_BR].dir = FOR;
02515                     motor[TIRE_BL].pwm = 60;
02516                     motor[TIRE_BL].dir = FOR;
02517                     mtc2=true;
02518                     break;
02519                 case 3:
02520                     motor[TIRE_FR].pwm = 20;
02521                     motor[TIRE_FR].dir = BACK;
02522                     motor[TIRE_FL].pwm = 0;
02523                     motor[TIRE_FL].dir = FREE;
02524                     motor[TIRE_BR].pwm = 0;
02525                     motor[TIRE_BR].dir = FREE;
02526                     motor[TIRE_BL].pwm = 20;
02527                     motor[TIRE_BL].dir = FOR;
02528                     mtc2=true;
02529                     break;
02530                 case 2:
02531                     motor[TIRE_FR].pwm = 30;
02532                     motor[TIRE_FR].dir = BACK;
02533                     motor[TIRE_FL].pwm = 0;
02534                     motor[TIRE_FL].dir = FREE;
02535                     motor[TIRE_BR].pwm = 0;
02536                     motor[TIRE_BR].dir = FREE;
02537                     motor[TIRE_BL].pwm = 30;
02538                     motor[TIRE_BL].dir = FOR;
02539                     mtc2=true;
02540                     break;
02541             }
02542             if(g[2]!=98&&mtc2==true) {
02543                 switch(g[2]) {
02544                     case 0:
02545                         mtc2=false;
02546                         break;
02547                     case 255:
02548                         motor[TIRE_FL].pwm += 5;
02549                         mtc2=false;
02550                         break;
02551                     case 253:
02552                         motor[TIRE_FL].pwm += 10;
02553                         mtc2=false;
02554                         break;
02555                     case 254:
02556                         motor[TIRE_FL].pwm += 20;
02557                         motor[TIRE_BL].pwm = 0;
02558                         mtc2=false;
02559                         break;
02560                     case 1:
02561                         motor[TIRE_BL].pwm += 5;
02562                         mtc2=false;
02563                         break;
02564                     case 3:
02565                         motor[TIRE_BL].pwm += 10;
02566                         mtc2=false;
02567                         break;
02568                     case 2:
02569                         motor[TIRE_BL].pwm += 20;
02570                         motor[TIRE_FL].pwm = 0;
02571                         mtc2=false;
02572                         break;
02573                 }
02574             }
02575         } else if(mode==61) {
02576             motor[TIRE_FR].pwm = 0;
02577             motor[TIRE_FR].dir = FREE;
02578             motor[TIRE_FL].pwm = 15;
02579             motor[TIRE_FL].dir = BACK;
02580             motor[TIRE_BR].pwm = 15;
02581             motor[TIRE_BR].dir = FOR;
02582             motor[TIRE_BL].pwm = 0;
02583             motor[TIRE_BL].dir = FREE;
02584             if(count>5000&&g[1]==0) {
02585                 count=0;
02586                 mode=62;
02587             }
02588         } else if(mode==62) {
02589             motor[TIRE_FR].pwm = 15;
02590             motor[TIRE_FR].dir = FOR;
02591             motor[TIRE_FL].pwm = 15;
02592             motor[TIRE_FL].dir = FOR;
02593             motor[TIRE_BR].pwm = 15;
02594             motor[TIRE_BR].dir = BACK;
02595             motor[TIRE_BL].pwm = 15;
02596             motor[TIRE_BL].dir = BACK;
02597             if(count>5000&&g[1]==0) {
02598                 count=0;
02599                 mode=70;
02600             }
02601         } else if(mode==70) {
02602             switch(g[1]) {
02603                 case 99:
02604                     if(count>=100000) {
02605                         mode=100;
02606                     }
02607                     break;
02608                 case 98:
02609                     motor[TIRE_FR].pwm = 30;
02610                     motor[TIRE_FR].dir = BACK;
02611                     motor[TIRE_FL].pwm = 30;
02612                     motor[TIRE_FL].dir = FOR;
02613                     motor[TIRE_BR].pwm = 30;
02614                     motor[TIRE_BR].dir = BACK;
02615                     motor[TIRE_BL].pwm = 30;
02616                     motor[TIRE_BL].dir = FOR;
02617                     break;
02618                 case 0:
02619                     motor[TIRE_FR].pwm = 60;
02620                     motor[TIRE_FR].dir = BACK;
02621                     motor[TIRE_FL].pwm = 60;
02622                     motor[TIRE_FL].dir = FOR;
02623                     motor[TIRE_BR].pwm = 60;
02624                     motor[TIRE_BR].dir = BACK;
02625                     motor[TIRE_BL].pwm = 60;
02626                     motor[TIRE_BL].dir = FOR;
02627                     mtc=true;
02628                     break;
02629                 case 255:
02630                     motor[TIRE_FR].pwm = 60;
02631                     motor[TIRE_FR].dir = BACK;
02632                     motor[TIRE_FL].pwm = 40;
02633                     motor[TIRE_FL].dir = FOR;
02634                     motor[TIRE_BR].pwm = 40;
02635                     motor[TIRE_BR].dir = BACK;
02636                     motor[TIRE_BL].pwm = 60;
02637                     motor[TIRE_BL].dir = FOR;
02638                     mtc=true;
02639                     break;
02640                 case 253:
02641                     motor[TIRE_FR].pwm = 20;
02642                     motor[TIRE_FR].dir = BACK;
02643                     motor[TIRE_FL].pwm = 0;
02644                     motor[TIRE_FL].dir = FREE;
02645                     motor[TIRE_BR].pwm = 0;
02646                     motor[TIRE_BR].dir = FREE;
02647                     motor[TIRE_BL].pwm = 20;
02648                     motor[TIRE_BL].dir = FOR;
02649                     mtc=true;
02650                     break;
02651                 case 254:
02652                     motor[TIRE_FR].pwm = 30;
02653                     motor[TIRE_FR].dir = BACK;
02654                     motor[TIRE_FL].pwm = 0;
02655                     motor[TIRE_FL].dir = FREE;
02656                     motor[TIRE_BR].pwm = 0;
02657                     motor[TIRE_BR].dir = FREE;
02658                     motor[TIRE_BL].pwm = 30;
02659                     motor[TIRE_BL].dir = FOR;
02660                     mtc=true;
02661                     break;
02662                 case 1:
02663                     motor[TIRE_FR].pwm = 40;
02664                     motor[TIRE_FR].dir = BACK;
02665                     motor[TIRE_FL].pwm = 60;
02666                     motor[TIRE_FL].dir = FOR;
02667                     motor[TIRE_BR].pwm = 60;
02668                     motor[TIRE_BR].dir = BACK;
02669                     motor[TIRE_BL].pwm = 40;
02670                     motor[TIRE_BL].dir = FOR;
02671                     mtc=true;
02672                     break;
02673                 case 3:
02674                     motor[TIRE_FR].pwm = 0;
02675                     motor[TIRE_FR].dir = FREE;
02676                     motor[TIRE_FL].pwm = 20;
02677                     motor[TIRE_FL].dir = FOR;
02678                     motor[TIRE_BR].pwm = 20;
02679                     motor[TIRE_BR].dir = BACK;
02680                     motor[TIRE_BL].pwm = 0;
02681                     motor[TIRE_BL].dir = FREE;
02682                     mtc=true;
02683                     break;
02684                 case 2:
02685                     motor[TIRE_FR].pwm = 0;
02686                     motor[TIRE_FR].dir = FREE;
02687                     motor[TIRE_FL].pwm = 30;
02688                     motor[TIRE_FL].dir = FOR;
02689                     motor[TIRE_BR].pwm = 30;
02690                     motor[TIRE_BR].dir = BACK;
02691                     motor[TIRE_BL].pwm = 0;
02692                     motor[TIRE_BL].dir = FREE;
02693                     mtc=true;
02694                     break;
02695             }
02696             if(g[1]!=98&&mtc==true) {
02697                 switch(g[0]) {
02698                     case 0:
02699                         mtc=false;
02700                         break;
02701                     case 255:
02702                         motor[TIRE_FR].pwm += 5;
02703                         mtc=false;
02704                         break;
02705                     case 253:
02706                         motor[TIRE_FR].pwm += 10;
02707                         mtc=false;
02708                         break;
02709                     case 254:
02710                         motor[TIRE_FR].pwm += 20;
02711                         motor[TIRE_FL].pwm = 0;
02712                         mtc=false;
02713                         break;
02714                     case 1:
02715                         motor[TIRE_FL].pwm += 5;
02716                         mtc=false;
02717                         break;
02718                     case 3:
02719                         motor[TIRE_FL].pwm += 10;
02720                         mtc=false;
02721                         break;
02722                     case 2:
02723                         motor[TIRE_FL].pwm += 20;
02724                         motor[TIRE_FR].pwm = 0;
02725                         mtc=false;
02726                         break;
02727                 }
02728             }
02729         } else if(mode==100) {
02730             motor[TIRE_FR].pwm = 20;
02731             motor[TIRE_FR].dir = FOR;
02732             motor[TIRE_FL].pwm = 20;
02733             motor[TIRE_FL].dir = BACK;
02734             motor[TIRE_BR].pwm = 23;
02735             motor[TIRE_BR].dir = FOR;
02736             motor[TIRE_BL].pwm = 20;
02737             motor[TIRE_BL].dir = BACK;
02738         } else {
02739             motor[TIRE_FR].pwm = 0;
02740             motor[TIRE_FR].dir = BRAKE;
02741             motor[TIRE_FL].pwm = 0;
02742             motor[TIRE_FL].dir = BRAKE;
02743             motor[TIRE_BR].pwm = 0;
02744             motor[TIRE_BR].dir = BRAKE;
02745             motor[TIRE_BL].pwm = 0;
02746             motor[TIRE_BL].dir = BRAKE;
02747         }
02748     }
02749 
02750 }
02751 
02752 #endif
02753 
02754 #if USE_PROCESS_NUM>3
02755 static void Process3()
02756 {
02757 
02758 }
02759 #endif
02760 
02761 #if USE_PROCESS_NUM>4
02762 static void Process4()
02763 {
02764 
02765 }
02766 #endif
02767 
02768 #if USE_PROCESS_NUM>5
02769 static void Process5()
02770 {
02771 
02772 }
02773 #endif
02774 
02775 #if USE_PROCESS_NUM>6
02776 static void Process6()
02777 {
02778 
02779 }
02780 #endif
02781 
02782 #if USE_PROCESS_NUM>7
02783 static void Process7()
02784 {
02785 
02786 }
02787 #endif
02788 
02789 #if USE_PROCESS_NUM>8
02790 static void Process8()
02791 {
02792 
02793 }
02794 #endif
02795 
02796 #if USE_PROCESS_NUM>9
02797 static void Process9()
02798 {
02799 
02800 }
02801 #endif
02802 #endif
02803 #pragma endregion PROCESS
02804 
02805 static void AllActuatorReset()
02806 {
02807 
02808 #ifdef USE_SOLENOID
02809     solenoid.all = ALL_SOLENOID_OFF;
02810 #endif
02811 
02812 #ifdef USE_MOTOR
02813     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
02814         motor[i].dir = FREE;
02815         motor[i].pwm = 0;
02816     }
02817 #endif
02818 }
02819 
02820 /*
02821 void BuzzerTimer_func()
02822 {
02823     buzzer = !buzzer;
02824     //LED_DEBUG0 = !LED_DEBUG0;
02825 }
02826 */
02827 void TapeLedEms_func()
02828 {
02829     sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
02830 }
02831 
02832 #pragma region USER-DEFINED-FUNCTIONS
02833 
02834 
02835 #pragma endregion