daad

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 
00269 
00270 #pragma region PROCESS
00271 #ifdef USE_SUBPROCESS
00272 #if USE_PROCESS_NUM>0
00273 static void Process0()
00274 {
00275 
00276 }
00277 #endif
00278 
00279 #if USE_PROCESS_NUM>1
00280 
00281 
00282 bool dz1=true;
00283 bool dz1i=false;
00284 bool dz2=true;
00285 bool dz2i=false;
00286 bool dz3=true;
00287 bool dz3i=false;
00288 bool dz4=true;
00289 bool dz4i=false;
00290 
00291 int mode=10;
00292 int mode1=0;
00293 int mode1g0=100000;
00294 
00295 
00296 static void Process1()
00297 {
00298     mode1g0=100000;
00299     mode=10;
00300     mode1=0;
00301     motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
00302     motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
00303     motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]);
00304     motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);
00305 
00306     motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
00307     motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
00308     motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
00309     motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
00310 
00311     if(controller->Button.UP||controller->Button.DOWN) {
00312 
00313         if(controller->Button.UP) {
00314             motor[MOTOR_0].pwm = 110;
00315             motor[MOTOR_0].dir = FOR;
00316         }
00317         if(controller->Button.DOWN) {
00318             motor[MOTOR_0].pwm = 50;
00319             motor[MOTOR_0].dir = BACK;
00320         }
00321     } else {
00322         motor[MOTOR_0].pwm = 0;
00323         motor[MOTOR_0].dir = BRAKE;
00324     }
00325     if(controller->Button.X) {
00326         if(dz1==true) {
00327             if(dz1i==false) {
00328                 solenoid.solenoid1 = SOLENOID_ON;
00329                 dz1i=true;
00330             } else {
00331                 solenoid.solenoid1 = SOLENOID_OFF;
00332                 dz1i=false;
00333             }
00334             dz1=false;
00335         }
00336     } else {
00337         dz1=true;
00338     }
00339 
00340     if(controller->Button.Y) {
00341         if(dz2==true) {
00342             if(dz2i==false) {
00343                 solenoid.solenoid2 = SOLENOID_ON;
00344                 dz2i=true;
00345             } else {
00346                 solenoid.solenoid2 = SOLENOID_OFF;
00347                 dz2i=false;
00348             }
00349             dz2=false;
00350         }
00351     } else {
00352         dz2=true;
00353     }
00354 
00355     if(controller->Button.A) {
00356         if(dz3==true) {
00357             if(dz3i==false) {
00358                 solenoid.solenoid3 = SOLENOID_ON;
00359                 dz3i=true;
00360             } else {
00361                 solenoid.solenoid3 = SOLENOID_OFF;
00362                 dz3i=false;
00363             }
00364             dz3=false;
00365         }
00366     } else {
00367         dz3=true;
00368     }
00369 
00370     if(controller->Button.B) {
00371         if(dz4==true) {
00372             if(dz4i==false) {
00373                 solenoid.solenoid4 = SOLENOID_ON;
00374                 dz4i=true;
00375             } else {
00376                 solenoid.solenoid4 = SOLENOID_OFF;
00377                 dz4i=false;
00378             }
00379             dz4=false;
00380         }
00381     } else {
00382         dz4=true;
00383     }
00384     /*
00385     if(controller->Button.RIGHT){
00386      motor[MOTOR_1].dir = FOR;
00387      motor[MOTOR_1].pwm = 60;
00388      if (ARM_1){
00389       motor[MOTOR_1].dir = BRAKE;
00390      }
00391     }
00392     else if(controller->Button.LEFT){
00393      motor[MOTOR_1].dir = BACK;
00394      motor[MOTOR_1].pwm = 60;
00395      if (ARM_0){
00396       motor[MOTOR_1].dir = BRAKE;
00397      }
00398     }
00399     */
00400 }
00401 #endif
00402 
00403 #if USE_PROCESS_NUM>2
00404 bool mtc=false;
00405 //追加
00406 bool mtc2 = false;
00407 
00408 static void Process2()
00409 {
00410 //    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]);
00411     mode1g0++;
00412     if(g[0]!=99&&mode==10) {
00413         switch(g[0]) {
00414             case 98:
00415                 motor[TIRE_FR].pwm = 30;
00416                 motor[TIRE_FR].dir = BACK;
00417                 motor[TIRE_FL].pwm = 30;
00418                 motor[TIRE_FL].dir = FOR;
00419                 motor[TIRE_BR].pwm = 30;
00420                 motor[TIRE_BR].dir = BACK;
00421                 motor[TIRE_BL].pwm = 30;
00422                 motor[TIRE_BL].dir = FOR;
00423                 if(mode1g0>100000) {
00424                     mode1++;
00425                     mode1g0=0;
00426                 }
00427                 mtc=true;
00428                 if(mode1==2) {
00429                     mode=11;
00430                     mode1g0=0;
00431                 }
00432                 break;
00433             case 0:
00434                 motor[TIRE_FR].pwm = 60;
00435                 motor[TIRE_FR].dir = BACK;
00436                 motor[TIRE_FL].pwm = 60;
00437                 motor[TIRE_FL].dir = FOR;
00438                 motor[TIRE_BR].pwm = 60;
00439                 motor[TIRE_BR].dir = BACK;
00440                 motor[TIRE_BL].pwm = 60;
00441                 motor[TIRE_BL].dir = FOR;
00442                 mtc=true;
00443                 break;
00444             case 255:
00445                 motor[TIRE_FR].pwm = 60;
00446                 motor[TIRE_FR].dir = BACK;
00447                 motor[TIRE_FL].pwm = 40;
00448                 motor[TIRE_FL].dir = FOR;
00449                 motor[TIRE_BR].pwm = 40;
00450                 motor[TIRE_BR].dir = BACK;
00451                 motor[TIRE_BL].pwm = 60;
00452                 motor[TIRE_BL].dir = FOR;
00453                 mtc=true;
00454                 break;
00455             case 253:
00456                 motor[TIRE_FR].pwm = 20;
00457                 motor[TIRE_FR].dir = BACK;
00458                 motor[TIRE_FL].pwm = 0;
00459                 motor[TIRE_FL].dir = FREE;
00460                 motor[TIRE_BR].pwm = 0;
00461                 motor[TIRE_BR].dir = FREE;
00462                 motor[TIRE_BL].pwm = 20;
00463                 motor[TIRE_BL].dir = FOR;
00464                 mtc=true;
00465                 break;
00466             case 254:
00467                 motor[TIRE_FR].pwm = 30;
00468                 motor[TIRE_FR].dir = BACK;
00469                 motor[TIRE_FL].pwm = 0;
00470                 motor[TIRE_FL].dir = FREE;
00471                 motor[TIRE_BR].pwm = 0;
00472                 motor[TIRE_BR].dir = FREE;
00473                 motor[TIRE_BL].pwm = 30;
00474                 motor[TIRE_BL].dir = FOR;
00475                 mtc=true;
00476                 break;
00477             case 1:
00478                 motor[TIRE_FR].pwm = 40;
00479                 motor[TIRE_FR].dir = BACK;
00480                 motor[TIRE_FL].pwm = 60;
00481                 motor[TIRE_FL].dir = FOR;
00482                 motor[TIRE_BR].pwm = 60;
00483                 motor[TIRE_BR].dir = BACK;
00484                 motor[TIRE_BL].pwm = 40;
00485                 motor[TIRE_BL].dir = FOR;
00486                 mtc=true;
00487                 break;
00488             case 3:
00489                 motor[TIRE_FR].pwm = 0;
00490                 motor[TIRE_FR].dir = FREE;
00491                 motor[TIRE_FL].pwm = 20;
00492                 motor[TIRE_FL].dir = FOR;
00493                 motor[TIRE_BR].pwm = 20;
00494                 motor[TIRE_BR].dir = BACK;
00495                 motor[TIRE_BL].pwm = 0;
00496                 motor[TIRE_BL].dir = FREE;
00497                 mtc=true;
00498                 break;
00499             case 2:
00500                 motor[TIRE_FR].pwm = 0;
00501                 motor[TIRE_FR].dir = FREE;
00502                 motor[TIRE_FL].pwm = 30;
00503                 motor[TIRE_FL].dir = FOR;
00504                 motor[TIRE_BR].pwm = 30;
00505                 motor[TIRE_BR].dir = BACK;
00506                 motor[TIRE_BL].pwm = 0;
00507                 motor[TIRE_BL].dir = FREE;
00508                 mtc=true;
00509                 break;
00510         }
00511         if(g[0]!=98&&mtc==true) {
00512             switch(g[1]) {
00513                 case 0:
00514                     mtc=false;
00515                     break;
00516                 case 255:
00517                     motor[TIRE_BR].pwm += 5;
00518                     mtc=false;
00519                     break;
00520                 case 253:
00521                     motor[TIRE_BR].pwm += 10;
00522                     mtc=false;
00523                     break;
00524                 case 254:
00525                     motor[TIRE_BR].pwm += 20;
00526                     motor[TIRE_BL].pwm = 0;
00527                     mtc=false;
00528                     break;
00529                 case 1:
00530                     motor[TIRE_BL].pwm += 5;
00531                     mtc=false;
00532                     break;
00533                 case 3:
00534                     motor[TIRE_BL].pwm += 10;
00535                     mtc=false;
00536                     break;
00537                 case 2:
00538                     motor[TIRE_BL].pwm += 20;
00539                     motor[TIRE_BR].pwm = 0;
00540                     mtc=false;
00541                     break;
00542             }
00543         }
00544     } else if(mode==11) {
00545         motor[TIRE_FR].pwm = 15;
00546         motor[TIRE_FR].dir = BACK;
00547         motor[TIRE_FL].pwm = 15;
00548         motor[TIRE_FL].dir = FOR;
00549         motor[TIRE_BR].pwm = 15;
00550         motor[TIRE_BR].dir = BACK;
00551         motor[TIRE_BL].pwm = 15;
00552         motor[TIRE_BL].dir = FOR;
00553         if(mode1g0>5000&&g[2]==0) {
00554             mode1g0=0;
00555             mode=12;
00556         }
00557     } else if(mode==12) {
00558         motor[TIRE_FR].pwm = 15;
00559         motor[TIRE_FR].dir = FOR;
00560         motor[TIRE_FL].pwm = 15;
00561         motor[TIRE_FL].dir = BACK;
00562         motor[TIRE_BR].pwm = 15;
00563         motor[TIRE_BR].dir = FOR;
00564         motor[TIRE_BL].pwm = 15;
00565         motor[TIRE_BL].dir = BACK;
00566         mode1g0++;
00567         if(mode1g0>10000&&g[2]==0) {
00568             mode1g0=0;
00569             mode1=0;
00570             mode=20;
00571         }
00572     } else if(mode==20) {
00573         switch(g[2]) {
00574             case 98:
00575                 motor[TIRE_FR].pwm = 30;
00576                 motor[TIRE_FR].dir = FOR;
00577                 motor[TIRE_FL].pwm = 30;
00578                 motor[TIRE_FL].dir = FOR;
00579                 motor[TIRE_BR].pwm = 30;
00580                 motor[TIRE_BR].dir = BACK;
00581                 motor[TIRE_BL].pwm = 30;
00582                 motor[TIRE_BL].dir = BACK;
00583                 if(mode1g0>100000) {
00584                     mode1++;
00585                     mode1g0=0;
00586                 }
00587                 mtc2=true;
00588                 if(mode1==3) {
00589                     mode=21;
00590                     mode1g0=0;
00591                     mode1=0;
00592                 }
00593                 break;
00594             case 0:
00595                 motor[TIRE_FR].pwm = 60;
00596                 motor[TIRE_FR].dir = FOR;
00597                 motor[TIRE_FL].pwm = 60;
00598                 motor[TIRE_FL].dir = FOR;
00599                 motor[TIRE_BR].pwm = 60;
00600                 motor[TIRE_BR].dir = BACK;
00601                 motor[TIRE_BL].pwm = 60;
00602                 motor[TIRE_BL].dir = BACK;
00603                 mtc2=true;
00604                 break;
00605             case 255:
00606                 motor[TIRE_FR].pwm = 40;
00607                 motor[TIRE_FR].dir = FOR;
00608                 motor[TIRE_FL].pwm = 60;
00609                 motor[TIRE_FL].dir = FOR;
00610                 motor[TIRE_BR].pwm = 60;
00611                 motor[TIRE_BR].dir = BACK;
00612                 motor[TIRE_BL].pwm = 40;
00613                 motor[TIRE_BL].dir = BACK;
00614                 mtc2=true;
00615                 break;
00616             case 253:
00617                 motor[TIRE_FR].pwm = 0;
00618                 motor[TIRE_FR].dir = FREE;
00619                 motor[TIRE_FL].pwm = 20;
00620                 motor[TIRE_FL].dir = FOR;
00621                 motor[TIRE_BR].pwm = 20;
00622                 motor[TIRE_BR].dir = BACK;
00623                 motor[TIRE_BL].pwm = 0;
00624                 motor[TIRE_BL].dir = FREE;
00625                 mtc2=true;
00626                 break;
00627             case 254:
00628                 motor[TIRE_FR].pwm = 0;
00629                 motor[TIRE_FR].dir = FREE;
00630                 motor[TIRE_FL].pwm = 30;
00631                 motor[TIRE_FL].dir = FOR;
00632                 motor[TIRE_BR].pwm = 30;
00633                 motor[TIRE_BR].dir = BACK;
00634                 motor[TIRE_BL].pwm = 0;
00635                 motor[TIRE_BL].dir = FREE;
00636                 mtc2=true;
00637                 break;
00638             case 1:
00639                 motor[TIRE_FR].pwm = 60;
00640                 motor[TIRE_FR].dir = FOR;
00641                 motor[TIRE_FL].pwm = 40;
00642                 motor[TIRE_FL].dir = FOR;
00643                 motor[TIRE_BR].pwm = 40;
00644                 motor[TIRE_BR].dir = BACK;
00645                 motor[TIRE_BL].pwm = 60;
00646                 motor[TIRE_BL].dir = BACK;
00647                 mtc2=true;
00648                 break;
00649             case 3:
00650                 motor[TIRE_FR].pwm = 20;
00651                 motor[TIRE_FR].dir = FOR;
00652                 motor[TIRE_FL].pwm = 0;
00653                 motor[TIRE_FL].dir = FREE;
00654                 motor[TIRE_BR].pwm = 0;
00655                 motor[TIRE_BR].dir = FREE;
00656                 motor[TIRE_BL].pwm = 20;
00657                 motor[TIRE_BL].dir = BACK;
00658                 mtc2=true;
00659                 break;
00660             case 2:
00661                 motor[TIRE_FR].pwm = 30;
00662                 motor[TIRE_FR].dir = FOR;
00663                 motor[TIRE_FL].pwm = 0;
00664                 motor[TIRE_FL].dir = FREE;
00665                 motor[TIRE_BR].pwm = 0;
00666                 motor[TIRE_BR].dir = FREE;
00667                 motor[TIRE_BL].pwm = 30;
00668                 motor[TIRE_BL].dir = BACK;
00669                 mtc2=true;
00670                 break;
00671         }
00672         if(g[2]!=98&&mtc2==true) {
00673             switch(g[3]) {
00674                 case 0:
00675                     mtc2=false;
00676                     break;
00677                 case 255:
00678                     motor[TIRE_BL].pwm += 5;
00679                     mtc2=false;
00680                     break;
00681                 case 253:
00682                     motor[TIRE_BL].pwm += 10;
00683                     mtc2=false;
00684                     break;
00685                 case 254:
00686                     motor[TIRE_BL].pwm += 20;
00687                     motor[TIRE_FL].pwm = 0;
00688                     mtc2=false;
00689                     break;
00690                 case 1:
00691                     motor[TIRE_FL].pwm += 5;
00692                     mtc2=false;
00693                     break;
00694                 case 3:
00695                     motor[TIRE_FL].pwm += 10;
00696                     mtc2=false;
00697                     break;
00698                 case 2:
00699                     motor[TIRE_FL].pwm += 20;
00700                     motor[TIRE_BL].pwm = 0;
00701                     mtc2=false;
00702                     break;
00703             }
00704         }
00705     } else if(mode==21) {
00706         motor[TIRE_FR].pwm = 15;
00707         motor[TIRE_FR].dir = FOR;
00708         motor[TIRE_FL].pwm = 15;
00709         motor[TIRE_FL].dir = FOR;
00710         motor[TIRE_BR].pwm = 15;
00711         motor[TIRE_BR].dir = BACK;
00712         motor[TIRE_BL].pwm = 15;
00713         motor[TIRE_BL].dir = BACK;
00714         if(mode1g0>5000&&g[0]==0) {
00715             mode1g0=0;
00716             mode=22;
00717         }
00718     } else if(mode==22) {
00719         motor[TIRE_FR].pwm = 15;
00720         motor[TIRE_FR].dir = BACK;
00721         motor[TIRE_FL].pwm = 15;
00722         motor[TIRE_FL].dir = BACK;
00723         motor[TIRE_BR].pwm = 15;
00724         motor[TIRE_BR].dir = FOR;
00725         motor[TIRE_BL].pwm = 15;
00726         motor[TIRE_BL].dir = FOR;
00727         if(mode1g0>10000&&g[0]==0) {
00728             mode1g0=0;
00729             mode1=0;
00730             mode=30;
00731         }
00732     } else if(mode==30) {
00733         switch(g[0]) {
00734             case 98:
00735                 motor[TIRE_FR].pwm = 30;
00736                 motor[TIRE_FR].dir = BACK;
00737                 motor[TIRE_FL].pwm = 30;
00738                 motor[TIRE_FL].dir = FOR;
00739                 motor[TIRE_BR].pwm = 30;
00740                 motor[TIRE_BR].dir = BACK;
00741                 motor[TIRE_BL].pwm = 30;
00742                 motor[TIRE_BL].dir = FOR;
00743                 if(mode1g0>100000) {
00744                     mode1++;
00745                     mode1g0=0;
00746                 }
00747                 mtc=true;
00748                 if(mode1==1) {
00749                     mode=31;
00750                     mode1g0=0;
00751                 }
00752                 break;
00753             case 0:
00754                 motor[TIRE_FR].pwm = 60;
00755                 motor[TIRE_FR].dir = BACK;
00756                 motor[TIRE_FL].pwm = 60;
00757                 motor[TIRE_FL].dir = FOR;
00758                 motor[TIRE_BR].pwm = 60;
00759                 motor[TIRE_BR].dir = BACK;
00760                 motor[TIRE_BL].pwm = 60;
00761                 motor[TIRE_BL].dir = FOR;
00762                 mtc=true;
00763                 break;
00764             case 255:
00765                 motor[TIRE_FR].pwm = 60;
00766                 motor[TIRE_FR].dir = BACK;
00767                 motor[TIRE_FL].pwm = 40;
00768                 motor[TIRE_FL].dir = FOR;
00769                 motor[TIRE_BR].pwm = 40;
00770                 motor[TIRE_BR].dir = BACK;
00771                 motor[TIRE_BL].pwm = 60;
00772                 motor[TIRE_BL].dir = FOR;
00773                 mtc=true;
00774                 break;
00775             case 253:
00776                 motor[TIRE_FR].pwm = 20;
00777                 motor[TIRE_FR].dir = BACK;
00778                 motor[TIRE_FL].pwm = 0;
00779                 motor[TIRE_FL].dir = FREE;
00780                 motor[TIRE_BR].pwm = 0;
00781                 motor[TIRE_BR].dir = FREE;
00782                 motor[TIRE_BL].pwm = 20;
00783                 motor[TIRE_BL].dir = FOR;
00784                 mtc=true;
00785                 break;
00786             case 254:
00787                 motor[TIRE_FR].pwm = 30;
00788                 motor[TIRE_FR].dir = BACK;
00789                 motor[TIRE_FL].pwm = 0;
00790                 motor[TIRE_FL].dir = FREE;
00791                 motor[TIRE_BR].pwm = 0;
00792                 motor[TIRE_BR].dir = FREE;
00793                 motor[TIRE_BL].pwm = 30;
00794                 motor[TIRE_BL].dir = FOR;
00795                 mtc=true;
00796                 break;
00797             case 1:
00798                 motor[TIRE_FR].pwm = 40;
00799                 motor[TIRE_FR].dir = BACK;
00800                 motor[TIRE_FL].pwm = 60;
00801                 motor[TIRE_FL].dir = FOR;
00802                 motor[TIRE_BR].pwm = 60;
00803                 motor[TIRE_BR].dir = BACK;
00804                 motor[TIRE_BL].pwm = 40;
00805                 motor[TIRE_BL].dir = FOR;
00806                 mtc=true;
00807                 break;
00808             case 3:
00809                 motor[TIRE_FR].pwm = 0;
00810                 motor[TIRE_FR].dir = FREE;
00811                 motor[TIRE_FL].pwm = 20;
00812                 motor[TIRE_FL].dir = FOR;
00813                 motor[TIRE_BR].pwm = 20;
00814                 motor[TIRE_BR].dir = BACK;
00815                 motor[TIRE_BL].pwm = 0;
00816                 motor[TIRE_BL].dir = FREE;
00817                 mtc=true;
00818                 break;
00819             case 2:
00820                 motor[TIRE_FR].pwm = 0;
00821                 motor[TIRE_FR].dir = FREE;
00822                 motor[TIRE_FL].pwm = 30;
00823                 motor[TIRE_FL].dir = FOR;
00824                 motor[TIRE_BR].pwm = 30;
00825                 motor[TIRE_BR].dir = BACK;
00826                 motor[TIRE_BL].pwm = 0;
00827                 motor[TIRE_BL].dir = FREE;
00828                 mtc=true;
00829                 break;
00830         }
00831         if(g[0]!=98&&mtc==true) {
00832             switch(g[1]) {
00833                 case 0:
00834                     mtc=false;
00835                     break;
00836                 case 255:
00837                     motor[TIRE_BR].pwm += 5;
00838                     mtc=false;
00839                     break;
00840                 case 253:
00841                     motor[TIRE_BR].pwm += 10;
00842                     mtc=false;
00843                     break;
00844                 case 254:
00845                     motor[TIRE_BR].pwm += 20;
00846                     motor[TIRE_BL].pwm = 0;
00847                     mtc=false;
00848                     break;
00849                 case 1:
00850                     motor[TIRE_BL].pwm += 5;
00851                     mtc=false;
00852                     break;
00853                 case 3:
00854                     motor[TIRE_BL].pwm += 10;
00855                     mtc=false;
00856                     break;
00857                 case 2:
00858                     motor[TIRE_BL].pwm += 20;
00859                     motor[TIRE_BR].pwm = 0;
00860                     mtc=false;
00861                     break;
00862             }
00863         }
00864     } else if(mode==31) {
00865         motor[TIRE_FR].pwm = 15;
00866         motor[TIRE_FR].dir = BACK;
00867         motor[TIRE_FL].pwm = 15;
00868         motor[TIRE_FL].dir = FOR;
00869         motor[TIRE_BR].pwm = 15;
00870         motor[TIRE_BR].dir = BACK;
00871         motor[TIRE_BL].pwm = 15;
00872         motor[TIRE_BL].dir = FOR;
00873         mode1g0++;
00874         if(mode1g0>5000&&g[3]==0) {
00875             mode1g0=0;
00876             mode=32;
00877         }
00878     } else if(mode==32) {
00879         motor[TIRE_FR].pwm = 15;
00880         motor[TIRE_FR].dir = FOR;
00881         motor[TIRE_FL].pwm = 15;
00882         motor[TIRE_FL].dir = BACK;
00883         motor[TIRE_BR].pwm = 15;
00884         motor[TIRE_BR].dir = FOR;
00885         motor[TIRE_BL].pwm = 15;
00886         motor[TIRE_BL].dir = BACK;
00887         mode1g0++;
00888         if(mode1g0>5000&&g[3]==0) {
00889             mode1g0=0;
00890             mode1=0;
00891             mode=40;
00892         }
00893     } else if(mode==40) {
00894         switch(g[3]) {
00895             case 98:
00896                 motor[TIRE_FR].pwm = 30;
00897                 motor[TIRE_FR].dir = BACK;
00898                 motor[TIRE_FL].pwm = 30;
00899                 motor[TIRE_FL].dir = BACK;
00900                 motor[TIRE_BR].pwm = 30;
00901                 motor[TIRE_BR].dir = FOR;
00902                 motor[TIRE_BL].pwm = 30;
00903                 motor[TIRE_BL].dir = FOR;
00904                 if(mode1g0>100000) {
00905                     mode1++;
00906                     mode1g0=0;
00907                 }
00908                 mtc2=true;
00909                 if(mode1==1) {
00910                     mode=41;
00911                     mode1g0=0;
00912                     mode1=0;
00913                 }
00914                 break;
00915             case 0:
00916                 motor[TIRE_FR].pwm = 60;
00917                 motor[TIRE_FR].dir = BACK;
00918                 motor[TIRE_FL].pwm = 60;
00919                 motor[TIRE_FL].dir = BACK;
00920                 motor[TIRE_BR].pwm = 60;
00921                 motor[TIRE_BR].dir = FOR;
00922                 motor[TIRE_BL].pwm = 60;
00923                 motor[TIRE_BL].dir = FOR;
00924                 mtc2=true;
00925                 break;
00926             case 255:
00927                 motor[TIRE_FR].pwm = 40;
00928                 motor[TIRE_FR].dir = BACK;
00929                 motor[TIRE_FL].pwm = 60;
00930                 motor[TIRE_FL].dir = BACK;
00931                 motor[TIRE_BR].pwm = 60;
00932                 motor[TIRE_BR].dir = FOR;
00933                 motor[TIRE_BL].pwm = 40;
00934                 motor[TIRE_BL].dir = FOR;
00935                 mtc2=true;
00936                 break;
00937             case 253:
00938                 motor[TIRE_FR].pwm = 0;
00939                 motor[TIRE_FR].dir = FREE;
00940                 motor[TIRE_FL].pwm = 20;
00941                 motor[TIRE_FL].dir = BACK;
00942                 motor[TIRE_BR].pwm = 20;
00943                 motor[TIRE_BR].dir = FOR;
00944                 motor[TIRE_BL].pwm = 0;
00945                 motor[TIRE_BL].dir = FREE;
00946                 mtc2=true;
00947                 break;
00948             case 254:
00949                 motor[TIRE_FR].pwm = 0;
00950                 motor[TIRE_FR].dir = FREE;
00951                 motor[TIRE_FL].pwm = 30;
00952                 motor[TIRE_FL].dir = BACK;
00953                 motor[TIRE_BR].pwm = 30;
00954                 motor[TIRE_BR].dir = FOR;
00955                 motor[TIRE_BL].pwm = 0;
00956                 motor[TIRE_BL].dir = FREE;
00957                 mtc2=true;
00958                 break;
00959             case 1:
00960                 motor[TIRE_FR].pwm = 60;
00961                 motor[TIRE_FR].dir = BACK;
00962                 motor[TIRE_FL].pwm = 40;
00963                 motor[TIRE_FL].dir = BACK;
00964                 motor[TIRE_BR].pwm = 40;
00965                 motor[TIRE_BR].dir = FOR;
00966                 motor[TIRE_BL].pwm = 60;
00967                 motor[TIRE_BL].dir = FOR;
00968                 mtc2=true;
00969                 break;
00970             case 3:
00971                 motor[TIRE_FR].pwm = 20;
00972                 motor[TIRE_FR].dir = BACK;
00973                 motor[TIRE_FL].pwm = 0;
00974                 motor[TIRE_FL].dir = FREE;
00975                 motor[TIRE_BR].pwm = 0;
00976                 motor[TIRE_BR].dir = FREE;
00977                 motor[TIRE_BL].pwm = 20;
00978                 motor[TIRE_BL].dir = FOR;
00979                 mtc2=true;
00980                 break;
00981             case 2:
00982                 motor[TIRE_FR].pwm = 30;
00983                 motor[TIRE_FR].dir = BACK;
00984                 motor[TIRE_FL].pwm = 0;
00985                 motor[TIRE_FL].dir = FREE;
00986                 motor[TIRE_BR].pwm = 0;
00987                 motor[TIRE_BR].dir = FREE;
00988                 motor[TIRE_BL].pwm = 30;
00989                 motor[TIRE_BL].dir = FOR;
00990                 mtc2=true;
00991                 break;
00992         }
00993         if(g[2]!=98&&mtc2==true) {
00994             switch(g[2]) {
00995                 case 0:
00996                     mtc2=false;
00997                     break;
00998                 case 255:
00999                     motor[TIRE_FL].pwm += 5;
01000                     mtc2=false;
01001                     break;
01002                 case 253:
01003                     motor[TIRE_FL].pwm += 10;
01004                     mtc2=false;
01005                     break;
01006                 case 254:
01007                     motor[TIRE_FL].pwm += 20;
01008                     motor[TIRE_BL].pwm = 0;
01009                     mtc2=false;
01010                     break;
01011                 case 1:
01012                     motor[TIRE_BL].pwm += 5;
01013                     mtc2=false;
01014                     break;
01015                 case 3:
01016                     motor[TIRE_BL].pwm += 10;
01017                     mtc2=false;
01018                     break;
01019                 case 2:
01020                     motor[TIRE_BL].pwm += 20;
01021                     motor[TIRE_FL].pwm = 0;
01022                     mtc2=false;
01023                     break;
01024             }
01025         }
01026     } else if(mode==41) {
01027         motor[TIRE_FR].pwm = 15;
01028         motor[TIRE_FR].dir = BACK;
01029         motor[TIRE_FL].pwm = 15;
01030         motor[TIRE_FL].dir = BACK;
01031         motor[TIRE_BR].pwm = 15;
01032         motor[TIRE_BR].dir = FOR;
01033         motor[TIRE_BL].pwm = 15;
01034         motor[TIRE_BL].dir = FOR;
01035         if(mode1g0>5000&&g[1]==0) {
01036             mode1g0=0;
01037             mode=42;
01038         }
01039     } else if(mode==42) {
01040         motor[TIRE_FR].pwm = 15;
01041         motor[TIRE_FR].dir = FOR;
01042         motor[TIRE_FL].pwm = 15;
01043         motor[TIRE_FL].dir = FOR;
01044         motor[TIRE_BR].pwm = 15;
01045         motor[TIRE_BR].dir = BACK;
01046         motor[TIRE_BL].pwm = 15;
01047         motor[TIRE_BL].dir = BACK;
01048         if(mode1g0>5000&&g[1]==0) {
01049             mode1g0=0;
01050             mode=50;
01051         }
01052     } else if(mode==50) {
01053         switch(g[1]) {
01054             case 98:
01055                 motor[TIRE_FR].pwm = 30;
01056                 motor[TIRE_FR].dir = FOR;
01057                 motor[TIRE_FL].pwm = 30;
01058                 motor[TIRE_FL].dir = BACK;
01059                 motor[TIRE_BR].pwm = 30;
01060                 motor[TIRE_BR].dir = FOR;
01061                 motor[TIRE_BL].pwm = 30;
01062                 motor[TIRE_BL].dir = BACK;
01063                 if(mode1g0>100000) {
01064                     mode1++;
01065                     mode1g0=0;
01066                 }
01067                 mtc=true;
01068                 if(mode1==1) {
01069                     mode=51;
01070                     mode1g0=0;
01071                 }
01072                 break;
01073             case 0:
01074                 motor[TIRE_FR].pwm = 60;
01075                 motor[TIRE_FR].dir = FOR;
01076                 motor[TIRE_FL].pwm = 60;
01077                 motor[TIRE_FL].dir = BACK;
01078                 motor[TIRE_BR].pwm = 60;
01079                 motor[TIRE_BR].dir = FOR;
01080                 motor[TIRE_BL].pwm = 60;
01081                 motor[TIRE_BL].dir = BACK;
01082                 mtc=true;
01083                 break;
01084             case 255:
01085                 motor[TIRE_FR].pwm = 60;
01086                 motor[TIRE_FR].dir = FOR;
01087                 motor[TIRE_FL].pwm = 40;
01088                 motor[TIRE_FL].dir = BACK;
01089                 motor[TIRE_BR].pwm = 40;
01090                 motor[TIRE_BR].dir = FOR;
01091                 motor[TIRE_BL].pwm = 60;
01092                 motor[TIRE_BL].dir = BACK;
01093                 mtc=true;
01094                 break;
01095             case 253:
01096                 motor[TIRE_FR].pwm = 20;
01097                 motor[TIRE_FR].dir = FOR;
01098                 motor[TIRE_FL].pwm = 0;
01099                 motor[TIRE_FL].dir = FREE;
01100                 motor[TIRE_BR].pwm = 0;
01101                 motor[TIRE_BR].dir = FREE;
01102                 motor[TIRE_BL].pwm = 20;
01103                 motor[TIRE_BL].dir = BACK;
01104                 mtc=true;
01105                 break;
01106             case 254:
01107                 motor[TIRE_FR].pwm = 30;
01108                 motor[TIRE_FR].dir = FOR;
01109                 motor[TIRE_FL].pwm = 0;
01110                 motor[TIRE_FL].dir = FREE;
01111                 motor[TIRE_BR].pwm = 0;
01112                 motor[TIRE_BR].dir = FREE;
01113                 motor[TIRE_BL].pwm = 30;
01114                 motor[TIRE_BL].dir = BACK;
01115                 mtc=true;
01116                 break;
01117             case 1:
01118                 motor[TIRE_FR].pwm = 40;
01119                 motor[TIRE_FR].dir = FOR;
01120                 motor[TIRE_FL].pwm = 60;
01121                 motor[TIRE_FL].dir = BACK;
01122                 motor[TIRE_BR].pwm = 60;
01123                 motor[TIRE_BR].dir = FOR;
01124                 motor[TIRE_BL].pwm = 40;
01125                 motor[TIRE_BL].dir = BACK;
01126                 mtc=true;
01127                 break;
01128             case 3:
01129                 motor[TIRE_FR].pwm = 0;
01130                 motor[TIRE_FR].dir = FREE;
01131                 motor[TIRE_FL].pwm = 20;
01132                 motor[TIRE_FL].dir = BACK;
01133                 motor[TIRE_BR].pwm = 20;
01134                 motor[TIRE_BR].dir = FOR;
01135                 motor[TIRE_BL].pwm = 0;
01136                 motor[TIRE_BL].dir = FREE;
01137                 mtc=true;
01138                 break;
01139             case 2:
01140                 motor[TIRE_FR].pwm = 0;
01141                 motor[TIRE_FR].dir = FREE;
01142                 motor[TIRE_FL].pwm = 30;
01143                 motor[TIRE_FL].dir = BACK;
01144                 motor[TIRE_BR].pwm = 30;
01145                 motor[TIRE_BR].dir = FOR;
01146                 motor[TIRE_BL].pwm = 0;
01147                 motor[TIRE_BL].dir = FREE;
01148                 mtc=true;
01149                 break;
01150         }
01151         if(g[1]!=98&&mtc==true) {
01152             switch(g[0]) {
01153                 case 0:
01154                     mtc=false;
01155                     break;
01156                 case 255:
01157                     motor[TIRE_FR].pwm += 5;
01158                     mtc=false;
01159                     break;
01160                 case 253:
01161                     motor[TIRE_FR].pwm += 10;
01162                     mtc=false;
01163                     break;
01164                 case 254:
01165                     motor[TIRE_FR].pwm += 20;
01166                     motor[TIRE_FL].pwm = 0;
01167                     mtc=false;
01168                     break;
01169                 case 1:
01170                     motor[TIRE_FL].pwm += 5;
01171                     mtc=false;
01172                     break;
01173                 case 3:
01174                     motor[TIRE_FL].pwm += 10;
01175                     mtc=false;
01176                     break;
01177                 case 2:
01178                     motor[TIRE_FL].pwm += 20;
01179                     motor[TIRE_FR].pwm = 0;
01180                     mtc=false;
01181                     break;
01182             }
01183         }
01184     } else if(mode==51) {
01185         motor[TIRE_FR].pwm = 15;
01186         motor[TIRE_FR].dir = FOR;
01187         motor[TIRE_FL].pwm = 15;
01188         motor[TIRE_FL].dir = BACK;
01189         motor[TIRE_BR].pwm = 15;
01190         motor[TIRE_BR].dir = FOR;
01191         motor[TIRE_BL].pwm = 15;
01192         motor[TIRE_BL].dir = BACK;
01193         mode1g0++;
01194         if(mode1g0>5000&&g[3]==0) {
01195             mode1g0=0;
01196             mode=52;
01197         }
01198     } else if(mode==52) {
01199         motor[TIRE_FR].pwm = 15;
01200         motor[TIRE_FR].dir = BACK;
01201         motor[TIRE_FL].pwm = 15;
01202         motor[TIRE_FL].dir = FOR;
01203         motor[TIRE_BR].pwm = 15;
01204         motor[TIRE_BR].dir = BACK;
01205         motor[TIRE_BL].pwm = 15;
01206         motor[TIRE_BL].dir = FOR;
01207         mode1g0++;
01208         if(mode1g0>5000&&g[3]==0) {
01209             mode1g0=0;
01210             mode1=0;
01211             mode=60;
01212         }
01213     } else if(mode==60) {
01214         switch(g[3]) {
01215             case 98:
01216                 motor[TIRE_FR].pwm = 30;
01217                 motor[TIRE_FR].dir = BACK;
01218                 motor[TIRE_FL].pwm = 30;
01219                 motor[TIRE_FL].dir = BACK;
01220                 motor[TIRE_BR].pwm = 30;
01221                 motor[TIRE_BR].dir = FOR;
01222                 motor[TIRE_BL].pwm = 30;
01223                 motor[TIRE_BL].dir = FOR;
01224                 if(mode1g0>100000) {
01225                     mode1++;
01226                     mode1g0=0;
01227                 }
01228                 mtc2=true;
01229                 if(mode1==1) {
01230                     mode=61;
01231                     mode1g0=0;
01232                     mode1=0;
01233                 }
01234                 break;
01235             case 0:
01236                 motor[TIRE_FR].pwm = 60;
01237                 motor[TIRE_FR].dir = BACK;
01238                 motor[TIRE_FL].pwm = 60;
01239                 motor[TIRE_FL].dir = BACK;
01240                 motor[TIRE_BR].pwm = 60;
01241                 motor[TIRE_BR].dir = FOR;
01242                 motor[TIRE_BL].pwm = 60;
01243                 motor[TIRE_BL].dir = FOR;
01244                 mtc2=true;
01245                 break;
01246             case 255:
01247                 motor[TIRE_FR].pwm = 40;
01248                 motor[TIRE_FR].dir = BACK;
01249                 motor[TIRE_FL].pwm = 60;
01250                 motor[TIRE_FL].dir = BACK;
01251                 motor[TIRE_BR].pwm = 60;
01252                 motor[TIRE_BR].dir = FOR;
01253                 motor[TIRE_BL].pwm = 40;
01254                 motor[TIRE_BL].dir = FOR;
01255                 mtc2=true;
01256                 break;
01257             case 253:
01258                 motor[TIRE_FR].pwm = 0;
01259                 motor[TIRE_FR].dir = FREE;
01260                 motor[TIRE_FL].pwm = 20;
01261                 motor[TIRE_FL].dir = BACK;
01262                 motor[TIRE_BR].pwm = 20;
01263                 motor[TIRE_BR].dir = FOR;
01264                 motor[TIRE_BL].pwm = 0;
01265                 motor[TIRE_BL].dir = FREE;
01266                 mtc2=true;
01267                 break;
01268             case 254:
01269                 motor[TIRE_FR].pwm = 0;
01270                 motor[TIRE_FR].dir = FREE;
01271                 motor[TIRE_FL].pwm = 30;
01272                 motor[TIRE_FL].dir = BACK;
01273                 motor[TIRE_BR].pwm = 30;
01274                 motor[TIRE_BR].dir = FOR;
01275                 motor[TIRE_BL].pwm = 0;
01276                 motor[TIRE_BL].dir = FREE;
01277                 mtc2=true;
01278                 break;
01279             case 1:
01280                 motor[TIRE_FR].pwm = 60;
01281                 motor[TIRE_FR].dir = BACK;
01282                 motor[TIRE_FL].pwm = 40;
01283                 motor[TIRE_FL].dir = BACK;
01284                 motor[TIRE_BR].pwm = 40;
01285                 motor[TIRE_BR].dir = FOR;
01286                 motor[TIRE_BL].pwm = 60;
01287                 motor[TIRE_BL].dir = FOR;
01288                 mtc2=true;
01289                 break;
01290             case 3:
01291                 motor[TIRE_FR].pwm = 20;
01292                 motor[TIRE_FR].dir = BACK;
01293                 motor[TIRE_FL].pwm = 0;
01294                 motor[TIRE_FL].dir = FREE;
01295                 motor[TIRE_BR].pwm = 0;
01296                 motor[TIRE_BR].dir = FREE;
01297                 motor[TIRE_BL].pwm = 20;
01298                 motor[TIRE_BL].dir = FOR;
01299                 mtc2=true;
01300                 break;
01301             case 2:
01302                 motor[TIRE_FR].pwm = 30;
01303                 motor[TIRE_FR].dir = BACK;
01304                 motor[TIRE_FL].pwm = 0;
01305                 motor[TIRE_FL].dir = FREE;
01306                 motor[TIRE_BR].pwm = 0;
01307                 motor[TIRE_BR].dir = FREE;
01308                 motor[TIRE_BL].pwm = 30;
01309                 motor[TIRE_BL].dir = FOR;
01310                 mtc2=true;
01311                 break;
01312         }
01313         if(g[2]!=98&&mtc2==true) {
01314             switch(g[2]) {
01315                 case 0:
01316                     mtc2=false;
01317                     break;
01318                 case 255:
01319                     motor[TIRE_FL].pwm += 5;
01320                     mtc2=false;
01321                     break;
01322                 case 253:
01323                     motor[TIRE_FL].pwm += 10;
01324                     mtc2=false;
01325                     break;
01326                 case 254:
01327                     motor[TIRE_FL].pwm += 20;
01328                     motor[TIRE_BL].pwm = 0;
01329                     mtc2=false;
01330                     break;
01331                 case 1:
01332                     motor[TIRE_BL].pwm += 5;
01333                     mtc2=false;
01334                     break;
01335                 case 3:
01336                     motor[TIRE_BL].pwm += 10;
01337                     mtc2=false;
01338                     break;
01339                 case 2:
01340                     motor[TIRE_BL].pwm += 20;
01341                     motor[TIRE_FL].pwm = 0;
01342                     mtc2=false;
01343                     break;
01344             }
01345         }
01346     } else if(mode==61) {
01347         motor[TIRE_FR].pwm = 15;
01348         motor[TIRE_FR].dir = BACK;
01349         motor[TIRE_FL].pwm = 15;
01350         motor[TIRE_FL].dir = BACK;
01351         motor[TIRE_BR].pwm = 15;
01352         motor[TIRE_BR].dir = FOR;
01353         motor[TIRE_BL].pwm = 15;
01354         motor[TIRE_BL].dir = FOR;
01355         if(mode1g0>5000&&g[1]==0) {
01356             mode1g0=0;
01357             mode=62;
01358         }
01359     } else if(mode==62) {
01360         motor[TIRE_FR].pwm = 15;
01361         motor[TIRE_FR].dir = FOR;
01362         motor[TIRE_FL].pwm = 15;
01363         motor[TIRE_FL].dir = FOR;
01364         motor[TIRE_BR].pwm = 15;
01365         motor[TIRE_BR].dir = BACK;
01366         motor[TIRE_BL].pwm = 15;
01367         motor[TIRE_BL].dir = BACK;
01368         if(mode1g0>5000&&g[1]==0) {
01369             mode1g0=0;
01370             mode=70;
01371         }
01372     } else if(mode==70) {
01373         switch(g[1]) {
01374             case 98:
01375                 motor[TIRE_FR].pwm = 30;
01376                 motor[TIRE_FR].dir = FOR;
01377                 motor[TIRE_FL].pwm = 30;
01378                 motor[TIRE_FL].dir = BACK;
01379                 motor[TIRE_BR].pwm = 30;
01380                 motor[TIRE_BR].dir = FOR;
01381                 motor[TIRE_BL].pwm = 30;
01382                 motor[TIRE_BL].dir = BACK;
01383                 if(mode1g0>100000) {
01384                     mode1++;
01385                     mode1g0=0;
01386                 }
01387                 mtc=true;
01388                 if(mode1==1) {
01389                     mode=71;
01390                     mode1g0=0;
01391                 }
01392                 break;
01393             case 0:
01394                 motor[TIRE_FR].pwm = 60;
01395                 motor[TIRE_FR].dir = FOR;
01396                 motor[TIRE_FL].pwm = 60;
01397                 motor[TIRE_FL].dir = BACK;
01398                 motor[TIRE_BR].pwm = 60;
01399                 motor[TIRE_BR].dir = FOR;
01400                 motor[TIRE_BL].pwm = 60;
01401                 motor[TIRE_BL].dir = BACK;
01402                 mtc=true;
01403                 break;
01404             case 255:
01405                 motor[TIRE_FR].pwm = 60;
01406                 motor[TIRE_FR].dir = FOR;
01407                 motor[TIRE_FL].pwm = 40;
01408                 motor[TIRE_FL].dir = BACK;
01409                 motor[TIRE_BR].pwm = 40;
01410                 motor[TIRE_BR].dir = FOR;
01411                 motor[TIRE_BL].pwm = 60;
01412                 motor[TIRE_BL].dir = BACK;
01413                 mtc=true;
01414                 break;
01415             case 253:
01416                 motor[TIRE_FR].pwm = 20;
01417                 motor[TIRE_FR].dir = FOR;
01418                 motor[TIRE_FL].pwm = 0;
01419                 motor[TIRE_FL].dir = FREE;
01420                 motor[TIRE_BR].pwm = 0;
01421                 motor[TIRE_BR].dir = FREE;
01422                 motor[TIRE_BL].pwm = 20;
01423                 motor[TIRE_BL].dir = BACK;
01424                 mtc=true;
01425                 break;
01426             case 254:
01427                 motor[TIRE_FR].pwm = 30;
01428                 motor[TIRE_FR].dir = FOR;
01429                 motor[TIRE_FL].pwm = 0;
01430                 motor[TIRE_FL].dir = FREE;
01431                 motor[TIRE_BR].pwm = 0;
01432                 motor[TIRE_BR].dir = FREE;
01433                 motor[TIRE_BL].pwm = 30;
01434                 motor[TIRE_BL].dir = BACK;
01435                 mtc=true;
01436                 break;
01437             case 1:
01438                 motor[TIRE_FR].pwm = 40;
01439                 motor[TIRE_FR].dir = FOR;
01440                 motor[TIRE_FL].pwm = 60;
01441                 motor[TIRE_FL].dir = BACK;
01442                 motor[TIRE_BR].pwm = 60;
01443                 motor[TIRE_BR].dir = FOR;
01444                 motor[TIRE_BL].pwm = 40;
01445                 motor[TIRE_BL].dir = BACK;
01446                 mtc=true;
01447                 break;
01448             case 3:
01449                 motor[TIRE_FR].pwm = 0;
01450                 motor[TIRE_FR].dir = FREE;
01451                 motor[TIRE_FL].pwm = 20;
01452                 motor[TIRE_FL].dir = BACK;
01453                 motor[TIRE_BR].pwm = 20;
01454                 motor[TIRE_BR].dir = FOR;
01455                 motor[TIRE_BL].pwm = 0;
01456                 motor[TIRE_BL].dir = FREE;
01457                 mtc=true;
01458                 break;
01459             case 2:
01460                 motor[TIRE_FR].pwm = 0;
01461                 motor[TIRE_FR].dir = FREE;
01462                 motor[TIRE_FL].pwm = 30;
01463                 motor[TIRE_FL].dir = BACK;
01464                 motor[TIRE_BR].pwm = 30;
01465                 motor[TIRE_BR].dir = FOR;
01466                 motor[TIRE_BL].pwm = 0;
01467                 motor[TIRE_BL].dir = FREE;
01468                 mtc=true;
01469                 break;
01470         }
01471         if(g[1]!=98&&mtc==true) {
01472             switch(g[0]) {
01473                 case 0:
01474                     mtc=false;
01475                     break;
01476                 case 255:
01477                     motor[TIRE_FR].pwm += 5;
01478                     mtc=false;
01479                     break;
01480                 case 253:
01481                     motor[TIRE_FR].pwm += 10;
01482                     mtc=false;
01483                     break;
01484                 case 254:
01485                     motor[TIRE_FR].pwm += 20;
01486                     motor[TIRE_FL].pwm = 0;
01487                     mtc=false;
01488                     break;
01489                 case 1:
01490                     motor[TIRE_FL].pwm += 5;
01491                     mtc=false;
01492                     break;
01493                 case 3:
01494                     motor[TIRE_FL].pwm += 10;
01495                     mtc=false;
01496                     break;
01497                 case 2:
01498                     motor[TIRE_FL].pwm += 20;
01499                     motor[TIRE_FR].pwm = 0;
01500                     mtc=false;
01501                     break;
01502             }
01503         }
01504     } else if(mode==71) {
01505         motor[TIRE_FR].pwm = 15;
01506         motor[TIRE_FR].dir = FOR;
01507         motor[TIRE_FL].pwm = 15;
01508         motor[TIRE_FL].dir = BACK;
01509         motor[TIRE_BR].pwm = 15;
01510         motor[TIRE_BR].dir = FOR;
01511         motor[TIRE_BL].pwm = 15;
01512         motor[TIRE_BL].dir = BACK;
01513         mode1g0++;
01514         if(mode1g0>5000&&g[3]==0) {
01515             mode1g0=0;
01516             mode=72;
01517         }
01518     } else if(mode==72) {
01519         motor[TIRE_FR].pwm = 15;
01520         motor[TIRE_FR].dir = BACK;
01521         motor[TIRE_FL].pwm = 15;
01522         motor[TIRE_FL].dir = FOR;
01523         motor[TIRE_BR].pwm = 15;
01524         motor[TIRE_BR].dir = BACK;
01525         motor[TIRE_BL].pwm = 15;
01526         motor[TIRE_BL].dir = FOR;
01527         mode1g0++;
01528         if(mode1g0>5000&&g[3]==0) {
01529             mode1g0=0;
01530             mode1=0;
01531             mode=80;
01532         }
01533     } else if(mode==80) {
01534         motor[TIRE_FR].pwm = 0;
01535         motor[TIRE_FR].dir = BRAKE;
01536         motor[TIRE_FL].pwm = 0;
01537         motor[TIRE_FL].dir = BRAKE;
01538         motor[TIRE_BR].pwm = 0;
01539         motor[TIRE_BR].dir = BRAKE;
01540         motor[TIRE_BL].pwm = 0;
01541         motor[TIRE_BL].dir = BRAKE;
01542     } else {
01543         motor[TIRE_FR].pwm = 0;
01544         motor[TIRE_FR].dir = BRAKE;
01545         motor[TIRE_FL].pwm = 0;
01546         motor[TIRE_FL].dir = BRAKE;
01547         motor[TIRE_BR].pwm = 0;
01548         motor[TIRE_BR].dir = BRAKE;
01549         motor[TIRE_BL].pwm = 0;
01550         motor[TIRE_BL].dir = BRAKE;
01551     }
01552 
01553 }
01554 
01555 #endif
01556 
01557 #if USE_PROCESS_NUM>3
01558 static void Process3()
01559 {
01560 
01561 }
01562 #endif
01563 
01564 #if USE_PROCESS_NUM>4
01565 static void Process4()
01566 {
01567 
01568 }
01569 #endif
01570 
01571 #if USE_PROCESS_NUM>5
01572 static void Process5()
01573 {
01574 
01575 }
01576 #endif
01577 
01578 #if USE_PROCESS_NUM>6
01579 static void Process6()
01580 {
01581 
01582 }
01583 #endif
01584 
01585 #if USE_PROCESS_NUM>7
01586 static void Process7()
01587 {
01588 
01589 }
01590 #endif
01591 
01592 #if USE_PROCESS_NUM>8
01593 static void Process8()
01594 {
01595 
01596 }
01597 #endif
01598 
01599 #if USE_PROCESS_NUM>9
01600 static void Process9()
01601 {
01602 
01603 }
01604 #endif
01605 #endif
01606 #pragma endregion PROCESS
01607 
01608 static void AllActuatorReset()
01609 {
01610 
01611 #ifdef USE_SOLENOID
01612     solenoid.all = ALL_SOLENOID_OFF;
01613 #endif
01614 
01615 #ifdef USE_MOTOR
01616     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
01617         motor[i].dir = FREE;
01618         motor[i].pwm = 0;
01619     }
01620 #endif
01621 }
01622 
01623 /*
01624 void BuzzerTimer_func()
01625 {
01626     buzzer = !buzzer;
01627     //LED_DEBUG0 = !LED_DEBUG0;
01628 }
01629 */
01630 void TapeLedEms_func()
01631 {
01632     sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
01633 }
01634 
01635 #pragma region USER-DEFINED-FUNCTIONS
01636 
01637 
01638 #pragma endregion