Taiki Maruyama
/
MainBoard2018_Auto_Master_A_neww
aaaaaaaaa
Fork of MainBoard2018_Auto_Master_A_new by
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Wed Jul 13 2022 01:10:06 by 1.7.2