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/Controller/Controller.h"
00009 #include "../../Input/ExternalInt/ExternalInt.h"
00010 #include "../../Input/Switch/Switch.h"
00011 #include "../../Input/ColorSensor/ColorSensor.h"
00012 #include "../../Input/AccelerationSensor/AccelerationSensor.h"
00013 #include "../../Input/Potentiometer/Potentiometer.h"
00014 #include "../../Input/Rotaryencoder/Rotaryencoder.h"
00015 #include "../../LED/LED.h"
00016 #include "../../Safty/Safty.h"
00017 #include "../Using.h"
00018 
00019 
00020 using namespace SWITCH;
00021 using namespace COLORSENSOR;
00022 using namespace ACCELERATIONSENSOR;
00023 using namespace PID_SPACE;
00024 using namespace ROTARYENCODER;
00025 
00026 static CONTROLLER::ControllerData *controller;
00027 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
00028 ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
00029 
00030 static bool lock;
00031 static bool processChangeComp;
00032 static int current;
00033 
00034 static void AllActuatorReset();
00035 
00036 #ifdef USE_SUBPROCESS
00037 static void (*Process[USE_PROCESS_NUM])(void);
00038 #endif
00039 
00040 #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
00041 
00042 /*Replace here with the definition code of your variables.*/
00043 
00044 Serial pc(USBTX, USBRX);
00045 
00046 //************メカナム********************
00047 
00048 const int mecanum[15][15]=
00049 {
00050     {   0,    5,     21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255},
00051     {  -5,    0,      5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255},
00052     { -21,   -5,      0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255},
00053     { -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255},
00054     { -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255},
00055     {-130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255},
00056     {-187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255},
00057     {-255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255},
00058     {-255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187},
00059     {-255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130},
00060     {-255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83},
00061     {-255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47},
00062     {-255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21},
00063     {-255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5},
00064     {-255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0}
00065 };
00066 
00067 const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
00068 uint8_t SetStatus(int);
00069 uint8_t SetStatus(int pwmVal){
00070     if(pwmVal < 0) return BACK;
00071     else if(pwmVal > 0) return FOR;
00072     else if(pwmVal == 0) return BRAKE;
00073     else return BRAKE;
00074 }
00075 uint8_t SetPWM(int);
00076 uint8_t SetPWM(int pwmVal){
00077     if(pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
00078     else return abs(pwmVal);
00079 }
00080 
00081 //************メカナム********************
00082 
00083 //************カラーセンサ********************
00084 
00085 int Color_A[3]; //[赤,緑,青]
00086 int Color_B[3];
00087 int Color_C[3];
00088 int Color_D[3];         
00089 int intergration = 50;
00090 
00091 int Avecolor_A[3];
00092 int Avecolor_B[3];
00093 int Avecolor_C[3];
00094 int Avecolor_D[3];
00095 
00096 void ColorIn();
00097 void ColorDetection();
00098 void getcolor();
00099 
00100 //************カラーセンサ********************
00101 
00102 //************ライントレース変数*******************
00103 int PointA[3] = {400, 700, 1000};//赤，緑，青
00104 int PointB[3] = {400, 700, 1000};//赤，緑，青
00105 int PointC[3] = {1000, 1700, 2400};//赤，緑，青
00106     
00107 int startP = 35;
00108 int downP = 5;
00109 
00110 int Asasult = 0;
00111 int Bsasult = 0;
00112 int Csasult = 0;
00113 int Dsasult = 0;
00114 
00115 bool compA = false;
00116 bool compB = false;
00117 bool compC = false;
00118 bool compD = false;
00119     
00120 bool invationA = false;
00121 bool invationB = false;
00122 bool invationC = false;
00123 bool invationD = false;
00124 
00125 Ticker Color_T;
00126 
00127 void ColorDetection();
00128 void Color_changeflag();
00129 
00130 //************ライントレース変数*******************
00131 
00132 //************ROタコン******************
00133 QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
00134 QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
00135 Ticker get_rpm;
00136 PID Rt_X = PID(0.03, -255, 255, 0.12, 0, 0);  
00137 PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0); 
00138 double rpmX;
00139 double rpmY;
00140 double disX;
00141 double disY;
00142 int palseX;
00143 int palseY;
00144 int RtpwmX;
00145 int RtpwmY; 
00146 double goalX = 900.000;
00147 double goalY = 700.000; 
00148 
00149 //double goalXB = 900.000;
00150 //double goalYB = 700.000;
00151 void filip();
00152 void filipB();
00153 
00154 //************ROタコン****************** 
00155 
00156 //************ジャイロ*******************
00157 bool Angle_flagI = false;
00158 int Angle;
00159 PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
00160 float rotateY;
00161 //初期値 -5
00162 int AngletargetX = 4; 
00163 int AngletargetY = -12;
00164 int AngletargetI = -5;
00165 //************ジャイロ*******************
00166 
00167 //************Buzzer******************
00168 //DigitalOut buzzer(BUZZER_PIN);
00169 PwmOut buzzer(BUZZER_PIN);
00170 void BuzzerTimer_func();
00171 Ticker BuzzerTimer;
00172 bool Emsflag = false;
00173 //************Buzzer******************
00174 
00175 //************TapeLed*****************
00176 void TapeLedEms_func();
00177 TapeLedData tapeLED;
00178 TapeLedData sendLedData;
00179 TapeLED_Mode ledMode = Normal;
00180 Ticker tapeLedTimer;
00181 //************TapaLed*****************
00182 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
00183 
00184 #ifdef USE_SUBPROCESS
00185 #if USE_PROCESS_NUM>0
00186 static void Process0(void);
00187 #endif
00188 #if USE_PROCESS_NUM>1
00189 static void Process1(void);
00190 #endif
00191 #if USE_PROCESS_NUM>2
00192 static void Process2(void);
00193 #endif
00194 #if USE_PROCESS_NUM>3
00195 static void Process3(void);
00196 #endif
00197 #if USE_PROCESS_NUM>4
00198 static void Process4(void);
00199 #endif
00200 #if USE_PROCESS_NUM>5
00201 static void Process5(void);
00202 #endif
00203 #if USE_PROCESS_NUM>6
00204 static void Process6(void);
00205 #endif
00206 #if USE_PROCESS_NUM>7
00207 static void Process7(void);
00208 #endif
00209 #if USE_PROCESS_NUM>8
00210 static void Process8(void);
00211 #endif
00212 #if USE_PROCESS_NUM>9
00213 static void Process9(void);
00214 #endif
00215 #endif
00216 
00217 void SystemProcessInitialize()
00218 {
00219     #pragma region USER-DEFINED_VARIABLE_INIT
00220     /*Replace here with the initialization code of your variables.*/
00221     get_rpm.attach_us(&filip,100);
00222     buzzer.period(1.0/800);
00223     
00224     #pragma endregion USER-DEFINED_VARIABLE_INIT
00225 
00226     lock = true;
00227     processChangeComp = true;
00228     current = DEFAULT_PROCESS;
00229 
00230     #ifdef USE_SUBPROCESS
00231     #if USE_PROCESS_NUM>0
00232     Process[0] = Process0;
00233     #endif
00234     #if USE_PROCESS_NUM>1
00235     Process[1] = Process1;
00236     #endif
00237     #if USE_PROCESS_NUM>2
00238     Process[2] = Process2;
00239     #endif
00240     #if USE_PROCESS_NUM>3
00241     Process[3] = Process3;
00242     #endif
00243     #if USE_PROCESS_NUM>4
00244     Process[4] = Process4;
00245     #endif
00246     #if USE_PROCESS_NUM>5
00247     Process[5] = Process5;
00248     #endif
00249     #if USE_PROCESS_NUM>6
00250     Process[6] = Process6;
00251     #endif
00252     #if USE_PROCESS_NUM>7
00253     Process[7] = Process7;
00254     #endif
00255     #if USE_PROCESS_NUM>8
00256     Process[8] = Process8;
00257     #endif
00258     #if USE_PROCESS_NUM>9
00259     Process[9] = Process9;
00260     #endif
00261     #endif
00262 }
00263 
00264 static void SystemProcessUpdate()
00265 {
00266     #ifdef USE_SUBPROCESS
00267     if(controller->Button.HOME) lock = false;
00268     
00269     if(controller->Button.START && processChangeComp)
00270     {
00271         current++;
00272         if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
00273         processChangeComp = false;
00274     }
00275     else if(controller->Button.SELECT && processChangeComp)
00276     {
00277         current--;
00278         if (current < 0) current = 0;
00279         processChangeComp = false;
00280     }
00281     else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
00282     #endif
00283     
00284     #ifdef USE_MOTOR
00285     ACTUATORHUB::MOTOR::Motor::Update(motor);
00286     #endif
00287     
00288     #ifdef USE_SOLENOID
00289     ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
00290     #endif
00291 
00292     #ifdef USE_RS485
00293     ACTUATORHUB::ActuatorHub::Update();
00294     #endif
00295     
00296 }
00297 
00298 
00299 
00300 void SystemProcess()
00301 {
00302     SystemProcessInitialize();
00303 
00304     while(1)
00305     {
00306         getcolor();
00307         pc.printf("R1:%d, G1:%d, B1:%d \r\n",Avecolor_A[0],Avecolor_A[1],Avecolor_A[2]);
00308         pc.printf("R2:%d, G2:%d, B2:%d \r\n",Avecolor_B[0],Avecolor_B[1],Avecolor_B[2]);
00309         pc.printf("R3:%d, G3:%d, B3:%d \r\n",Avecolor_C[0],Avecolor_C[1],Avecolor_C[2]);
00310         pc.printf("R4:%d, G4:%d, B4:%d \r\n",Avecolor_D[0],Avecolor_D[1],Avecolor_D[2]);
00311         
00312         #ifdef USE_MU
00313         controller = CONTROLLER::Controller::GetData();
00314         #endif
00315 
00316         #ifdef USE_ERRORCHECK
00317         if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
00318         {
00319             CONTROLLER::Controller::DataReset();
00320             AllActuatorReset();
00321             lock = true;
00322             buzzer = 0.5;
00323             BuzzerTimer.attach(BuzzerTimer_func, 0.5);
00324         }
00325         else
00326         #endif
00327         {
00328 
00329             #ifdef USE_SUBPROCESS
00330             if(!lock)
00331             {
00332                 Process[current]();
00333             }
00334             else
00335             #endif
00336             {
00337                 //ロック時の処理
00338             }
00339         }
00340         
00341         if ((EMS_0 || EMS_1) && !Emsflag){
00342             buzzer = 0.5;
00343             BuzzerTimer.attach(BuzzerTimer_func, 1.2);
00344             Emsflag = true;
00345             ledMode = EMS;
00346             current = 0;
00347             tapeLedTimer.attach(TapeLedEms_func, 1.2);
00348             sendLedData.code = (uint32_t)Red;
00349         }
00350         
00351         if(!EMS_0 && !EMS_1) {
00352             buzzer = 0;
00353             BuzzerTimer.detach();
00354             Emsflag = false;
00355             if(ledMode == EMS) ledMode = Normal;
00356             tapeLedTimer.detach();
00357         }
00358         
00359         switch(ledMode)
00360         {
00361             case EMS :
00362                 break;
00363 
00364             case Normal :
00365                 sendLedData.code = tapeLED.code;
00366 
00367             default:
00368                 break;
00369         }
00370         
00371         SystemProcessUpdate();
00372     }
00373 }
00374 
00375 
00376     
00377 
00378 #pragma region PROCESS
00379 #ifdef USE_SUBPROCESS
00380 #if USE_PROCESS_NUM>0
00381 static void Process0() 
00382 {   
00383     tapeLED.code = (uint32_t)Green;
00384     if(RedSW){
00385         current = 1;
00386     }
00387     if(BlueSW){
00388         current = 2;
00389     }
00390 }
00391 #endif
00392 
00393 #if USE_PROCESS_NUM>1
00394 static void Process1() //手動
00395 {
00396     tapeLED.code = (uint32_t)Orange;
00397     motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]);
00398     motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]);
00399     motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]); 
00400     motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);
00401          
00402     motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X])    *0.8;
00403     motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X])       *0.8;
00404     motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
00405     motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y])    *0.8;
00406                   
00407     if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
00408     motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
00409     motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
00410     }
00411     
00412     if(controller->Button.R){
00413         motor[Angle_R].dir = FOR;
00414         motor[Angle_L].dir = BACK;
00415         motor[Angle_R].pwm = 150;
00416         motor[Angle_L].pwm = 150;
00417     }else if(controller->Button.L){
00418         motor[Angle_R].dir = BACK;
00419         motor[Angle_L].dir = FOR;
00420         motor[Angle_R].pwm = 150;
00421         motor[Angle_L].pwm = 150;
00422     }else{
00423         motor[Angle_R].dir = BRAKE;
00424         motor[Angle_L].dir = BRAKE;
00425         motor[Angle_R].pwm = 255;
00426         motor[Angle_L].pwm = 255;
00427     }
00428     
00429     if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
00430         motor[Angle_R].dir = BRAKE;
00431         motor[Angle_L].dir = BRAKE;
00432         motor[Angle_R].pwm = 255;
00433         motor[Angle_L].pwm = 255;
00434     }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
00435         motor[Angle_R].dir = BRAKE;
00436         motor[Angle_L].dir = BRAKE;
00437         motor[Angle_R].pwm = 255;
00438         motor[Angle_L].pwm = 255;
00439     }
00440     /*for(int i = 0;i<20;i++){
00441         float y = 0;
00442         y = acc[1]*1000;
00443         float rotateY = (y - 305)/2.21 - 90;
00444         Angle += rotateY;
00445     }
00446     Angle = Angle/20;
00447     pc.printf("Y:%d \r\n",Angle);*/
00448     
00449     //wheel.getPulses()...どちらの方向にどれだけ回ったか
00450     //pc.printf("Pulses:%07d \r\n",wheel.getPulses());
00451     //軸が何回転したか
00452     //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
00453 }
00454 #endif
00455 
00456 #if USE_PROCESS_NUM>2
00457 static void Process2() //trace
00458 {   
00459     tapeLED.code = (uint32_t)Yellow;
00460     static bool color_flag = false;
00461     
00462     static bool traceon = false;//fase1
00463     static bool yokofla = false;//fase2
00464     static bool boxslip = false;//fase3
00465     
00466     //static bool syu = false;
00467     
00468     ColorDetection();
00469     Color_changeflag();
00470     
00471     if(controller->Button.B && !color_flag)
00472     {
00473         traceon = true;
00474         color_flag = true;
00475     }
00476     else if(!controller->Button.B)color_flag = false;
00477     
00478     if(traceon)
00479     {
00480         Color_changeflag();
00481         if(!invationA && !compA && !invationB && !compB)
00482         {
00483         motor[TIRE_FR].dir = FOR;
00484         motor[TIRE_FL].dir = FOR;
00485         motor[TIRE_BR].dir = BACK;
00486         motor[TIRE_BL].dir = BACK;
00487         
00488         motor[TIRE_FR].pwm = startP;
00489         motor[TIRE_FL].pwm = startP;
00490         motor[TIRE_BR].pwm = startP;
00491         motor[TIRE_BL].pwm = startP;
00492         
00493         Color_changeflag();
00494         }
00495         else if(invationC && compC && !invationB && !compB)
00496         {
00497         for(int i = 0; i<1000; i++){
00498         motor[TIRE_FR].dir = BRAKE;
00499         motor[TIRE_FL].dir = BRAKE;
00500         motor[TIRE_BR].dir = BRAKE;
00501         motor[TIRE_BL].dir = BRAKE;
00502         
00503         motor[TIRE_FR].pwm = 255;
00504         motor[TIRE_FL].pwm = 255;
00505         motor[TIRE_BR].pwm = 255;
00506         motor[TIRE_BL].pwm = 255;
00507         }
00508         
00509         yokofla = true;
00510         traceon = false;
00511         }
00512     }
00513     
00514     
00515     if(yokofla && !traceon)
00516     {
00517         //pointcalculation();
00518     Color_changeflag();
00519     if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
00520     {
00521         motor[TIRE_FR].dir = BRAKE;
00522         motor[TIRE_FL].dir = BRAKE;
00523         motor[TIRE_BR].dir = BRAKE;
00524         motor[TIRE_BL].dir = BRAKE;
00525         
00526         motor[TIRE_FR].pwm = 255;
00527         motor[TIRE_FL].pwm = 255;
00528         motor[TIRE_BR].pwm = 255;
00529         motor[TIRE_BL].pwm = 255;
00530         
00531         wait(2);
00532         
00533         boxslip = true;
00534         yokofla = false;
00535     }
00536     else if(compA && compB && compC)
00537     {
00538         motor[TIRE_FR].dir = FOR;
00539         motor[TIRE_FL].dir = BACK;
00540         motor[TIRE_BR].dir = FOR;
00541         motor[TIRE_BL].dir = BACK;
00542         
00543         motor[TIRE_FR].pwm = startP;
00544         motor[TIRE_FL].pwm = startP;
00545         motor[TIRE_BR].pwm = startP;
00546         motor[TIRE_BL].pwm = startP;
00547         
00548         Color_changeflag();
00549     }
00550     else if(invationA && invationB && invationC)
00551     {
00552         motor[TIRE_FR].dir = FREE;
00553         motor[TIRE_FL].dir = BACK;
00554         motor[TIRE_BR].dir = FOR;
00555         motor[TIRE_BL].dir = FREE;
00556         
00557         //motor[TIRE_FR].pwm = startP;
00558         motor[TIRE_FL].pwm = startP;
00559         motor[TIRE_BR].pwm = startP;
00560         //motor[TIRE_BL].pwm = startP;
00561     }
00562     else if(!invationA && !invationB && !invationC)
00563     {
00564         motor[TIRE_FR].dir = FOR;
00565         motor[TIRE_FL].dir = FREE;
00566         motor[TIRE_BR].dir = FREE;
00567         motor[TIRE_BL].dir = BACK;
00568         
00569         motor[TIRE_FR].pwm = startP;
00570         //motor[TIRE_FL].pwm = startP;
00571         //motor[TIRE_BR].pwm = startP;
00572         motor[TIRE_BL].pwm = startP;
00573     }
00574     else if(!invationA && compC && invationC)//C固定A下
00575         {
00576         motor[TIRE_FR].dir = BRAKE;
00577         motor[TIRE_FL].dir = BRAKE;
00578         motor[TIRE_BR].dir = BACK;
00579         motor[TIRE_BL].dir = BACK;
00580         
00581         motor[TIRE_FR].pwm = 255;
00582         motor[TIRE_FL].pwm = 100;
00583         motor[TIRE_BR].pwm = 55;
00584         motor[TIRE_BL].pwm = startP;
00585         
00586         Color_changeflag();
00587         }
00588     else if(compA && compB && !invationC)//AB固定C下
00589         {
00590         motor[TIRE_FR].dir = FOR;
00591         motor[TIRE_FL].dir = FOR;
00592         motor[TIRE_BR].dir = BRAKE;
00593         motor[TIRE_BL].dir = BRAKE;
00594         
00595         motor[TIRE_FR].pwm = 55;
00596         motor[TIRE_FL].pwm = startP;
00597         motor[TIRE_BR].pwm = 255;
00598         motor[TIRE_BL].pwm = 100;
00599         
00600         Color_changeflag();
00601         }
00602         else if(compA && compB && !compC && invationC)//AB固定C上
00603         {
00604         motor[TIRE_FR].dir = BACK;
00605         motor[TIRE_FL].dir = BACK;
00606         motor[TIRE_BR].dir = BRAKE;
00607         motor[TIRE_BL].dir = BRAKE;
00608         
00609         motor[TIRE_FR].pwm = startP;
00610         motor[TIRE_FL].pwm = 55;
00611         motor[TIRE_BR].pwm = 255;
00612         motor[TIRE_BL].pwm = 100;
00613         
00614         Color_changeflag();
00615         }
00616         else if(!compA && invationA && compC)//C固定A上
00617         {
00618         motor[TIRE_FR].dir = BRAKE;
00619         motor[TIRE_FL].dir = BRAKE;
00620         motor[TIRE_BR].dir = FOR;
00621         motor[TIRE_BL].dir = FOR;
00622         
00623         motor[TIRE_FR].pwm = 255;
00624         motor[TIRE_FL].pwm = 100;
00625         motor[TIRE_BR].pwm = startP;
00626         motor[TIRE_BL].pwm = 55;
00627         
00628         Color_changeflag();
00629         }
00630   }
00631   
00632    if(boxslip)
00633    {
00634         motor[TIRE_FR].dir = BRAKE;
00635         motor[TIRE_FL].dir = BRAKE;
00636         motor[TIRE_BR].dir = BRAKE;
00637         motor[TIRE_BL].dir = BRAKE;
00638         
00639         motor[TIRE_FR].pwm = 255;
00640         motor[TIRE_FL].pwm = 255;
00641         motor[TIRE_BR].pwm = 255;
00642         motor[TIRE_BL].pwm = 255;
00643    }
00644 }
00645 #endif
00646 
00647 #if USE_PROCESS_NUM>3
00648 static void Process3() //Blue Zone
00649 {
00650     filipB();
00651     
00652     static bool Rt_flagX = false;
00653     static bool Rt_flagY = false;
00654     
00655     if(Rt_flagX)
00656     {
00657       filipB();
00658         if(disX < goalX - 5){
00659         filipB();
00660         
00661         motor[TIRE_FR].dir = SetStatus(-RtpwmX);
00662         motor[TIRE_FL].dir = SetStatus(-RtpwmX);
00663         motor[TIRE_BR].dir = SetStatus(RtpwmX);
00664         motor[TIRE_BL].dir = SetStatus(RtpwmX);
00665         motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
00666         motor[TIRE_FL].pwm = SetPWM(RtpwmX);
00667         motor[TIRE_BR].pwm = SetPWM(RtpwmX);
00668         motor[TIRE_BL].pwm = SetPWM(RtpwmX);
00669     }
00670     else if(disX > goalX - 5){
00671         
00672         for(int i = 0; i<200; i++){
00673         motor[TIRE_FR].dir = BRAKE;
00674         motor[TIRE_FL].dir = BRAKE;
00675         motor[TIRE_BR].dir = BRAKE;
00676         motor[TIRE_BL].dir = BRAKE;
00677         
00678         motor[TIRE_FR].pwm = 255;
00679         motor[TIRE_FL].pwm = 255;
00680         motor[TIRE_BR].pwm = 255;
00681         motor[TIRE_BL].pwm = 255;
00682         }
00683         
00684         Rt_flagY = true;
00685         Rt_flagX = false;
00686     }
00687   }
00688     
00689 if(Rt_flagY && !Rt_flagX){
00690     filipB();
00691      if(disY < goalY - 5){
00692         filipB();
00693         motor[TIRE_FR].dir = SetStatus(-RtpwmY);
00694         motor[TIRE_FL].dir = SetStatus(RtpwmY);
00695         motor[TIRE_BR].dir = SetStatus(-RtpwmY);
00696         motor[TIRE_BL].dir = SetStatus(RtpwmY);
00697         motor[TIRE_FR].pwm = SetPWM(RtpwmY);
00698         motor[TIRE_FL].pwm = SetPWM(RtpwmY);
00699         motor[TIRE_BR].pwm = SetPWM(RtpwmY);
00700         motor[TIRE_BL].pwm = SetPWM(RtpwmY);
00701     }
00702     else if(disY > goalY - 5)
00703     {
00704         filipB();
00705         motor[TIRE_FR].dir = BRAKE;
00706         motor[TIRE_FL].dir = BRAKE;
00707         motor[TIRE_BR].dir = BRAKE;
00708         motor[TIRE_BL].dir = BRAKE;
00709         motor[TIRE_FR].pwm = 255*0.85;
00710         motor[TIRE_FL].pwm = 255;
00711         motor[TIRE_BR].pwm = 255;
00712         motor[TIRE_BL].pwm = 255;
00713     }
00714   }
00715 }
00716 #endif
00717 
00718 #if USE_PROCESS_NUM>4
00719 static void Process4() 
00720 {   
00721 
00722 }
00723 #endif
00724 
00725 #if USE_PROCESS_NUM>5
00726 static void Process5() //ロタコンXY 
00727 {
00728     tapeLED.code = (uint32_t)White;
00729     
00730     static bool nopushed = false; 
00731     static bool Rt_flagX = false;
00732     static bool Rt_flagY = false;
00733     
00734     if(controller->Button.A && !nopushed){
00735         Rt_flagX = true;
00736         nopushed = true;
00737         
00738         RtX.reset();
00739         RtY.reset();
00740     }else if(!controller->Button.A)nopushed = false;
00741 
00742     filip();
00743 
00744     if(Rt_flagX)
00745     {
00746       filip();
00747         if(disX < goalX - 5){
00748         filip();
00749         
00750         motor[TIRE_FR].dir = SetStatus(-RtpwmX);
00751         motor[TIRE_FL].dir = SetStatus(-RtpwmX);
00752         motor[TIRE_BR].dir = SetStatus(RtpwmX);
00753         motor[TIRE_BL].dir = SetStatus(RtpwmX);
00754         motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
00755         motor[TIRE_FL].pwm = SetPWM(RtpwmX);
00756         motor[TIRE_BR].pwm = SetPWM(RtpwmX);
00757         motor[TIRE_BL].pwm = SetPWM(RtpwmX);
00758     }
00759     else if(disX > goalX - 5){
00760         
00761         for(int i = 0; i<200; i++){
00762         motor[TIRE_FR].dir = BRAKE;
00763         motor[TIRE_FL].dir = BRAKE;
00764         motor[TIRE_BR].dir = BRAKE;
00765         motor[TIRE_BL].dir = BRAKE;
00766         
00767         motor[TIRE_FR].pwm = 255;
00768         motor[TIRE_FL].pwm = 255;
00769         motor[TIRE_BR].pwm = 255;
00770         motor[TIRE_BL].pwm = 255;
00771         }
00772         
00773         Rt_flagY = true;
00774         Rt_flagX = false;
00775     }
00776   }
00777     
00778     
00779 if(Rt_flagY && !Rt_flagX){
00780     filip();
00781      if(disY < goalY - 5){
00782         filip();
00783         motor[TIRE_FR].dir = SetStatus(-RtpwmY);
00784         motor[TIRE_FL].dir = SetStatus(RtpwmY);
00785         motor[TIRE_BR].dir = SetStatus(-RtpwmY);
00786         motor[TIRE_BL].dir = SetStatus(RtpwmY);
00787         motor[TIRE_FR].pwm = SetPWM(RtpwmY);
00788         motor[TIRE_FL].pwm = SetPWM(RtpwmY);
00789         motor[TIRE_BR].pwm = SetPWM(RtpwmY);
00790         motor[TIRE_BL].pwm = SetPWM(RtpwmY);
00791     }
00792     else if(disY > goalY - 5)
00793     {
00794         filip();
00795         motor[TIRE_FR].dir = BRAKE;
00796         motor[TIRE_FL].dir = BRAKE;
00797         motor[TIRE_BR].dir = BRAKE;
00798         motor[TIRE_BL].dir = BRAKE;
00799         motor[TIRE_FR].pwm = 255*0.85;
00800         motor[TIRE_FL].pwm = 255;
00801         motor[TIRE_BR].pwm = 255;
00802         motor[TIRE_BL].pwm = 255;
00803     }
00804   }
00805 }
00806 #endif
00807 
00808 #if USE_PROCESS_NUM>6
00809 static void Process6() 
00810 {
00811     tapeLED.code = (uint32_t)Yellow;
00812     static bool color_flag = false;
00813     
00814     static bool traceon = false;//fase1
00815     static bool yokofla = false;//fase2
00816     static bool boxslip = false;//fase3
00817     
00818     static bool nopushed = false; 
00819     static bool Rt_flagX = false;
00820     static bool Rt_flagY = false;
00821     
00822     //static bool syu = false;
00823     
00824     ColorDetection();
00825     Color_changeflag();
00826     
00827     if(controller->Button.B && !color_flag)
00828     {
00829         traceon = true;
00830         color_flag = true;
00831     }
00832     else if(!controller->Button.B)color_flag = false;
00833     
00834     if(traceon)
00835     {
00836         Color_changeflag();
00837         if(!invationA && !compA && !invationB && !compB)
00838         {
00839         motor[TIRE_FR].dir = FOR;
00840         motor[TIRE_FL].dir = FOR;
00841         motor[TIRE_BR].dir = BACK;
00842         motor[TIRE_BL].dir = BACK;
00843         
00844         motor[TIRE_FR].pwm = startP;
00845         motor[TIRE_FL].pwm = startP;
00846         motor[TIRE_BR].pwm = startP;
00847         motor[TIRE_BL].pwm = startP;
00848         
00849         Color_changeflag();
00850         }
00851         else if(invationC && compC && !invationB && !compB)
00852         {
00853         for(int i = 0; i<1000; i++){
00854         motor[TIRE_FR].dir = BRAKE;
00855         motor[TIRE_FL].dir = BRAKE;
00856         motor[TIRE_BR].dir = BRAKE;
00857         motor[TIRE_BL].dir = BRAKE;
00858         
00859         motor[TIRE_FR].pwm = 255;
00860         motor[TIRE_FL].pwm = 255;
00861         motor[TIRE_BR].pwm = 255;
00862         motor[TIRE_BL].pwm = 255;
00863         }
00864         
00865         yokofla = true;
00866         traceon = false;
00867         }
00868     }
00869     
00870     
00871     if(yokofla && !traceon)
00872     {
00873         //pointcalculation();
00874     Color_changeflag();
00875     if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
00876     {
00877         motor[TIRE_FR].dir = BRAKE;
00878         motor[TIRE_FL].dir = BRAKE;
00879         motor[TIRE_BR].dir = BRAKE;
00880         motor[TIRE_BL].dir = BRAKE;
00881         
00882         motor[TIRE_FR].pwm = 255;
00883         motor[TIRE_FL].pwm = 255;
00884         motor[TIRE_BR].pwm = 255;
00885         motor[TIRE_BL].pwm = 255;
00886         
00887         wait(2);
00888         
00889         boxslip = true;
00890         yokofla = false;
00891     }
00892     else if(compA && compB && compC)
00893     {
00894         motor[TIRE_FR].dir = FOR;
00895         motor[TIRE_FL].dir = BACK;
00896         motor[TIRE_BR].dir = FOR;
00897         motor[TIRE_BL].dir = BACK;
00898         
00899         motor[TIRE_FR].pwm = startP;
00900         motor[TIRE_FL].pwm = startP;
00901         motor[TIRE_BR].pwm = startP;
00902         motor[TIRE_BL].pwm = startP;
00903         
00904         Color_changeflag();
00905     }
00906     else if(invationA && invationB && invationC)
00907     {
00908         motor[TIRE_FR].dir = FREE;
00909         motor[TIRE_FL].dir = BACK;
00910         motor[TIRE_BR].dir = FOR;
00911         motor[TIRE_BL].dir = FREE;
00912         
00913         //motor[TIRE_FR].pwm = startP;
00914         motor[TIRE_FL].pwm = startP;
00915         motor[TIRE_BR].pwm = startP;
00916         //motor[TIRE_BL].pwm = startP;
00917     }
00918     else if(!invationA && !invationB && !invationC)
00919     {
00920         motor[TIRE_FR].dir = FOR;
00921         motor[TIRE_FL].dir = FREE;
00922         motor[TIRE_BR].dir = FREE;
00923         motor[TIRE_BL].dir = BACK;
00924         
00925         motor[TIRE_FR].pwm = startP;
00926         //motor[TIRE_FL].pwm = startP;
00927         //motor[TIRE_BR].pwm = startP;
00928         motor[TIRE_BL].pwm = startP;
00929     }
00930     else if(!invationA && compC && invationC)//C固定A下
00931         {
00932         motor[TIRE_FR].dir = BRAKE;
00933         motor[TIRE_FL].dir = BRAKE;
00934         motor[TIRE_BR].dir = BACK;
00935         motor[TIRE_BL].dir = BACK;
00936         
00937         motor[TIRE_FR].pwm = 255;
00938         motor[TIRE_FL].pwm = 100;
00939         motor[TIRE_BR].pwm = 55;
00940         motor[TIRE_BL].pwm = startP;
00941         
00942         Color_changeflag();
00943         }
00944     else if(compA && compB && !invationC)//AB固定C下
00945         {
00946         motor[TIRE_FR].dir = FOR;
00947         motor[TIRE_FL].dir = FOR;
00948         motor[TIRE_BR].dir = BRAKE;
00949         motor[TIRE_BL].dir = BRAKE;
00950         
00951         motor[TIRE_FR].pwm = 55;
00952         motor[TIRE_FL].pwm = startP;
00953         motor[TIRE_BR].pwm = 255;
00954         motor[TIRE_BL].pwm = 100;
00955         
00956         Color_changeflag();
00957         }
00958         else if(compA && compB && !compC && invationC)//AB固定C上
00959         {
00960         motor[TIRE_FR].dir = BACK;
00961         motor[TIRE_FL].dir = BACK;
00962         motor[TIRE_BR].dir = BRAKE;
00963         motor[TIRE_BL].dir = BRAKE;
00964         
00965         motor[TIRE_FR].pwm = startP;
00966         motor[TIRE_FL].pwm = 55;
00967         motor[TIRE_BR].pwm = 255;
00968         motor[TIRE_BL].pwm = 100;
00969         
00970         Color_changeflag();
00971         }
00972         else if(!compA && invationA && compC)//C固定A上
00973         {
00974         motor[TIRE_FR].dir = BRAKE;
00975         motor[TIRE_FL].dir = BRAKE;
00976         motor[TIRE_BR].dir = FOR;
00977         motor[TIRE_BL].dir = FOR;
00978         
00979         motor[TIRE_FR].pwm = 255;
00980         motor[TIRE_FL].pwm = 100;
00981         motor[TIRE_BR].pwm = startP;
00982         motor[TIRE_BL].pwm = 55;
00983         
00984         Color_changeflag();
00985         }
00986   }
00987   
00988    if(boxslip)
00989    {
00990         for(int i = 0; i<500; i++)
00991         motor[TIRE_FR].dir = BRAKE;
00992         motor[TIRE_FL].dir = BRAKE;
00993         motor[TIRE_BR].dir = BRAKE;
00994         motor[TIRE_BL].dir = BRAKE;
00995         
00996         motor[TIRE_FR].pwm = 255;
00997         motor[TIRE_FL].pwm = 255;
00998         motor[TIRE_BR].pwm = 255;
00999         motor[TIRE_BL].pwm = 255;
01000         
01001         Rt_flagX = true;
01002         nopushed = true;
01003         
01004         RtX.reset();
01005         RtY.reset();
01006    }
01007   
01008     
01009     /*if(controller->Button.A && !nopushed){
01010         Rt_flagX = true;
01011         nopushed = true;
01012         
01013         RtX.reset();
01014         RtY.reset();
01015     }else if(!controller->Button.A)nopushed = false;
01016     */
01017     filip();
01018 
01019     if(Rt_flagX)
01020     {
01021       filip();
01022         if(disX < goalX - 5){
01023         filip();
01024         
01025         motor[TIRE_FR].dir = SetStatus(-RtpwmX);
01026         motor[TIRE_FL].dir = SetStatus(-RtpwmX);
01027         motor[TIRE_BR].dir = SetStatus(RtpwmX);
01028         motor[TIRE_BL].dir = SetStatus(RtpwmX);
01029         motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
01030         motor[TIRE_FL].pwm = SetPWM(RtpwmX);
01031         motor[TIRE_BR].pwm = SetPWM(RtpwmX);
01032         motor[TIRE_BL].pwm = SetPWM(RtpwmX);
01033     }
01034     else if(disX > goalX - 5){
01035         
01036         for(int i = 0; i<500; i++){
01037         motor[TIRE_FR].dir = BRAKE;
01038         motor[TIRE_FL].dir = BRAKE;
01039         motor[TIRE_BR].dir = BRAKE;
01040         motor[TIRE_BL].dir = BRAKE;
01041         
01042         motor[TIRE_FR].pwm = 255;
01043         motor[TIRE_FL].pwm = 255;
01044         motor[TIRE_BR].pwm = 255;
01045         motor[TIRE_BL].pwm = 255;
01046         }
01047         
01048         Rt_flagY = true;
01049         Rt_flagX = false;
01050     }
01051   }
01052     
01053     
01054 if(Rt_flagY && !Rt_flagX){
01055     filip();
01056      if(disY < goalY - 5){
01057         filip();
01058         motor[TIRE_FR].dir = SetStatus(-RtpwmY);
01059         motor[TIRE_FL].dir = SetStatus(RtpwmY);
01060         motor[TIRE_BR].dir = SetStatus(-RtpwmY);
01061         motor[TIRE_BL].dir = SetStatus(RtpwmY);
01062         motor[TIRE_FR].pwm = SetPWM(RtpwmY);
01063         motor[TIRE_FL].pwm = SetPWM(RtpwmY);
01064         motor[TIRE_BR].pwm = SetPWM(RtpwmY);
01065         motor[TIRE_BL].pwm = SetPWM(RtpwmY);
01066     }
01067     else if(disY > goalY - 5)
01068     {
01069         filip();
01070         motor[TIRE_FR].dir = BRAKE;
01071         motor[TIRE_FL].dir = BRAKE;
01072         motor[TIRE_BR].dir = BRAKE;
01073         motor[TIRE_BL].dir = BRAKE;
01074         motor[TIRE_FR].pwm = 255;
01075         motor[TIRE_FL].pwm = 255;
01076         motor[TIRE_BR].pwm = 255;
01077         motor[TIRE_BL].pwm = 255;
01078     }
01079   }
01080    
01081 }
01082 #endif
01083 
01084 #if USE_PROCESS_NUM>7
01085 static void Process7()
01086 {
01087     tapeLED.code = (uint32_t)Hotpink;
01088     static bool Xnopush = false;
01089     static bool Ynopush = false;
01090     static bool Inopush = false;
01091     
01092     static bool Angle_flagX = false;
01093     static bool Angle_flagY = false;
01094     static bool ANgle_flagI = false;
01095     
01096     if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
01097         motor[Angle_R].dir = BRAKE;
01098         motor[Angle_L].dir = BRAKE;
01099         motor[Angle_R].pwm = 255;
01100         motor[Angle_L].pwm = 255;
01101     }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
01102         motor[Angle_R].dir = BRAKE;
01103         motor[Angle_L].dir = BRAKE;
01104         motor[Angle_R].pwm = 255;
01105         motor[Angle_L].pwm = 255;
01106     }
01107     for(int i = 0;i<20;i++){
01108         float y = 0;
01109         y = acc[1]*1000;
01110         float rotateY = (y - 305)/2.21 - 90;
01111         Angle += rotateY;
01112     }
01113     Angle = Angle /20;
01114     
01115     int gyropwmX = gyro.SetPV(Angle,AngletargetX);
01116     int gyropwmY = gyro.SetPV(Angle,AngletargetY);
01117     int gyropwmI = gyro.SetPV(Angle,AngletargetI);
01118     
01119     if(controller->Button.X && !Xnopush){
01120         Angle_flagX = true;
01121         Xnopush = true;
01122     }else if(!controller->Button.X)Xnopush = false; 
01123     
01124     if(controller->Button.Y && !Ynopush){
01125         Angle_flagY = true;
01126         Ynopush = true;
01127     }else if(!controller->Button.Y)Ynopush = false;
01128     
01129     if(controller->Button.A && !Inopush){
01130         Angle_flagI = true;
01131         Inopush = true;
01132     }else if(!controller->Button.A)Inopush = false;
01133     
01134     if (Angle_flagX){
01135         motor[Angle_R].dir = SetStatus(gyropwmX);
01136         motor[Angle_L].dir = SetStatus(-gyropwmX);
01137         motor[Angle_R].pwm = SetPWM(gyropwmX);
01138         motor[Angle_L].pwm = SetPWM(gyropwmX);
01139         if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
01140         motor[Angle_R].dir = BRAKE;
01141         motor[Angle_L].dir = BRAKE;
01142         motor[Angle_R].pwm = 255;
01143         motor[Angle_L].pwm = 255;
01144         Angle_flagX = false;
01145         }
01146     }
01147     
01148     if (Angle_flagY){
01149         motor[Angle_R].dir = SetStatus(-gyropwmY);
01150         motor[Angle_L].dir = SetStatus(gyropwmY);
01151         motor[Angle_R].pwm = SetPWM(gyropwmY);
01152         motor[Angle_L].pwm = SetPWM(gyropwmY);
01153         if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
01154         motor[Angle_R].dir = BRAKE;
01155         motor[Angle_L].dir = BRAKE;
01156         motor[Angle_R].pwm = 255;
01157         motor[Angle_L].pwm = 255;
01158         Angle_flagY = false;
01159         }
01160     }
01161     
01162     if (Angle_flagI){
01163         if(Angle < 0)
01164         {
01165             motor[Angle_R].dir = SetStatus(-gyropwmI);
01166             motor[Angle_L].dir = SetStatus(gyropwmI);
01167             motor[Angle_R].pwm = SetPWM(gyropwmI);
01168             motor[Angle_L].pwm = SetPWM(gyropwmI);
01169 
01170             if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
01171             motor[Angle_R].dir = BRAKE;
01172             motor[Angle_L].dir = BRAKE;
01173             motor[Angle_R].pwm = 255;
01174             motor[Angle_L].pwm = 255;
01175             Angle_flagI = false;
01176             }
01177         }
01178         else if(Angle > 0)
01179         {   
01180             motor[Angle_R].dir = FOR;
01181             motor[Angle_L].dir = BACK;
01182             motor[Angle_R].pwm = 150;
01183             motor[Angle_L].pwm = 150;
01184             
01185             if(Angle < 0){
01186                 motor[Angle_R].dir = SetStatus(gyropwmI);
01187                 motor[Angle_L].dir = SetStatus(-gyropwmI);
01188                 motor[Angle_R].pwm = SetPWM(gyropwmI);
01189                 motor[Angle_L].pwm = SetPWM(gyropwmI);
01190 
01191                 if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
01192                 motor[Angle_R].dir = BRAKE;
01193                 motor[Angle_L].dir = BRAKE;
01194                 motor[Angle_R].pwm = 255;
01195                 motor[Angle_L].pwm = 255;
01196                 Angle_flagI = false;
01197                 }
01198             }
01199         }   
01200     }
01201     else{
01202         motor[Angle_R].dir = BRAKE;
01203         motor[Angle_L].dir = BRAKE;
01204         motor[Angle_R].pwm = 255;
01205         motor[Angle_L].pwm = 255;
01206     }
01207 }
01208 #endif
01209 
01210 #if USE_PROCESS_NUM>8 //kakudo
01211 static void Process8()
01212 {
01213     
01214 }
01215 #endif
01216 
01217 #if USE_PROCESS_NUM>9
01218 static void Process9()
01219 {
01220     
01221 }
01222 #endif
01223 #endif
01224 #pragma endregion PROCESS
01225 
01226 static void AllActuatorReset()
01227 {
01228 
01229     #ifdef USE_SOLENOID
01230     solenoid.all = ALL_SOLENOID_OFF;
01231     #endif
01232 
01233     #ifdef USE_MOTOR
01234     for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
01235     {
01236         motor[i].dir = FREE;
01237         motor[i].pwm = 0;
01238     }
01239     #endif
01240 }
01241 
01242 #pragma region USER-DEFINED-FUNCTIONS
01243 
01244 void filip(){
01245     palseX = RtX.getPulses();
01246     palseY = RtY.getPulses();
01247     
01248     rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
01249     rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
01250     
01251     disX = 48*3.141*rpmX;
01252     disY = 48*3.141*rpmY;
01253     
01254     RtpwmX = (int)Rt_X.SetPV(disX , goalX);
01255     RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
01256 }
01257 
01258 void filipB(){
01259     palseX = RtX.getPulses();
01260     palseY = RtY.getPulses();
01261     
01262     rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
01263     rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
01264     
01265     disX = abs(48*3.141*rpmX);
01266     disY = 48*3.141*rpmY;
01267     
01268     RtpwmX = (int)Rt_X.SetPV(disX , goalX);
01269     RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
01270 }
01271 
01272 unsigned long ColorIn(int index)
01273 {
01274     int result = 0;
01275     bool rtn = false;
01276     for(int i=0; i<12; i++)
01277     {
01278         CK[index] = 1;
01279         rtn = DOUT[index];
01280         CK[index] = 0;
01281         if(rtn)
01282         {
01283            result|=(1 << i);
01284         }
01285     }
01286     return result;
01287 }
01288 
01289 void ColorDetection(){
01290     GATE = 0;
01291            
01292     CK[0] = 0;
01293     CK[1] = 0;
01294     CK[2] = 0;
01295     CK[3] = 0;
01296         
01297     RANGE = 1;
01298         
01299     GATE = 1;
01300     wait_ms(intergration);
01301     GATE = 0;
01302     wait_us(4);
01303         
01304     Color_A[0] = ColorIn(0); //赤
01305     wait_us(3);
01306     Color_A[1] = ColorIn(0); //青
01307     wait_us(3);
01308     Color_A[2] = ColorIn(0); //緑
01309         
01310     Color_B[0] = ColorIn(1);
01311     wait_us(3);
01312     Color_B[1] = ColorIn(1);
01313     wait_us(3);
01314     Color_B[2] = ColorIn(1);
01315          
01316     Color_C[0] = ColorIn(2);
01317     wait_us(3);
01318     Color_C[1] = ColorIn(2);
01319     wait_us(3);
01320     Color_C[2] = ColorIn(2);
01321         
01322     Color_D[0] = ColorIn(3);
01323     wait_us(3);
01324     Color_D[1] = ColorIn(3);
01325     wait_us(3);
01326     Color_D[2] = ColorIn(3);
01327 }
01328 
01329 void Color_changeflag(){
01330     ColorDetection();
01331     
01332     if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
01333     {
01334     invationA ^= 1;//start false,over true
01335     compA = true;//on true,noon false
01336     }   
01337     else if(!(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2]))compA = false;//茶
01338     
01339     if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
01340     {
01341     invationB ^= 1;//start false,over true
01342     compB = true;//on true,noon false
01343     }   
01344     else if(!(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2]))compB = false;//茶
01345     
01346     if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
01347     {
01348     invationC ^= 1;//start false,over true
01349     compC = true;//on true,noon false
01350     }   
01351     else if(!(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2]))compC = false;//茶
01352     /*
01353     if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
01354     {
01355     invationD ^= 1;//start false,over true
01356     compD = true;//on true,noon false
01357     }   
01358     else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
01359     */
01360 }
01361 
01362 void getcolor(){
01363     for(int i=0;i<10;i++){
01364         ColorDetection();
01365         
01366         Avecolor_A[0] += Color_A[0];
01367         Avecolor_A[1] += Color_A[1];
01368         Avecolor_A[2] += Color_A[2];
01369         Avecolor_B[0] += Color_B[0];
01370         Avecolor_B[1] += Color_B[1];
01371         Avecolor_B[2] += Color_B[2];
01372         Avecolor_C[0] += Color_C[0];
01373         Avecolor_C[1] += Color_C[1];
01374         Avecolor_C[2] += Color_C[2];
01375         Avecolor_D[0] += Color_D[0];
01376         Avecolor_D[1] += Color_D[1];
01377         Avecolor_D[2] += Color_D[2];
01378 }
01379 
01380 Avecolor_A[0] = Avecolor_A[0]/10;
01381 Avecolor_A[1] = Avecolor_A[1]/10;
01382 Avecolor_A[2] = Avecolor_A[2]/10;
01383 Avecolor_B[0] = Avecolor_B[0]/10;
01384 Avecolor_B[1] = Avecolor_B[1]/10;
01385 Avecolor_B[2] = Avecolor_B[2]/10;
01386 Avecolor_C[0] = Avecolor_C[0]/10;
01387 Avecolor_C[1] = Avecolor_C[1]/10;
01388 Avecolor_C[2] = Avecolor_C[2]/10;
01389 Avecolor_D[0] = Avecolor_D[0]/10;
01390 Avecolor_D[1] = Avecolor_D[1]/10;
01391 Avecolor_D[2] = Avecolor_D[2]/10;
01392 
01393 }
01394 
01395 void BuzzerTimer_func() {
01396     if(buzzer == 0.5){
01397         buzzer = 0;
01398     }
01399     else if(buzzer == 0){
01400         buzzer = 0.5;
01401     }
01402 }
01403 
01404 void TapeLedEms_func() {
01405     sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
01406 }
01407 #pragma endregion