Nagano kosen robocon

Dependencies:   mbed QEI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Process.cpp Source File

Process.cpp

00001 #include "mbed.h"
00002 #include "Process.h"
00003 #include "QEI.h"
00004 #include <math.h>
00005 
00006 #include "../../CommonLibraries/PID/PID.h"
00007 #include "../../CommonLibraries/Ping/Ping.h"
00008 #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
00009 #include "../../Communication/RS485/Master/Master.h"
00010 #include "../../Communication/Controller/Controller.h"
00011 #include "../../Input/ColorSensor/ColorSensor.h"
00012 #include "../../Input/Switch/Switch.h"
00013 #include "../../LED/LED.h"
00014 #include "../../Safty/Safty.h"
00015 #include "../Using.h"
00016 
00017 
00018 using namespace SWITCH;
00019 using namespace PID_SPACE;
00020 using namespace COLORSENSOR;
00021 
00022 static CONTROLLER::ControllerData *controller;
00023 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
00024 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
00025 
00026 static bool lock;
00027 static bool processChangeComp;
00028 static int current;
00029 
00030 static void AllActuatorReset();
00031 
00032 #ifdef USE_SUBPROCESS
00033 static void (*Process[USE_PROCESS_NUM])(void);
00034 #endif
00035 
00036 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
00037 
00038 /*Replace here with the definition code of your variables.*/
00039 //************足回り*****************
00040 void AllBrake();
00041 void GoOshituke();
00042 void GoMae();
00043 void GoFront();
00044 void GoBack();
00045 void GoLeft();
00046 void GoRight();
00047 void GoHidariNanameMae();
00048 void GoHidariNanameUshiro();
00049 void GoMigiNanameMae();
00050 void GoMigiNanameUshiro();
00051 
00052 void RightTurning();
00053 void LeftTurning();
00054 uint8_t SetStatus(int);
00055 uint8_t SetPWM(int);
00056 bool move_flag = false;
00057 //************足回り*****************
00058 
00059 //************カラーセンサ********************
00060 
00061 int Color_FR[3]; //[赤,緑,青]
00062 int Color_FL[3];
00063 int Color_RL[3];
00064 int Color_RR[3];         
00065 int intergration = 40;
00066 
00067 int Avecolor_FR[3];
00068 int Avecolor_FL[3];
00069 int Avecolor_RL[3];
00070 int Avecolor_RR[3];
00071 
00072 void ColorIn();
00073 void ColorDetection();
00074 void getcolor();
00075 //Ticker catch_colordata;
00076 //************カラーセンサ********************
00077 
00078 //************ライントレース変数*******************
00079 
00080 int line_FR[3] = {500, 1000, 1400}; 
00081 int line_FL[3] = {400, 1000, 1400};
00082 int line_RL[3] = {180, 260, 150};
00083 int line_RR[3] = {300, 1400, 1600};
00084 /*
00085 int line_FR[3] = {500, 1100, 1500}; 
00086 int line_FL[3] = {300, 500, 380};
00087 int line_RL[3] = {50, 100, 70};
00088 int line_RR[3] = {300, 500, 300};
00089 */    
00090 bool white_FR = false;
00091 bool white_FL = false;
00092 bool white_RL = false;
00093 bool white_RR = false;
00094  
00095 bool flag = false;    
00096 bool invation_FR = false;
00097 bool invation_FL = false;
00098 bool invation_RL = false;
00099 bool invation_RR = false;
00100 bool on_line_flag = false;
00101 
00102 void Move_On_the_Line();
00103 void Move_On_the_Line_Trace();
00104 void Move_On_the_Line_BackTrace();
00105 void Move_On_the_Line_BACK();
00106 void ColorDetection();
00107 void Color_Changeflag();
00108 //************ライントレース変数*******************
00109 
00110 //************ROタコン******************
00111 //QEI RtX(Rota_XA_PIN, Rota_XB_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
00112 //QEI RtY(Rota_YA_PIN, Rota_YB_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
00113 QEI Rota_FR(Rota_FR_A_PIN, Rota_FR_B_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
00114 QEI Rota_FL(Rota_FL_A_PIN, Rota_FL_B_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
00115 QEI Rota_RL(Rota_RL_A_PIN, Rota_RL_B_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
00116 QEI Rota_RR(Rota_RR_A_PIN, Rota_RR_B_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
00117 
00118 Ticker ugokunoka_rps;
00119 Ticker catch_rps;
00120 //PID Rota_X = PID(0.03, -100, 50, 0.05, 0, 0);
00121 //PID Rota_Y = PID(0.03, -100, 50, 0.05, 0, 0);
00122 PID Get_pwm_FR = PID(0.03, -80, 80, 0.3,0,0);
00123 PID Get_pwm_FL = PID(0.03, -80, 80, 0.3, 0, 0);
00124 PID Get_pwm_RL = PID(0.03, -80, 80, 0.3, 0, 0);
00125 PID Get_pwm_RR = PID(0.03, -80, 80, 0.3, 0, 0);
00126 
00127 PID Catch_pwm_FR = PID(0.03, -100, 100, 0.3,0,0);
00128 PID Catch_pwm_FL = PID(0.03, -100, 100, 0.3, 0, 0);
00129 PID Catch_pwm_RL = PID(0.03, -100, 100, 0.3, 0, 0);
00130 PID Catch_pwm_RR = PID(0.03, -100, 100, 0.3, 0, 0);
00131 
00132 typedef struct {
00133     double FR;
00134     double FL;
00135     double RL;
00136     double RR;
00137 }Tire;
00138 
00139 Tire rps;
00140 Tire p_rps;
00141 Tire rotate;
00142 Tire now;
00143 Tire dist;
00144 
00145 typedef struct {
00146     double x;
00147     double y;
00148 }Vector;
00149 //double target = 2561.00;
00150 double target_FR = 0.00;
00151 double target_FL = 0.00;
00152 double target_RL = 0.00;
00153 double target_RR = 0.00;
00154 
00155 Vector goal = {1000.00,1000.00};
00156 Vector rpm;
00157 Vector dis;
00158 
00159 int palseX;
00160 int palseY;
00161 
00162 double wantVal_FL = 0;
00163 double wantVal_FR = 0;
00164 double wantVal_RL = 0;
00165 double wantVal_RR = 0;
00166 
00167 typedef struct {
00168     int FR;
00169     int FL;
00170     int RL;
00171     int RR;
00172 }Pwm;
00173 
00174 int pwm_FR;
00175 int pwm_FL;
00176 int pwm_RL;
00177 int pwm_RR;
00178 
00179 Pwm pwm;
00180 int hontoka = 0;
00181 
00182 int vx;
00183 int vy;
00184 double max;
00185 double ratio;
00186 double max_rotate = 4.5;
00187 
00188 void Rota_SetStatus(char,double);
00189 void Get_rps();
00190 void Get_Move();
00191 void GoBack_Rota();
00192 //************Buzzer*************
00193 DigitalOut buzzer(BUZZER_PIN);
00194 Ticker BuzzerTimer;
00195 void BuzzerTimer_func();
00196 
00197 //************動き***********
00198 bool spot2400_flag = false;
00199 bool spot1200_flag = false;
00200 bool spot1500_flag = false;
00201 bool spot1800_flag = false;
00202 bool next_flag = true;
00203 bool bluezone = true;
00204 bool redzone = false;
00205 bool once_flag = true;
00206 bool debug_flag1 = true;
00207 
00208 void CheckPoint();
00209 void MovePoint();
00210 void GoShoot();
00211 /******超音波*****/
00212 Ping Ping_0(PC_2);
00213 Ping Ping_1(PC_3);
00214 Ping Ping_2(PC_0);
00215 Ping Ping_3(PC_1);
00216 int select_kyori=0;
00217 int select = 0;
00218 int distance0 = 0;
00219 int distance1 = 0;
00220 int distance2 = 0;
00221 int distance3 = 0;
00222 bool ugoite_flag = true;
00223 bool debug_flag = true;
00224 bool temae_flag = false;
00225 bool ugokanaide_flag = false;
00226 bool ok_flag = false;
00227 int zyouge = 0;
00228 Serial pc(USBTX, USBRX);
00229 bool ushiroate_flag = false;
00230 bool kaeru_flag = false;
00231 bool twin_flag = false;
00232 bool idoutable_flag = false;
00233 bool itido_flag = true;
00234 
00235 int maeR = 0;
00236 int maeL = 0;
00237 int ushiroL = 0;
00238 int ushiroR = 0;
00239 
00240 Timeout launchTimer;
00241 void launchTimer_func();
00242 void launchTimer_func1();
00243 void launchTimer_func2();
00244 bool reset_flag = false;
00245 bool rotarota_flag = false;
00246 bool roller_flag = true;
00247 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
00248 
00249 #ifdef USE_SUBPROCESS
00250 #if USE_PROCESS_NUM>0
00251 static void Process0(void);
00252 #endif
00253 #if USE_PROCESS_NUM>1
00254 static void Process1(void);
00255 #endif
00256 #if USE_PROCESS_NUM>2
00257 static void Process2(void);
00258 #endif
00259 #if USE_PROCESS_NUM>3
00260 static void Process3(void);
00261 #endif
00262 #if USE_PROCESS_NUM>4
00263 static void Process4(void);
00264 #endif
00265 #if USE_PROCESS_NUM>5
00266 static void Process5(void);
00267 #endif
00268 #if USE_PROCESS_NUM>6
00269 static void Process6(void);
00270 #endif
00271 #if USE_PROCESS_NUM>7
00272 static void Process7(void);
00273 #endif
00274 #if USE_PROCESS_NUM>8
00275 static void Process8(void);
00276 #endif
00277 #if USE_PROCESS_NUM>9
00278 static void Process9(void);
00279 #endif
00280 #endif
00281 
00282 void SystemProcessInitialize()
00283 {
00284 #pragma region USER-DEFINED_VARIABLE_INIT
00285     /*Replace here with the initialization code of your variables.*/
00286     catch_rps.attach_us(&Get_rps,500);
00287     //catch_colordata.attach_us(&ColorDetection,30);
00288     //ugokunoka_rps.attach(&Get_Move,0.1);
00289     
00290 #pragma endregion USER-DEFINED_VARIABLE_INIT
00291 
00292     AllActuatorReset();
00293 
00294     lock = true;
00295     processChangeComp = true;
00296     current = DEFAULT_PROCESS;
00297 
00298 #ifdef USE_SUBPROCESS
00299 #if USE_PROCESS_NUM>0
00300     Process[0] = Process0;
00301 #endif
00302 #if USE_PROCESS_NUM>1
00303     Process[1] = Process1;
00304 #endif
00305 #if USE_PROCESS_NUM>2
00306     Process[2] = Process2;
00307 #endif
00308 #if USE_PROCESS_NUM>3
00309     Process[3] = Process3;
00310 #endif
00311 #if USE_PROCESS_NUM>4
00312     Process[4] = Process4;
00313 #endif
00314 #if USE_PROCESS_NUM>5
00315     Process[5] = Process5;
00316 #endif
00317 #if USE_PROCESS_NUM>6
00318     Process[6] = Process6;
00319 #endif
00320 #if USE_PROCESS_NUM>7
00321     Process[7] = Process7;
00322 #endif
00323 #if USE_PROCESS_NUM>8
00324     Process[8] = Process8;
00325 #endif
00326 #if USE_PROCESS_NUM>9
00327     Process[9] = Process9;
00328 #endif
00329 #endif
00330 }
00331 
00332 static void SystemProcessUpdate()
00333 {
00334 #ifdef USE_SUBPROCESS
00335     if(controller->Button.HOME) lock = false;
00336 
00337     if(controller->Button.START && processChangeComp) {
00338         current++;
00339         if (USE_PROCESS_NUM-1 < current) current = 0;
00340         processChangeComp = false;
00341     } else if(controller->Button.SELECT && processChangeComp) {
00342         current--;
00343         if (current < 0) current = USE_PROCESS_NUM-1;
00344         processChangeComp = false;
00345     } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
00346 #endif
00347 
00348 #ifdef USE_MOTOR
00349     ACTUATORHUB::MOTOR::Motor::Update(motor);
00350 #endif
00351 
00352 #ifdef USE_SOLENOID
00353     ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
00354 #endif
00355 
00356 #ifdef USE_RS485
00357     MASTER::Master::Update();
00358     ACTUATORHUB::ActuatorHub::Update();
00359 #endif
00360 
00361 }
00362 
00363 void SystemProcess()
00364 {
00365     SystemProcessInitialize();
00366 
00367     while(1) {
00368 #ifdef USE_MU
00369         controller = CONTROLLER::Controller::GetData();
00370 #endif
00371 
00372 #ifdef USE_ERRORCHECK
00373         if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) {
00374             CONTROLLER::Controller::DataReset();
00375             //AllActuatorReset();
00376             //lock = true;
00377         } else
00378 #endif
00379         {
00380 #ifdef USE_SUBPROCESS
00381             if(!lock) {
00382                 Process[current]();
00383             } else
00384 #endif
00385             {
00386                 //ロック時の処理
00387             }
00388         }
00389         LED_DEBUG0 = masterReceive.ems ? LED_ON : LED_OFF;
00390         if(masterReceive.zoneIsRed)
00391         {
00392             redzone = true;
00393             bluezone = false;
00394         }else if(!masterReceive.zoneIsRed)
00395         {
00396             redzone = false;
00397             bluezone = true;
00398         }
00399         if(masterReceive.ems)
00400         {
00401             masterSend.lowerRoller = false;
00402             masterSend.higherRoller = false;
00403             masterSend.launch = false;
00404             move_flag = false;
00405             white_FR = false;
00406             white_FL = false;
00407             white_RL = false;
00408             white_RR = false;
00409             flag = false;    
00410             invation_FR = false;
00411             invation_FL = false;
00412             invation_RL = false;
00413             invation_RR = false;
00414             on_line_flag = false;
00415             spot2400_flag = false;
00416             spot1200_flag = false;
00417             spot1500_flag = false;
00418             spot1800_flag = false;
00419             next_flag = true;
00420             once_flag = true;
00421             debug_flag1 = true;
00422             ugoite_flag = true;
00423             debug_flag = true;
00424             temae_flag = false;
00425             ugokanaide_flag = false;
00426             ok_flag = false;
00427             zyouge = 0;
00428             ushiroate_flag = false;
00429             kaeru_flag = false;
00430             twin_flag = false;
00431             idoutable_flag = false;
00432             itido_flag = true;
00433             roller_flag = true;
00434             reset_flag = false;
00435             current = 0;
00436             AllBrake();
00437             Rota_FR.reset();
00438             Rota_FL.reset();
00439             Rota_RL.reset();
00440             Rota_RR.reset();
00441             rotarota_flag = false;
00442             masterSend.rollerHeight = STOP;
00443             lock = false;
00444         }
00445         masterSend.processNum = lock ? 0x0f : current;
00446         
00447         static uint16_t cnt = 0;
00448         if(cnt >= 0x3fff) {
00449             LED_DEBUG0 = !LED_DEBUG0;
00450             cnt = 0;
00451         } else cnt++;
00452         
00453         SystemProcessUpdate();
00454     }
00455 }
00456 
00457 #pragma region PROCESS
00458 #ifdef USE_SUBPROCESS
00459 #if USE_PROCESS_NUM>0
00460 static void Process0()//二段テーブルへ移動か
00461 {
00462     /*
00463     motor[TIRE_FR].dir = FOR;
00464     motor[TIRE_FL].dir = BACK;
00465     motor[TIRE_RL].dir = BACK;
00466     motor[TIRE_RR].dir = FOR;
00467     motor[TIRE_FR].pwm = 49+maeR;
00468     motor[TIRE_FL].pwm = 52+maeL;
00469     motor[TIRE_RL].pwm = 55+ushiroL;
00470     motor[TIRE_RR].pwm = 57+ushiroR;
00471     if(controller->Button.R)
00472     {
00473         maeR++;
00474     }else if(controller->Button.L)
00475     {
00476         maeL++;
00477     }else if(controller->Button.ZL)
00478     {
00479         ushiroL++;
00480     }else if(controller->Button.ZR)
00481     {
00482         ushiroR++;
00483     }else if(controller->Button.A)
00484     {
00485         maeR = 0;
00486         maeL = 0;
00487         ushiroL = 0;
00488         ushiroR = 0;
00489     }*/
00490     /*
00491     if(controller->Button.UP)
00492     {
00493         GoMae();
00494     }else if(controller->Button.LEFT)
00495     {
00496         GoHidariNanameMae();
00497     }else if(controller->Button.RIGHT)
00498     {
00499         GoMigiNanameMae();
00500     }else{
00501         AllBrake();
00502     }
00503     */
00504     
00505     //ColorDetection();
00506     Color_Changeflag();
00507     if(bluezone)
00508     {
00509         if(masterReceive.startSw)
00510         {
00511             if(!(GO_2400up||GO_2400down) && GO_1800)
00512             {
00513                 move_flag = true;
00514                 Rota_FR.reset();
00515                 Rota_FL.reset();
00516                 Rota_RL.reset();
00517                 Rota_RR.reset();
00518                 target_FR = 3500;
00519                 target_FL = 3300;
00520                 target_RL = -3500;
00521                 target_RR = -3300;
00522                 masterSend.lowerRoller = true;
00523                 masterSend.rollerHeight = UE;
00524                 zyouge = 4;
00525                 idoutable_flag = true;
00526             }else if(!(GO_2400up||GO_2400down) && GO_1500)
00527             {
00528                 move_flag = true;
00529                 Rota_FR.reset();
00530                 Rota_FL.reset();
00531                 Rota_RL.reset();
00532                 Rota_RR.reset();
00533                 target_FR = 3300;
00534                 target_FL = 2900;
00535                 target_RL = -3300;
00536                 target_RR = -2900;
00537                 masterSend.lowerRoller = true;
00538                 masterSend.rollerHeight = MANNAKA;
00539                 zyouge = 3;
00540                 idoutable_flag = true;
00541             }else if(!(GO_2400up||GO_2400down) && GO_1200)
00542             {
00543                 move_flag = true;
00544                 Rota_FR.reset();
00545                 Rota_FL.reset();
00546                 Rota_RL.reset();
00547                 Rota_RR.reset();
00548                 target_FR = 2000;
00549                 target_FL = 1900;
00550                 target_RL = -2000;
00551                 target_RR = -1900;
00552                 masterSend.lowerRoller = true;
00553                 masterSend.rollerHeight = SHITA;
00554                 zyouge = 2;
00555                 idoutable_flag = true;
00556             }else if(GO_2400up && !GO_2400down)
00557             {
00558                 move_flag = true;
00559                 Rota_FR.reset();
00560                 Rota_FL.reset();
00561                 Rota_RL.reset();
00562                 Rota_RR.reset();
00563                 target_FR = 300;
00564                 target_FL = 2000;
00565                 target_RL = -300;
00566                 target_RR = -2000;
00567                 masterSend.lowerRoller = false;
00568                 masterSend.higherRoller = true;
00569                 masterSend.rollerHeight = SHITA;
00570                 zyouge = 2;
00571                 twin_flag = true;
00572             }else if(!GO_2400up && GO_2400down)
00573             {
00574                 move_flag = true;
00575                 Rota_FR.reset();
00576                 Rota_FL.reset();
00577                 Rota_RL.reset();
00578                 Rota_RR.reset();
00579                 target_FR = 300;
00580                 target_FL = 2000;
00581                 target_RL = -300;
00582                 target_RR = -2000;
00583                 masterSend.lowerRoller = true;
00584                 masterSend.higherRoller = false;
00585                 masterSend.rollerHeight = SHITA;
00586                 zyouge = 2;
00587                 twin_flag = true;
00588             }else if(GO_2400up && GO_2400down)
00589             {
00590                 move_flag = true;
00591                 Rota_FR.reset();
00592                 Rota_FL.reset();
00593                 Rota_RL.reset();
00594                 Rota_RR.reset();
00595                 target_FR = 300;
00596                 target_FL = 2000;
00597                 target_RL = -300;
00598                 target_RR = -2000;
00599                 masterSend.lowerRoller = true;
00600                 masterSend.higherRoller = true;
00601                 masterSend.rollerHeight = SHITA;
00602                 zyouge = 2;
00603                 twin_flag = true;
00604             }
00605         }
00606     }else if(redzone)
00607     {
00608         if(masterReceive.startSw)
00609         {
00610             if(!(GO_2400up||GO_2400down) && GO_1800)
00611             {
00612                 move_flag = true;
00613                 Rota_FR.reset();
00614                 Rota_FL.reset();
00615                 Rota_RL.reset();
00616                 Rota_RR.reset();
00617                 target_FR = -3300;
00618                 target_FL = -3500;
00619                 target_RL = 3300;
00620                 target_RR = 3500;
00621                 masterSend.lowerRoller = true;
00622                 masterSend.rollerHeight = UE;
00623                 zyouge = 4;
00624                 idoutable_flag = true;
00625             }else if(!(GO_2400up||GO_2400down) && GO_1500)
00626             {
00627                 move_flag = true;
00628                 Rota_FR.reset();
00629                 Rota_FL.reset();
00630                 Rota_RL.reset();
00631                 Rota_RR.reset();
00632                 target_FR = -2900;
00633                 target_FL = -3300;
00634                 target_RL = 2900;
00635                 target_RR = 3300;
00636                 masterSend.lowerRoller = true;
00637                 masterSend.rollerHeight = MANNAKA;
00638                 zyouge = 3;
00639                 idoutable_flag = true;
00640             }else if(!(GO_2400up||GO_2400down) && GO_1200)
00641             {
00642                 move_flag = true;
00643                 Rota_FR.reset();
00644                 Rota_FL.reset();
00645                 Rota_RL.reset();
00646                 Rota_RR.reset();
00647                 target_FR = -1900;
00648                 target_FL = -2000;
00649                 target_RL = 1900;
00650                 target_RR = 2000;
00651                 masterSend.lowerRoller = true;
00652                 masterSend.rollerHeight = SHITA;
00653                 zyouge = 2;
00654                 idoutable_flag = true;
00655             }else if(!GO_2400up && GO_2400down)
00656             {
00657                 move_flag = true;
00658                 Rota_FR.reset();
00659                 Rota_FL.reset();
00660                 Rota_RL.reset();
00661                 Rota_RR.reset();
00662                 target_FR = -2000;
00663                 target_FL = -300;
00664                 target_RL = 2000;
00665                 target_RR = 300;
00666                 masterSend.lowerRoller = true;
00667                 masterSend.higherRoller = false;
00668                 masterSend.rollerHeight = SHITA;
00669                 zyouge = 2;
00670                 twin_flag = true;
00671             }else if(GO_2400up && !GO_2400down)
00672             {
00673                 move_flag = true;
00674                 Rota_FR.reset();
00675                 Rota_FL.reset();
00676                 Rota_RL.reset();
00677                 Rota_RR.reset();
00678                 target_FR = -2000;
00679                 target_FL = -300;
00680                 target_RL = 2000;
00681                 target_RR = 300;
00682                 masterSend.lowerRoller = false;
00683                 masterSend.higherRoller = true;
00684                 masterSend.rollerHeight = SHITA;
00685                 zyouge = 2;
00686                 twin_flag = true;
00687             }else if(GO_2400up||GO_2400down)
00688             {
00689                 move_flag = true;
00690                 Rota_FR.reset();
00691                 Rota_FL.reset();
00692                 Rota_RL.reset();
00693                 Rota_RR.reset();
00694                 target_FR = -2000;
00695                 target_FL = -300;
00696                 target_RL = 2000;
00697                 target_RR = 300;
00698                 masterSend.lowerRoller = true;
00699                 masterSend.higherRoller = true;
00700                 masterSend.rollerHeight = SHITA;
00701                 zyouge = 2;
00702                 twin_flag = true;
00703             }         
00704         }
00705     }
00706     
00707     if(bluezone)
00708     {
00709         if(twin_flag)
00710         {
00711             if(move_flag)
00712             {
00713                 motor[TIRE_FR].dir = SetStatus(pwm.FR);
00714                 motor[TIRE_FL].dir = SetStatus(pwm.FL);
00715                 motor[TIRE_RL].dir = SetStatus(pwm.RL);
00716                 motor[TIRE_RR].dir = SetStatus(pwm.RR);
00717                 motor[TIRE_FR].pwm = SetPWM(pwm.FR);
00718                 motor[TIRE_FL].pwm = SetPWM(pwm.FL);
00719                 motor[TIRE_RL].pwm = SetPWM(pwm.RL);
00720                 motor[TIRE_RR].pwm = SetPWM(pwm.RR);
00721             }
00722             if(dist.FL > target_FL - 5)current = 1;
00723         }
00724         if(idoutable_flag)
00725         {
00726             if(move_flag){
00727                 motor[TIRE_FR].dir = SetStatus(pwm.FR);
00728                 motor[TIRE_FL].dir = SetStatus(pwm.FL);
00729                 motor[TIRE_RL].dir = SetStatus(pwm.RL);
00730                 motor[TIRE_RR].dir = SetStatus(pwm.RR);
00731                 motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.8;
00732                 motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.3;
00733                 motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.8;
00734                 motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.3;
00735             }
00736             if(dist.FR > target_FR - 100) current = 4;
00737         }
00738     }
00739     if(redzone)
00740     {
00741         if(twin_flag)
00742         {
00743             if(move_flag)
00744             {
00745                 motor[TIRE_FR].dir = SetStatus(pwm.FR);
00746                 motor[TIRE_FL].dir = SetStatus(pwm.FL);
00747                 motor[TIRE_RL].dir = SetStatus(pwm.RL);
00748                 motor[TIRE_RR].dir = SetStatus(pwm.RR);
00749                 motor[TIRE_FR].pwm = SetPWM(pwm.FR);
00750                 motor[TIRE_FL].pwm = SetPWM(pwm.FL);
00751                 motor[TIRE_RL].pwm = SetPWM(pwm.RL);
00752                 motor[TIRE_RR].pwm = SetPWM(pwm.RR);
00753             }
00754             if(dist.RL > target_RL - 5)current = 1;
00755         }
00756         if(idoutable_flag)
00757         {
00758             if(move_flag){
00759                 motor[TIRE_FR].dir = SetStatus(pwm.FR);
00760                 motor[TIRE_FL].dir = SetStatus(pwm.FL);
00761                 motor[TIRE_RL].dir = SetStatus(pwm.RL);
00762                 motor[TIRE_RR].dir = SetStatus(pwm.RR);
00763                 motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.3;
00764                 motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.8;
00765                 motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.3;
00766                 motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.8;
00767             }
00768             if(dist.RR > target_RR - 100) current = 4;
00769         }
00770     }
00771     
00772     if(controller->Button.B){
00773         AllBrake();
00774         Rota_FR.reset();
00775         Rota_FL.reset();
00776         Rota_RL.reset();
00777         Rota_RR.reset();
00778         move_flag = false;
00779     }
00780 }
00781 #endif
00782 
00783 #if USE_PROCESS_NUM>1
00784 static void Process1()//二段テーブルの位置合わせ
00785 {
00786     move_flag = false;
00787     ColorDetection();
00788     Color_Changeflag();
00789 /*
00790     pc.printf("FR: red %d g %d b %d \n\r",Color_FR[0],Color_FR[1],Color_FR[2]);
00791     pc.printf("FL: red %d g %d b %d \n\r",Color_FL[0],Color_FL[1],Color_FL[2]);
00792     pc.printf("RL: red %d g %d b %d \n\r",Color_RL[0],Color_RL[1],Color_RL[2]);
00793     pc.printf("RR: red %d g %d b %d \n\r",Color_RR[0],Color_RR[1],Color_RR[2]);
00794 */    
00795     if(!flag)
00796     {
00797         if(LS_FL && LS_FR) 
00798         {
00799             AllBrake();
00800             flag = true;
00801         } else if(!LS_FL && LS_FR) {
00802             GoFront();
00803         }else if(LS_FL && !LS_FR) {
00804             GoFront();
00805         }else if(!LS_FL && !LS_FR){
00806             GoFront();
00807         }
00808     }
00809     if(flag){
00810         Move_On_the_Line();
00811     }
00812     if(on_line_flag)
00813     {
00814         ugokanaide_flag = true;
00815     }
00816     if(ugokanaide_flag)
00817     {
00818         if(LS_FL && LS_FR) 
00819         {
00820             masterSend.launch = true;
00821             AllBrake();
00822             if(roller_flag)
00823             {
00824                 launchTimer.attach(launchTimer_func, 7);
00825                 roller_flag = false;
00826             }
00827         } else if(!LS_FL && LS_FR) {
00828             GoFront();
00829         }else if(LS_FL && !LS_FR) {
00830             GoFront();
00831         }else if(!LS_FL && !LS_FR){
00832             GoFront();
00833         }
00834     }
00835     
00836     if(controller->Button.B){
00837         AllBrake();
00838         Rota_FR.reset();
00839         Rota_FL.reset();
00840         Rota_RL.reset();
00841         Rota_RR.reset();
00842         move_flag = false;
00843     }
00844 }
00845 #endif
00846 
00847 #if USE_PROCESS_NUM>2
00848 static void Process2()//二段テーブル終了後ろへ or スタートゾーンへ戻る
00849 {
00850     masterSend.higherRoller = false;
00851     move_flag = false;
00852     if(GO_1200 || GO_1500 || GO_1800)
00853     {
00854         if(LS_RL && LS_RR) 
00855         {
00856             Rota_FR.reset();
00857             Rota_FL.reset();
00858             Rota_RL.reset();
00859             Rota_RR.reset();
00860             AllBrake();
00861             target_FR = 0;
00862             target_FL = 0;
00863             target_RL = 0;
00864             target_RR = 0;
00865             dist.FR = 0;
00866             dist.FL = 0;
00867             dist.RL = 0;
00868             dist.RR = 0;
00869             rotarota_flag = false;
00870             current = 3;
00871         }else if(!LS_RL && LS_RR) 
00872         {
00873             motor[TIRE_FR].dir = FOR;
00874             motor[TIRE_FL].dir = FOR;
00875             motor[TIRE_RL].dir = FOR;
00876             motor[TIRE_RR].dir = FOR;
00877             motor[TIRE_FR].pwm = 0;
00878             motor[TIRE_FL].pwm = 60;
00879             motor[TIRE_RL].pwm = 0;
00880             motor[TIRE_RR].pwm = 0;
00881         }else if(LS_RL && !LS_RR) 
00882         {
00883             motor[TIRE_FR].dir = BACK;
00884             motor[TIRE_FL].dir = BACK;
00885             motor[TIRE_RL].dir = BACK;
00886             motor[TIRE_RR].dir = BACK;
00887             motor[TIRE_FR].pwm = 60;
00888             motor[TIRE_FL].pwm = 0;
00889             motor[TIRE_RL].pwm = 0;
00890             motor[TIRE_RR].pwm = 0;
00891         }else if(!LS_RL && !LS_RR){
00892             GoBack_Rota();
00893         }
00894     }
00895     if(bluezone)
00896     {    
00897         if(!GO_1200 && !GO_1500 && !GO_1800)
00898         {
00899             if(itido_flag)
00900             {
00901                 Rota_FR.reset();
00902                 Rota_FL.reset();
00903                 Rota_RL.reset();
00904                 Rota_RR.reset();
00905                 target_FR = -550;
00906                 target_FL = -2550;
00907                 target_RL = 550;
00908                 target_RR = 2550;
00909                 itido_flag = false;
00910             }
00911             motor[TIRE_FR].dir = SetStatus(pwm.FR);
00912             motor[TIRE_FL].dir = SetStatus(pwm.FL);
00913             motor[TIRE_RL].dir = SetStatus(pwm.RL);
00914             motor[TIRE_RR].dir = SetStatus(pwm.RR);
00915             motor[TIRE_FR].pwm = SetPWM(pwm.FR);
00916             motor[TIRE_FL].pwm = SetPWM(pwm.FL);
00917             motor[TIRE_RL].pwm = SetPWM(pwm.RL);
00918             motor[TIRE_RR].pwm = SetPWM(pwm.RR);
00919         }
00920     }else if(redzone)
00921     {
00922         if(!GO_1200 && !GO_1500 && !GO_1800)
00923         {
00924             if(itido_flag)
00925             {
00926                 Rota_FR.reset();
00927                 Rota_FL.reset();
00928                 Rota_RL.reset();
00929                 Rota_RR.reset();
00930                 
00931                 target_FR = 2650;
00932                 target_FL = 400;
00933                 target_RL = -2650;
00934                 target_RR = -400;
00935                 itido_flag = false;
00936             }
00937             motor[TIRE_FR].dir = SetStatus(pwm.FR);
00938             motor[TIRE_FL].dir = SetStatus(pwm.FL);
00939             motor[TIRE_RL].dir = SetStatus(pwm.RL);
00940             motor[TIRE_RR].dir = SetStatus(pwm.RR);
00941             motor[TIRE_FR].pwm = SetPWM(pwm.FR);
00942             motor[TIRE_FL].pwm = SetPWM(pwm.FL);
00943             motor[TIRE_RL].pwm = SetPWM(pwm.RL);
00944             motor[TIRE_RR].pwm = SetPWM(pwm.RR);
00945         }
00946     }
00947 }
00948 #endif
00949 
00950 #if USE_PROCESS_NUM>3
00951 static void Process3()//移動テーブルのライン手前までロタコンで移動
00952 {
00953     if(!rotarota_flag)
00954     {
00955         if(bluezone)
00956         {
00957             if(GO_1200)
00958             {
00959                 
00960                 Rota_FR.reset();
00961                 Rota_FL.reset();
00962                 Rota_RL.reset();
00963                 Rota_RR.reset();
00964                 rotarota_flag = true;        
00965                 target_FR = 1000;
00966                 target_FL = 900;
00967                 target_RL = -1000;
00968                 target_RR = -900;
00969                 masterSend.lowerRoller = true;
00970                 masterSend.rollerHeight = SHITA;
00971                 zyouge = 2;
00972             }else if(!GO_1200 && GO_1500)
00973             {
00974                 Rota_FR.reset();
00975                 Rota_FL.reset();
00976                 Rota_RL.reset();
00977                 Rota_RR.reset();
00978                 rotarota_flag = true;
00979                 target_FR = 1600;
00980                 target_FL = 1500;
00981                 target_RL = -1600;
00982                 target_RR = -1500;
00983                 masterSend.lowerRoller = true;
00984                 masterSend.rollerHeight = MANNAKA;
00985                 zyouge = 3;
00986             }else if(!GO_1200 && !GO_1500 && GO_1800)
00987             {   
00988                 Rota_FR.reset();
00989                 Rota_FL.reset();
00990                 Rota_RL.reset();
00991                 Rota_RR.reset();
00992                 rotarota_flag = true;
00993                 target_FR = 2800;
00994                 target_FL = 2700;
00995                 target_RL = -2800;
00996                 target_RR = -2700;
00997                 masterSend.lowerRoller = true;
00998                 masterSend.rollerHeight = UE;
00999                 zyouge = 4;
01000             }
01001         }else if(redzone)
01002         {
01003             if(GO_1200)
01004             {
01005                 Rota_FR.reset();
01006                 Rota_FL.reset();
01007                 Rota_RL.reset();
01008                 Rota_RR.reset();
01009                 rotarota_flag = true;
01010                 target_FR = -900;
01011                 target_FL = -1000;
01012                 target_RL = 900;
01013                 target_RR = 1000;
01014                 masterSend.lowerRoller = true;
01015                 masterSend.rollerHeight = SHITA;
01016                 zyouge = 2;
01017             }else if(!GO_1200 && GO_1500)
01018             {
01019                 Rota_FR.reset();
01020                 Rota_FL.reset();
01021                 Rota_RL.reset();
01022                 Rota_RR.reset();
01023                 rotarota_flag = true;
01024                 target_FR = -1500;
01025                 target_FL = -1600;
01026                 target_RL = 1500;
01027                 target_RR = 1600;
01028                 masterSend.lowerRoller = true;
01029                 masterSend.rollerHeight = MANNAKA;
01030                 zyouge = 3;
01031             }else if(!GO_1200 && !GO_1500 && GO_1800)
01032             {   
01033                 Rota_FR.reset();
01034                 Rota_FL.reset();
01035                 Rota_RL.reset();
01036                 Rota_RR.reset();
01037                 rotarota_flag = true;
01038                 target_FR = -2700;
01039                 target_FL = -2800;
01040                 target_RL = 2700;
01041                 target_RR = 2800;
01042                 masterSend.lowerRoller = true;
01043                 masterSend.rollerHeight = UE;
01044                 zyouge = 4;
01045             }
01046         }
01047     }
01048     if(bluezone)
01049     {
01050         if(rotarota_flag)
01051         {
01052             motor[TIRE_FR].dir = SetStatus(pwm.FR);
01053             motor[TIRE_FL].dir = SetStatus(pwm.FL);
01054             motor[TIRE_RL].dir = SetStatus(pwm.RL);
01055             motor[TIRE_RR].dir = SetStatus(pwm.RR);
01056             motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.8;
01057             motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.3;
01058             motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.8;
01059             motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.3;
01060             if(dist.FR > target_FR-100)
01061             {
01062                 current = 4;
01063             }
01064         }
01065     }else if(redzone)
01066     {
01067         if(rotarota_flag)
01068         {
01069             motor[TIRE_FR].dir = SetStatus(pwm.FR);
01070             motor[TIRE_FL].dir = SetStatus(pwm.FL);
01071             motor[TIRE_RL].dir = SetStatus(pwm.RL);
01072             motor[TIRE_RR].dir = SetStatus(pwm.RR);
01073             motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.3;
01074             motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.8;
01075             motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.3;
01076             motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.8;
01077             if(dist.RR > (target_RR-100))
01078             {
01079                 current = 4;
01080             }
01081         }
01082     }
01083 }
01084 #endif
01085 
01086 #if USE_PROCESS_NUM>4
01087 static void Process4()//ライン位置合わせ
01088 {
01089     rotarota_flag = false;
01090     masterSend.launch = false;
01091     roller_flag = true;
01092     once_flag = true;
01093     ColorDetection();
01094     Color_Changeflag();
01095     if (debug_flag)
01096     {
01097         on_line_flag = false;
01098         invation_FR = false;
01099         invation_FL = false;
01100         invation_RL = false;
01101         invation_RR = false; 
01102         flag = false;
01103         ok_flag = false;
01104         debug_flag = false;
01105         //ushiroate_flag = false;
01106     }
01107     Move_On_the_Line_BackTrace();
01108     if(on_line_flag)
01109     {
01110         if(LS_RL && LS_RR) 
01111             {
01112                 current = 5;
01113                 ok_flag = true;
01114                 AllBrake();
01115             }else if(!LS_RL && LS_RR) 
01116             {
01117                 motor[TIRE_FR].dir = FOR;
01118                 motor[TIRE_FL].dir = FOR;
01119                 motor[TIRE_RL].dir = FOR;
01120                 motor[TIRE_RR].dir = FOR;
01121                 motor[TIRE_FR].pwm = 0;
01122                 motor[TIRE_FL].pwm = 40;
01123                 motor[TIRE_RL].pwm = 0;
01124                 motor[TIRE_RR].pwm = 0;
01125             }else if(LS_RL && !LS_RR) 
01126             {
01127                 motor[TIRE_FR].dir = BACK;
01128                 motor[TIRE_FL].dir = BACK;
01129                 motor[TIRE_RL].dir = BACK;
01130                 motor[TIRE_RR].dir = BACK;
01131                 motor[TIRE_FR].pwm = 40;
01132                 motor[TIRE_FL].pwm = 0;
01133                 motor[TIRE_RL].pwm = 0;
01134                 motor[TIRE_RR].pwm = 0;
01135             }else{
01136                 GoBack();
01137             }
01138     }
01139     
01140     if(controller->Button.B){
01141         AllBrake();
01142         Rota_FR.reset();
01143         Rota_FL.reset();
01144         Rota_RL.reset();
01145         Rota_RR.reset();
01146         move_flag = false;
01147     }
01148 }
01149 #endif
01150 
01151 #if USE_PROCESS_NUM>5
01152 static void Process5()//最初の移動テーブルへライントレースし、投射
01153 {
01154     if(LS_FL && LS_FR) 
01155     {
01156         masterSend.launch = true;
01157         rotarota_flag = false;
01158         AllBrake();
01159         move_flag = false;
01160         if(roller_flag)
01161         { 
01162             launchTimer.attach(launchTimer_func1, 3);
01163             roller_flag = false;
01164         }
01165     }else if(!LS_FL && LS_FR) {
01166         GoOshituke();
01167     }else if(LS_FL && !LS_FR) {
01168         GoOshituke();
01169     }else if(!LS_FL && !LS_FR){
01170         Move_On_the_Line_Trace();
01171     }
01172 }
01173 #endif
01174 
01175 #if USE_PROCESS_NUM>6//下がる 
01176 static void Process6()
01177 {
01178     if(LS_RL && LS_RR) {
01179         Rota_FR.reset();
01180         Rota_FL.reset();
01181         Rota_RL.reset();
01182         Rota_RR.reset();
01183         AllBrake();
01184         rotarota_flag = false;
01185         target_FR = 0;
01186         target_FL = 0;
01187         target_RL = 0;
01188         target_RR = 0;
01189         dist.FR = 0;
01190         dist.FL = 0;
01191         dist.RL = 0;
01192         dist.RR = 0;
01193         current = 7;
01194     }else if(!LS_RL && LS_RR) {
01195         motor[TIRE_FR].dir = FOR;
01196         motor[TIRE_FL].dir = FOR;
01197         motor[TIRE_RL].dir = FOR;
01198         motor[TIRE_RR].dir = FOR;
01199         motor[TIRE_FR].pwm = 0;
01200         motor[TIRE_FL].pwm = 40;
01201         motor[TIRE_RL].pwm = 0;
01202         motor[TIRE_RR].pwm = 0;
01203     }else if(LS_RL && !LS_RR) {
01204         motor[TIRE_FR].dir = BACK;
01205         motor[TIRE_FL].dir = BACK;
01206         motor[TIRE_RL].dir = BACK;
01207         motor[TIRE_RR].dir = BACK;
01208         motor[TIRE_FR].pwm = 40;
01209         motor[TIRE_FL].pwm = 0;
01210         motor[TIRE_RL].pwm = 0;
01211         motor[TIRE_RR].pwm = 0;
01212     }else if(!LS_RL && !LS_RR){
01213         GoBack_Rota();
01214     }
01215 }
01216 #endif
01217 
01218 #if USE_PROCESS_NUM>7
01219 static void Process7()//次の移動テーブルのライン手前までロタコンで移動 or スタートゾーンへ戻る
01220 {
01221     masterSend.launch = false;
01222     debug_flag1  = true;
01223     if(!rotarota_flag)
01224     {
01225         if(bluezone)
01226         {
01227             if(zyouge==2)
01228             {
01229                 if(GO_1500)
01230                 {
01231                     masterSend.rollerHeight = MANNAKA;
01232                     Rota_FR.reset();
01233                     Rota_FL.reset();
01234                     Rota_RL.reset();
01235                     Rota_RR.reset();
01236                     rotarota_flag = true;
01237                     target_FR = 400;
01238                     target_FL = 300;
01239                     target_RL = -400;
01240                     target_RR = -300;
01241                     zyouge = 3;
01242                 }else if(!GO_1500 && GO_1800)
01243                 {
01244                     masterSend.rollerHeight = UE;
01245                     Rota_FR.reset();
01246                     Rota_FL.reset();
01247                     Rota_RL.reset();
01248                     Rota_RR.reset();
01249                     rotarota_flag = true;
01250                     target_FR = 1100;
01251                     target_FL = 1000;
01252                     target_RL = -1100;
01253                     target_RR = -1000;
01254                     zyouge = 4;
01255                 }else if(!GO_1500 && !GO_1800)
01256                 {
01257                     masterSend.rollerHeight = SHITA;
01258                     kaeru_flag = true;
01259                 }
01260             }else if(zyouge==3){
01261                 if(GO_1800)
01262                 {
01263                     masterSend.rollerHeight = UE;
01264                     Rota_FR.reset();
01265                     Rota_FL.reset();
01266                     Rota_RL.reset();
01267                     Rota_RR.reset();
01268                     rotarota_flag = true;
01269                     target_FR = 400;
01270                     target_FL = 300;
01271                     target_RL = -400;
01272                     target_RR = -300;
01273                     zyouge = 4;
01274                 }else if(!GO_1800)
01275                 {
01276                     masterSend.rollerHeight = MANNAKA;
01277                     kaeru_flag = true;
01278                 }
01279             }else if(zyouge==4){
01280                 masterSend.rollerHeight = MANNAKA;
01281                 kaeru_flag = true;
01282             }
01283         }else if(redzone)
01284         {
01285             if(zyouge==2)
01286             {
01287                 if(GO_1500)
01288                 {
01289                     masterSend.rollerHeight = MANNAKA;
01290                     Rota_FR.reset();
01291                     Rota_FL.reset();
01292                     Rota_RL.reset();
01293                     Rota_RR.reset();
01294                     rotarota_flag = true;
01295                     target_FR = -300;
01296                     target_FL = -400;
01297                     target_RL = 300;
01298                     target_RR = 400;
01299                     zyouge = 3;
01300                 }else if(!GO_1500 && GO_1800)
01301                 {
01302                     masterSend.rollerHeight = UE;
01303                     Rota_FR.reset();
01304                     Rota_FL.reset();
01305                     Rota_RL.reset();
01306                     Rota_RR.reset();
01307                     rotarota_flag = true;
01308                     target_FR = -1000;
01309                     target_FL = -1100;
01310                     target_RL = 1000;
01311                     target_RR = 1100;
01312                     zyouge = 4;
01313                 }else if(!GO_1500 && !GO_1800)
01314                 {
01315                     masterSend.rollerHeight = MANNAKA;
01316                     kaeru_flag = true;
01317                 }
01318             }else if(zyouge==3){
01319                 if(GO_1800)
01320                 {
01321                     masterSend.rollerHeight = UE;
01322                     Rota_FR.reset();
01323                     Rota_FL.reset();
01324                     Rota_RL.reset();
01325                     Rota_RR.reset();
01326                     
01327                     rotarota_flag = true;
01328                     target_FR = -300;
01329                     target_FL = -400;
01330                     target_RL = 300;
01331                     target_RR = 400;
01332                     zyouge = 4;
01333                 }else if(!GO_1800)
01334                 {
01335                     masterSend.rollerHeight = MANNAKA;
01336                     kaeru_flag = true;
01337                 }
01338             }else if(zyouge==4){
01339                 masterSend.rollerHeight = MANNAKA;
01340                 kaeru_flag = true;
01341             }
01342         }
01343     }
01344     if(bluezone)
01345     {
01346         if(rotarota_flag)
01347         {
01348             motor[TIRE_FR].dir = SetStatus(pwm.FR);
01349             motor[TIRE_FL].dir = SetStatus(pwm.FL);
01350             motor[TIRE_RL].dir = SetStatus(pwm.RL);
01351             motor[TIRE_RR].dir = SetStatus(pwm.RR);
01352             motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.8;
01353             motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.3;
01354             motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.8;
01355             motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.3;
01356             if(dist.FR > (target_FR-100))
01357             {
01358                 current = 8;
01359             }
01360         }
01361         if(kaeru_flag)
01362         {
01363             motor[TIRE_FR].dir = FOR;
01364             motor[TIRE_FL].dir = FOR;
01365             motor[TIRE_RL].dir = BACK;
01366             motor[TIRE_RR].dir = BACK;
01367             motor[TIRE_FR].pwm = 40;
01368             motor[TIRE_FL].pwm = 100;
01369             motor[TIRE_RL].pwm = 40;
01370             motor[TIRE_RR].pwm = 100;
01371             masterSend.rollerHeight = MANNAKA;
01372         }
01373     }else if(redzone)
01374     {
01375         if(rotarota_flag)
01376         {
01377             motor[TIRE_FR].dir = SetStatus(pwm.FR);
01378             motor[TIRE_FL].dir = SetStatus(pwm.FL);
01379             motor[TIRE_RL].dir = SetStatus(pwm.RL);
01380             motor[TIRE_RR].dir = SetStatus(pwm.RR);
01381             motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.3;
01382             motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.8;
01383             motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.3;
01384             motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.8;
01385             if(dist.RR > (target_RR-100))
01386             {
01387                 current = 8;
01388             }
01389         }
01390         if(kaeru_flag)
01391         {
01392             motor[TIRE_FR].dir = BACK;
01393             motor[TIRE_FL].dir = BACK;
01394             motor[TIRE_RL].dir = FOR;
01395             motor[TIRE_RR].dir = FOR;
01396             motor[TIRE_FR].pwm = 100;
01397             motor[TIRE_FL].pwm = 40;
01398             motor[TIRE_RL].pwm = 100;
01399             motor[TIRE_RR].pwm = 40;
01400             masterSend.rollerHeight = MANNAKA;
01401         }
01402     }
01403     
01404     
01405 }
01406 #endif
01407 
01408 #if USE_PROCESS_NUM>8
01409 static void Process8()//ライン位置合わせ
01410 {
01411     roller_flag = true;
01412     ColorDetection();
01413     Color_Changeflag();
01414     
01415     if (debug_flag1)
01416     {
01417         //zyouge++;
01418         on_line_flag = false;
01419         invation_FR = false;
01420         invation_FL = false;
01421         invation_RL = false;
01422         invation_RR = false; 
01423         debug_flag1 = false;
01424         ugoite_flag = false;
01425         flag = false;
01426         temae_flag = false;
01427         ok_flag = false;
01428     }
01429     
01430     Move_On_the_Line_BackTrace();
01431     
01432     if(on_line_flag)
01433     {
01434         if(LS_RL && LS_RR) 
01435         {
01436             AllBrake();
01437             current = 9;
01438             ok_flag = true;
01439         }else if(!LS_RL && LS_RR) 
01440         {
01441             motor[TIRE_FR].dir = FOR;
01442             motor[TIRE_FL].dir = FOR;
01443             motor[TIRE_RL].dir = FOR;
01444             motor[TIRE_RR].dir = FOR;
01445             motor[TIRE_FR].pwm = 0;
01446             motor[TIRE_FL].pwm = 40;
01447             motor[TIRE_RL].pwm = 0;
01448             motor[TIRE_RR].pwm = 0;
01449         }else if(LS_RL && !LS_RR) 
01450         {
01451             motor[TIRE_FR].dir = BACK;
01452             motor[TIRE_FL].dir = BACK;
01453             motor[TIRE_RL].dir = BACK;
01454             motor[TIRE_RR].dir = BACK;
01455             motor[TIRE_FR].pwm = 40;
01456             motor[TIRE_FL].pwm = 0;
01457             motor[TIRE_RL].pwm = 0;
01458             motor[TIRE_RR].pwm = 0;
01459         }else{
01460             GoBack();
01461         }
01462     }
01463     
01464     if(controller->Button.B){
01465         AllBrake();
01466         Rota_FR.reset();
01467         Rota_FL.reset();
01468         Rota_RL.reset();
01469         Rota_RR.reset();
01470         move_flag = false;
01471     }
01472 }
01473 #endif
01474 
01475 #if USE_PROCESS_NUM>9
01476 static void Process9()//ライントレースし、投射->Process6へ戻る
01477 {
01478     once_flag = true;
01479     rotarota_flag = false;
01480     move_flag = false;
01481     Move_On_the_Line_Trace();
01482     if(LS_FL && LS_FR) 
01483     {
01484         target_FR = 0;
01485         target_FL = 0;
01486         target_RL = 0;
01487         target_RR = 0;
01488         dist.FR = 0;
01489         dist.FL = 0;
01490         dist.RL = 0;
01491         dist.RR = 0;
01492     
01493         masterSend.launch = true; 
01494         AllBrake();
01495         move_flag = false;
01496         if(roller_flag)
01497         {
01498             launchTimer.attach(launchTimer_func2, 3);
01499             roller_flag = false;
01500         }
01501     }else if(!LS_FL && LS_FR) {
01502         GoOshituke();
01503     }else if(LS_FL && !LS_FR) {
01504         GoOshituke();
01505     }
01506 }
01507 #endif
01508 #endif
01509 #pragma endregion PROCESS
01510 
01511 static void AllActuatorReset()
01512 {
01513 
01514 #ifdef USE_SOLENOID
01515     solenoid.all = ALL_SOLENOID_OFF;
01516 #endif
01517 
01518 #ifdef USE_MOTOR
01519     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
01520         motor[i].dir = FREE;
01521         motor[i].pwm = 0x00;
01522     }
01523 #endif
01524 }
01525 
01526 #pragma region USER-DEFINED-FUNCTIONS
01527 
01528 void Get_Move()
01529 {
01530     rps.FR = (rotate.FR-p_rps.FR)/0.1;
01531     rps.FL = (rotate.FL-p_rps.FL)/0.1;
01532     rps.RL = (rotate.RL-p_rps.RL)/0.1;
01533     rps.RR = (rotate.RR-p_rps.RR)/0.1;
01534     pwm_FR = (int)Catch_pwm_FR.SetPV(rps.FR , wantVal_FR);
01535     pwm_FL = (int)Catch_pwm_FL.SetPV(rps.FL , wantVal_FL);
01536     pwm_RL = (int)Catch_pwm_RL.SetPV(rps.RL , wantVal_RL);
01537     pwm_RR = (int)Catch_pwm_RR.SetPV(rps.RR , wantVal_RR);
01538     
01539     p_rps.FR = rotate.FR;
01540     p_rps.FL = rotate.FL;
01541     p_rps.RL = rotate.RL;
01542     p_rps.RR = rotate.RR;
01543 }
01544 
01545 void AllBrake()
01546  {
01547     motor[TIRE_FR].dir = BRAKE;
01548     motor[TIRE_FL].dir = BRAKE;
01549     motor[TIRE_RL].dir = BRAKE;
01550     motor[TIRE_RR].dir = BRAKE;
01551     motor[TIRE_FR].pwm = 255;
01552     motor[TIRE_FL].pwm = 255;
01553     motor[TIRE_RL].pwm = 255;
01554     motor[TIRE_RR].pwm = 255;
01555 }
01556 
01557 void GoOshituke()
01558 {
01559     motor[TIRE_FR].dir = FOR;
01560     motor[TIRE_FL].dir = BACK;
01561     motor[TIRE_RL].dir = BACK;
01562     motor[TIRE_RR].dir = FOR;
01563     motor[TIRE_FR].pwm = 48;
01564     motor[TIRE_FL].pwm = 48;
01565     motor[TIRE_RL].pwm = 48;
01566     motor[TIRE_RR].pwm = 48;
01567 }
01568 void GoFront()
01569 {
01570     motor[TIRE_FR].dir = FOR;
01571     motor[TIRE_FL].dir = BACK;
01572     motor[TIRE_RL].dir = BACK;
01573     motor[TIRE_RR].dir = FOR;
01574     motor[TIRE_FR].pwm = 48;
01575     motor[TIRE_FL].pwm = 48;
01576     motor[TIRE_RL].pwm = 48;
01577     motor[TIRE_RR].pwm = 48;
01578 }
01579 
01580 void GoMae()
01581 {
01582     motor[TIRE_FR].dir = FOR;
01583     motor[TIRE_FL].dir = BACK;
01584     motor[TIRE_RL].dir = BACK;
01585     motor[TIRE_RR].dir = FOR;
01586     motor[TIRE_FR].pwm = 29;
01587     motor[TIRE_FL].pwm = 32;
01588     motor[TIRE_RL].pwm = 35;
01589     motor[TIRE_RR].pwm = 37;
01590     
01591 }
01592 void GoBack()
01593 {
01594     motor[TIRE_FR].dir = BACK;
01595     motor[TIRE_FL].dir = FOR;
01596     motor[TIRE_RL].dir = FOR;
01597     motor[TIRE_RR].dir = BACK;
01598     motor[TIRE_FR].pwm = 25;
01599     motor[TIRE_FL].pwm = 22;
01600     motor[TIRE_RL].pwm = 22;
01601     motor[TIRE_RR].pwm = 25;
01602 }
01603 
01604 void GoBack_Rota()
01605 {
01606     if(once_flag)
01607     {
01608         dist.FR = 0;
01609         dist.FL = 0;
01610         dist.RL = 0;
01611         dist.RR = 0;
01612         Rota_FR.reset();
01613         Rota_FL.reset();
01614         Rota_RL.reset();
01615         Rota_RR.reset();
01616         target_FR = 8000;
01617         target_FL = -8000;
01618         target_RL = -8000;
01619         target_RR = 8000;
01620         once_flag = false;
01621     }
01622     
01623     motor[TIRE_FR].dir = SetStatus(pwm.FR);
01624     motor[TIRE_FL].dir = SetStatus(pwm.FL);
01625     motor[TIRE_RL].dir = SetStatus(pwm.RL);
01626     motor[TIRE_RR].dir = SetStatus(pwm.RR);
01627     motor[TIRE_FR].pwm = SetPWM(pwm.FR)*0.51;
01628     motor[TIRE_FL].pwm = SetPWM(pwm.FL)*0.52;
01629     motor[TIRE_RL].pwm = SetPWM(pwm.RL)*0.53;
01630     motor[TIRE_RR].pwm = SetPWM(pwm.RR)*0.47;
01631     
01632 }
01633 
01634 void GoLeft()
01635 {
01636     motor[TIRE_FR].dir = FOR;
01637     motor[TIRE_FL].dir = FOR;
01638     motor[TIRE_RL].dir = BACK;
01639     motor[TIRE_RR].dir = BACK;
01640     motor[TIRE_FR].pwm = 24;
01641     motor[TIRE_FL].pwm = 24;
01642     motor[TIRE_RL].pwm = 24;
01643     motor[TIRE_RR].pwm = 24;
01644 }
01645 
01646 void GoMigiNanameMae()
01647 {
01648     motor[TIRE_FR].dir = FOR;
01649     motor[TIRE_FL].dir = BACK;
01650     motor[TIRE_RL].dir = BACK;
01651     motor[TIRE_RR].dir = FOR;
01652     motor[TIRE_FR].pwm = 23;
01653     motor[TIRE_FL].pwm = 35;
01654     motor[TIRE_RL].pwm = 23;
01655     motor[TIRE_RR].pwm = 30;
01656 }
01657 
01658 void GoMigiNanameUshiro()
01659 {
01660     motor[TIRE_FR].dir = BACK;
01661     motor[TIRE_FL].dir = BACK;
01662     motor[TIRE_RL].dir = FOR;
01663     motor[TIRE_RR].dir = FOR;
01664     motor[TIRE_FR].pwm = 30;
01665     motor[TIRE_FL].pwm = 0;
01666     motor[TIRE_RL].pwm = 30;
01667     motor[TIRE_RR].pwm = 0;
01668 }
01669 
01670 void GoRight()
01671 {
01672     motor[TIRE_FR].dir = BACK;
01673     motor[TIRE_FL].dir = BACK;
01674     motor[TIRE_RL].dir = FOR;
01675     motor[TIRE_RR].dir = FOR;
01676     motor[TIRE_FR].pwm = 24;
01677     motor[TIRE_FL].pwm = 24;
01678     motor[TIRE_RL].pwm = 24;
01679     motor[TIRE_RR].pwm = 24;
01680 }
01681 
01682 void GoHidariNanameMae()
01683 {
01684     motor[TIRE_FR].dir = FOR;
01685     motor[TIRE_FL].dir = BACK;
01686     motor[TIRE_RL].dir = BACK;
01687     motor[TIRE_RR].dir = FOR;
01688     motor[TIRE_FR].pwm = 30;
01689     motor[TIRE_FL].pwm = 23;
01690     motor[TIRE_RL].pwm = 35;
01691     motor[TIRE_RR].pwm = 23;
01692 }
01693 
01694 void GoHidariNanameUshiro()
01695 {
01696     motor[TIRE_FR].dir = FOR;
01697     motor[TIRE_FL].dir = FOR;
01698     motor[TIRE_RL].dir = BACK;
01699     motor[TIRE_RR].dir = BACK;
01700     motor[TIRE_FR].pwm = 0;
01701     motor[TIRE_FL].pwm = 30;
01702     motor[TIRE_RL].pwm = 0;
01703     motor[TIRE_RR].pwm = 30;
01704 }
01705 
01706 void RightTurning()
01707 {
01708     motor[TIRE_FR].dir = BACK;
01709     motor[TIRE_FL].dir = BACK;
01710     motor[TIRE_RL].dir = BACK;
01711     motor[TIRE_RR].dir = BACK;
01712     motor[TIRE_FR].pwm = 0;
01713     motor[TIRE_FL].pwm = 35;
01714     motor[TIRE_RL].pwm = 35;
01715     motor[TIRE_RR].pwm = 30;
01716 }
01717 
01718 void LeftTurning()
01719 {
01720     motor[TIRE_FR].dir = FOR;
01721     motor[TIRE_FL].dir = FOR;
01722     motor[TIRE_RL].dir = FOR;
01723     motor[TIRE_RR].dir = FOR;
01724     motor[TIRE_FR].pwm = 30;
01725     motor[TIRE_FL].pwm = 0;
01726     motor[TIRE_RL].pwm = 35;
01727     motor[TIRE_RR].pwm = 30;
01728 }
01729 
01730 void Get_rps()
01731 {
01732     rotate.FR = ((double)Rota_FR.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
01733     rotate.FL = ((double)Rota_FL.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
01734     rotate.RL = ((double)Rota_RL.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
01735     rotate.RR = ((double)Rota_RR.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
01736 
01737     dist.FR = 103.0*3.141*rotate.FR;
01738     dist.FL = 103.0*3.141*rotate.FL;
01739     dist.RL = 103.0*3.141*rotate.RL;
01740     dist.RR = 103.0*3.141*rotate.RR;
01741     
01742     pwm.FR = (int)Get_pwm_FR.SetPV(dist.FR , target_FR);
01743     pwm.FL = (int)Get_pwm_FL.SetPV(dist.FL , target_FL);
01744     pwm.RL = (int)Get_pwm_RL.SetPV(dist.RL , target_RL);
01745     pwm.RR = (int)Get_pwm_RR.SetPV(dist.RR , target_RR);
01746 }
01747 
01748 void Rota_SetStatus(char position,double percent)
01749 {
01750     switch(position){
01751         case TIRE_FR :
01752                         wantVal_FR = (percent/100.0)*max_rotate;
01753                         //pwm_FR = (int)Catch_pwm_FR.SetPV(rps.FR , wantVal_FR);
01754                         motor[TIRE_FR].dir = SetStatus(pwm_FR);
01755                         motor[TIRE_FR].pwm = SetPWM(pwm_FR);
01756                         break;
01757         case TIRE_FL :
01758                         wantVal_FL = (percent/100.0)*max_rotate;
01759                         //pwm_FL = (int)Catch_pwm_FL.SetPV(rps.FL , wantVal_FL);
01760                         motor[TIRE_FL].dir = SetStatus(pwm_FL);
01761                         motor[TIRE_FL].pwm = SetPWM(pwm_FL);
01762                         break;
01763         case TIRE_RL :
01764                         wantVal_RL = (percent/100.0)*max_rotate;
01765                         //pwm_RL = (int)Catch_pwm_RL.SetPV(rps.RL , wantVal_RL);
01766                         motor[TIRE_RL].dir = SetStatus(pwm_RL);
01767                         motor[TIRE_RL].pwm = SetPWM(pwm_RL);
01768                         break;
01769         case TIRE_RR :
01770                         wantVal_RR = (percent/100.0)*max_rotate;
01771                         //pwm_RR = (int)Catch_pwm_RR.SetPV(rps.RR , wantVal_RR);
01772                         motor[TIRE_RR].dir = SetStatus(pwm_RR);
01773                         motor[TIRE_RR].pwm = SetPWM(pwm_RR);
01774                         break;
01775     }
01776 }
01777 
01778 uint8_t SetStatus(int pwmVal)
01779 {
01780     if(pwmVal < 0) return BACK;
01781     else if(pwmVal > 0) return FOR;
01782     else return BRAKE;
01783 }
01784 
01785 uint8_t SetPWM(int pwmVal)
01786 {
01787     if(pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
01788     else return abs(pwmVal);
01789 }
01790 unsigned long ColorIn(int index)
01791 {
01792     int result = 0;
01793     bool rtn = false;
01794     for(int i=0; i<12; i++)
01795     {
01796         CK = 1;
01797         rtn = DOUT[index];
01798         CK = 0;
01799         if(rtn)
01800         {
01801            result|=(1 << i);
01802         }
01803     }
01804     return result;
01805 }
01806 
01807 void ColorDetection(){
01808     GATE = 0;
01809            
01810     CK = 0;
01811         
01812     RANGE = 1;
01813         
01814     GATE = 1;
01815     wait_ms(intergration);
01816     GATE = 0;
01817     wait_us(4);
01818     switch(select++){
01819         case 0 :
01820                     Color_FR[0] = ColorIn(0); //赤
01821                     wait_us(3);
01822                     Color_FR[1] = ColorIn(0); //青
01823                     wait_us(3);
01824                     Color_FR[2] = ColorIn(0); //緑
01825                     break;
01826         case 1 :
01827                     Color_FL[0] = ColorIn(1);
01828                     wait_us(3);
01829                     Color_FL[1] = ColorIn(1);
01830                     wait_us(3);
01831                     Color_FL[2] = ColorIn(1);
01832                     break;
01833         case 2 :
01834                     Color_RL[0] = ColorIn(2);
01835                     wait_us(3);
01836                     Color_RL[1] = ColorIn(2);
01837                     wait_us(3);
01838                     Color_RL[2] = ColorIn(2);
01839                     break;
01840         case 3 :
01841                     Color_RR[0] = ColorIn(3);
01842                     wait_us(3);
01843                     Color_RR[1] = ColorIn(3);
01844                     wait_us(3);
01845                     Color_RR[2] = ColorIn(3);
01846                     select = 0;
01847                     break;
01848     }    
01849 }
01850 
01851 void Color_Changeflag(){
01852     ColorDetection();
01853     
01854     if(Color_FR[0] > line_FR[0] && Color_FR[1] > line_FR[1] && Color_FR[2] > line_FR[2] && !white_FR)//白
01855     {
01856     //invation_FR ^= 1;//start false,over true
01857     white_FR = true;//on true,noon false
01858     }   
01859     else if(!(Color_FR[0] > line_FR[0] && Color_FR[1] > line_FR[1] && Color_FR[2] > line_FR[2]))white_FR = false;//茶
01860     
01861     if(Color_FL[0] > line_FL[0] && Color_FL[1] > line_FL[1] && Color_FL[2] > line_FL[2] && !white_FL)//白
01862     {
01863     //invation_FL ^= 1;//start false,over true
01864     white_FL = true;//on true,noon false
01865     }   
01866     else if(!(Color_FL[0] > line_FL[0] && Color_FL[1] > line_FL[1] && Color_FL[2] > line_FL[2]))white_FL = false;//茶
01867     
01868     if(Color_RL[0] > line_RL[0] && Color_RL[1] > line_RL[1] && Color_RL[2] > line_RL[2] && !white_RL)//白
01869     {
01870     //invation_RL ^= 1;//start false,over true
01871     white_RL = true;//on true,noon false
01872     }   
01873     else if(!(Color_RL[0] > line_RL[0] && Color_RL[1] > line_RL[1] && Color_RL[2] > line_RL[2]))white_RL = false;//茶
01874 
01875     if(Color_RR[0] > line_RR[0] && Color_RR[1] > line_RR[1] && Color_RR[2] > line_RR[2] && !white_RR)//白
01876     {
01877     //invation_RR ^= 1;//start false,over true
01878     white_RR = true;//on true,noon false
01879     }   
01880     else if(!(Color_RR[0] > line_RR[0] && Color_RR[1] > line_RR[1] && Color_RR[2] > line_RR[2]))white_RR = false;//茶
01881 }
01882 
01883 void Move_On_the_Line()
01884 {
01885     Color_Changeflag();
01886     if(bluezone)
01887     {
01888         if(white_FL && !white_FR)
01889         {
01890             GoLeft();
01891             invation_FL = false;
01892             invation_FR = true;
01893             on_line_flag = false;
01894         }
01895         if(!white_FL && white_FR)
01896         {
01897             GoRight();
01898             invation_FL = false;
01899             invation_FR = false;
01900             on_line_flag = false;
01901         }
01902         if(!white_FL && !white_FR)
01903         {
01904             if(!white_RR)
01905             {
01906                 if(invation_FR)
01907                 {
01908                     GoLeft();
01909                     on_line_flag = false;
01910                 }else if(!invation_FR)
01911                 {
01912                     GoRight();
01913                     on_line_flag = false;
01914                 }
01915             }
01916         }
01917         if(white_RR)
01918         {
01919             GoMae();
01920             on_line_flag = true;    
01921         }
01922     }else if(redzone)
01923     {
01924         
01925         if(white_FL && !white_FR)
01926         {
01927             GoLeft();
01928             invation_FL = false;
01929             invation_FR = false;
01930             on_line_flag = false;
01931         }
01932         if(white_FR && !white_FL)
01933         {
01934             GoRight();
01935             invation_FL = true;
01936             invation_FR = false;
01937             on_line_flag = false;
01938         }
01939         if(!white_FL && !white_FR)
01940         {
01941             if(!white_RR)
01942             {
01943                 if(invation_FL)
01944                 {
01945                     GoRight();
01946                     on_line_flag = false;
01947                 }else if(!invation_FL)
01948                 {
01949                     GoLeft();
01950                     on_line_flag = false;
01951                 }
01952             }
01953         }
01954         if(white_RR)
01955         {
01956             GoMae();
01957             on_line_flag = true;    
01958         }
01959     }
01960 }
01961 void Move_On_the_Line_Trace()
01962 {
01963     Color_Changeflag();
01964     if(bluezone)
01965     {
01966         if(white_FL && !white_FR && !white_RR)//右に行きすぎ
01967         {
01968             invation_FR = true;
01969             invation_FL = false;
01970             invation_RR = true;
01971             GoHidariNanameMae();
01972             on_line_flag = false;
01973         }else if(!white_FL && white_FR && !white_RR)//左に行き過ぎ
01974         {
01975             invation_FR = false;
01976             invation_FL = false;
01977             invation_RR = false;
01978             GoMigiNanameMae();
01979             on_line_flag = false;
01980         }else if(!white_FL && !white_FR && !white_RR)//線の右か左
01981         {
01982             if(!invation_FL && !invation_FR && !invation_RR)
01983             {
01984                 GoMigiNanameMae();
01985                 on_line_flag = false;
01986                 
01987             }
01988             if(invation_FL && invation_FR && invation_RR)
01989             {
01990                 GoHidariNanameMae();
01991                 on_line_flag = false;
01992             }
01993         }else if(white_FL && !white_FR && white_RR)//ちょい右
01994         {
01995             invation_FR = true;
01996             invation_FL = false;
01997             invation_RR = false; 
01998             GoHidariNanameMae();            
01999             on_line_flag = false;
02000         }else if(!white_FL && white_FR && white_RR)//ちょい左
02001         {
02002             invation_FR = true;
02003             invation_FL = false;
02004             invation_RR = true;
02005             GoMigiNanameMae();
02006             on_line_flag = false;
02007         }else if(white_FL && white_FR && !white_RR)//機体が斜め
02008         {
02009             if(invation_RR)
02010             {
02011                 GoMae();
02012                 on_line_flag = false;
02013             }
02014             if(!invation_RR)
02015             {
02016                 GoMae();
02017                 on_line_flag  = false;
02018             }
02019         }else if(white_FL && white_FR && white_RR)//十字の時
02020         {
02021             GoMae();
02022             on_line_flag  = true;
02023         }else if(!white_FL && !white_FR && white_RR)//OK
02024         {
02025             GoMae();
02026             on_line_flag  = true;
02027         }
02028     }else if(redzone)
02029     {
02030         if(white_FL && !white_FR && !white_RR)//右に行きすぎ
02031         {
02032             invation_FR = false;
02033             invation_FL = false;
02034             invation_RR = false;
02035             GoHidariNanameMae();
02036             on_line_flag = false;
02037         }else if(!white_FL && white_FR && !white_RR)//左に行き過ぎ
02038         {
02039             invation_FR = false;
02040             invation_FL = true;
02041             invation_RR = true;
02042             GoMigiNanameMae();
02043             on_line_flag = false;
02044         }else if(!white_FL && !white_FR && !white_RR)//線の右か左
02045         {
02046             if(!invation_FL && !invation_FR && !invation_RR)
02047             {
02048                 GoHidariNanameMae();
02049                 on_line_flag = false;
02050                 
02051             }
02052             if(invation_FL && invation_FR && invation_RR)
02053             {
02054                 GoMigiNanameMae();
02055                 on_line_flag = false;
02056             }
02057         }else if(white_FL && !white_FR && white_RR)//ちょい右
02058         {
02059             invation_FR = false;
02060             invation_FL = false;
02061             invation_RR = false; 
02062             GoHidariNanameMae();            
02063             on_line_flag = false;
02064         }else if(!white_FL && white_FR && white_RR)//ちょい左
02065         {
02066             invation_FR = false;
02067             invation_FL = true;
02068             invation_RR = false;
02069             GoMigiNanameMae();
02070             on_line_flag = false;
02071         }else if(white_FL && white_FR && !white_RR)//機体が斜め
02072         {
02073             if(invation_RR)
02074             {
02075                 GoMae();
02076                 on_line_flag = false;
02077             }
02078             if(!invation_RR)
02079             {
02080                 GoMae();
02081                 on_line_flag  = false;
02082             }
02083         }else if(white_FL && white_FR && white_RR)//十字の時
02084         {
02085             GoMae();
02086             on_line_flag  = true;
02087         }else if(!white_FL && !white_FR && white_RR)//OK
02088         {
02089             GoMae();
02090             on_line_flag  = true;
02091         }
02092     }
02093 }
02094 
02095 void Move_On_the_Line_BackTrace()
02096 {
02097     Color_Changeflag();
02098     if(bluezone)
02099     {
02100         if(white_FL && !white_FR && !white_RR)//右に行きすぎ
02101         {
02102             invation_FR = true;
02103             invation_FL = false;
02104             invation_RR = true;
02105             GoHidariNanameUshiro();
02106             on_line_flag = false;
02107         }else if(!white_FL && white_FR && !white_RR)//左に行き過ぎ
02108         {
02109             invation_FR = false;
02110             invation_FL = false;
02111             invation_RR = false;
02112             GoMigiNanameUshiro();
02113             on_line_flag = false;
02114         }else if(!white_FL && !white_FR && !white_RR)//線の右か左
02115         {
02116             if(!invation_FL && !invation_FR && !invation_RR)
02117             {
02118                 GoMigiNanameUshiro();
02119                 on_line_flag = false;
02120                 
02121             }
02122             if(invation_FL && invation_FR && invation_RR)
02123             {
02124                 GoHidariNanameUshiro();
02125                 on_line_flag = false;
02126             }
02127         }else if(white_FL && !white_FR && white_RR)//ちょい右
02128         {
02129             invation_FR = true;
02130             invation_FL = false;
02131             invation_RR = false; 
02132             GoHidariNanameUshiro();            
02133             on_line_flag = false;
02134         }else if(!white_FL && white_FR && white_RR)//ちょい左
02135         {
02136             invation_FR = true;
02137             invation_FL = false;
02138             invation_RR = true;
02139             GoMigiNanameUshiro();
02140             on_line_flag = false;
02141         }else if(white_FL && white_FR && !white_RR)//機体が斜め
02142         {
02143             if(invation_RR)
02144             {
02145                 RightTurning();
02146                 on_line_flag = false;
02147             }
02148             if(!invation_RR)
02149             {
02150                 LeftTurning();
02151                 on_line_flag  = false;
02152             }
02153         }else if(white_FL && white_FR && white_RR)//十字の時
02154         {
02155             GoBack();
02156             on_line_flag  = true;
02157         }else if(!white_FL && !white_FR && white_RR)//OK
02158         {
02159             GoBack();
02160             on_line_flag  = true;
02161         }
02162     }else if(redzone)
02163     {
02164         if(white_FL && !white_FR && !white_RR)//右に行きすぎ
02165         {
02166             invation_FR = false;
02167             invation_FL = false;
02168             invation_RR = false;
02169             GoHidariNanameUshiro();
02170             on_line_flag = false;
02171         }else if(!white_FL && white_FR && !white_RR)//左に行き過ぎ
02172         {
02173             invation_FR = false;
02174             invation_FL = true;
02175             invation_RR = true;
02176             GoMigiNanameUshiro();
02177             on_line_flag = false;
02178         }else if(!white_FL && !white_FR && !white_RR)//線の右か左
02179         {
02180             if(!invation_FL && !invation_FR && !invation_RR)
02181             {
02182                 GoHidariNanameUshiro();
02183                 on_line_flag = false;
02184                 
02185             }
02186             if(invation_FL && invation_FR && invation_RR)
02187             {
02188                 GoMigiNanameUshiro();
02189                 on_line_flag = false;
02190             }
02191         }else if(white_FL && !white_FR && white_RR)//ちょい右
02192         {
02193             invation_FR = false;
02194             invation_FL = false;
02195             invation_RR = false; 
02196             GoHidariNanameUshiro();            
02197             on_line_flag = false;
02198         }else if(!white_FL && white_FR && white_RR)//ちょい左
02199         {
02200             invation_FR = false;
02201             invation_FL = true;
02202             invation_RR = false;
02203             GoMigiNanameUshiro();
02204             on_line_flag = false;
02205         }else if(white_FL && white_FR && !white_RR)//機体が斜め
02206         {
02207             if(invation_RR)
02208             {
02209                 LeftTurning();
02210                 on_line_flag = false;
02211             }
02212             if(!invation_RR)
02213             {
02214                 RightTurning();
02215                 on_line_flag  = false;
02216             }
02217         }else if(white_FL && white_FR && white_RR)//十字の時
02218         {
02219             GoBack();
02220             on_line_flag  = true;
02221         }else if(!white_FL && !white_FR && white_RR)//OK
02222         {
02223             GoBack();
02224             on_line_flag  = true;
02225         }
02226     }
02227 }
02228 void BuzzerTimer_func()
02229 {
02230     buzzer = !buzzer;
02231 }
02232 
02233 void launchTimer_func() {
02234     
02235     current = 2;
02236 }
02237 void launchTimer_func1(){
02238     //masterSend.launch = true;
02239     current = 6;
02240 }
02241 void launchTimer_func2()
02242 {
02243     //masterSend.launch = true;
02244     current = 6;
02245 }
02246 #pragma endregion