aa

Dependencies:   mbed TrapezoidControl QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Process.cpp Source File

Process.cpp

00001 
00002 #include "mbed.h"
00003 #include "Process.h"
00004 #include "QEI.h"
00005 
00006 #include "../../CommonLibraries/PID/PID.h"
00007 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
00008 #include "../../Communication/RS485/LineHub/LineHub.h"
00009 #include "../../Communication/Controller/Controller.h"
00010 #include "../../Input/ExternalInt/ExternalInt.h"
00011 #include "../../Input/Switch/Switch.h"
00012 #include "../../Input/Potentiometer/Potentiometer.h"
00013 #include "../../Input/Encoder/Encoder.h"
00014 #include "../../LED/LED.h"
00015 #include "../../Safty/Safty.h"
00016 #include "../Using.h"
00017 
00018 using namespace SWITCH;
00019 using namespace PID_SPACE;
00020 using namespace ENCODER;
00021 using namespace LINEHUB;
00022 
00023 static CONTROLLER::ControllerData *controller;
00024 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
00025 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
00026 
00027 static bool lock;
00028 static bool processChangeComp;
00029 static int current;
00030 
00031 static void AllActuatorReset();
00032 
00033 #ifdef USE_SUBPROCESS
00034 static void (*Process[USE_PROCESS_NUM])(void);
00035 #endif
00036 
00037 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
00038 
00039 /*Replace here with the definition code of your variables.*/
00040 
00041 Serial pc(USBTX, USBRX);
00042 
00043 //**************Encoder***************
00044 const int PerRev = 256;
00045 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
00046 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
00047 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
00048 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
00049 //**************Encoder***************
00050 
00051 //**************Buzzer****************
00052 //DigitalOut buzzer(BUZZER_PIN);
00053 void BuzzerTimer_func();
00054 Ticker BuzzerTimer;
00055 bool EMGflag = false;
00056 //PWMOut buzzer(BUZZER_PIN);
00057 //**************Buzzer****************
00058 
00059 //************TapeLed*****************
00060 void TapeLedEms_func();
00061 TapeLedData tapeLED;
00062 TapeLedData sendLedData;
00063 TapeLED_Mode ledMode = Normal;
00064 Ticker tapeLedTimer;
00065 //************TapaLed*****************
00066 
00067 float tireProRPM[4];
00068 float tireTarRPM[4];
00069 float tirepwm[4];
00070 
00071 const int omni[15][15] = {
00072     {    0,     5,    21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255 },
00073     {   -5,     0,     5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255 },
00074     {  -21,    -5,     0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255 },
00075     {  -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255 },
00076     {  -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255 },
00077     { -130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255 },
00078     { -187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255 },
00079     { -255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255 },
00080     { -255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187 },
00081     { -255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130 },
00082     { -255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83 },
00083     { -255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47 },
00084     { -255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21 },
00085     { -255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5 },
00086     { -255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0 }
00087 };
00088 
00089 const int curve[15] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 };
00090 //{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200}
00091 
00092 uint8_t SetStatus(int);
00093 uint8_t SetStatus(int pwmVal)
00094 {
00095     if (pwmVal < 0) return BACK;
00096     else if (pwmVal > 0) return FOR;
00097     else if (pwmVal == 0) return BRAKE;
00098     else return BRAKE;
00099 }
00100 uint8_t Setpwm(int);
00101 uint8_t Setpwm(int pwmVal)
00102 {
00103     if (pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
00104     else return abs(pwmVal);
00105 }
00106 
00107 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
00108 
00109 #ifdef USE_SUBPROCESS
00110 #if USE_PROCESS_NUM>0
00111 static void Process0(void);
00112 #endif
00113 #if USE_PROCESS_NUM>1
00114 static void Process1(void);
00115 #endif
00116 #if USE_PROCESS_NUM>2
00117 static void Process2(void);
00118 #endif
00119 #if USE_PROCESS_NUM>3
00120 static void Process3(void);
00121 #endif
00122 #if USE_PROCESS_NUM>4
00123 static void Process4(void);
00124 #endif
00125 #if USE_PROCESS_NUM>5
00126 static void Process5(void);
00127 #endif
00128 #if USE_PROCESS_NUM>6
00129 static void Process6(void);
00130 #endif
00131 #if USE_PROCESS_NUM>7
00132 static void Process7(void);
00133 #endif
00134 #if USE_PROCESS_NUM>8
00135 static void Process8(void);
00136 #endif
00137 #if USE_PROCESS_NUM>9
00138 static void Process9(void);
00139 #endif
00140 #endif
00141 
00142 void SystemProcessInitialize()
00143 {
00144     #pragma region USER-DEFINED_VARIABLE_INIT
00145     /*Replace here with the initialization code of your variables.*/
00146     #pragma endregion USER-DEFINED_VARIABLE_INIT
00147 
00148     lock = true;
00149     processChangeComp = true;
00150     current = DEFAULT_PROCESS;
00151 
00152 #ifdef USE_SUBPROCESS
00153 #if USE_PROCESS_NUM>0
00154     Process[0] = Process0;
00155 #endif
00156 #if USE_PROCESS_NUM>1
00157     Process[1] = Process1;
00158 #endif
00159 #if USE_PROCESS_NUM>2
00160     Process[2] = Process2;
00161 #endif
00162 #if USE_PROCESS_NUM>3
00163     Process[3] = Process3;
00164 #endif
00165 #if USE_PROCESS_NUM>4
00166     Process[4] = Process4;
00167 #endif
00168 #if USE_PROCESS_NUM>5
00169     Process[5] = Process5;
00170 #endif
00171 #if USE_PROCESS_NUM>6
00172     Process[6] = Process6;
00173 #endif
00174 #if USE_PROCESS_NUM>7
00175     Process[7] = Process7;
00176 #endif
00177 #if USE_PROCESS_NUM>8
00178     Process[8] = Process8;
00179 #endif
00180 #if USE_PROCESS_NUM>9
00181     Process[9] = Process9;
00182 #endif
00183 #endif
00184 }
00185 
00186 static void SystemProcessUpdate()
00187 {
00188 #ifdef USE_SUBPROCESS
00189     if(controller->Button.HOME) lock = false;
00190 
00191     if(controller->Button.START && processChangeComp) {
00192         current++;
00193         if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
00194         processChangeComp = false;
00195     } else if(controller->Button.SELECT && processChangeComp) {
00196         current--;
00197         if (current < 0) current = 0;
00198         processChangeComp = false;
00199     } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
00200 #endif
00201 
00202 #ifdef USE_MOTOR
00203     ACTUATORHUB::MOTOR::Motor::Update(motor);
00204 #endif
00205 
00206 #ifdef USE_SOLENOID
00207     ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
00208 #endif
00209 
00210 #ifdef USE_RS485
00211     ACTUATORHUB::ActuatorHub::Update();
00212 #endif
00213 
00214 }
00215 
00216 int g[8];
00217 
00218 void SystemProcess()
00219 {
00220     SystemProcessInitialize();
00221 
00222     while(1) {
00223         for(int i = 0; i < 8; i++) {
00224             g[i] = LineHub::GetPara(i);
00225         }
00226 
00227 #ifdef USE_MU
00228         controller = CONTROLLER::Controller::GetData();
00229 #endif
00230 
00231 #ifdef USE_ERRORCHECK
00232         if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
00233             CONTROLLER::Controller::DataReset();
00234             AllActuatorReset();
00235             lock = true;
00236         } else
00237 #endif
00238         {
00239 
00240 #ifdef USE_SUBPROCESS
00241             if(!lock) {
00242                 Process[current]();
00243             } else
00244 #endif
00245             {
00246                 //ロック時の処理
00247             }
00248         }
00249         /*
00250         //Emergency!
00251         if(!EMG_0 && !EMG_1 && !EMGflag) {
00252             buzzer = 0;
00253             BuzzerTimer.attach(BuzzerTimer_func, 1);
00254             EMGflag = true;
00255             LED_DEBUG0 = 1;
00256         }
00257         if(EMG_0 && EMG_1 && EMGflag) {
00258             buzzer = 1;
00259             BuzzerTimer.detach();
00260             EMGflag = false;
00261         }
00262         */
00263         SystemProcessUpdate();
00264     }
00265 }
00266 
00267 
00268 int Twsh;
00269 
00270 #pragma region PROCESS
00271 #ifdef USE_SUBPROCESS
00272 #if USE_PROCESS_NUM>0
00273 
00274 static void Process0()
00275 {
00276     if(StertSW) {
00277         if(ZoneSW) {
00278             current = 1;
00279             Twsh=2;
00280         } else {
00281             current = 1;
00282             Twsh=3;
00283         }
00284     }
00285 }
00286 #endif
00287 
00288 #if USE_PROCESS_NUM>1
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 =1;
00300 int cross=0;//十字数える用
00301 int count=100000;//wait代替え
00302 
00303 
00304 static void Process1()
00305 {
00306     count=100000;
00307     mode=1;
00308     cross=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     count++;
00420     if(mode==1) {//スタートゾーンから白線検知
00421         motor[TIRE_FR].pwm = 30;
00422         motor[TIRE_FR].dir = BACK;
00423         motor[TIRE_FL].pwm = 30;
00424         motor[TIRE_FL].dir = FOR;
00425         motor[TIRE_BR].pwm = 30;
00426         motor[TIRE_BR].dir = BACK;
00427         motor[TIRE_BL].pwm = 30;
00428         motor[TIRE_BL].dir = FOR;
00429         if(g[2]==0) {
00430             count=100000;
00431             cross=0;
00432             mode=2;
00433         }
00434     } else if(mode==2) {//横移動
00435         if(g[2]==0) {
00436             count=100000;
00437             cross=0;
00438             mode=3;
00439             mtc2=true;
00440         } else {
00441             motor[TIRE_FR].pwm = 30;
00442             motor[TIRE_FR].dir = FOR;
00443             motor[TIRE_FL].pwm = 30;
00444             motor[TIRE_FL].dir = FOR;
00445             motor[TIRE_BR].pwm = 30;
00446             motor[TIRE_BR].dir = BACK;
00447             motor[TIRE_BL].pwm = 30;
00448             motor[TIRE_BL].dir = BACK;
00449         }
00450     } else if(mode==3) { //横ライントレースから縦ライントレースへ
00451         motor[TIRE_FR].pwm = 0;
00452         motor[TIRE_FR].dir = FREE;
00453         motor[TIRE_FL].pwm = 20;
00454         motor[TIRE_FL].dir = FOR;
00455         motor[TIRE_BR].pwm = 20;
00456         motor[TIRE_BR].dir = BACK;
00457         motor[TIRE_BL].pwm = 0;
00458         motor[TIRE_BL].dir = FREE;
00459         if(count>100000&&g[0]==0) {
00460             count=0;
00461             mode=10;
00462         }
00463     } else if(mode==4) { //横ライントレースから縦ライントレースへ
00464         motor[TIRE_FR].pwm = 15;
00465         motor[TIRE_FR].dir = BACK;
00466         motor[TIRE_FL].pwm = 15;
00467         motor[TIRE_FL].dir = BACK;
00468         motor[TIRE_BR].pwm = 15;
00469         motor[TIRE_BR].dir = FOR;
00470         motor[TIRE_BL].pwm = 15;
00471         motor[TIRE_BL].dir = FOR;
00472         if(count>30000&&g[0]==0) {
00473             count=0;
00474             cross=0;
00475             mode=10;
00476         }
00477     } else if(g[0]!=99&&mode==10) { //縦ライントレース
00478         switch(g[0]) {
00479             case 98:
00480                 motor[TIRE_FR].pwm = 30;
00481                 motor[TIRE_FR].dir = BACK;
00482                 motor[TIRE_FL].pwm = 30;
00483                 motor[TIRE_FL].dir = FOR;
00484                 motor[TIRE_BR].pwm = 30;
00485                 motor[TIRE_BR].dir = BACK;
00486                 motor[TIRE_BL].pwm = 30;
00487                 motor[TIRE_BL].dir = FOR;
00488                 if(count>100000) {
00489                     cross++;
00490                     count=0;
00491                 }
00492                 mtc=true;
00493                 if(cross==1) {
00494                     mode=11;
00495                     count=0;
00496                 }
00497                 break;
00498             case 0:
00499                 motor[TIRE_FR].pwm = 60;
00500                 motor[TIRE_FR].dir = BACK;
00501                 motor[TIRE_FL].pwm = 60;
00502                 motor[TIRE_FL].dir = FOR;
00503                 motor[TIRE_BR].pwm = 60;
00504                 motor[TIRE_BR].dir = BACK;
00505                 motor[TIRE_BL].pwm = 60;
00506                 motor[TIRE_BL].dir = FOR;
00507                 mtc=true;
00508                 break;
00509             case 255:
00510                 motor[TIRE_FR].pwm = 60;
00511                 motor[TIRE_FR].dir = BACK;
00512                 motor[TIRE_FL].pwm = 40;
00513                 motor[TIRE_FL].dir = FOR;
00514                 motor[TIRE_BR].pwm = 40;
00515                 motor[TIRE_BR].dir = BACK;
00516                 motor[TIRE_BL].pwm = 60;
00517                 motor[TIRE_BL].dir = FOR;
00518                 mtc=true;
00519                 break;
00520             case 253:
00521                 motor[TIRE_FR].pwm = 20;
00522                 motor[TIRE_FR].dir = BACK;
00523                 motor[TIRE_FL].pwm = 0;
00524                 motor[TIRE_FL].dir = FREE;
00525                 motor[TIRE_BR].pwm = 0;
00526                 motor[TIRE_BR].dir = FREE;
00527                 motor[TIRE_BL].pwm = 20;
00528                 motor[TIRE_BL].dir = FOR;
00529                 mtc=true;
00530                 break;
00531             case 254:
00532                 motor[TIRE_FR].pwm = 30;
00533                 motor[TIRE_FR].dir = BACK;
00534                 motor[TIRE_FL].pwm = 0;
00535                 motor[TIRE_FL].dir = FREE;
00536                 motor[TIRE_BR].pwm = 0;
00537                 motor[TIRE_BR].dir = FREE;
00538                 motor[TIRE_BL].pwm = 30;
00539                 motor[TIRE_BL].dir = FOR;
00540                 mtc=true;
00541                 break;
00542             case 1:
00543                 motor[TIRE_FR].pwm = 40;
00544                 motor[TIRE_FR].dir = BACK;
00545                 motor[TIRE_FL].pwm = 60;
00546                 motor[TIRE_FL].dir = FOR;
00547                 motor[TIRE_BR].pwm = 60;
00548                 motor[TIRE_BR].dir = BACK;
00549                 motor[TIRE_BL].pwm = 40;
00550                 motor[TIRE_BL].dir = FOR;
00551                 mtc=true;
00552                 break;
00553             case 3:
00554                 motor[TIRE_FR].pwm = 0;
00555                 motor[TIRE_FR].dir = FREE;
00556                 motor[TIRE_FL].pwm = 20;
00557                 motor[TIRE_FL].dir = FOR;
00558                 motor[TIRE_BR].pwm = 20;
00559                 motor[TIRE_BR].dir = BACK;
00560                 motor[TIRE_BL].pwm = 0;
00561                 motor[TIRE_BL].dir = FREE;
00562                 mtc=true;
00563                 break;
00564             case 2:
00565                 motor[TIRE_FR].pwm = 0;
00566                 motor[TIRE_FR].dir = FREE;
00567                 motor[TIRE_FL].pwm = 30;
00568                 motor[TIRE_FL].dir = FOR;
00569                 motor[TIRE_BR].pwm = 30;
00570                 motor[TIRE_BR].dir = BACK;
00571                 motor[TIRE_BL].pwm = 0;
00572                 motor[TIRE_BL].dir = FREE;
00573                 mtc=true;
00574                 break;
00575         }
00576         if(g[0]!=98&&mtc==true) {
00577             switch(g[1]) {
00578                 case 0:
00579                     mtc=false;
00580                     break;
00581                 case 255:
00582                     motor[TIRE_BR].pwm += 5;
00583                     mtc=false;
00584                     break;
00585                 case 253:
00586                     motor[TIRE_BR].pwm += 10;
00587                     mtc=false;
00588                     break;
00589                 case 254:
00590                     motor[TIRE_BR].pwm += 20;
00591                     motor[TIRE_BL].pwm = 0;
00592                     mtc=false;
00593                     break;
00594                 case 1:
00595                     motor[TIRE_BL].pwm += 5;
00596                     mtc=false;
00597                     break;
00598                 case 3:
00599                     motor[TIRE_BL].pwm += 10;
00600                     mtc=false;
00601                     break;
00602                 case 2:
00603                     motor[TIRE_BL].pwm += 20;
00604                     motor[TIRE_BR].pwm = 0;
00605                     mtc=false;
00606                     break;
00607             }
00608         }
00609     } else if(mode==11) {
00610         motor[TIRE_FR].pwm = 0;
00611         motor[TIRE_FR].dir = FREE;
00612         motor[TIRE_FL].pwm = 15;
00613         motor[TIRE_FL].dir = FOR;
00614         motor[TIRE_BR].pwm = 15;
00615         motor[TIRE_BR].dir = BACK;
00616         motor[TIRE_BL].pwm = 0;
00617         motor[TIRE_BL].dir = FREE;
00618         if(count>5000&&g[2]==0) {
00619             count=0;
00620             mode=12;
00621         }
00622     } else if(mode==12) {
00623         motor[TIRE_FR].pwm = 15;
00624         motor[TIRE_FR].dir = FOR;
00625         motor[TIRE_FL].pwm = 15;
00626         motor[TIRE_FL].dir = BACK;
00627         motor[TIRE_BR].pwm = 15;
00628         motor[TIRE_BR].dir = FOR;
00629         motor[TIRE_BL].pwm = 15;
00630         motor[TIRE_BL].dir = BACK;
00631         if(count>10000&&g[2]==0) {
00632             count=0;
00633             cross=0;
00634             mode=20;
00635         }
00636     } else if(mode==20) {
00637         switch(g[2]) {
00638             case 98:
00639                 motor[TIRE_FR].pwm = 30;
00640                 motor[TIRE_FR].dir = FOR;
00641                 motor[TIRE_FL].pwm = 30;
00642                 motor[TIRE_FL].dir = FOR;
00643                 motor[TIRE_BR].pwm = 30;
00644                 motor[TIRE_BR].dir = BACK;
00645                 motor[TIRE_BL].pwm = 30;
00646                 motor[TIRE_BL].dir = BACK;
00647                 if(count>50000) {
00648                     cross++;
00649                     count=0;
00650                 }
00651                 mtc2=true;
00652                 if(cross==3) {
00653                     mode=21;
00654                     count=0;
00655                     cross=0;
00656                 }
00657                 break;
00658             case 0:
00659                 motor[TIRE_FR].pwm = 60;
00660                 motor[TIRE_FR].dir = FOR;
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 = 60;
00666                 motor[TIRE_BL].dir = BACK;
00667                 mtc2=true;
00668                 break;
00669             case 255:
00670                 motor[TIRE_FR].pwm = 40;
00671                 motor[TIRE_FR].dir = FOR;
00672                 motor[TIRE_FL].pwm = 60;
00673                 motor[TIRE_FL].dir = FOR;
00674                 motor[TIRE_BR].pwm = 60;
00675                 motor[TIRE_BR].dir = BACK;
00676                 motor[TIRE_BL].pwm = 40;
00677                 motor[TIRE_BL].dir = BACK;
00678                 mtc2=true;
00679                 break;
00680             case 253:
00681                 motor[TIRE_FR].pwm = 0;
00682                 motor[TIRE_FR].dir = FREE;
00683                 motor[TIRE_FL].pwm = 20;
00684                 motor[TIRE_FL].dir = FOR;
00685                 motor[TIRE_BR].pwm = 20;
00686                 motor[TIRE_BR].dir = BACK;
00687                 motor[TIRE_BL].pwm = 0;
00688                 motor[TIRE_BL].dir = FREE;
00689                 mtc2=true;
00690                 break;
00691             case 254:
00692                 motor[TIRE_FR].pwm = 0;
00693                 motor[TIRE_FR].dir = FREE;
00694                 motor[TIRE_FL].pwm = 30;
00695                 motor[TIRE_FL].dir = FOR;
00696                 motor[TIRE_BR].pwm = 30;
00697                 motor[TIRE_BR].dir = BACK;
00698                 motor[TIRE_BL].pwm = 0;
00699                 motor[TIRE_BL].dir = FREE;
00700                 mtc2=true;
00701                 break;
00702             case 1:
00703                 motor[TIRE_FR].pwm = 60;
00704                 motor[TIRE_FR].dir = FOR;
00705                 motor[TIRE_FL].pwm = 40;
00706                 motor[TIRE_FL].dir = FOR;
00707                 motor[TIRE_BR].pwm = 40;
00708                 motor[TIRE_BR].dir = BACK;
00709                 motor[TIRE_BL].pwm = 60;
00710                 motor[TIRE_BL].dir = BACK;
00711                 mtc2=true;
00712                 break;
00713             case 3:
00714                 motor[TIRE_FR].pwm = 20;
00715                 motor[TIRE_FR].dir = FOR;
00716                 motor[TIRE_FL].pwm = 0;
00717                 motor[TIRE_FL].dir = FREE;
00718                 motor[TIRE_BR].pwm = 0;
00719                 motor[TIRE_BR].dir = FREE;
00720                 motor[TIRE_BL].pwm = 20;
00721                 motor[TIRE_BL].dir = BACK;
00722                 mtc2=true;
00723                 break;
00724             case 2:
00725                 motor[TIRE_FR].pwm = 30;
00726                 motor[TIRE_FR].dir = FOR;
00727                 motor[TIRE_FL].pwm = 0;
00728                 motor[TIRE_FL].dir = FREE;
00729                 motor[TIRE_BR].pwm = 0;
00730                 motor[TIRE_BR].dir = FREE;
00731                 motor[TIRE_BL].pwm = 30;
00732                 motor[TIRE_BL].dir = BACK;
00733                 mtc2=true;
00734                 break;
00735         }
00736         if(g[2]!=98&&mtc2==true) {
00737             switch(g[3]) {
00738                 case 0:
00739                     mtc2=false;
00740                     break;
00741                 case 255:
00742                     motor[TIRE_BL].pwm += 5;
00743                     mtc2=false;
00744                     break;
00745                 case 253:
00746                     motor[TIRE_BL].pwm += 10;
00747                     mtc2=false;
00748                     break;
00749                 case 254:
00750                     motor[TIRE_BL].pwm += 20;
00751                     motor[TIRE_FL].pwm = 0;
00752                     mtc2=false;
00753                     break;
00754                 case 1:
00755                     motor[TIRE_FL].pwm += 5;
00756                     mtc2=false;
00757                     break;
00758                 case 3:
00759                     motor[TIRE_FL].pwm += 10;
00760                     mtc2=false;
00761                     break;
00762                 case 2:
00763                     motor[TIRE_FL].pwm += 20;
00764                     motor[TIRE_BL].pwm = 0;
00765                     mtc2=false;
00766                     break;
00767             }
00768         }
00769     } else if(mode==21) {
00770         motor[TIRE_FR].pwm = 0;
00771         motor[TIRE_FR].dir = FREE;
00772         motor[TIRE_FL].pwm = 15;
00773         motor[TIRE_FL].dir = FOR;
00774         motor[TIRE_BR].pwm = 15;
00775         motor[TIRE_BR].dir = BACK;
00776         motor[TIRE_BL].pwm = 0;
00777         motor[TIRE_BL].dir = FREE;
00778         if(count>5000&&g[0]==0) {
00779             count=0;
00780             mode=22;
00781         }
00782     } else if(mode==22) {
00783         motor[TIRE_FR].pwm = 15;
00784         motor[TIRE_FR].dir = BACK;
00785         motor[TIRE_FL].pwm = 15;
00786         motor[TIRE_FL].dir = BACK;
00787         motor[TIRE_BR].pwm = 15;
00788         motor[TIRE_BR].dir = FOR;
00789         motor[TIRE_BL].pwm = 15;
00790         motor[TIRE_BL].dir = FOR;
00791         if(count>10000&&g[0]==0) {
00792             count=0;
00793             cross=0;
00794             mode=30;
00795         }
00796     } else if(mode==30) {
00797         switch(g[0]) {
00798             case 98:
00799                 motor[TIRE_FR].pwm = 30;
00800                 motor[TIRE_FR].dir = BACK;
00801                 motor[TIRE_FL].pwm = 30;
00802                 motor[TIRE_FL].dir = FOR;
00803                 motor[TIRE_BR].pwm = 30;
00804                 motor[TIRE_BR].dir = BACK;
00805                 motor[TIRE_BL].pwm = 30;
00806                 motor[TIRE_BL].dir = FOR;
00807                 if(count>100000) {
00808                     cross++;
00809                     count=0;
00810                 }
00811                 mtc=true;
00812                 if(cross==1) {
00813                     mode=31;
00814                     count=0;
00815                 }
00816                 break;
00817             case 0:
00818                 motor[TIRE_FR].pwm = 60;
00819                 motor[TIRE_FR].dir = BACK;
00820                 motor[TIRE_FL].pwm = 60;
00821                 motor[TIRE_FL].dir = FOR;
00822                 motor[TIRE_BR].pwm = 60;
00823                 motor[TIRE_BR].dir = BACK;
00824                 motor[TIRE_BL].pwm = 60;
00825                 motor[TIRE_BL].dir = FOR;
00826                 mtc=true;
00827                 break;
00828             case 255:
00829                 motor[TIRE_FR].pwm = 60;
00830                 motor[TIRE_FR].dir = BACK;
00831                 motor[TIRE_FL].pwm = 40;
00832                 motor[TIRE_FL].dir = FOR;
00833                 motor[TIRE_BR].pwm = 40;
00834                 motor[TIRE_BR].dir = BACK;
00835                 motor[TIRE_BL].pwm = 60;
00836                 motor[TIRE_BL].dir = FOR;
00837                 mtc=true;
00838                 break;
00839             case 253:
00840                 motor[TIRE_FR].pwm = 20;
00841                 motor[TIRE_FR].dir = BACK;
00842                 motor[TIRE_FL].pwm = 0;
00843                 motor[TIRE_FL].dir = FREE;
00844                 motor[TIRE_BR].pwm = 0;
00845                 motor[TIRE_BR].dir = FREE;
00846                 motor[TIRE_BL].pwm = 20;
00847                 motor[TIRE_BL].dir = FOR;
00848                 mtc=true;
00849                 break;
00850             case 254:
00851                 motor[TIRE_FR].pwm = 30;
00852                 motor[TIRE_FR].dir = BACK;
00853                 motor[TIRE_FL].pwm = 0;
00854                 motor[TIRE_FL].dir = FREE;
00855                 motor[TIRE_BR].pwm = 0;
00856                 motor[TIRE_BR].dir = FREE;
00857                 motor[TIRE_BL].pwm = 30;
00858                 motor[TIRE_BL].dir = FOR;
00859                 mtc=true;
00860                 break;
00861             case 1:
00862                 motor[TIRE_FR].pwm = 40;
00863                 motor[TIRE_FR].dir = BACK;
00864                 motor[TIRE_FL].pwm = 60;
00865                 motor[TIRE_FL].dir = FOR;
00866                 motor[TIRE_BR].pwm = 60;
00867                 motor[TIRE_BR].dir = BACK;
00868                 motor[TIRE_BL].pwm = 40;
00869                 motor[TIRE_BL].dir = FOR;
00870                 mtc=true;
00871                 break;
00872             case 3:
00873                 motor[TIRE_FR].pwm = 0;
00874                 motor[TIRE_FR].dir = FREE;
00875                 motor[TIRE_FL].pwm = 20;
00876                 motor[TIRE_FL].dir = FOR;
00877                 motor[TIRE_BR].pwm = 20;
00878                 motor[TIRE_BR].dir = BACK;
00879                 motor[TIRE_BL].pwm = 0;
00880                 motor[TIRE_BL].dir = FREE;
00881                 mtc=true;
00882                 break;
00883             case 2:
00884                 motor[TIRE_FR].pwm = 0;
00885                 motor[TIRE_FR].dir = FREE;
00886                 motor[TIRE_FL].pwm = 30;
00887                 motor[TIRE_FL].dir = FOR;
00888                 motor[TIRE_BR].pwm = 30;
00889                 motor[TIRE_BR].dir = BACK;
00890                 motor[TIRE_BL].pwm = 0;
00891                 motor[TIRE_BL].dir = FREE;
00892                 mtc=true;
00893                 break;
00894         }
00895         if(g[0]!=98&&mtc==true) {
00896             switch(g[1]) {
00897                 case 0:
00898                     mtc=false;
00899                     break;
00900                 case 255:
00901                     motor[TIRE_BR].pwm += 5;
00902                     mtc=false;
00903                     break;
00904                 case 253:
00905                     motor[TIRE_BR].pwm += 10;
00906                     mtc=false;
00907                     break;
00908                 case 254:
00909                     motor[TIRE_BR].pwm += 20;
00910                     motor[TIRE_BL].pwm = 0;
00911                     mtc=false;
00912                     break;
00913                 case 1:
00914                     motor[TIRE_BL].pwm += 5;
00915                     mtc=false;
00916                     break;
00917                 case 3:
00918                     motor[TIRE_BL].pwm += 10;
00919                     mtc=false;
00920                     break;
00921                 case 2:
00922                     motor[TIRE_BL].pwm += 20;
00923                     motor[TIRE_BR].pwm = 0;
00924                     mtc=false;
00925                     break;
00926             }
00927         }
00928     } else if(mode==31) {
00929         motor[TIRE_FR].pwm = 15;
00930         motor[TIRE_FR].dir = BACK;
00931         motor[TIRE_FL].pwm = 0;
00932         motor[TIRE_FL].dir = FREE;
00933         motor[TIRE_BR].pwm = 0;
00934         motor[TIRE_BR].dir = FREE;
00935         motor[TIRE_BL].pwm = 15;
00936         motor[TIRE_BL].dir = FOR;
00937         if(count>5000&&g[3]==0) {
00938             count=0;
00939             mode=32;
00940         }
00941     } else if(mode==32) {
00942         motor[TIRE_FR].pwm = 15;
00943         motor[TIRE_FR].dir = FOR;
00944         motor[TIRE_FL].pwm = 15;
00945         motor[TIRE_FL].dir = BACK;
00946         motor[TIRE_BR].pwm = 15;
00947         motor[TIRE_BR].dir = FOR;
00948         motor[TIRE_BL].pwm = 15;
00949         motor[TIRE_BL].dir = BACK;
00950         if(count>5000&&g[3]==0) {
00951             count=0;
00952             cross=0;
00953             mode=40;
00954         }
00955     } else if(mode==40) {
00956         switch(g[3]) {
00957             case 98:
00958                 motor[TIRE_FR].pwm = 30;
00959                 motor[TIRE_FR].dir = BACK;
00960                 motor[TIRE_FL].pwm = 30;
00961                 motor[TIRE_FL].dir = BACK;
00962                 motor[TIRE_BR].pwm = 30;
00963                 motor[TIRE_BR].dir = FOR;
00964                 motor[TIRE_BL].pwm = 30;
00965                 motor[TIRE_BL].dir = FOR;
00966                 if(count>30000) {
00967                     cross++;
00968                     count=0;
00969                 }
00970                 mtc2=true;
00971                 if(cross==1) {
00972                     mode=41;
00973                     count=0;
00974                     cross=0;
00975                 }
00976                 break;
00977             case 0:
00978                 motor[TIRE_FR].pwm = 60;
00979                 motor[TIRE_FR].dir = BACK;
00980                 motor[TIRE_FL].pwm = 60;
00981                 motor[TIRE_FL].dir = BACK;
00982                 motor[TIRE_BR].pwm = 60;
00983                 motor[TIRE_BR].dir = FOR;
00984                 motor[TIRE_BL].pwm = 60;
00985                 motor[TIRE_BL].dir = FOR;
00986                 mtc2=true;
00987                 break;
00988             case 255:
00989                 motor[TIRE_FR].pwm = 40;
00990                 motor[TIRE_FR].dir = BACK;
00991                 motor[TIRE_FL].pwm = 60;
00992                 motor[TIRE_FL].dir = BACK;
00993                 motor[TIRE_BR].pwm = 60;
00994                 motor[TIRE_BR].dir = FOR;
00995                 motor[TIRE_BL].pwm = 40;
00996                 motor[TIRE_BL].dir = FOR;
00997                 mtc2=true;
00998                 break;
00999             case 253:
01000                 motor[TIRE_FR].pwm = 0;
01001                 motor[TIRE_FR].dir = FREE;
01002                 motor[TIRE_FL].pwm = 20;
01003                 motor[TIRE_FL].dir = BACK;
01004                 motor[TIRE_BR].pwm = 20;
01005                 motor[TIRE_BR].dir = FOR;
01006                 motor[TIRE_BL].pwm = 0;
01007                 motor[TIRE_BL].dir = FREE;
01008                 mtc2=true;
01009                 break;
01010             case 254:
01011                 motor[TIRE_FR].pwm = 0;
01012                 motor[TIRE_FR].dir = FREE;
01013                 motor[TIRE_FL].pwm = 30;
01014                 motor[TIRE_FL].dir = BACK;
01015                 motor[TIRE_BR].pwm = 30;
01016                 motor[TIRE_BR].dir = FOR;
01017                 motor[TIRE_BL].pwm = 0;
01018                 motor[TIRE_BL].dir = FREE;
01019                 mtc2=true;
01020                 break;
01021             case 1:
01022                 motor[TIRE_FR].pwm = 60;
01023                 motor[TIRE_FR].dir = BACK;
01024                 motor[TIRE_FL].pwm = 40;
01025                 motor[TIRE_FL].dir = BACK;
01026                 motor[TIRE_BR].pwm = 40;
01027                 motor[TIRE_BR].dir = FOR;
01028                 motor[TIRE_BL].pwm = 60;
01029                 motor[TIRE_BL].dir = FOR;
01030                 mtc2=true;
01031                 break;
01032             case 3:
01033                 motor[TIRE_FR].pwm = 20;
01034                 motor[TIRE_FR].dir = BACK;
01035                 motor[TIRE_FL].pwm = 0;
01036                 motor[TIRE_FL].dir = FREE;
01037                 motor[TIRE_BR].pwm = 0;
01038                 motor[TIRE_BR].dir = FREE;
01039                 motor[TIRE_BL].pwm = 20;
01040                 motor[TIRE_BL].dir = FOR;
01041                 mtc2=true;
01042                 break;
01043             case 2:
01044                 motor[TIRE_FR].pwm = 30;
01045                 motor[TIRE_FR].dir = BACK;
01046                 motor[TIRE_FL].pwm = 0;
01047                 motor[TIRE_FL].dir = FREE;
01048                 motor[TIRE_BR].pwm = 0;
01049                 motor[TIRE_BR].dir = FREE;
01050                 motor[TIRE_BL].pwm = 30;
01051                 motor[TIRE_BL].dir = FOR;
01052                 mtc2=true;
01053                 break;
01054         }
01055         if(g[2]!=98&&mtc2==true) {
01056             switch(g[2]) {
01057                 case 0:
01058                     mtc2=false;
01059                     break;
01060                 case 255:
01061                     motor[TIRE_FL].pwm += 5;
01062                     mtc2=false;
01063                     break;
01064                 case 253:
01065                     motor[TIRE_FL].pwm += 10;
01066                     mtc2=false;
01067                     break;
01068                 case 254:
01069                     motor[TIRE_FL].pwm += 20;
01070                     motor[TIRE_BL].pwm = 0;
01071                     mtc2=false;
01072                     break;
01073                 case 1:
01074                     motor[TIRE_BL].pwm += 5;
01075                     mtc2=false;
01076                     break;
01077                 case 3:
01078                     motor[TIRE_BL].pwm += 10;
01079                     mtc2=false;
01080                     break;
01081                 case 2:
01082                     motor[TIRE_BL].pwm += 20;
01083                     motor[TIRE_FL].pwm = 0;
01084                     mtc2=false;
01085                     break;
01086             }
01087         }
01088     } else if(mode==41) {
01089         motor[TIRE_FR].pwm = 0;
01090         motor[TIRE_FR].dir = FREE;
01091         motor[TIRE_FL].pwm = 15;
01092         motor[TIRE_FL].dir = BACK;
01093         motor[TIRE_BR].pwm = 15;
01094         motor[TIRE_BR].dir = FOR;
01095         motor[TIRE_BL].pwm = 0;
01096         motor[TIRE_BL].dir = FREE;
01097         if(count>5000&&g[1]==0) {
01098             count=0;
01099             mode=42;
01100         }
01101     } else if(mode==42) {
01102         motor[TIRE_FR].pwm = 15;
01103         motor[TIRE_FR].dir = FOR;
01104         motor[TIRE_FL].pwm = 15;
01105         motor[TIRE_FL].dir = FOR;
01106         motor[TIRE_BR].pwm = 15;
01107         motor[TIRE_BR].dir = BACK;
01108         motor[TIRE_BL].pwm = 15;
01109         motor[TIRE_BL].dir = BACK;
01110         if(count>5000&&g[1]==0) {
01111             count=0;
01112             mode=50;
01113         }
01114     } else if(mode==50) {
01115         switch(g[1]) {
01116             case 98:
01117                 motor[TIRE_FR].pwm = 30;
01118                 motor[TIRE_FR].dir = FOR;
01119                 motor[TIRE_FL].pwm = 30;
01120                 motor[TIRE_FL].dir = BACK;
01121                 motor[TIRE_BR].pwm = 30;
01122                 motor[TIRE_BR].dir = FOR;
01123                 motor[TIRE_BL].pwm = 30;
01124                 motor[TIRE_BL].dir = BACK;
01125                 if(count>100000) {
01126                     cross++;
01127                     count=0;
01128                 }
01129                 mtc=true;
01130                 if(cross==1) {
01131                     mode=51;
01132                     count=0;
01133                 }
01134                 break;
01135             case 0:
01136                 motor[TIRE_FR].pwm = 60;
01137                 motor[TIRE_FR].dir = FOR;
01138                 motor[TIRE_FL].pwm = 60;
01139                 motor[TIRE_FL].dir = BACK;
01140                 motor[TIRE_BR].pwm = 60;
01141                 motor[TIRE_BR].dir = FOR;
01142                 motor[TIRE_BL].pwm = 60;
01143                 motor[TIRE_BL].dir = BACK;
01144                 mtc=true;
01145                 break;
01146             case 255:
01147                 motor[TIRE_FR].pwm = 60;
01148                 motor[TIRE_FR].dir = FOR;
01149                 motor[TIRE_FL].pwm = 40;
01150                 motor[TIRE_FL].dir = BACK;
01151                 motor[TIRE_BR].pwm = 40;
01152                 motor[TIRE_BR].dir = FOR;
01153                 motor[TIRE_BL].pwm = 60;
01154                 motor[TIRE_BL].dir = BACK;
01155                 mtc=true;
01156                 break;
01157             case 253:
01158                 motor[TIRE_FR].pwm = 20;
01159                 motor[TIRE_FR].dir = FOR;
01160                 motor[TIRE_FL].pwm = 0;
01161                 motor[TIRE_FL].dir = FREE;
01162                 motor[TIRE_BR].pwm = 0;
01163                 motor[TIRE_BR].dir = FREE;
01164                 motor[TIRE_BL].pwm = 20;
01165                 motor[TIRE_BL].dir = BACK;
01166                 mtc=true;
01167                 break;
01168             case 254:
01169                 motor[TIRE_FR].pwm = 30;
01170                 motor[TIRE_FR].dir = FOR;
01171                 motor[TIRE_FL].pwm = 0;
01172                 motor[TIRE_FL].dir = FREE;
01173                 motor[TIRE_BR].pwm = 0;
01174                 motor[TIRE_BR].dir = FREE;
01175                 motor[TIRE_BL].pwm = 30;
01176                 motor[TIRE_BL].dir = BACK;
01177                 mtc=true;
01178                 break;
01179             case 1:
01180                 motor[TIRE_FR].pwm = 40;
01181                 motor[TIRE_FR].dir = FOR;
01182                 motor[TIRE_FL].pwm = 60;
01183                 motor[TIRE_FL].dir = BACK;
01184                 motor[TIRE_BR].pwm = 60;
01185                 motor[TIRE_BR].dir = FOR;
01186                 motor[TIRE_BL].pwm = 40;
01187                 motor[TIRE_BL].dir = BACK;
01188                 mtc=true;
01189                 break;
01190             case 3:
01191                 motor[TIRE_FR].pwm = 0;
01192                 motor[TIRE_FR].dir = FREE;
01193                 motor[TIRE_FL].pwm = 20;
01194                 motor[TIRE_FL].dir = BACK;
01195                 motor[TIRE_BR].pwm = 20;
01196                 motor[TIRE_BR].dir = FOR;
01197                 motor[TIRE_BL].pwm = 0;
01198                 motor[TIRE_BL].dir = FREE;
01199                 mtc=true;
01200                 break;
01201             case 2:
01202                 motor[TIRE_FR].pwm = 0;
01203                 motor[TIRE_FR].dir = FREE;
01204                 motor[TIRE_FL].pwm = 30;
01205                 motor[TIRE_FL].dir = BACK;
01206                 motor[TIRE_BR].pwm = 30;
01207                 motor[TIRE_BR].dir = FOR;
01208                 motor[TIRE_BL].pwm = 0;
01209                 motor[TIRE_BL].dir = FREE;
01210                 mtc=true;
01211                 break;
01212         }
01213         if(g[1]!=98&&mtc==true) {
01214             switch(g[0]) {
01215                 case 0:
01216                     mtc=false;
01217                     break;
01218                 case 255:
01219                     motor[TIRE_FL].pwm += 5;
01220                     mtc=false;
01221                     break;
01222                 case 253:
01223                     motor[TIRE_FL].pwm += 10;
01224                     mtc=false;
01225                     break;
01226                 case 254:
01227                     motor[TIRE_FL].pwm += 20;
01228                     motor[TIRE_FR].pwm = 0;
01229                     mtc=false;
01230                     break;
01231                 case 1:
01232                     motor[TIRE_FR].pwm += 5;
01233                     mtc=false;
01234                     break;
01235                 case 3:
01236                     motor[TIRE_FR].pwm += 10;
01237                     mtc=false;
01238                     break;
01239                 case 2:
01240                     motor[TIRE_FR].pwm += 20;
01241                     motor[TIRE_FL].pwm = 0;
01242                     mtc=false;
01243                     break;
01244             }
01245         }
01246     } else if(mode==51) {
01247         motor[TIRE_FR].pwm = 0;
01248         motor[TIRE_FR].dir = FREE;
01249         motor[TIRE_FL].pwm = 15;
01250         motor[TIRE_FL].dir = BACK;
01251         motor[TIRE_BR].pwm = 15;
01252         motor[TIRE_BR].dir = FOR;
01253         motor[TIRE_BL].pwm = 0;
01254         motor[TIRE_BL].dir = FREE;
01255         if(count>5000&&g[3]==0) {
01256             count=0;
01257             mode=52;
01258         }
01259     } else if(mode==52) {
01260         motor[TIRE_FR].pwm = 15;
01261         motor[TIRE_FR].dir = BACK;
01262         motor[TIRE_FL].pwm = 15;
01263         motor[TIRE_FL].dir = FOR;
01264         motor[TIRE_BR].pwm = 15;
01265         motor[TIRE_BR].dir = BACK;
01266         motor[TIRE_BL].pwm = 15;
01267         motor[TIRE_BL].dir = FOR;
01268         if(count>5000&&g[3]==0) {
01269             count=0;
01270             cross=0;
01271             mode=60;
01272         }
01273     } else if(mode==60) {
01274         switch(g[3]) {
01275             case 98:
01276                 motor[TIRE_FR].pwm = 30;
01277                 motor[TIRE_FR].dir = BACK;
01278                 motor[TIRE_FL].pwm = 30;
01279                 motor[TIRE_FL].dir = BACK;
01280                 motor[TIRE_BR].pwm = 30;
01281                 motor[TIRE_BR].dir = FOR;
01282                 motor[TIRE_BL].pwm = 30;
01283                 motor[TIRE_BL].dir = FOR;
01284                 if(count>50000) {
01285                     cross++;
01286                     count=0;
01287                 }
01288                 mtc2=true;
01289                 if(cross==2) {
01290                     mode=61;
01291                     count=0;
01292                     cross=0;
01293                 }
01294                 break;
01295             case 0:
01296                 motor[TIRE_FR].pwm = 60;
01297                 motor[TIRE_FR].dir = BACK;
01298                 motor[TIRE_FL].pwm = 60;
01299                 motor[TIRE_FL].dir = BACK;
01300                 motor[TIRE_BR].pwm = 60;
01301                 motor[TIRE_BR].dir = FOR;
01302                 motor[TIRE_BL].pwm = 60;
01303                 motor[TIRE_BL].dir = FOR;
01304                 mtc2=true;
01305                 break;
01306             case 255:
01307                 motor[TIRE_FR].pwm = 40;
01308                 motor[TIRE_FR].dir = BACK;
01309                 motor[TIRE_FL].pwm = 60;
01310                 motor[TIRE_FL].dir = BACK;
01311                 motor[TIRE_BR].pwm = 60;
01312                 motor[TIRE_BR].dir = FOR;
01313                 motor[TIRE_BL].pwm = 40;
01314                 motor[TIRE_BL].dir = FOR;
01315                 mtc2=true;
01316                 break;
01317             case 253:
01318                 motor[TIRE_FR].pwm = 0;
01319                 motor[TIRE_FR].dir = FREE;
01320                 motor[TIRE_FL].pwm = 20;
01321                 motor[TIRE_FL].dir = BACK;
01322                 motor[TIRE_BR].pwm = 20;
01323                 motor[TIRE_BR].dir = FOR;
01324                 motor[TIRE_BL].pwm = 0;
01325                 motor[TIRE_BL].dir = FREE;
01326                 mtc2=true;
01327                 break;
01328             case 254:
01329                 motor[TIRE_FR].pwm = 0;
01330                 motor[TIRE_FR].dir = FREE;
01331                 motor[TIRE_FL].pwm = 30;
01332                 motor[TIRE_FL].dir = BACK;
01333                 motor[TIRE_BR].pwm = 30;
01334                 motor[TIRE_BR].dir = FOR;
01335                 motor[TIRE_BL].pwm = 0;
01336                 motor[TIRE_BL].dir = FREE;
01337                 mtc2=true;
01338                 break;
01339             case 1:
01340                 motor[TIRE_FR].pwm = 60;
01341                 motor[TIRE_FR].dir = BACK;
01342                 motor[TIRE_FL].pwm = 40;
01343                 motor[TIRE_FL].dir = BACK;
01344                 motor[TIRE_BR].pwm = 40;
01345                 motor[TIRE_BR].dir = FOR;
01346                 motor[TIRE_BL].pwm = 60;
01347                 motor[TIRE_BL].dir = FOR;
01348                 mtc2=true;
01349                 break;
01350             case 3:
01351                 motor[TIRE_FR].pwm = 20;
01352                 motor[TIRE_FR].dir = BACK;
01353                 motor[TIRE_FL].pwm = 0;
01354                 motor[TIRE_FL].dir = FREE;
01355                 motor[TIRE_BR].pwm = 0;
01356                 motor[TIRE_BR].dir = FREE;
01357                 motor[TIRE_BL].pwm = 20;
01358                 motor[TIRE_BL].dir = FOR;
01359                 mtc2=true;
01360                 break;
01361             case 2:
01362                 motor[TIRE_FR].pwm = 30;
01363                 motor[TIRE_FR].dir = BACK;
01364                 motor[TIRE_FL].pwm = 0;
01365                 motor[TIRE_FL].dir = FREE;
01366                 motor[TIRE_BR].pwm = 0;
01367                 motor[TIRE_BR].dir = FREE;
01368                 motor[TIRE_BL].pwm = 30;
01369                 motor[TIRE_BL].dir = FOR;
01370                 mtc2=true;
01371                 break;
01372         }
01373         if(g[2]!=98&&mtc2==true) {
01374             switch(g[2]) {
01375                 case 0:
01376                     mtc2=false;
01377                     break;
01378                 case 255:
01379                     motor[TIRE_FL].pwm += 5;
01380                     mtc2=false;
01381                     break;
01382                 case 253:
01383                     motor[TIRE_FL].pwm += 10;
01384                     mtc2=false;
01385                     break;
01386                 case 254:
01387                     motor[TIRE_FL].pwm += 20;
01388                     motor[TIRE_BL].pwm = 0;
01389                     mtc2=false;
01390                     break;
01391                 case 1:
01392                     motor[TIRE_BL].pwm += 5;
01393                     mtc2=false;
01394                     break;
01395                 case 3:
01396                     motor[TIRE_BL].pwm += 10;
01397                     mtc2=false;
01398                     break;
01399                 case 2:
01400                     motor[TIRE_BL].pwm += 20;
01401                     motor[TIRE_FL].pwm = 0;
01402                     mtc2=false;
01403                     break;
01404             }
01405         }
01406     } else if(mode==61) {
01407         motor[TIRE_FR].pwm = 0;
01408         motor[TIRE_FR].dir = FREE;
01409         motor[TIRE_FL].pwm = 15;
01410         motor[TIRE_FL].dir = BACK;
01411         motor[TIRE_BR].pwm = 15;
01412         motor[TIRE_BR].dir = FOR;
01413         motor[TIRE_BL].pwm = 0;
01414         motor[TIRE_BL].dir = FREE;
01415         if(count>5000&&g[1]==0) {
01416             count=0;
01417             mode=62;
01418         }
01419     } else if(mode==62) {
01420         motor[TIRE_FR].pwm = 15;
01421         motor[TIRE_FR].dir = FOR;
01422         motor[TIRE_FL].pwm = 15;
01423         motor[TIRE_FL].dir = FOR;
01424         motor[TIRE_BR].pwm = 15;
01425         motor[TIRE_BR].dir = BACK;
01426         motor[TIRE_BL].pwm = 15;
01427         motor[TIRE_BL].dir = BACK;
01428         if(count>5000&&g[1]==0) {
01429             count=0;
01430             mode=70;
01431         }
01432     } else if(mode==70) {
01433         switch(g[1]) {
01434             case 98:
01435                 motor[TIRE_FR].pwm = 30;
01436                 motor[TIRE_FR].dir = FOR;
01437                 motor[TIRE_FL].pwm = 30;
01438                 motor[TIRE_FL].dir = BACK;
01439                 motor[TIRE_BR].pwm = 30;
01440                 motor[TIRE_BR].dir = FOR;
01441                 motor[TIRE_BL].pwm = 30;
01442                 motor[TIRE_BL].dir = BACK;
01443                 if(count>100000) {
01444                     cross++;
01445                     count=0;
01446                 }
01447                 mtc=true;
01448                 if(cross==1) {
01449                     mode=71;
01450                     count=0;
01451                 }
01452                 break;
01453             case 0:
01454                 motor[TIRE_FR].pwm = 60;
01455                 motor[TIRE_FR].dir = FOR;
01456                 motor[TIRE_FL].pwm = 60;
01457                 motor[TIRE_FL].dir = BACK;
01458                 motor[TIRE_BR].pwm = 60;
01459                 motor[TIRE_BR].dir = FOR;
01460                 motor[TIRE_BL].pwm = 60;
01461                 motor[TIRE_BL].dir = BACK;
01462                 mtc=true;
01463                 break;
01464             case 255:
01465                 motor[TIRE_FR].pwm = 60;
01466                 motor[TIRE_FR].dir = FOR;
01467                 motor[TIRE_FL].pwm = 40;
01468                 motor[TIRE_FL].dir = BACK;
01469                 motor[TIRE_BR].pwm = 40;
01470                 motor[TIRE_BR].dir = FOR;
01471                 motor[TIRE_BL].pwm = 60;
01472                 motor[TIRE_BL].dir = BACK;
01473                 mtc=true;
01474                 break;
01475             case 253:
01476                 motor[TIRE_FR].pwm = 20;
01477                 motor[TIRE_FR].dir = FOR;
01478                 motor[TIRE_FL].pwm = 0;
01479                 motor[TIRE_FL].dir = FREE;
01480                 motor[TIRE_BR].pwm = 0;
01481                 motor[TIRE_BR].dir = FREE;
01482                 motor[TIRE_BL].pwm = 20;
01483                 motor[TIRE_BL].dir = BACK;
01484                 mtc=true;
01485                 break;
01486             case 254:
01487                 motor[TIRE_FR].pwm = 30;
01488                 motor[TIRE_FR].dir = FOR;
01489                 motor[TIRE_FL].pwm = 0;
01490                 motor[TIRE_FL].dir = FREE;
01491                 motor[TIRE_BR].pwm = 0;
01492                 motor[TIRE_BR].dir = FREE;
01493                 motor[TIRE_BL].pwm = 30;
01494                 motor[TIRE_BL].dir = BACK;
01495                 mtc=true;
01496                 break;
01497             case 1:
01498                 motor[TIRE_FR].pwm = 40;
01499                 motor[TIRE_FR].dir = FOR;
01500                 motor[TIRE_FL].pwm = 60;
01501                 motor[TIRE_FL].dir = BACK;
01502                 motor[TIRE_BR].pwm = 60;
01503                 motor[TIRE_BR].dir = FOR;
01504                 motor[TIRE_BL].pwm = 40;
01505                 motor[TIRE_BL].dir = BACK;
01506                 mtc=true;
01507                 break;
01508             case 3:
01509                 motor[TIRE_FR].pwm = 0;
01510                 motor[TIRE_FR].dir = FREE;
01511                 motor[TIRE_FL].pwm = 20;
01512                 motor[TIRE_FL].dir = BACK;
01513                 motor[TIRE_BR].pwm = 20;
01514                 motor[TIRE_BR].dir = FOR;
01515                 motor[TIRE_BL].pwm = 0;
01516                 motor[TIRE_BL].dir = FREE;
01517                 mtc=true;
01518                 break;
01519             case 2:
01520                 motor[TIRE_FR].pwm = 0;
01521                 motor[TIRE_FR].dir = FREE;
01522                 motor[TIRE_FL].pwm = 30;
01523                 motor[TIRE_FL].dir = BACK;
01524                 motor[TIRE_BR].pwm = 30;
01525                 motor[TIRE_BR].dir = FOR;
01526                 motor[TIRE_BL].pwm = 0;
01527                 motor[TIRE_BL].dir = FREE;
01528                 mtc=true;
01529                 break;
01530         }
01531         if(g[1]!=98&&mtc==true) {
01532             switch(g[0]) {
01533                 case 0:
01534                     mtc=false;
01535                     break;
01536                 case 255:
01537                     motor[TIRE_FL].pwm += 5;
01538                     mtc=false;
01539                     break;
01540                 case 253:
01541                     motor[TIRE_FL].pwm += 10;
01542                     mtc=false;
01543                     break;
01544                 case 254:
01545                     motor[TIRE_FL].pwm += 20;
01546                     motor[TIRE_FR].pwm = 0;
01547                     mtc=false;
01548                     break;
01549                 case 1:
01550                     motor[TIRE_FR].pwm += 5;
01551                     mtc=false;
01552                     break;
01553                 case 3:
01554                     motor[TIRE_FR].pwm += 10;
01555                     mtc=false;
01556                     break;
01557                 case 2:
01558                     motor[TIRE_FR].pwm += 20;
01559                     motor[TIRE_FL].pwm = 0;
01560                     mtc=false;
01561                     break;
01562             }
01563         }
01564     } else if(mode==71) {
01565         motor[TIRE_FR].pwm = 15;
01566         motor[TIRE_FR].dir = FOR;
01567         motor[TIRE_FL].pwm = 15;
01568         motor[TIRE_FL].dir = BACK;
01569         motor[TIRE_BR].pwm = 15;
01570         motor[TIRE_BR].dir = FOR;
01571         motor[TIRE_BL].pwm = 15;
01572         motor[TIRE_BL].dir = BACK;
01573         if(count>5000&&g[3]==0) {
01574             count=0;
01575             mode=72;
01576         }
01577     } else if(mode==72) {
01578         motor[TIRE_FR].pwm = 15;
01579         motor[TIRE_FR].dir = BACK;
01580         motor[TIRE_FL].pwm = 15;
01581         motor[TIRE_FL].dir = FOR;
01582         motor[TIRE_BR].pwm = 15;
01583         motor[TIRE_BR].dir = BACK;
01584         motor[TIRE_BL].pwm = 15;
01585         motor[TIRE_BL].dir = FOR;
01586         count++;
01587         if(count>50000&&g[3]==0) {
01588             count=0;
01589             cross=0;
01590             mode=80;
01591         }
01592     } else if(mode==80) {
01593         if(count>=400000) {
01594             mode=100;
01595         }
01596         motor[TIRE_FR].pwm = 40;
01597         motor[TIRE_FR].dir = FOR;
01598         motor[TIRE_FL].pwm = 40;
01599         motor[TIRE_FL].dir = BACK;
01600         motor[TIRE_BR].pwm = 40;
01601         motor[TIRE_BR].dir = FOR;
01602         motor[TIRE_BL].pwm = 40;
01603         motor[TIRE_BL].dir = BACK;
01604     } else if(mode==100) {
01605         motor[TIRE_FR].pwm = 0;
01606         motor[TIRE_FR].dir = BRAKE;
01607         motor[TIRE_FL].pwm = 0;
01608         motor[TIRE_FL].dir = BRAKE;
01609         motor[TIRE_BR].pwm = 0;
01610         motor[TIRE_BR].dir = BRAKE;
01611         motor[TIRE_BL].pwm = 0;
01612         motor[TIRE_BL].dir = BRAKE;
01613     } else {
01614         motor[TIRE_FR].pwm = 0;
01615         motor[TIRE_FR].dir = BRAKE;
01616         motor[TIRE_FL].pwm = 0;
01617         motor[TIRE_FL].dir = BRAKE;
01618         motor[TIRE_BR].pwm = 0;
01619         motor[TIRE_BR].dir = BRAKE;
01620         motor[TIRE_BL].pwm = 0;
01621         motor[TIRE_BL].dir = BRAKE;
01622     }
01623 
01624 }
01625 
01626 #endif
01627 
01628 #if USE_PROCESS_NUM>3
01629 static void Process3()
01630 {
01631 
01632 }
01633 #endif
01634 
01635 #if USE_PROCESS_NUM>4
01636 static void Process4()
01637 {
01638 
01639 }
01640 #endif
01641 
01642 #if USE_PROCESS_NUM>5
01643 static void Process5()
01644 {
01645 
01646 }
01647 #endif
01648 
01649 #if USE_PROCESS_NUM>6
01650 static void Process6()
01651 {
01652 
01653 }
01654 #endif
01655 
01656 #if USE_PROCESS_NUM>7
01657 static void Process7()
01658 {
01659 
01660 }
01661 #endif
01662 
01663 #if USE_PROCESS_NUM>8
01664 static void Process8()
01665 {
01666 
01667 }
01668 #endif
01669 
01670 #if USE_PROCESS_NUM>9
01671 static void Process9()
01672 {
01673 
01674 }
01675 #endif
01676 #endif
01677 #pragma endregion PROCESS
01678 
01679 static void AllActuatorReset()
01680 {
01681 
01682 #ifdef USE_SOLENOID
01683     solenoid.all = ALL_SOLENOID_OFF;
01684 #endif
01685 
01686 #ifdef USE_MOTOR
01687     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
01688         motor[i].dir = FREE;
01689         motor[i].pwm = 0;
01690     }
01691 #endif
01692 }
01693 
01694 /*
01695 void BuzzerTimer_func()
01696 {
01697     buzzer = !buzzer;
01698     //LED_DEBUG0 = !LED_DEBUG0;
01699 }
01700 */
01701 void TapeLedEms_func()
01702 {
01703     sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
01704 }
01705 
01706 #pragma region USER-DEFINED-FUNCTIONS
01707 
01708 
01709 #pragma endregion