![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
uuu
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
System/Process/Process.cpp
- Committer:
- kishibekairohan
- Date:
- 2018-10-01
- Revision:
- 7:e88c5d47a3be
- Parent:
- 6:10e22bc327ce
- Child:
- 8:6fb3723f7747
File content as of revision 7:e88c5d47a3be:
#include "mbed.h" #include "Process.h" #include "QEI.h" #include "../../CommonLibraries/PID/PID.h" #include "../../Communication/RS485/ActuatorHub/ActuatorHub.h" #include "../../Communication/Controller/Controller.h" #include "../../Input/ExternalInt/ExternalInt.h" #include "../../Input/Switch/Switch.h" #include "../../Input/ColorSensor/ColorSensor.h" #include "../../Input/AccelerationSensor/AccelerationSensor.h" #include "../../Input/Potentiometer/Potentiometer.h" #include "../../Input/Rotaryencoder/Rotaryencoder.h" #include "../../LED/LED.h" #include "../../Safty/Safty.h" #include "../Using.h" using namespace SWITCH; using namespace COLORSENSOR; using namespace ACCELERATIONSENSOR; using namespace PID_SPACE; using namespace ROTARYENCODER; static CONTROLLER::ControllerData *controller; ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; static bool lock; static bool processChangeComp; static int current; static void AllActuatorReset(); #ifdef USE_SUBPROCESS static void (*Process[USE_PROCESS_NUM])(void); #endif #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE /*Replace here with the definition code of your variables.*/ Serial pc(USBTX, USBRX); unsigned long ColorIn(int index) { int result = 0; bool rtn = false; for(int i=0; i<12; i++) { CK[index] = 1; rtn = DOUT[index]; CK[index] = 0; if(rtn) { result|=(1 << i); } } return result; } #define TIRE_FR 4 //足回り前右 #define TIRE_FL 5 //足回り前左 #define TIRE_BR 2 //足回り後右 #define TIRE_BL 3 //足回り後左 #define Angle_R 0 //角度調節右 #define Angle_L 1 //角度調節左 #define Lim_AR 3 //角度調節右 #define Lim_AL 4 //角度調節左 #define Lim_R 0 //センター右 #define Lim_L 1 //センター左 //************メカナム******************** const int mecanum[15][15]= { { 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255}, { -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255}, { -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255}, { -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255}, { -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255}, {-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255}, {-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255}, {-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255}, {-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187}, {-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130}, {-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83}, {-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47}, {-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21}, {-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5}, {-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0} }; const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204}; uint8_t SetStatus(int); uint8_t SetStatus(int pwmVal){ if(pwmVal < 0) return BACK; else if(pwmVal > 0) return FOR; else if(pwmVal == 0) return BRAKE; else return BRAKE; } uint8_t SetPWM(int); uint8_t SetPWM(int pwmVal){ if(pwmVal == 0 || pwmVal > 255 || pwmVal < -255) return 255; else return abs(pwmVal); } //************メカナム******************** //************カラーセンサ******************** int Color_A[3]; //[赤,緑,青] int Color_B[3]; int Color_C[3]; int Color_D[3]; int intergration = 50; void ColorDetection(); //************カラーセンサ******************** //************ライントレース変数******************* int Point[3] = {234, 466, 590};//赤,緑,青 int startP = 35; int downP = 5; //************ライントレース変数******************* //ROタコン QEI wheel(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING); Ticker get_rpm; int rpm; int palse; double goalpoint = 3000.0000; PID startup = PID(0.03, -255, 255, 0.3, 0, 0); //************ジャイロ******************* float Angle; PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0); bool Angle_flagX = false; bool Angle_flagY = false; bool Angle_flagI = false; float rotateY; int AngletargetX = 50; int AngletargetY = -50; int Angle_I = -5; //************ジャイロ******************* #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 static void Process0(void); #endif #if USE_PROCESS_NUM>1 static void Process1(void); #endif #if USE_PROCESS_NUM>2 static void Process2(void); #endif #if USE_PROCESS_NUM>3 static void Process3(void); #endif #if USE_PROCESS_NUM>4 static void Process4(void); #endif #if USE_PROCESS_NUM>5 static void Process5(void); #endif #if USE_PROCESS_NUM>6 static void Process6(void); #endif #if USE_PROCESS_NUM>7 static void Process7(void); #endif #if USE_PROCESS_NUM>8 static void Process8(void); #endif #if USE_PROCESS_NUM>9 static void Process9(void); #endif #endif void SystemProcessInitialize() { #pragma region USER-DEFINED_VARIABLE_INIT /*Replace here with the initialization code of your variables.*/ #pragma endregion USER-DEFINED_VARIABLE_INIT lock = true; processChangeComp = true; current = DEFAULT_PROCESS; #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 Process[0] = Process0; #endif #if USE_PROCESS_NUM>1 Process[1] = Process1; #endif #if USE_PROCESS_NUM>2 Process[2] = Process2; #endif #if USE_PROCESS_NUM>3 Process[3] = Process3; #endif #if USE_PROCESS_NUM>4 Process[4] = Process4; #endif #if USE_PROCESS_NUM>5 Process[5] = Process5; #endif #if USE_PROCESS_NUM>6 Process[6] = Process6; #endif #if USE_PROCESS_NUM>7 Process[7] = Process7; #endif #if USE_PROCESS_NUM>8 Process[8] = Process8; #endif #if USE_PROCESS_NUM>9 Process[9] = Process9; #endif #endif } static void SystemProcessUpdate() { #ifdef USE_SUBPROCESS if(controller->Button.HOME) lock = false; if(controller->Button.START && processChangeComp) { current++; if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; processChangeComp = false; } else if(controller->Button.SELECT && processChangeComp) { current--; if (current < 0) current = 0; processChangeComp = false; } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; #endif #ifdef USE_MOTOR ACTUATORHUB::MOTOR::Motor::Update(motor); #endif #ifdef USE_SOLENOID ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); #endif #ifdef USE_RS485 ACTUATORHUB::ActuatorHub::Update(); #endif } void SystemProcess() { SystemProcessInitialize(); while(1) { /*wait(0.1); //wheel.getPulses()...どちらの方向にどれだけ回ったか pc.printf("Pulses:%07d \r\n",wheel.getPulses()); //軸が何回転したか pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2)); */ /* float x = 0, y= 0, z = 0; x = acc[0]*1000; y = acc[1]*1000; z = acc[2]*1000; pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z); float rotateX = (x - 306)/2.22 - 90; float rotateY = (y - 305)/2.21 - 90; float rotateZ = (z - 300)/1.18 - 90; pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ); wait_ms(50); */ /*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON; else LED_DEBUG0 = LED_OFF;*/ #ifdef USE_MU controller = CONTROLLER::Controller::GetData(); #endif #ifdef USE_ERRORCHECK if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) { CONTROLLER::Controller::DataReset(); AllActuatorReset(); lock = true; } else #endif { #ifdef USE_SUBPROCESS if(!lock) { Process[current](); } else #endif { //ロック時の処理 } } SystemProcessUpdate(); } } #pragma region PROCESS #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 static void Process0() { if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; motor[Angle_R].pwm = 255; motor[Angle_L].pwm = 255; }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_L].dir == BACK && motor[Angle_L].dir == FOR){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; motor[Angle_R].pwm = 255; motor[Angle_L].pwm = 255; } for(int i = 0;i<20;i++){ float y = 0; y = acc[1]*1000; float rotateY = (y - 305)/2.21 - 90; Angle += rotateY; } Angle = Angle /20; int gyropwm = gyro.SetPV(Angle,Angle_I); if(controller->Button.A){ Angle_flagI = true; } if (Angle_flagI){ motor[Angle_R].dir = SetStatus(gyropwm); motor[Angle_L].dir = SetStatus(-gyropwm); motor[Angle_R].pwm = SetPWM(gyropwm); motor[Angle_L].pwm = SetPWM(gyropwm); if(Angle_I - 2 < Angle && Angle < Angle_I + 2){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; Angle_flagI = false; } } } #endif #if USE_PROCESS_NUM>1 static void Process1() { motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]) *0.8; motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]) *0.8; motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8; motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8; if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){ motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3; motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3; } //wheel.getPulses()...どちらの方向にどれだけ回ったか pc.printf("Pulses:%07d \r\n",wheel.getPulses()); //軸が何回転したか pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2)); } #endif #if USE_PROCESS_NUM>2 static void Process2() { static bool color_flag = false; static bool traceon = false;//fase1 static bool yokofla = false;//fase2 static bool boxslip = false;//fase3 static bool compA = false; static bool compB = false; static bool compC = false; static bool compD = false; static bool invationA = false; static bool invationB = false; static bool invationC = false; static bool invationD = false; ColorDetection(); // if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白 { invationA ^= 1;//start false,over true compA = true;//on true,noon false } else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶 if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白 { invationB ^= 1;//start false,over true compB = true;//on true,noon false } else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶 if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白 { invationC ^= 1;//start false,over true compC = true;//on true,noon false } else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶 if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白 { invationD ^= 1;//start false,over true compD = true;//on true,noon false } else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶 // if(controller->Button.B && !color_flag) { traceon ^= 1; color_flag = true; } else if(!controller->Button.B)color_flag = false; if(traceon && !yokofla && !boxslip) { if(!invationA && !compA && !invationB && !compB) { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_BL].dir = BACK; motor[TIRE_FR].pwm = startP; motor[TIRE_FL].pwm = startP; motor[TIRE_BR].pwm = startP; motor[TIRE_BL].pwm = startP; } else if(invationA && compA && !invationB && !compB) { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_BL].dir = BACK; motor[TIRE_FR].pwm = startP - downP; motor[TIRE_FL].pwm = startP - downP; motor[TIRE_BR].pwm = startP - downP; motor[TIRE_BL].pwm = startP - downP; } else if(invationA && !compA && !invationB && !compB) { motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; wait(2); yokofla = true; traceon = false; } else{ motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; } } if(!traceon && yokofla && !boxslip) { if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; wait(2); boxslip = true; yokofla = false; } else if(invationA && !compA && invationB) { motor[TIRE_FR].dir = BACK; motor[TIRE_FL].dir = BACK; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].dir = FOR; motor[TIRE_FR].pwm = startP; motor[TIRE_FL].pwm = startP; motor[TIRE_BR].pwm = startP; motor[TIRE_BL].pwm = startP; } else if(!invationA && !compB && !invationB) { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_BL].dir = BACK; motor[TIRE_FR].pwm = startP; motor[TIRE_FL].pwm = startP; motor[TIRE_BR].pwm = startP; motor[TIRE_BL].pwm = startP; } else if(invationA && compA && !invationB && !compB) { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_BL].dir = BACK; motor[TIRE_FR].pwm = startP; motor[TIRE_FL].pwm = startP; motor[TIRE_BR].pwm = startP; motor[TIRE_BL].pwm = startP; } else if(compB && invationB) { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = BACK; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].dir = BACK; motor[TIRE_FR].pwm = startP; motor[TIRE_FL].pwm = startP; motor[TIRE_BR].pwm = startP; motor[TIRE_BL].pwm = startP; } else { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = BACK; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].dir = BACK; motor[TIRE_FR].pwm = startP; motor[TIRE_FL].pwm = startP; motor[TIRE_BR].pwm = startP; motor[TIRE_BL].pwm = startP; } } if(!traceon && !yokofla && boxslip) { if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = FOR; motor[TIRE_BR].dir = BACK; motor[TIRE_BL].dir = BACK; motor[TIRE_FR].pwm = startP; motor[TIRE_FL].pwm = startP; motor[TIRE_BR].pwm = startP; motor[TIRE_BL].pwm = startP; } else if(!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; } } /*//// motor[0].dir = BACK; motor[1].dir = BACK; motor[2].dir = FOR; motor[3].dir = FOR; motor[0].pwm = startP; motor[1].pwm = startP; motor[2].pwm = startP; motor[3].pwm = startP; else if() { motor[0].dir = BRAKE; motor[1].dir = BRAKE; motor[2].dir = BRAKE; motor[3].dir = BRAKE; motor[0].pwm = 255; motor[1].pwm = 255; motor[2].pwm = 255; motor[3].pwm = 255; }*/////// } #endif #if USE_PROCESS_NUM>3 static void Process3() { if(controller->Button.R){ motor[Angle_R].dir = FOR; motor[Angle_L].dir = BACK; motor[Angle_R].pwm = 150; motor[Angle_L].pwm = 150; }else if(controller->Button.L){ motor[Angle_R].dir = BACK; motor[Angle_L].dir = FOR; motor[Angle_R].pwm = 150; motor[Angle_L].pwm = 150; }else{ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; } if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; motor[Angle_R].pwm = 255; motor[Angle_L].pwm = 255; }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; motor[Angle_R].pwm = 255; motor[Angle_L].pwm = 255; } } #endif #if USE_PROCESS_NUM>4 static void Process4() { if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; motor[Angle_R].pwm = 255; motor[Angle_L].pwm = 255; }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; motor[Angle_R].pwm = 255; motor[Angle_L].pwm = 255; } for(int i = 0;i<20;i++){ float y = 0; y = acc[1]*1000; float rotateY = (y - 305)/2.21 - 90; Angle += rotateY; } Angle = Angle /20; int gyropwmX = gyro.SetPV(Angle,AngletargetX); int gyropwmY = gyro.SetPV(Angle,AngletargetY); if(controller->Button.X){ Angle_flagX = true; } if(controller->Button.Y){ Angle_flagY = true; } if (Angle_flagX){ motor[Angle_R].dir = SetStatus(gyropwmX); motor[Angle_L].dir = SetStatus(-gyropwmX); motor[Angle_R].pwm = SetPWM(gyropwmX); motor[Angle_L].pwm = SetPWM(gyropwmX); if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; Angle_flagX = false; } } if (Angle_flagY){ motor[Angle_R].dir = SetStatus(gyropwmY); motor[Angle_L].dir = SetStatus(-gyropwmY); motor[Angle_R].pwm = SetPWM(gyropwmY); motor[Angle_L].pwm = SetPWM(gyropwmY); if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; Angle_flagY = false; } } /*float y = 0; y = acc[1]*1000; float rotateY = (y - 305)/2.21 - 90; int gyropwm = gyro.SetPV(rotateY , Angletarget); if(controller->Button.X){ Angle_flag = true; } if (Angle_flag){ motor[Angle_R].dir = SetStatus(gyropwm); motor[Angle_L].dir = SetStatus(-gyropwm); motor[Angle_R].pwm = SetPWM(gyropwm); motor[Angle_L].pwm = SetPWM(gyropwm); if(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; Angle_flag = false; } }*/ else{ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; } } #endif #if USE_PROCESS_NUM>5 static void Process5() { } #endif #if USE_PROCESS_NUM>6 static void Process6() { } #endif #if USE_PROCESS_NUM>7 static void Process7() { } #endif #if USE_PROCESS_NUM>8 static void Process8() { } #endif #if USE_PROCESS_NUM>9 static void Process9() { } #endif #endif #pragma endregion PROCESS static void AllActuatorReset() { #ifdef USE_SOLENOID solenoid.all = ALL_SOLENOID_OFF; #endif #ifdef USE_MOTOR for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { motor[i].dir = FREE; motor[i].pwm = 0; } #endif } #pragma region USER-DEFINED-FUNCTIONS void rotconpulex() { } void ColorDetection(){ GATE = 0; CK[0] = 0; CK[1] = 0; CK[2] = 0; CK[3] = 0; RANGE = 1; GATE = 1; wait_ms(intergration); GATE = 0; wait_us(4); Color_A[0] = ColorIn(0); //赤 wait_us(3); Color_A[1] = ColorIn(0); //青 wait_us(3); Color_A[2] = ColorIn(0); //緑 Color_B[0] = ColorIn(1); wait_us(3); Color_B[1] = ColorIn(1); wait_us(3); Color_B[2] = ColorIn(1); Color_C[0] = ColorIn(2); wait_us(3); Color_C[1] = ColorIn(2); wait_us(3); Color_C[2] = ColorIn(2); Color_D[0] = ColorIn(3); wait_us(3); Color_D[1] = ColorIn(3); wait_us(3); Color_D[2] = ColorIn(3); } #pragma endregion