![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Diff: System/Process/Process.cpp
- Revision:
- 12:c09b3e08a316
- Parent:
- 11:028a150943b5
- Child:
- 13:b6e02d6261d7
--- a/System/Process/Process.cpp Sat Oct 06 08:30:58 2018 +0000 +++ b/System/Process/Process.cpp Sun Oct 07 09:08:18 2018 +0000 @@ -42,22 +42,6 @@ Serial pc(USBTX, USBRX); -#define TIRE_FR 0 //足回り前右 -#define TIRE_FL 1 //足回り前左 -#define TIRE_BR 2 //足回り後右 -#define TIRE_BL 3 //足回り後左 - -#define Angle_R 4 //角度調節右 -#define Angle_L 5 //角度調節左 - -#define Lim_AR 3 //角度調節右 -#define Lim_AL 4 //角度調節左 -#define Lim_R 0 //センター右 -#define Lim_L 1 //センター左 -#define EMS_0 LimitSw::IsPressed(8) -#define EMS_1 LimitSw::IsPressed(9) -#define LS LimitSw::IsPressed(7) //赤ゾーン用スイッチ -#define BS LimitSw::IsPressed(6) //青ゾーン用スイッチ //************メカナム******************** const int mecanum[15][15]= @@ -103,24 +87,14 @@ int Color_D[3]; int intergration = 50; -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; -} +int Avecolor_A[3]; +int Avecolor_B[3]; +int Avecolor_C[3]; +int Avecolor_D[3]; + +void ColorIn(); void ColorDetection(); - +void getcolor(); //************カラーセンサ******************** //************ライントレース変数******************* @@ -139,7 +113,6 @@ Ticker Color_T; //************ライントレース変数******************* - //************ROタコン****************** QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); @@ -157,12 +130,10 @@ double goalX = 1200.000; double goalY = 900.000; void filip(); -//PID startup = PID(0.03, -255, 255, 0.3, 0, 0); - + //************ROタコン****************** //************ジャイロ******************* - bool Angle_flagI = false; int Angle; PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0); @@ -170,7 +141,7 @@ //初期値 -5 int AngletargetX = 4; int AngletargetY = -12; -int Angle_I = -5; +int AngletargetI = -5; //************ジャイロ******************* //************Buzzer****************** @@ -312,6 +283,12 @@ while(1) { + getcolor(); + pc.printf("R1:%d, G1:%d, B1:%d \r\n",Avecolor_A[0],Avecolor_A[1],Avecolor_A[2]); + pc.printf("R2:%d, G2:%d, B2:%d \r\n",Avecolor_B[0],Avecolor_B[1],Avecolor_B[2]); + pc.printf("R3:%d, G3:%d, B3:%d \r\n",Avecolor_C[0],Avecolor_C[1],Avecolor_C[2]); + pc.printf("R4:%d, G4:%d, B4:%d \r\n",Avecolor_D[0],Avecolor_D[1],Avecolor_D[2]); + #ifdef USE_MU controller = CONTROLLER::Controller::GetData(); #endif @@ -379,65 +356,16 @@ #pragma region PROCESS #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 -static void Process0() +static void Process0() { - static bool Xnopush = false; - static bool Angle_flagX = false; - - 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 gyropwmX = gyro.SetPV(Angle,AngletargetX); - - if(controller->Button.X && !Xnopush){ - Angle_flagX = true; - Xnopush = true; - }else if(!controller->Button.X)Xnopush = false; - - - 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; - motor[Angle_R].pwm = 255; - motor[Angle_L].pwm = 255; - Angle_flagX = false; - } - } - - else{ - motor[Angle_R].dir = BRAKE; - motor[Angle_L].dir = BRAKE; - motor[Angle_R].pwm = 255; - motor[Angle_L].pwm = 255; - } + tapeLED.code = (uint32_t)Green; } #endif #if USE_PROCESS_NUM>1 -static void Process1() +static void Process1() //手動 { - - + tapeLED.code = (uint32_t)Orange; 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]); @@ -453,17 +381,408 @@ motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3; } + 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; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; + } + + 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; + pc.printf("Y:%d \r\n",Angle);*/ - //wheel.getPulses()...どちらの方向にどれだけ回ったか - //pc.printf("Pulses:%07d \r\n",wheel.getPulses()); - //軸が何回転したか - //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); + //wheel.getPulses()...どちらの方向にどれだけ回ったか + //pc.printf("Pulses:%07d \r\n",wheel.getPulses()); + //軸が何回転したか + //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); } #endif #if USE_PROCESS_NUM>2 -static void Process2() +static void Process2() //自動角度調節 +{ + tapeLED.code = (uint32_t)Hotpink; + static bool Xnopush = false; + static bool Ynopush = false; + static bool Inopush = false; + + static bool Angle_flagX = false; + static bool Angle_flagY = false; + static bool ANgle_flagI = false; + + 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); + int gyropwmI = gyro.SetPV(Angle,AngletargetI); + + if(controller->Button.X && !Xnopush){ + Angle_flagX = true; + Xnopush = true; + }else if(!controller->Button.X)Xnopush = false; + + if(controller->Button.Y && !Ynopush){ + Angle_flagY = true; + Ynopush = true; + }else if(!controller->Button.Y)Ynopush = false; + + if(controller->Button.A && !Inopush){ + Angle_flagI = true; + Inopush = true; + }else if(!controller->Button.A)Inopush = false; + + 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; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; + 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; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; + Angle_flagY = false; + } + } + + if (Angle_flagI){ + if(Angle < 0) + { + motor[Angle_R].dir = SetStatus(-gyropwmI); + motor[Angle_L].dir = SetStatus(gyropwmI); + motor[Angle_R].pwm = SetPWM(gyropwmI); + motor[Angle_L].pwm = SetPWM(gyropwmI); + + if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){ + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; + Angle_flagI = false; + } + } + else if(Angle > 0) + { + motor[Angle_R].dir = FOR; + motor[Angle_L].dir = BACK; + motor[Angle_R].pwm = 150; + motor[Angle_L].pwm = 150; + + if(Angle < 0){ + motor[Angle_R].dir = SetStatus(gyropwmI); + motor[Angle_L].dir = SetStatus(-gyropwmI); + motor[Angle_R].pwm = SetPWM(gyropwmI); + motor[Angle_L].pwm = SetPWM(gyropwmI); + + if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){ + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; + Angle_flagI = false; + } + } + } + } + else{ + 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>3 +static void Process3() // ロタコンX { + tapeLED.code = (uint32_t)Blue; + static bool nopushed = false; + static bool Rt_flagX = false; + + if(controller->Button.A && !nopushed){ + Rt_flagX = true; + nopushed = true; + }else if(!controller->Button.A)nopushed = false; + + + if (Rt_flagX && SetPWM(RtpwmX) > 0){ + filip(); + + motor[TIRE_FR].dir = SetStatus(-RtpwmX); + motor[TIRE_FL].dir = SetStatus(-RtpwmX); + motor[TIRE_BR].dir = SetStatus(RtpwmX); + motor[TIRE_BL].dir = SetStatus(RtpwmX); + motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.72; + motor[TIRE_FL].pwm = SetPWM(RtpwmX); + motor[TIRE_BR].pwm = SetPWM(RtpwmX); + motor[TIRE_BL].pwm = SetPWM(RtpwmX); + } + else if(Rt_flagX && SetPWM(RtpwmX) < 0) + { + filip(); + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + + motor[TIRE_FR].pwm = 255; + motor[TIRE_FL].pwm = 255; + motor[TIRE_BR].pwm = 255; + motor[TIRE_BL].pwm = 255; + + } + else + { + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + + motor[TIRE_FR].pwm = 255; + motor[TIRE_FL].pwm = 255; + motor[TIRE_BR].pwm = 255; + motor[TIRE_BL].pwm = 255; + } + //pc.printf("X:%d \r\n",RtpwmX); +} +#endif + +#if USE_PROCESS_NUM>4 +static void Process4() //ロタコンY +{ + tapeLED.code = (uint32_t)Violet; + static bool nopushed = false; + static bool Rt_flagY = false; + /* wait(0.1); + //RtX.getPulses();//...どちらの方向にどれだけ回ったか + //RtY.getPulses(); + pc.printf("Pulses:%07d \r\n",RtX.getPulses()); + pc.printf("Pulses:%07d \r\n",RtY.getPulses()); + //軸が何回転したか + pc.printf("Rotate:%04.3f \r\n",(double)RtX.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); + */ + + + if(controller->Button.B && !nopushed){ + Rt_flagY = true; + nopushed = true; + }else if(!controller->Button.B)nopushed = false; + + + if (Rt_flagY && SetPWM(RtpwmY) > 0){ + filip(); + + motor[TIRE_FR].dir = SetStatus(-RtpwmY); + motor[TIRE_FL].dir = SetStatus(RtpwmY); + motor[TIRE_BR].dir = SetStatus(-RtpwmY); + motor[TIRE_BL].dir = SetStatus(RtpwmY); + motor[TIRE_FR].pwm = SetPWM(RtpwmY); + motor[TIRE_FL].pwm = SetPWM(RtpwmY); + motor[TIRE_BR].pwm = SetPWM(RtpwmY); + motor[TIRE_BL].pwm = SetPWM(RtpwmY); + } + else if(Rt_flagY && SetPWM(RtpwmY) < 0) + { + filip(); + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_FR].pwm = 255; + motor[TIRE_FL].pwm = 255; + motor[TIRE_BR].pwm = 255; + motor[TIRE_BL].pwm = 255; + + } + else + { + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_FR].pwm = 255; + motor[TIRE_FL].pwm = 255; + motor[TIRE_BR].pwm = 255; + motor[TIRE_BL].pwm = 255; + } + + //pc.printf("RtpwmX:%f \r\n", RtpwmX); + + + //pc.printf("PWM:%d \r\n", RtpwmY); + //pc.printf("回転数:%f \r\n" ,rpmY); + //pc.printf("距離:%f \r\n", disY); +} +#endif + +#if USE_PROCESS_NUM>5 +static void Process5() //ロタコンXY +{ + tapeLED.code = (uint32_t)White; + static bool nopushed = false; + static bool Rt_flagX = false; + static bool Rt_flagY = false; + + if(controller->Button.A && !nopushed){ + Rt_flagX = true; + nopushed = true; + }else if(!controller->Button.A)nopushed = false; + + if (Rt_flagX && SetPWM(RtpwmX) > 0){ + filip(); + + motor[TIRE_FR].dir = SetStatus(-RtpwmX); + motor[TIRE_FL].dir = SetStatus(-RtpwmX); + motor[TIRE_BR].dir = SetStatus(RtpwmX); + motor[TIRE_BL].dir = SetStatus(RtpwmX); + motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.72; + motor[TIRE_FL].pwm = SetPWM(RtpwmX); + motor[TIRE_BR].pwm = SetPWM(RtpwmX); + motor[TIRE_BL].pwm = SetPWM(RtpwmX); + } + else if(Rt_flagX && SetPWM(RtpwmX) < 0){ + filip(); + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + + motor[TIRE_FR].pwm = 255; + motor[TIRE_FL].pwm = 255; + motor[TIRE_BR].pwm = 255; + motor[TIRE_BL].pwm = 255; + } + + if(SetPWM(RtpwmX)== 0 && Rt_flagX){ + wait(2); + + Rt_flagX = false; + Rt_flagY = true; + } + + if (Rt_flagY && SetPWM(RtpwmY) > 0){ + filip(); + + motor[TIRE_FR].dir = SetStatus(-RtpwmY); + motor[TIRE_FL].dir = SetStatus(RtpwmY); + motor[TIRE_BR].dir = SetStatus(-RtpwmY); + motor[TIRE_BL].dir = SetStatus(RtpwmY); + motor[TIRE_FR].pwm = SetPWM(RtpwmY); + motor[TIRE_FL].pwm = SetPWM(RtpwmY); + motor[TIRE_BR].pwm = SetPWM(RtpwmY); + motor[TIRE_BL].pwm = SetPWM(RtpwmY); + } + else if(Rt_flagY && SetPWM(RtpwmY) < 0) + { + filip(); + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_FR].pwm = 255; + motor[TIRE_FL].pwm = 255; + motor[TIRE_BR].pwm = 255; + motor[TIRE_BL].pwm = 255; + + } + else + { + motor[TIRE_FR].dir = BRAKE; + motor[TIRE_FL].dir = BRAKE; + motor[TIRE_BR].dir = BRAKE; + motor[TIRE_BL].dir = BRAKE; + motor[TIRE_FR].pwm = 255; + motor[TIRE_FL].pwm = 255; + motor[TIRE_BR].pwm = 255; + motor[TIRE_BL].pwm = 255; + } +} +#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() + { static bool color_flag = false; static bool traceon = false;//fase1 @@ -688,466 +1007,6 @@ }*/////// } #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; - motor[Angle_R].pwm = 255; - motor[Angle_L].pwm = 255; - } - - 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; - pc.printf("Y:%d \r\n",Angle); -} -#endif - -#if USE_PROCESS_NUM>4 -static void Process4() -{ - static bool Xnopush = false; - static bool Ynopush = false; - - static bool Angle_flagX = false; - static bool Angle_flagY = false; - - - 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 && !Xnopush){ - Angle_flagX = true; - Xnopush = true; - }else if(!controller->Button.X)Xnopush = false; - - if(controller->Button.Y && !Ynopush){ - Angle_flagY = true; - Ynopush = true; - }else if(!controller->Button.Y)Ynopush = false; - - 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; - motor[Angle_R].pwm = 255; - motor[Angle_L].pwm = 255; - 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; - motor[Angle_R].pwm = 255; - motor[Angle_L].pwm = 255; - 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; - motor[Angle_R].pwm = 255; - motor[Angle_L].pwm = 255; - } - //pc.printf("%d \r\n",gyropwmY); -} -#endif - -#if USE_PROCESS_NUM>5 -static void Process5() -{ - static bool nopushed = false; - static bool Rt_flagY = false; - /* wait(0.1); - //RtX.getPulses();//...どちらの方向にどれだけ回ったか - //RtY.getPulses(); - pc.printf("Pulses:%07d \r\n",RtX.getPulses()); - pc.printf("Pulses:%07d \r\n",RtY.getPulses()); - //軸が何回転したか - pc.printf("Rotate:%04.3f \r\n",(double)RtX.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); - */ - - - if(controller->Button.B && !nopushed){ - Rt_flagY = true; - nopushed = true; - }else if(!controller->Button.B)nopushed = false; - - - if (Rt_flagY && SetPWM(RtpwmY) > 0){ - filip(); - - motor[TIRE_FR].dir = SetStatus(-RtpwmY); - motor[TIRE_FL].dir = SetStatus(RtpwmY); - motor[TIRE_BR].dir = SetStatus(-RtpwmY); - motor[TIRE_BL].dir = SetStatus(RtpwmY); - motor[TIRE_FR].pwm = SetPWM(RtpwmY); - motor[TIRE_FL].pwm = SetPWM(RtpwmY); - motor[TIRE_BR].pwm = SetPWM(RtpwmY); - motor[TIRE_BL].pwm = SetPWM(RtpwmY); - } - else if(Rt_flagY && SetPWM(RtpwmY) < 0) - { - filip(); - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - motor[TIRE_FR].pwm = 255; - motor[TIRE_FL].pwm = 255; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 255; - - } - else - { - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - motor[TIRE_FR].pwm = 255; - motor[TIRE_FL].pwm = 255; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 255; - } - - //pc.printf("RtpwmX:%f \r\n", RtpwmX); - - - //pc.printf("PWM:%d \r\n", RtpwmY); - //pc.printf("回転数:%f \r\n" ,rpmY); - //pc.printf("距離:%f \r\n", disY); - - -} -#endif - -#if USE_PROCESS_NUM>6 -static void Process6() -{ - /*static bool color_flag = false; - - static bool traceon = false;//fase1 - static bool yokofla = false;//fase2 - static bool boxslip = false;//fase3 - - static bool syu = false; - - 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 && !syu) - { - 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 = BRAKE; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - - wait(2); - - syu = true; - yokofla = false; - traceon = false; - } - else{ - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - } - } - - pointcalculation(); - - if(syu && !traceon && !yokofla && !boxslip) - { - if(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10) - { - 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; - syu = false; - } - else if(Asasult < Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10)) - { - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].dir = FOR; - 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(Asasult > Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10)) - { - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].dir = BACK; - 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 - { - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - } - } - - if(!syu && !traceon && yokofla && !boxslip) - { - 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 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 - { - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - } - */ -} -#endif - -#if USE_PROCESS_NUM>7 -static void Process7() -{ - static bool nopushed = false; - static bool Rt_flagX = false; - - if(controller->Button.A && !nopushed){ - Rt_flagX = true; - nopushed = true; - }else if(!controller->Button.A)nopushed = false; - - - if (Rt_flagX && SetPWM(RtpwmX) > 0){ - filip(); - - motor[TIRE_FR].dir = SetStatus(-RtpwmX); - motor[TIRE_FL].dir = SetStatus(-RtpwmX); - motor[TIRE_BR].dir = SetStatus(RtpwmX); - motor[TIRE_BL].dir = SetStatus(RtpwmX); - motor[TIRE_FR].pwm = SetPWM(RtpwmX); - motor[TIRE_FL].pwm = SetPWM(RtpwmX); - motor[TIRE_BR].pwm = SetPWM(RtpwmX); - motor[TIRE_BL].pwm = SetPWM(RtpwmX); - } - else if(Rt_flagX && SetPWM(RtpwmX) < 0) - { - filip(); - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - - motor[TIRE_FR].pwm = 255; - motor[TIRE_FL].pwm = 255; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 255; - - } - else - { - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - - motor[TIRE_FR].pwm = 255; - motor[TIRE_FL].pwm = 255; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 255; - } - //pc.printf("X:%d \r\n",RtpwmX); -} -#endif - -#if USE_PROCESS_NUM>8 -static void Process8() -{ - -} -#endif - -#if USE_PROCESS_NUM>9 -static void Process9() -{ - -} -#endif #endif #pragma endregion PROCESS @@ -1196,6 +1055,22 @@ RtpwmY = (int)Rt_Y.SetPV(disY , goalY); } +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; +} void ColorDetection(){ GATE = 0; @@ -1236,6 +1111,41 @@ wait_us(3); Color_D[2] = ColorIn(3); } + +void getcolor(){ + for(int i=0;i<20;i++){ + ColorDetection(); + + Avecolor_A[0] += Color_A[0]; + Avecolor_A[1] += Color_A[1]; + Avecolor_A[2] += Color_A[2]; + Avecolor_B[0] += Color_B[0]; + Avecolor_B[1] += Color_B[1]; + Avecolor_B[2] += Color_B[2]; + Avecolor_C[0] += Color_C[0]; + Avecolor_C[1] += Color_C[1]; + Avecolor_C[2] += Color_C[2]; + Avecolor_D[0] += Color_D[0]; + Avecolor_D[1] += Color_D[1]; + Avecolor_D[2] += Color_D[2]; +} + +Avecolor_A[0] = Avecolor_A[0]/20; +Avecolor_A[1] = Avecolor_A[1]/20; +Avecolor_A[2] = Avecolor_A[2]/20; +Avecolor_B[0] = Avecolor_B[0]/20; +Avecolor_B[1] = Avecolor_B[1]/20; +Avecolor_B[2] = Avecolor_B[2]/20; +Avecolor_C[0] = Avecolor_C[0]/20; +Avecolor_C[1] = Avecolor_C[1]/20; +Avecolor_C[2] = Avecolor_C[2]/20; +Avecolor_D[0] = Avecolor_D[0]/20; +Avecolor_D[1] = Avecolor_D[1]/20; +Avecolor_D[2] = Avecolor_D[2]/20; +} + + + void BuzzerTimer_func() { if(buzzer == 0.5){ buzzer = 0;