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