da

Dependencies:   mbed TrapezoidControl QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Process.cpp Source File

Process.cpp

00001 
00002 #include "mbed.h"
00003 #include "Process.h"
00004 #include "QEI.h"
00005 
00006 #include "../../CommonLibraries/PID/PID.h"
00007 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
00008 #include "../../Communication/RS485/LineHub/LineHub.h"
00009 #include "../../Communication/Controller/Controller.h"
00010 #include "../../Input/ExternalInt/ExternalInt.h"
00011 #include "../../Input/Switch/Switch.h"
00012 #include "../../Input/Potentiometer/Potentiometer.h"
00013 #include "../../Input/Encoder/Encoder.h"
00014 #include "../../LED/LED.h"
00015 #include "../../Safty/Safty.h"
00016 #include "../Using.h"
00017 
00018 using namespace SWITCH;
00019 using namespace PID_SPACE;
00020 using namespace ENCODER;
00021 using namespace LINEHUB;
00022 
00023 static CONTROLLER::ControllerData *controller;
00024 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
00025 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
00026 
00027 static bool lock;
00028 static bool processChangeComp;
00029 static int current;
00030 
00031 static void AllActuatorReset();
00032 
00033 #ifdef USE_SUBPROCESS
00034 static void (*Process[USE_PROCESS_NUM])(void);
00035 #endif
00036 
00037 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
00038 
00039 /*Replace here with the definition code of your variables.*/
00040 
00041 Serial pc(USBTX, USBRX);
00042 
00043 //**************Encoder***************
00044 const int PerRev = 256;
00045 QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
00046 QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
00047 QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
00048 QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
00049 //**************Encoder***************
00050 
00051 //**************Buzzer****************
00052 //DigitalOut buzzer(BUZZER_PIN);
00053 void BuzzerTimer_func();
00054 Ticker BuzzerTimer;
00055 bool EMGflag = false;
00056 //PWMOut buzzer(BUZZER_PIN);
00057 //**************Buzzer****************
00058 
00059 //************TapeLed*****************
00060 void TapeLedEms_func();
00061 TapeLedData tapeLED;
00062 TapeLedData sendLedData;
00063 TapeLED_Mode ledMode = Normal;
00064 Ticker tapeLedTimer;
00065 //************TapaLed*****************
00066 
00067 float tireProRPM[4];
00068 float tireTarRPM[4];
00069 float tirepwm[4];
00070 
00071 const int omni[15][15] = {
00072     {    0,     5,    21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255 },
00073     {   -5,     0,     5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255 },
00074     {  -21,    -5,     0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255 },
00075     {  -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255 },
00076     {  -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255 },
00077     { -130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255 },
00078     { -187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255 },
00079     { -255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255 },
00080     { -255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187 },
00081     { -255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130 },
00082     { -255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83 },
00083     { -255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47 },
00084     { -255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21 },
00085     { -255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5 },
00086     { -255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0 }
00087 };
00088 
00089 const int curve[15] = { -152, -98, -54, -18, 0, 0, 0, 0, 0, 0, 0, 18, 54, 98, 152 };
00090 //{-200,-146,-102,-66,-36,-16,0,0,0,16,36,66,102,146,200}
00091 
00092 uint8_t SetStatus(int);
00093 uint8_t SetStatus(int pwmVal)
00094 {
00095     if (pwmVal < 0) return BACK;
00096     else if (pwmVal > 0) return FOR;
00097     else if (pwmVal == 0) return BRAKE;
00098     else return BRAKE;
00099 }
00100 uint8_t Setpwm(int);
00101 uint8_t Setpwm(int pwmVal)
00102 {
00103     if (pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
00104     else return abs(pwmVal);
00105 }
00106 
00107 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
00108 
00109 #ifdef USE_SUBPROCESS
00110 #if USE_PROCESS_NUM>0
00111 static void Process0(void);
00112 #endif
00113 #if USE_PROCESS_NUM>1
00114 static void Process1(void);
00115 #endif
00116 #if USE_PROCESS_NUM>2
00117 static void Process2(void);
00118 #endif
00119 #if USE_PROCESS_NUM>3
00120 static void Process3(void);
00121 #endif
00122 #if USE_PROCESS_NUM>4
00123 static void Process4(void);
00124 #endif
00125 #if USE_PROCESS_NUM>5
00126 static void Process5(void);
00127 #endif
00128 #if USE_PROCESS_NUM>6
00129 static void Process6(void);
00130 #endif
00131 #if USE_PROCESS_NUM>7
00132 static void Process7(void);
00133 #endif
00134 #if USE_PROCESS_NUM>8
00135 static void Process8(void);
00136 #endif
00137 #if USE_PROCESS_NUM>9
00138 static void Process9(void);
00139 #endif
00140 #endif
00141 
00142 void SystemProcessInitialize()
00143 {
00144     #pragma region USER-DEFINED_VARIABLE_INIT
00145     /*Replace here with the initialization code of your variables.*/
00146     #pragma endregion USER-DEFINED_VARIABLE_INIT
00147 
00148     lock = true;
00149     processChangeComp = true;
00150     current = DEFAULT_PROCESS;
00151 
00152 #ifdef USE_SUBPROCESS
00153 #if USE_PROCESS_NUM>0
00154     Process[0] = Process0;
00155 #endif
00156 #if USE_PROCESS_NUM>1
00157     Process[1] = Process1;
00158 #endif
00159 #if USE_PROCESS_NUM>2
00160     Process[2] = Process2;
00161 #endif
00162 #if USE_PROCESS_NUM>3
00163     Process[3] = Process3;
00164 #endif
00165 #if USE_PROCESS_NUM>4
00166     Process[4] = Process4;
00167 #endif
00168 #if USE_PROCESS_NUM>5
00169     Process[5] = Process5;
00170 #endif
00171 #if USE_PROCESS_NUM>6
00172     Process[6] = Process6;
00173 #endif
00174 #if USE_PROCESS_NUM>7
00175     Process[7] = Process7;
00176 #endif
00177 #if USE_PROCESS_NUM>8
00178     Process[8] = Process8;
00179 #endif
00180 #if USE_PROCESS_NUM>9
00181     Process[9] = Process9;
00182 #endif
00183 #endif
00184 }
00185 
00186 static void SystemProcessUpdate()
00187 {
00188 #ifdef USE_SUBPROCESS
00189     if(controller->Button.HOME) lock = false;
00190 
00191     if(controller->Button.START && processChangeComp) {
00192         current++;
00193         if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
00194         processChangeComp = false;
00195     } else if(controller->Button.SELECT && processChangeComp) {
00196         current--;
00197         if (current < 0) current = 0;
00198         processChangeComp = false;
00199     } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
00200 #endif
00201 
00202 #ifdef USE_MOTOR
00203     ACTUATORHUB::MOTOR::Motor::Update(motor);
00204 #endif
00205 
00206 #ifdef USE_SOLENOID
00207     ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
00208 #endif
00209 
00210 #ifdef USE_RS485
00211     ACTUATORHUB::ActuatorHub::Update();
00212 #endif
00213 
00214 }
00215 
00216 int g[8];
00217 
00218 void SystemProcess()
00219 {
00220     SystemProcessInitialize();
00221 
00222     while(1) {
00223         for(int i = 0; i < 8; i++) {
00224             g[i] = LineHub::GetPara(i);
00225         }
00226 
00227 #ifdef USE_MU
00228         controller = CONTROLLER::Controller::GetData();
00229 #endif
00230 
00231 #ifdef USE_ERRORCHECK
00232         if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
00233             CONTROLLER::Controller::DataReset();
00234             AllActuatorReset();
00235             lock = true;
00236         } else
00237 #endif
00238         {
00239 
00240 #ifdef USE_SUBPROCESS
00241             if(!lock) {
00242                 Process[current]();
00243             } else
00244 #endif
00245             {
00246                 //ロック時の処理
00247             }
00248         }
00249         /*
00250         //Emergency!
00251         if(!EMG_0 && !EMG_1 && !EMGflag) {
00252             buzzer = 0;
00253             BuzzerTimer.attach(BuzzerTimer_func, 1);
00254             EMGflag = true;
00255             LED_DEBUG0 = 1;
00256         }
00257         if(EMG_0 && EMG_1 && EMGflag) {
00258             buzzer = 1;
00259             BuzzerTimer.detach();
00260             EMGflag = false;
00261         }
00262         */
00263         SystemProcessUpdate();
00264     }
00265 }
00266 
00267 
00268 
00269 
00270 #pragma region PROCESS
00271 #ifdef USE_SUBPROCESS
00272 #if USE_PROCESS_NUM>0
00273 static void Process0()
00274 {
00275 
00276 }
00277 #endif
00278 
00279 #if USE_PROCESS_NUM>1
00280 
00281 
00282 bool dz1=true;
00283 bool dz1i=false;
00284 bool dz2=true;
00285 bool dz2i=false;
00286 bool dz3=true;
00287 bool dz3i=false;
00288 bool dz4=true;
00289 bool dz4i=false;
00290 
00291 int mode=10;
00292 int mode1=0;
00293 int mode1g0=100000;
00294 
00295 
00296 static void Process1()
00297 {
00298     mode1g0=100000;
00299     mode=10;
00300     mode1=0;
00301     motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
00302     motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
00303     motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]);
00304     motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);
00305 
00306     motor[TIRE_FR].pwm = Setpwm(omni[controller->AnalogL.Y][14-controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
00307     motor[TIRE_FL].pwm = Setpwm(omni[controller->AnalogL.Y][controller->AnalogL.X]+ curve[controller->AnalogR.X])*0.2;
00308     motor[TIRE_BR].pwm = Setpwm(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
00309     motor[TIRE_BL].pwm = Setpwm(omni[controller->AnalogL.X][14-controller->AnalogL.Y]+ curve[controller->AnalogR.X])*0.2;
00310 
00311     if(controller->Button.UP||controller->Button.DOWN) {
00312 
00313         if(controller->Button.UP) {
00314             motor[MOTOR_0].pwm = 110;
00315             motor[MOTOR_0].dir = FOR;
00316         }
00317         if(controller->Button.DOWN) {
00318             motor[MOTOR_0].pwm = 50;
00319             motor[MOTOR_0].dir = BACK;
00320         }
00321     } else {
00322         motor[MOTOR_0].pwm = 0;
00323         motor[MOTOR_0].dir = BRAKE;
00324     }
00325     if(controller->Button.X) {
00326         if(dz1==true) {
00327             if(dz1i==false) {
00328                 solenoid.solenoid1 = SOLENOID_ON;
00329                 dz1i=true;
00330             } else {
00331                 solenoid.solenoid1 = SOLENOID_OFF;
00332                 dz1i=false;
00333             }
00334             dz1=false;
00335         }
00336     } else {
00337         dz1=true;
00338     }
00339 
00340     if(controller->Button.Y) {
00341         if(dz2==true) {
00342             if(dz2i==false) {
00343                 solenoid.solenoid2 = SOLENOID_ON;
00344                 dz2i=true;
00345             } else {
00346                 solenoid.solenoid2 = SOLENOID_OFF;
00347                 dz2i=false;
00348             }
00349             dz2=false;
00350         }
00351     } else {
00352         dz2=true;
00353     }
00354 
00355     if(controller->Button.A) {
00356         if(dz3==true) {
00357             if(dz3i==false) {
00358                 solenoid.solenoid3 = SOLENOID_ON;
00359                 dz3i=true;
00360             } else {
00361                 solenoid.solenoid3 = SOLENOID_OFF;
00362                 dz3i=false;
00363             }
00364             dz3=false;
00365         }
00366     } else {
00367         dz3=true;
00368     }
00369 
00370     if(controller->Button.B) {
00371         if(dz4==true) {
00372             if(dz4i==false) {
00373                 solenoid.solenoid4 = SOLENOID_ON;
00374                 dz4i=true;
00375             } else {
00376                 solenoid.solenoid4 = SOLENOID_OFF;
00377                 dz4i=false;
00378             }
00379             dz4=false;
00380         }
00381     } else {
00382         dz4=true;
00383     }
00384     /*
00385     if(controller->Button.RIGHT){
00386      motor[MOTOR_1].dir = FOR;
00387      motor[MOTOR_1].pwm = 60;
00388      if (ARM_1){
00389       motor[MOTOR_1].dir = BRAKE;
00390      }
00391     }
00392     else if(controller->Button.LEFT){
00393      motor[MOTOR_1].dir = BACK;
00394      motor[MOTOR_1].pwm = 60;
00395      if (ARM_0){
00396       motor[MOTOR_1].dir = BRAKE;
00397      }
00398     }
00399     */
00400 }
00401 #endif
00402 
00403 #if USE_PROCESS_NUM>2
00404 bool mtc=false;
00405 //追加
00406 bool mtc2 = false;
00407 
00408 static void Process2()
00409 {
00410 //    printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
00411     mode1g0++;
00412     if(g[0]!=99&&mode==10) {
00413         switch(g[0]) {
00414             case 98:
00415                 motor[TIRE_FR].pwm = 30;
00416                 motor[TIRE_FR].dir = BACK;
00417                 motor[TIRE_FL].pwm = 30;
00418                 motor[TIRE_FL].dir = FOR;
00419                 motor[TIRE_BR].pwm = 30;
00420                 motor[TIRE_BR].dir = BACK;
00421                 motor[TIRE_BL].pwm = 30;
00422                 motor[TIRE_BL].dir = FOR;
00423                 if(mode1g0>100000) {
00424                     mode1++;
00425                     mode1g0=0;
00426                 }
00427                 mtc=true;
00428                 if(mode1==2) {
00429                     mode=11;
00430                     mode1g0=0;
00431                 }
00432                 break;
00433             case 0:
00434                 motor[TIRE_FR].pwm = 60;
00435                 motor[TIRE_FR].dir = BACK;
00436                 motor[TIRE_FL].pwm = 60;
00437                 motor[TIRE_FL].dir = FOR;
00438                 motor[TIRE_BR].pwm = 60;
00439                 motor[TIRE_BR].dir = BACK;
00440                 motor[TIRE_BL].pwm = 60;
00441                 motor[TIRE_BL].dir = FOR;
00442                 mtc=true;
00443                 break;
00444             case 255:
00445                 motor[TIRE_FR].pwm = 60;
00446                 motor[TIRE_FR].dir = BACK;
00447                 motor[TIRE_FL].pwm = 40;
00448                 motor[TIRE_FL].dir = FOR;
00449                 motor[TIRE_BR].pwm = 40;
00450                 motor[TIRE_BR].dir = BACK;
00451                 motor[TIRE_BL].pwm = 60;
00452                 motor[TIRE_BL].dir = FOR;
00453                 mtc=true;
00454                 break;
00455             case 253:
00456                 motor[TIRE_FR].pwm = 20;
00457                 motor[TIRE_FR].dir = BACK;
00458                 motor[TIRE_FL].pwm = 0;
00459                 motor[TIRE_FL].dir = FREE;
00460                 motor[TIRE_BR].pwm = 0;
00461                 motor[TIRE_BR].dir = FREE;
00462                 motor[TIRE_BL].pwm = 20;
00463                 motor[TIRE_BL].dir = FOR;
00464                 mtc=true;
00465                 break;
00466             case 254:
00467                 motor[TIRE_FR].pwm = 30;
00468                 motor[TIRE_FR].dir = BACK;
00469                 motor[TIRE_FL].pwm = 0;
00470                 motor[TIRE_FL].dir = FREE;
00471                 motor[TIRE_BR].pwm = 0;
00472                 motor[TIRE_BR].dir = FREE;
00473                 motor[TIRE_BL].pwm = 30;
00474                 motor[TIRE_BL].dir = FOR;
00475                 mtc=true;
00476                 break;
00477             case 1:
00478                 motor[TIRE_FR].pwm = 40;
00479                 motor[TIRE_FR].dir = BACK;
00480                 motor[TIRE_FL].pwm = 60;
00481                 motor[TIRE_FL].dir = FOR;
00482                 motor[TIRE_BR].pwm = 60;
00483                 motor[TIRE_BR].dir = BACK;
00484                 motor[TIRE_BL].pwm = 40;
00485                 motor[TIRE_BL].dir = FOR;
00486                 mtc=true;
00487                 break;
00488             case 3:
00489                 motor[TIRE_FR].pwm = 0;
00490                 motor[TIRE_FR].dir = FREE;
00491                 motor[TIRE_FL].pwm = 20;
00492                 motor[TIRE_FL].dir = FOR;
00493                 motor[TIRE_BR].pwm = 20;
00494                 motor[TIRE_BR].dir = BACK;
00495                 motor[TIRE_BL].pwm = 0;
00496                 motor[TIRE_BL].dir = FREE;
00497                 mtc=true;
00498                 break;
00499             case 2:
00500                 motor[TIRE_FR].pwm = 0;
00501                 motor[TIRE_FR].dir = FREE;
00502                 motor[TIRE_FL].pwm = 30;
00503                 motor[TIRE_FL].dir = FOR;
00504                 motor[TIRE_BR].pwm = 30;
00505                 motor[TIRE_BR].dir = BACK;
00506                 motor[TIRE_BL].pwm = 0;
00507                 motor[TIRE_BL].dir = FREE;
00508                 mtc=true;
00509                 break;
00510         }
00511         if(g[0]!=98&&mtc==true) {
00512             switch(g[1]) {
00513                 case 0:
00514                     mtc=false;
00515                     break;
00516                 case 255:
00517                     motor[TIRE_BR].pwm += 5;
00518                     mtc=false;
00519                     break;
00520                 case 253:
00521                     motor[TIRE_BR].pwm += 10;
00522                     mtc=false;
00523                     break;
00524                 case 254:
00525                     motor[TIRE_BR].pwm += 20;
00526                     motor[TIRE_BL].pwm = 0;
00527                     mtc=false;
00528                     break;
00529                 case 1:
00530                     motor[TIRE_BL].pwm += 5;
00531                     mtc=false;
00532                     break;
00533                 case 3:
00534                     motor[TIRE_BL].pwm += 10;
00535                     mtc=false;
00536                     break;
00537                 case 2:
00538                     motor[TIRE_BL].pwm += 20;
00539                     motor[TIRE_BR].pwm = 0;
00540                     mtc=false;
00541                     break;
00542             }
00543         }
00544     } else if(mode==11) {
00545         motor[TIRE_FR].pwm = 15;
00546         motor[TIRE_FR].dir = BACK;
00547         motor[TIRE_FL].pwm = 15;
00548         motor[TIRE_FL].dir = FOR;
00549         motor[TIRE_BR].pwm = 15;
00550         motor[TIRE_BR].dir = BACK;
00551         motor[TIRE_BL].pwm = 15;
00552         motor[TIRE_BL].dir = FOR;
00553         if(mode1g0>5000&&g[2]==0) {
00554             mode1g0=0;
00555             mode=12;
00556         }
00557     } else if(mode==12) {
00558         motor[TIRE_FR].pwm = 15;
00559         motor[TIRE_FR].dir = FOR;
00560         motor[TIRE_FL].pwm = 15;
00561         motor[TIRE_FL].dir = BACK;
00562         motor[TIRE_BR].pwm = 15;
00563         motor[TIRE_BR].dir = FOR;
00564         motor[TIRE_BL].pwm = 15;
00565         motor[TIRE_BL].dir = BACK;
00566         mode1g0++;
00567         if(mode1g0>10000&&g[2]==0) {
00568             mode1g0=0;
00569             mode1=0;
00570             mode=20;
00571         }
00572     } else if(mode==20) {
00573         switch(g[2]) {
00574             case 98:
00575                 motor[TIRE_FR].pwm = 30;
00576                 motor[TIRE_FR].dir = FOR;
00577                 motor[TIRE_FL].pwm = 30;
00578                 motor[TIRE_FL].dir = FOR;
00579                 motor[TIRE_BR].pwm = 30;
00580                 motor[TIRE_BR].dir = BACK;
00581                 motor[TIRE_BL].pwm = 30;
00582                 motor[TIRE_BL].dir = BACK;
00583                 if(mode1g0>100000) {
00584                     mode1++;
00585                     mode1g0=0;
00586                 }
00587                 mtc2=true;
00588                 if(mode1==3) {
00589                     mode=21;
00590                     mode1g0=0;
00591                     mode1=0;
00592                 }
00593                 break;
00594             case 0:
00595                 motor[TIRE_FR].pwm = 60;
00596                 motor[TIRE_FR].dir = FOR;
00597                 motor[TIRE_FL].pwm = 60;
00598                 motor[TIRE_FL].dir = FOR;
00599                 motor[TIRE_BR].pwm = 60;
00600                 motor[TIRE_BR].dir = BACK;
00601                 motor[TIRE_BL].pwm = 60;
00602                 motor[TIRE_BL].dir = BACK;
00603                 mtc2=true;
00604                 break;
00605             case 255:
00606                 motor[TIRE_FR].pwm = 40;
00607                 motor[TIRE_FR].dir = FOR;
00608                 motor[TIRE_FL].pwm = 60;
00609                 motor[TIRE_FL].dir = FOR;
00610                 motor[TIRE_BR].pwm = 60;
00611                 motor[TIRE_BR].dir = BACK;
00612                 motor[TIRE_BL].pwm = 40;
00613                 motor[TIRE_BL].dir = BACK;
00614                 mtc2=true;
00615                 break;
00616             case 253:
00617                 motor[TIRE_FR].pwm = 0;
00618                 motor[TIRE_FR].dir = FREE;
00619                 motor[TIRE_FL].pwm = 20;
00620                 motor[TIRE_FL].dir = FOR;
00621                 motor[TIRE_BR].pwm = 20;
00622                 motor[TIRE_BR].dir = BACK;
00623                 motor[TIRE_BL].pwm = 0;
00624                 motor[TIRE_BL].dir = FREE;
00625                 mtc2=true;
00626                 break;
00627             case 254:
00628                 motor[TIRE_FR].pwm = 0;
00629                 motor[TIRE_FR].dir = FREE;
00630                 motor[TIRE_FL].pwm = 30;
00631                 motor[TIRE_FL].dir = FOR;
00632                 motor[TIRE_BR].pwm = 30;
00633                 motor[TIRE_BR].dir = BACK;
00634                 motor[TIRE_BL].pwm = 0;
00635                 motor[TIRE_BL].dir = FREE;
00636                 mtc2=true;
00637                 break;
00638             case 1:
00639                 motor[TIRE_FR].pwm = 60;
00640                 motor[TIRE_FR].dir = FOR;
00641                 motor[TIRE_FL].pwm = 40;
00642                 motor[TIRE_FL].dir = FOR;
00643                 motor[TIRE_BR].pwm = 40;
00644                 motor[TIRE_BR].dir = BACK;
00645                 motor[TIRE_BL].pwm = 60;
00646                 motor[TIRE_BL].dir = BACK;
00647                 mtc2=true;
00648                 break;
00649             case 3:
00650                 motor[TIRE_FR].pwm = 20;
00651                 motor[TIRE_FR].dir = FOR;
00652                 motor[TIRE_FL].pwm = 0;
00653                 motor[TIRE_FL].dir = FREE;
00654                 motor[TIRE_BR].pwm = 0;
00655                 motor[TIRE_BR].dir = FREE;
00656                 motor[TIRE_BL].pwm = 20;
00657                 motor[TIRE_BL].dir = BACK;
00658                 mtc2=true;
00659                 break;
00660             case 2:
00661                 motor[TIRE_FR].pwm = 30;
00662                 motor[TIRE_FR].dir = FOR;
00663                 motor[TIRE_FL].pwm = 0;
00664                 motor[TIRE_FL].dir = FREE;
00665                 motor[TIRE_BR].pwm = 0;
00666                 motor[TIRE_BR].dir = FREE;
00667                 motor[TIRE_BL].pwm = 30;
00668                 motor[TIRE_BL].dir = BACK;
00669                 mtc2=true;
00670                 break;
00671         }
00672         if(g[2]!=98&&mtc2==true) {
00673             switch(g[3]) {
00674                 case 0:
00675                     mtc2=false;
00676                     break;
00677                 case 255:
00678                     motor[TIRE_BL].pwm += 5;
00679                     mtc2=false;
00680                     break;
00681                 case 253:
00682                     motor[TIRE_BL].pwm += 10;
00683                     mtc2=false;
00684                     break;
00685                 case 254:
00686                     motor[TIRE_BL].pwm += 20;
00687                     motor[TIRE_FL].pwm = 0;
00688                     mtc2=false;
00689                     break;
00690                 case 1:
00691                     motor[TIRE_FL].pwm += 5;
00692                     mtc2=false;
00693                     break;
00694                 case 3:
00695                     motor[TIRE_FL].pwm += 10;
00696                     mtc2=false;
00697                     break;
00698                 case 2:
00699                     motor[TIRE_FL].pwm += 20;
00700                     motor[TIRE_BL].pwm = 0;
00701                     mtc2=false;
00702                     break;
00703             }
00704         }
00705     } else if(mode==21) {
00706         motor[TIRE_FR].pwm = 15;
00707         motor[TIRE_FR].dir = FOR;
00708         motor[TIRE_FL].pwm = 15;
00709         motor[TIRE_FL].dir = FOR;
00710         motor[TIRE_BR].pwm = 15;
00711         motor[TIRE_BR].dir = BACK;
00712         motor[TIRE_BL].pwm = 15;
00713         motor[TIRE_BL].dir = BACK;
00714         if(mode1g0>5000&&g[0]==0) {
00715             mode1g0=0;
00716             mode=22;
00717         }
00718     } else if(mode==22) {
00719         motor[TIRE_FR].pwm = 15;
00720         motor[TIRE_FR].dir = BACK;
00721         motor[TIRE_FL].pwm = 15;
00722         motor[TIRE_FL].dir = BACK;
00723         motor[TIRE_BR].pwm = 15;
00724         motor[TIRE_BR].dir = FOR;
00725         motor[TIRE_BL].pwm = 15;
00726         motor[TIRE_BL].dir = FOR;
00727         if(mode1g0>10000&&g[0]==0) {
00728             mode1g0=0;
00729             mode1=0;
00730             mode=30;
00731         }
00732     } else if(mode==30) {
00733         switch(g[0]) {
00734             case 98:
00735                 motor[TIRE_FR].pwm = 30;
00736                 motor[TIRE_FR].dir = BACK;
00737                 motor[TIRE_FL].pwm = 30;
00738                 motor[TIRE_FL].dir = FOR;
00739                 motor[TIRE_BR].pwm = 30;
00740                 motor[TIRE_BR].dir = BACK;
00741                 motor[TIRE_BL].pwm = 30;
00742                 motor[TIRE_BL].dir = FOR;
00743                 if(mode1g0>100000) {
00744                     mode1++;
00745                     mode1g0=0;
00746                 }
00747                 mtc=true;
00748                 if(mode1==1) {
00749                     mode=31;
00750                     mode1g0=0;
00751                 }
00752                 break;
00753             case 0:
00754                 motor[TIRE_FR].pwm = 60;
00755                 motor[TIRE_FR].dir = BACK;
00756                 motor[TIRE_FL].pwm = 60;
00757                 motor[TIRE_FL].dir = FOR;
00758                 motor[TIRE_BR].pwm = 60;
00759                 motor[TIRE_BR].dir = BACK;
00760                 motor[TIRE_BL].pwm = 60;
00761                 motor[TIRE_BL].dir = FOR;
00762                 mtc=true;
00763                 break;
00764             case 255:
00765                 motor[TIRE_FR].pwm = 60;
00766                 motor[TIRE_FR].dir = BACK;
00767                 motor[TIRE_FL].pwm = 40;
00768                 motor[TIRE_FL].dir = FOR;
00769                 motor[TIRE_BR].pwm = 40;
00770                 motor[TIRE_BR].dir = BACK;
00771                 motor[TIRE_BL].pwm = 60;
00772                 motor[TIRE_BL].dir = FOR;
00773                 mtc=true;
00774                 break;
00775             case 253:
00776                 motor[TIRE_FR].pwm = 20;
00777                 motor[TIRE_FR].dir = BACK;
00778                 motor[TIRE_FL].pwm = 0;
00779                 motor[TIRE_FL].dir = FREE;
00780                 motor[TIRE_BR].pwm = 0;
00781                 motor[TIRE_BR].dir = FREE;
00782                 motor[TIRE_BL].pwm = 20;
00783                 motor[TIRE_BL].dir = FOR;
00784                 mtc=true;
00785                 break;
00786             case 254:
00787                 motor[TIRE_FR].pwm = 30;
00788                 motor[TIRE_FR].dir = BACK;
00789                 motor[TIRE_FL].pwm = 0;
00790                 motor[TIRE_FL].dir = FREE;
00791                 motor[TIRE_BR].pwm = 0;
00792                 motor[TIRE_BR].dir = FREE;
00793                 motor[TIRE_BL].pwm = 30;
00794                 motor[TIRE_BL].dir = FOR;
00795                 mtc=true;
00796                 break;
00797             case 1:
00798                 motor[TIRE_FR].pwm = 40;
00799                 motor[TIRE_FR].dir = BACK;
00800                 motor[TIRE_FL].pwm = 60;
00801                 motor[TIRE_FL].dir = FOR;
00802                 motor[TIRE_BR].pwm = 60;
00803                 motor[TIRE_BR].dir = BACK;
00804                 motor[TIRE_BL].pwm = 40;
00805                 motor[TIRE_BL].dir = FOR;
00806                 mtc=true;
00807                 break;
00808             case 3:
00809                 motor[TIRE_FR].pwm = 0;
00810                 motor[TIRE_FR].dir = FREE;
00811                 motor[TIRE_FL].pwm = 20;
00812                 motor[TIRE_FL].dir = FOR;
00813                 motor[TIRE_BR].pwm = 20;
00814                 motor[TIRE_BR].dir = BACK;
00815                 motor[TIRE_BL].pwm = 0;
00816                 motor[TIRE_BL].dir = FREE;
00817                 mtc=true;
00818                 break;
00819             case 2:
00820                 motor[TIRE_FR].pwm = 0;
00821                 motor[TIRE_FR].dir = FREE;
00822                 motor[TIRE_FL].pwm = 30;
00823                 motor[TIRE_FL].dir = FOR;
00824                 motor[TIRE_BR].pwm = 30;
00825                 motor[TIRE_BR].dir = BACK;
00826                 motor[TIRE_BL].pwm = 0;
00827                 motor[TIRE_BL].dir = FREE;
00828                 mtc=true;
00829                 break;
00830         }
00831         if(g[0]!=98&&mtc==true) {
00832             switch(g[1]) {
00833                 case 0:
00834                     mtc=false;
00835                     break;
00836                 case 255:
00837                     motor[TIRE_BR].pwm += 5;
00838                     mtc=false;
00839                     break;
00840                 case 253:
00841                     motor[TIRE_BR].pwm += 10;
00842                     mtc=false;
00843                     break;
00844                 case 254:
00845                     motor[TIRE_BR].pwm += 20;
00846                     motor[TIRE_BL].pwm = 0;
00847                     mtc=false;
00848                     break;
00849                 case 1:
00850                     motor[TIRE_BL].pwm += 5;
00851                     mtc=false;
00852                     break;
00853                 case 3:
00854                     motor[TIRE_BL].pwm += 10;
00855                     mtc=false;
00856                     break;
00857                 case 2:
00858                     motor[TIRE_BL].pwm += 20;
00859                     motor[TIRE_BR].pwm = 0;
00860                     mtc=false;
00861                     break;
00862             }
00863         }
00864     } else if(mode==31) {
00865         motor[TIRE_FR].pwm = 15;
00866         motor[TIRE_FR].dir = BACK;
00867         motor[TIRE_FL].pwm = 15;
00868         motor[TIRE_FL].dir = FOR;
00869         motor[TIRE_BR].pwm = 15;
00870         motor[TIRE_BR].dir = BACK;
00871         motor[TIRE_BL].pwm = 15;
00872         motor[TIRE_BL].dir = FOR;
00873         mode1g0++;
00874         if(mode1g0>5000&&g[3]==0) {
00875             mode1g0=0;
00876             mode=32;
00877         }
00878     } else if(mode==32) {
00879         motor[TIRE_FR].pwm = 15;
00880         motor[TIRE_FR].dir = FOR;
00881         motor[TIRE_FL].pwm = 15;
00882         motor[TIRE_FL].dir = BACK;
00883         motor[TIRE_BR].pwm = 15;
00884         motor[TIRE_BR].dir = FOR;
00885         motor[TIRE_BL].pwm = 15;
00886         motor[TIRE_BL].dir = BACK;
00887         mode1g0++;
00888         if(mode1g0>5000&&g[3]==0) {
00889             mode1g0=0;
00890             mode1=0;
00891             mode=40;
00892         }
00893     } else if(mode==40) {
00894         switch(g[3]) {
00895             case 98:
00896                 motor[TIRE_FR].pwm = 30;
00897                 motor[TIRE_FR].dir = BACK;
00898                 motor[TIRE_FL].pwm = 30;
00899                 motor[TIRE_FL].dir = BACK;
00900                 motor[TIRE_BR].pwm = 30;
00901                 motor[TIRE_BR].dir = FOR;
00902                 motor[TIRE_BL].pwm = 30;
00903                 motor[TIRE_BL].dir = FOR;
00904                 if(mode1g0>100000) {
00905                     mode1++;
00906                     mode1g0=0;
00907                 }
00908                 mtc2=true;
00909                 if(mode1==1) {
00910                     mode=41;
00911                     mode1g0=0;
00912                     mode1=0;
00913                 }
00914                 break;
00915             case 0:
00916                 motor[TIRE_FR].pwm = 60;
00917                 motor[TIRE_FR].dir = BACK;
00918                 motor[TIRE_FL].pwm = 60;
00919                 motor[TIRE_FL].dir = BACK;
00920                 motor[TIRE_BR].pwm = 60;
00921                 motor[TIRE_BR].dir = FOR;
00922                 motor[TIRE_BL].pwm = 60;
00923                 motor[TIRE_BL].dir = FOR;
00924                 mtc2=true;
00925                 break;
00926             case 255:
00927                 motor[TIRE_FR].pwm = 40;
00928                 motor[TIRE_FR].dir = BACK;
00929                 motor[TIRE_FL].pwm = 60;
00930                 motor[TIRE_FL].dir = BACK;
00931                 motor[TIRE_BR].pwm = 60;
00932                 motor[TIRE_BR].dir = FOR;
00933                 motor[TIRE_BL].pwm = 40;
00934                 motor[TIRE_BL].dir = FOR;
00935                 mtc2=true;
00936                 break;
00937             case 253:
00938                 motor[TIRE_FR].pwm = 0;
00939                 motor[TIRE_FR].dir = FREE;
00940                 motor[TIRE_FL].pwm = 20;
00941                 motor[TIRE_FL].dir = BACK;
00942                 motor[TIRE_BR].pwm = 20;
00943                 motor[TIRE_BR].dir = FOR;
00944                 motor[TIRE_BL].pwm = 0;
00945                 motor[TIRE_BL].dir = FREE;
00946                 mtc2=true;
00947                 break;
00948             case 254:
00949                 motor[TIRE_FR].pwm = 0;
00950                 motor[TIRE_FR].dir = FREE;
00951                 motor[TIRE_FL].pwm = 30;
00952                 motor[TIRE_FL].dir = BACK;
00953                 motor[TIRE_BR].pwm = 30;
00954                 motor[TIRE_BR].dir = FOR;
00955                 motor[TIRE_BL].pwm = 0;
00956                 motor[TIRE_BL].dir = FREE;
00957                 mtc2=true;
00958                 break;
00959             case 1:
00960                 motor[TIRE_FR].pwm = 60;
00961                 motor[TIRE_FR].dir = BACK;
00962                 motor[TIRE_FL].pwm = 40;
00963                 motor[TIRE_FL].dir = BACK;
00964                 motor[TIRE_BR].pwm = 40;
00965                 motor[TIRE_BR].dir = FOR;
00966                 motor[TIRE_BL].pwm = 60;
00967                 motor[TIRE_BL].dir = FOR;
00968                 mtc2=true;
00969                 break;
00970             case 3:
00971                 motor[TIRE_FR].pwm = 20;
00972                 motor[TIRE_FR].dir = BACK;
00973                 motor[TIRE_FL].pwm = 0;
00974                 motor[TIRE_FL].dir = FREE;
00975                 motor[TIRE_BR].pwm = 0;
00976                 motor[TIRE_BR].dir = FREE;
00977                 motor[TIRE_BL].pwm = 20;
00978                 motor[TIRE_BL].dir = FOR;
00979                 mtc2=true;
00980                 break;
00981             case 2:
00982                 motor[TIRE_FR].pwm = 30;
00983                 motor[TIRE_FR].dir = BACK;
00984                 motor[TIRE_FL].pwm = 0;
00985                 motor[TIRE_FL].dir = FREE;
00986                 motor[TIRE_BR].pwm = 0;
00987                 motor[TIRE_BR].dir = FREE;
00988                 motor[TIRE_BL].pwm = 30;
00989                 motor[TIRE_BL].dir = FOR;
00990                 mtc2=true;
00991                 break;
00992         }
00993         if(g[2]!=98&&mtc2==true) {
00994             switch(g[2]) {
00995                 case 0:
00996                     mtc2=false;
00997                     break;
00998                 case 255:
00999                     motor[TIRE_FL].pwm += 5;
01000                     mtc2=false;
01001                     break;
01002                 case 253:
01003                     motor[TIRE_FL].pwm += 10;
01004                     mtc2=false;
01005                     break;
01006                 case 254:
01007                     motor[TIRE_FL].pwm += 20;
01008                     motor[TIRE_BL].pwm = 0;
01009                     mtc2=false;
01010                     break;
01011                 case 1:
01012                     motor[TIRE_BL].pwm += 5;
01013                     mtc2=false;
01014                     break;
01015                 case 3:
01016                     motor[TIRE_BL].pwm += 10;
01017                     mtc2=false;
01018                     break;
01019                 case 2:
01020                     motor[TIRE_BL].pwm += 20;
01021                     motor[TIRE_FL].pwm = 0;
01022                     mtc2=false;
01023                     break;
01024             }
01025         }
01026     } else if(mode==41) {
01027         motor[TIRE_FR].pwm = 15;
01028         motor[TIRE_FR].dir = BACK;
01029         motor[TIRE_FL].pwm = 15;
01030         motor[TIRE_FL].dir = BACK;
01031         motor[TIRE_BR].pwm = 15;
01032         motor[TIRE_BR].dir = FOR;
01033         motor[TIRE_BL].pwm = 15;
01034         motor[TIRE_BL].dir = FOR;
01035         if(mode1g0>5000&&g[1]==0) {
01036             mode1g0=0;
01037             mode=42;
01038         }
01039     } else if(mode==42) {
01040         motor[TIRE_FR].pwm = 15;
01041         motor[TIRE_FR].dir = FOR;
01042         motor[TIRE_FL].pwm = 15;
01043         motor[TIRE_FL].dir = FOR;
01044         motor[TIRE_BR].pwm = 15;
01045         motor[TIRE_BR].dir = BACK;
01046         motor[TIRE_BL].pwm = 15;
01047         motor[TIRE_BL].dir = BACK;
01048         if(mode1g0>5000&&g[1]==0) {
01049             mode1g0=0;
01050             mode=50;
01051         } else if(mode==50) {
01052             switch(g[1]) {
01053                 case 98:
01054                     motor[TIRE_FR].pwm = 30;
01055                     motor[TIRE_FR].dir = FOR;
01056                     motor[TIRE_FL].pwm = 30;
01057                     motor[TIRE_FL].dir = BACK;
01058                     motor[TIRE_BR].pwm = 30;
01059                     motor[TIRE_BR].dir = FOR;
01060                     motor[TIRE_BL].pwm = 30;
01061                     motor[TIRE_BL].dir = BACK;
01062                     if(mode1g0>100000) {
01063                         mode1++;
01064                         mode1g0=0;
01065                     }
01066                     mtc=true;
01067                     if(mode1==1) {
01068                         mode=51;
01069                         mode1g0=0;
01070                     }
01071                     break;
01072                 case 0:
01073                     motor[TIRE_FR].pwm = 60;
01074                     motor[TIRE_FR].dir = FOR;
01075                     motor[TIRE_FL].pwm = 60;
01076                     motor[TIRE_FL].dir = BACK;
01077                     motor[TIRE_BR].pwm = 60;
01078                     motor[TIRE_BR].dir = FOR;
01079                     motor[TIRE_BL].pwm = 60;
01080                     motor[TIRE_BL].dir = BACK;
01081                     mtc=true;
01082                     break;
01083                 case 255:
01084                     motor[TIRE_FR].pwm = 60;
01085                     motor[TIRE_FR].dir = FOR;
01086                     motor[TIRE_FL].pwm = 40;
01087                     motor[TIRE_FL].dir = BACK;
01088                     motor[TIRE_BR].pwm = 40;
01089                     motor[TIRE_BR].dir = FOR;
01090                     motor[TIRE_BL].pwm = 60;
01091                     motor[TIRE_BL].dir = BACK;
01092                     mtc=true;
01093                     break;
01094                 case 253:
01095                     motor[TIRE_FR].pwm = 20;
01096                     motor[TIRE_FR].dir = FOR;
01097                     motor[TIRE_FL].pwm = 0;
01098                     motor[TIRE_FL].dir = FREE;
01099                     motor[TIRE_BR].pwm = 0;
01100                     motor[TIRE_BR].dir = FREE;
01101                     motor[TIRE_BL].pwm = 20;
01102                     motor[TIRE_BL].dir = BACK;
01103                     mtc=true;
01104                     break;
01105                 case 254:
01106                     motor[TIRE_FR].pwm = 30;
01107                     motor[TIRE_FR].dir = FOR;
01108                     motor[TIRE_FL].pwm = 0;
01109                     motor[TIRE_FL].dir = FREE;
01110                     motor[TIRE_BR].pwm = 0;
01111                     motor[TIRE_BR].dir = FREE;
01112                     motor[TIRE_BL].pwm = 30;
01113                     motor[TIRE_BL].dir = BACK;
01114                     mtc=true;
01115                     break;
01116                 case 1:
01117                     motor[TIRE_FR].pwm = 40;
01118                     motor[TIRE_FR].dir = FOR;
01119                     motor[TIRE_FL].pwm = 60;
01120                     motor[TIRE_FL].dir = BACK;
01121                     motor[TIRE_BR].pwm = 60;
01122                     motor[TIRE_BR].dir = FOR;
01123                     motor[TIRE_BL].pwm = 40;
01124                     motor[TIRE_BL].dir = BACK;
01125                     mtc=true;
01126                     break;
01127                 case 3:
01128                     motor[TIRE_FR].pwm = 0;
01129                     motor[TIRE_FR].dir = FREE;
01130                     motor[TIRE_FL].pwm = 20;
01131                     motor[TIRE_FL].dir = BACK;
01132                     motor[TIRE_BR].pwm = 20;
01133                     motor[TIRE_BR].dir = FOR;
01134                     motor[TIRE_BL].pwm = 0;
01135                     motor[TIRE_BL].dir = FREE;
01136                     mtc=true;
01137                     break;
01138                 case 2:
01139                     motor[TIRE_FR].pwm = 0;
01140                     motor[TIRE_FR].dir = FREE;
01141                     motor[TIRE_FL].pwm = 30;
01142                     motor[TIRE_FL].dir = BACK;
01143                     motor[TIRE_BR].pwm = 30;
01144                     motor[TIRE_BR].dir = FOR;
01145                     motor[TIRE_BL].pwm = 0;
01146                     motor[TIRE_BL].dir = FREE;
01147                     mtc=true;
01148                     break;
01149             }
01150             if(g[1]!=98&&mtc==true) {
01151                 switch(g[0]) {
01152                     case 0:
01153                         mtc=false;
01154                         break;
01155                     case 255:
01156                         motor[TIRE_FR].pwm += 5;
01157                         mtc=false;
01158                         break;
01159                     case 253:
01160                         motor[TIRE_FR].pwm += 10;
01161                         mtc=false;
01162                         break;
01163                     case 254:
01164                         motor[TIRE_FR].pwm += 20;
01165                         motor[TIRE_FL].pwm = 0;
01166                         mtc=false;
01167                         break;
01168                     case 1:
01169                         motor[TIRE_FL].pwm += 5;
01170                         mtc=false;
01171                         break;
01172                     case 3:
01173                         motor[TIRE_FL].pwm += 10;
01174                         mtc=false;
01175                         break;
01176                     case 2:
01177                         motor[TIRE_FL].pwm += 20;
01178                         motor[TIRE_FR].pwm = 0;
01179                         mtc=false;
01180                         break;
01181                 }
01182             }
01183         }
01184     } else if(mode==51) {
01185         motor[TIRE_FR].pwm = 0;
01186         motor[TIRE_FR].dir = BRAKE;
01187         motor[TIRE_FL].pwm = 0;
01188         motor[TIRE_FL].dir = BRAKE;
01189         motor[TIRE_BR].pwm = 0;
01190         motor[TIRE_BR].dir = BRAKE;
01191         motor[TIRE_BL].pwm = 0;
01192         motor[TIRE_BL].dir = BRAKE;
01193     } else if(mode==52) {
01194     } else {
01195         motor[TIRE_FR].pwm = 0;
01196         motor[TIRE_FR].dir = BRAKE;
01197         motor[TIRE_FL].pwm = 0;
01198         motor[TIRE_FL].dir = BRAKE;
01199         motor[TIRE_BR].pwm = 0;
01200         motor[TIRE_BR].dir = BRAKE;
01201         motor[TIRE_BL].pwm = 0;
01202         motor[TIRE_BL].dir = BRAKE;
01203     }
01204 
01205 }
01206 
01207 #endif
01208 
01209 #if USE_PROCESS_NUM>3
01210 static void Process3()
01211 {
01212 
01213 }
01214 #endif
01215 
01216 #if USE_PROCESS_NUM>4
01217 static void Process4()
01218 {
01219 
01220 }
01221 #endif
01222 
01223 #if USE_PROCESS_NUM>5
01224 static void Process5()
01225 {
01226 
01227 }
01228 #endif
01229 
01230 #if USE_PROCESS_NUM>6
01231 static void Process6()
01232 {
01233 
01234 }
01235 #endif
01236 
01237 #if USE_PROCESS_NUM>7
01238 static void Process7()
01239 {
01240 
01241 }
01242 #endif
01243 
01244 #if USE_PROCESS_NUM>8
01245 static void Process8()
01246 {
01247 
01248 }
01249 #endif
01250 
01251 #if USE_PROCESS_NUM>9
01252 static void Process9()
01253 {
01254 
01255 }
01256 #endif
01257 #endif
01258 #pragma endregion PROCESS
01259 
01260 static void AllActuatorReset()
01261 {
01262 
01263 #ifdef USE_SOLENOID
01264     solenoid.all = ALL_SOLENOID_OFF;
01265 #endif
01266 
01267 #ifdef USE_MOTOR
01268     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
01269         motor[i].dir = FREE;
01270         motor[i].pwm = 0;
01271     }
01272 #endif
01273 }
01274 
01275 /*
01276 void BuzzerTimer_func()
01277 {
01278     buzzer = !buzzer;
01279     //LED_DEBUG0 = !LED_DEBUG0;
01280 }
01281 */
01282 void TapeLedEms_func()
01283 {
01284     sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
01285 }
01286 
01287 #pragma region USER-DEFINED-FUNCTIONS
01288 
01289 
01290 #pragma endregion