![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Diff: System/Process/Process.cpp
- Revision:
- 14:93776ca449a4
- Parent:
- 13:b6e02d6261d7
--- a/System/Process/Process.cpp Mon Oct 08 15:51:15 2018 +0000 +++ b/System/Process/Process.cpp Thu Oct 25 09:00:19 2018 +0000 @@ -78,7 +78,7 @@ else return abs(pwmVal); } -//************メカナム******************** +//**************************************************************************************************** //************カラーセンサ******************** @@ -97,20 +97,12 @@ void ColorDetection(); void getcolor(); -//************カラーセンサ******************** +//*************************************************************************************************************** //************ライントレース変数******************* -int PointA[3] = {400, 700, 1000};//赤,緑,青 -int PointB[3] = {400, 700, 1000};//赤,緑,青 +int PointA[3] = {350, 600, 800};//赤,緑,青 +int PointB[3] = {350, 600, 800};//赤,緑,青 int PointC[3] = {1000, 1700, 2400};//赤,緑,青 - -int startP = 35; -int downP = 5; - -int Asasult = 0; -int Bsasult = 0; -int Csasult = 0; -int Dsasult = 0; bool compA = false; bool compB = false; @@ -125,27 +117,30 @@ Ticker Color_T; void ColorDetection(); -void Color_changeflag(); - -//************ライントレース変数******************* +void Color_changeflagA(); +void Color_changeflagB(); +//***************************************************************************************************** //************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); Ticker get_rpm; -PID Rt_X = PID(0.03, -255, 255, 0.12, 0, 0); -PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0); -double rpmX; -double rpmY; -double disX; -double disY; +PID Rt_X = PID(0.03, -255, 255, 0.15, 0, 0); +PID Rt_Y = PID(0.03, -255, 255, 0.17, 0, 0); +double rpmX = 0, rpmXB =0; +double rpmY = 0, rpmYB =0; +double disX =0,disXB =0; +double disY =0,disYB =0; int palseX; int palseY; -int RtpwmX; -int RtpwmY; -double goalX = 900.000; -double goalY = 700.000; +int RtpwmX =0,RtpwmXB =0; +int RtpwmY =0,RtpwmYB =0; +double goalAX = 1000.000; +double goalAY = 850.000; +double goalBX = -430.000; +double goalBY = 1000.000; void filip(); +void filipB(); //************ROタコン****************** @@ -165,6 +160,7 @@ PwmOut buzzer(BUZZER_PIN); void BuzzerTimer_func(); Ticker BuzzerTimer; +Ticker LostTimer; bool Emsflag = false; //************Buzzer****************** @@ -174,6 +170,8 @@ TapeLedData sendLedData; TapeLED_Mode ledMode = Normal; Ticker tapeLedTimer; +Ticker LostLedTimer; +//void LostLed_func(); //************TapaLed***************** #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE @@ -298,13 +296,24 @@ SystemProcessInitialize(); 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]); + + palseX = RtX.getPulses(); + palseY = RtY.getPulses(); + + pc.printf("X:%d",palseX); + pc.printf("Y:%d",palseY); */ + //wheel.getPulses()...どちらの方向にどれだけ回ったか + //pc.printf("Pulses:%07d \r\n",wheel.getPulses()); + //軸が何回転したか + //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); #ifdef USE_MU controller = CONTROLLER::Controller::GetData(); #endif @@ -315,6 +324,11 @@ CONTROLLER::Controller::DataReset(); AllActuatorReset(); lock = true; + //BuzzerTimer.attach(BuzzerTimer_func, 0.5); + //LostLedTimer.attach(LostLed_func, 0.5); + //sendLedData.code = (uint32_t)Yellow; + //buzzer = 0.5; + //LostTimer.attach(BuzzerTimer_func, 1.2); } else #endif @@ -376,10 +390,10 @@ { tapeLED.code = (uint32_t)Green; if(RedSW){ - current = 1; + current = 6; } if(BlueSW){ - current = 2; + current = 5; } } #endif @@ -388,6 +402,7 @@ 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]); @@ -444,49 +459,55 @@ //pc.printf("Pulses:%07d \r\n",wheel.getPulses()); //軸が何回転したか //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); + + //filip(); + //pc.printf("AKApwmX:%d , AKApwmY:%d\r\n",RtpwmX,RtpwmY); + + //filipB(); + //pc.printf("BLUEpwmX:%d , BLUEpwmY:%d \r\n",RtpwmXB,RtpwmYB); } #endif -#if USE_PROCESS_NUM>2 +#if USE_PROCESS_NUM>2 //Blue Zone static void Process2() //trace { - tapeLED.code = (uint32_t)Yellow; - static bool color_flag = false; - + /*//tapeLED.code = (uint32_t)Blue; static bool traceon = false;//fase1 static bool yokofla = false;//fase2 static bool boxslip = false;//fase3 - //static bool syu = false; + static bool Rt_flagX = false; + static bool Rt_flagY = false; + ColorDetection(); - Color_changeflag(); + Color_changeflagA(); - if(controller->Button.B && !color_flag) + /*if(controller->Button.B && !color_nopush) { traceon = true; - color_flag = true; + color_nopush = true; } - else if(!controller->Button.B)color_flag = false; + traceon = true; + + //else if(!controller->Button.B)color_nopush = false; if(traceon) { - Color_changeflag(); - if(!invationA && !compA && !invationB && !compB) + Color_changeflagA(); + if(!invationA && !compA && !invationB && !compB && !invationC && !compC) { - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].dir = BACK; - motor[TIRE_BL].dir = BACK; + 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_FR].pwm = 42; motor[TIRE_FL].pwm = startP; motor[TIRE_BR].pwm = startP; motor[TIRE_BL].pwm = startP; - - Color_changeflag(); } - else if(invationC && compC && !invationB && !compB) + else if(invationB || compB || invationA || compA || invationC || compC) { for(int i = 0; i<1000; i++){ motor[TIRE_FR].dir = BRAKE; @@ -509,8 +530,345 @@ if(yokofla && !traceon) { //pointcalculation(); - Color_changeflag(); - if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)) + Color_changeflagA(); + 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[TIRE_FR].pwm = 255; + motor[TIRE_FL].pwm = 255; + motor[TIRE_BR].pwm = 255; + motor[TIRE_BL].pwm = 255; + + + boxslip = true; + yokofla = false; + } + 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 = 47; + } +} + + if(boxslip) + { + for(int i = 0; i<500; i++){ + 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; + } + + RtX.reset(); + RtY.reset(); + + Rt_flagX = true; + boxslip = false; + } + + filipB(); + if(Rt_flagX && !boxslip) + { + filipB(); + if(disX > -goalAX + 5){ + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].dir = FOR; + motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8; + motor[TIRE_FL].pwm = SetPWM(RtpwmX); + motor[TIRE_BR].pwm = SetPWM(RtpwmX); + motor[TIRE_BL].pwm = SetPWM(RtpwmX); + } + else if(disX < -goalAX + 5){ + 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; + + Rt_flagY = true; + Rt_flagX = false; + } + } + + +if(Rt_flagY && !Rt_flagX){ + filipB(); + if(disY < goalAY - 5){ + 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)*1.2; + } + else if(disY > goalAY - 5) + { + 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>3 +static void Process3() //Red Zone +{ + /*tapeLED.code = (uint32_t)Red; + + //static bool color_nopush = false; + + static bool traceon = false;//fase1 + static bool yokofla = false;//fase2 + static bool boxslip = false;//fase3 + + static bool Rt_flagX = false; + static bool Rt_flagY = false; + + //static bool syu = false; + + ColorDetection(); + Color_changeflagA(); + + /*if(controller->Button.B && !color_nopush) + { + traceon = true; + color_nopush = true; + } + //else if(!controller->Button.B)color_nopush = false; + traceon = true; + + if(traceon) + { + Color_changeflagA(); + if(!invationA && !compA && !invationB && !compB && !invationC && !compC) + { + 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 || invationC || compC) + { + 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; + + yokofla = true; + traceon = false; + } + } + + + if(yokofla && !traceon) + { + //pointcalculation(); + Color_changeflagA(); + 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[TIRE_FR].pwm = 255; + motor[TIRE_FL].pwm = 255; + motor[TIRE_BR].pwm = 255; + motor[TIRE_BL].pwm = 255; + + boxslip = true; + yokofla = false; + } + 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(boxslip && !yokofla) + { + 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; + + + RtX.reset(); + RtY.reset(); + + Rt_flagX = true; + boxslip = false; + } + + if(Rt_flagX && !boxslip) + { + filip(); + if(disX < goalAX - 5){ + + motor[TIRE_FR].dir = FOR; + motor[TIRE_FL].dir = FOR; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].dir = BACK; + motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8; + motor[TIRE_FL].pwm = SetPWM(RtpwmX); + motor[TIRE_BR].pwm = SetPWM(RtpwmX); + motor[TIRE_BL].pwm = SetPWM(RtpwmX); + } + else if(disX > goalAX - 5){ + + 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; + + Rt_flagY = true; + Rt_flagX = false; + } + } + + +if(Rt_flagY && !Rt_flagX){ + filip(); + if(disY < goalAY - 5){ + 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(disY > goalAY - 5) + { + 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>4 +static void Process4() +{ + +} +#endif + +#if USE_PROCESS_NUM>5 +static void Process5()//traceB +{ + static bool traceonB = true;//fase1 + static bool yokoflaB = false;//fase2 + static bool boxslipB = false;//fase3 + + static bool Rt_flagXB = false; + static bool Rt_flagYB = false; + + ColorDetection(); + Color_changeflagB(); + + if(traceonB && !Rt_flagXB && !Rt_flagYB && !boxslipB) + { + Color_changeflagB(); + if(!invationA && !compA && !invationB && !compB && !invationC && !compC) + { + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].dir = FOR; + + motor[TIRE_FR].pwm = 48; + motor[TIRE_FL].pwm = 43; + motor[TIRE_BR].pwm = 43; + motor[TIRE_BL].pwm = 43; + } + else if(invationA || compA || invationB || compB || invationC || compC) + { + 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; + + yokoflaB = true; + traceonB = false; + } + } + + + if(yokoflaB && !traceonB) + { + Color_changeflagB(); + if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; @@ -522,172 +880,113 @@ motor[TIRE_BR].pwm = 255; motor[TIRE_BL].pwm = 255; - wait(2); - - boxslip = true; - yokofla = false; + boxslipB = true; + yokoflaB = false; } - else if(compA && compB && compC) + else if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L)) { - motor[TIRE_FR].dir = FOR; + 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; - - Color_changeflag(); + motor[TIRE_FR].pwm = 55; + motor[TIRE_FL].pwm = 55; + motor[TIRE_BR].pwm = 55; + motor[TIRE_BL].pwm = 55; } - else if(invationA && invationB && invationC) - { - motor[TIRE_FR].dir = FREE; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].dir = FREE; - - //motor[TIRE_FR].pwm = startP; - motor[TIRE_FL].pwm = startP; - motor[TIRE_BR].pwm = startP; - //motor[TIRE_BL].pwm = startP; - } - else if(!invationA && !invationB && !invationC) + else if(invationA || invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))) { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = FREE; motor[TIRE_BR].dir = FREE; 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; + motor[TIRE_FR].pwm = 55; + //motor[TIRE_FL].pwm = 30; + //motor[TIRE_BR].pwm = 30; + motor[TIRE_BL].pwm = 55; + } + else if(!invationA || !invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))) + { + motor[TIRE_FR].dir = FREE; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].dir = FREE; + + //motor[TIRE_FR].pwm = 30; + motor[TIRE_FL].pwm = 55; + motor[TIRE_BR].pwm = 55; + //motor[TIRE_BL].pwm = 30; } - else if(!invationA && compC && invationC)//C固定A下 - { - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; + 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 = 50; + motor[TIRE_FL].pwm = 50; + motor[TIRE_BR].pwm = 50; + motor[TIRE_BL].pwm = 50; + } +} + + + /*********************************/ + if(boxslipB && !yokoflaB && !traceonB) + { + Color_changeflagB(); + if((compA && compB) || compC){ + + RtX.reset(); + RtY.reset(); + + Rt_flagXB = true; + boxslipB = false; + } + else if(invationA || 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 = 255; - motor[TIRE_FL].pwm = 100; - motor[TIRE_BR].pwm = 55; - motor[TIRE_BL].pwm = startP; - - Color_changeflag(); + motor[TIRE_FR].pwm = 25; + motor[TIRE_FL].pwm = 25; + motor[TIRE_BR].pwm = 25; + motor[TIRE_BL].pwm = 25; } - else if(compA && compB && !invationC)//AB固定C下 - { - motor[TIRE_FR].dir = FOR; - motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - - motor[TIRE_FR].pwm = 55; - motor[TIRE_FL].pwm = startP; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 100; - - Color_changeflag(); - } - else if(compA && compB && !compC && invationC)//AB固定C上 - { + else if(!invationA || !invationB){ motor[TIRE_FR].dir = BACK; motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - - motor[TIRE_FR].pwm = startP; - motor[TIRE_FL].pwm = 55; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 100; - - Color_changeflag(); - } - else if(!compA && invationA && compC)//C固定A上 - { - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = FOR; motor[TIRE_BL].dir = FOR; - motor[TIRE_FR].pwm = 255; - motor[TIRE_FL].pwm = 100; - motor[TIRE_BR].pwm = startP; - motor[TIRE_BL].pwm = 55; - - Color_changeflag(); + motor[TIRE_FR].pwm = 25; + motor[TIRE_FL].pwm = 25; + motor[TIRE_BR].pwm = 25; + motor[TIRE_BL].pwm = 25; } - } - - if(boxslip) - { - 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>3 -static void Process3() -{ - -} -#endif - -#if USE_PROCESS_NUM>4 -static void Process4() -{ -} -#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; - - RtX.reset(); - RtY.reset(); - }else if(!controller->Button.A)nopushed = false; - - filip(); - - if(Rt_flagX) + + + + filipB(); + if(Rt_flagXB && !boxslipB) { - filip(); - if(disX < goalX - 5){ - 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); + filipB(); + if(disXB > goalBX + 5){ + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].dir = FOR; motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8; motor[TIRE_FL].pwm = SetPWM(RtpwmX); motor[TIRE_BR].pwm = SetPWM(RtpwmX); motor[TIRE_BL].pwm = SetPWM(RtpwmX); } - else if(disX > goalX - 5){ - - for(int i = 0; i<200; i++){ + else if(disXB < goalBX + 5){ motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; @@ -697,18 +996,18 @@ motor[TIRE_FL].pwm = 255; motor[TIRE_BR].pwm = 255; motor[TIRE_BL].pwm = 255; - } + + wait(1); - Rt_flagY = true; - Rt_flagX = false; + Rt_flagYB = true; + Rt_flagXB = false; } } -if(Rt_flagY && !Rt_flagX){ - filip(); - if(disY < goalY - 5){ - filip(); +if(Rt_flagYB && !Rt_flagXB){ + filipB(); + if(disYB < goalBY - 5){ motor[TIRE_FR].dir = SetStatus(-RtpwmY); motor[TIRE_FL].dir = SetStatus(RtpwmY); motor[TIRE_BR].dir = SetStatus(-RtpwmY); @@ -716,16 +1015,15 @@ motor[TIRE_FR].pwm = SetPWM(RtpwmY); motor[TIRE_FL].pwm = SetPWM(RtpwmY); motor[TIRE_BR].pwm = SetPWM(RtpwmY); - motor[TIRE_BL].pwm = SetPWM(RtpwmY); + motor[TIRE_BL].pwm = SetPWM(RtpwmY)*1.2; } - else if(disY > goalY - 5) + else if(disYB > goalBY - 5) { - 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*0.85; + motor[TIRE_FR].pwm = 255; motor[TIRE_FL].pwm = 255; motor[TIRE_BR].pwm = 255; motor[TIRE_BL].pwm = 255; @@ -734,52 +1032,36 @@ } #endif -#if USE_PROCESS_NUM>6 +#if USE_PROCESS_NUM>6//traceA static void Process6() -{ - tapeLED.code = (uint32_t)Yellow; - static bool color_flag = false; - - static bool traceon = false;//fase1 +{ + static bool traceon = true;//fase1 static bool yokofla = false;//fase2 static bool boxslip = false;//fase3 - static bool nopushed = false; static bool Rt_flagX = false; static bool Rt_flagY = false; - //static bool syu = false; + ColorDetection(); + Color_changeflagA(); - ColorDetection(); - Color_changeflag(); - - if(controller->Button.B && !color_flag) + if(traceon && !Rt_flagX && !Rt_flagY && !boxslip ) { - traceon = true; - color_flag = true; - } - else if(!controller->Button.B)color_flag = false; - - if(traceon) - { - Color_changeflag(); - if(!invationA && !compA && !invationB && !compB) + Color_changeflagA(); + if(!invationA && !compA && !invationB && !compB && !invationC && !compC) { 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; - - Color_changeflag(); + motor[TIRE_FR].pwm = 42; + motor[TIRE_FL].pwm = 42; + motor[TIRE_BR].pwm = 42; + motor[TIRE_BL].pwm = 42; } - else if(invationC && compC && !invationB && !compB) + else if(invationA || compA || invationB || compB || invationC || compC) { - for(int i = 0; i<1000; i++){ motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; @@ -789,7 +1071,6 @@ motor[TIRE_FL].pwm = 255; motor[TIRE_BR].pwm = 255; motor[TIRE_BL].pwm = 255; - } yokofla = true; traceon = false; @@ -799,9 +1080,8 @@ if(yokofla && !traceon) { - //pointcalculation(); - Color_changeflag(); - if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)) + Color_changeflagA(); + if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; @@ -813,26 +1093,22 @@ motor[TIRE_BR].pwm = 255; motor[TIRE_BL].pwm = 255; - wait(2); - boxslip = true; yokofla = false; } - else if(compA && compB && compC) + else if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L)) { - motor[TIRE_FR].dir = FOR; + 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; - - Color_changeflag(); + motor[TIRE_FR].pwm = 55; + motor[TIRE_FL].pwm = 55; + motor[TIRE_BR].pwm = 55; + motor[TIRE_BL].pwm = 55; } - else if(invationA && invationB && invationC) + else if((invationA || invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = FREE; motor[TIRE_FL].dir = BACK; @@ -840,85 +1116,93 @@ motor[TIRE_BL].dir = FREE; //motor[TIRE_FR].pwm = startP; - motor[TIRE_FL].pwm = startP; - motor[TIRE_BR].pwm = startP; + motor[TIRE_FL].pwm = 55; + motor[TIRE_BR].pwm = 55; //motor[TIRE_BL].pwm = startP; } - else if(!invationA && !invationB && !invationC) + else if((!invationA || !invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = FREE; motor[TIRE_BR].dir = FREE; motor[TIRE_BL].dir = BACK; - motor[TIRE_FR].pwm = startP; + motor[TIRE_FR].pwm = 55; //motor[TIRE_FL].pwm = startP; - //motor[TIRE_BR].pwm = startP; - motor[TIRE_BL].pwm = startP; + //motor[TIRE_BR].pwm = startP; + motor[TIRE_BL].pwm = 55; + } + 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 = 50; + motor[TIRE_FL].pwm = 50; + motor[TIRE_BR].pwm = 50; + motor[TIRE_BL].pwm = 50; } - else if(!invationA && compC && invationC)//C固定A下 - { - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; +} + + + /*********************************/ + if(boxslip && !yokofla && !traceon) + { + Color_changeflagA(); + if((compA && compB) || compC){ + + RtX.reset(); + RtY.reset(); + + Rt_flagX = true; + boxslip = false; + } + else if(invationA || invationB && !compC && !(compA && compB)){ + motor[TIRE_FR].dir = BACK; + motor[TIRE_FL].dir = BACK; + motor[TIRE_BR].dir = FOR; + motor[TIRE_BL].dir = FOR; + + motor[TIRE_FR].pwm = 25; + motor[TIRE_FL].pwm = 25; + motor[TIRE_BR].pwm = 25; + motor[TIRE_BL].pwm = 25; + } + else if(!invationA || !invationB && !compC && !(compA && 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 = 255; - motor[TIRE_FL].pwm = 100; - motor[TIRE_BR].pwm = 55; - motor[TIRE_BL].pwm = startP; + motor[TIRE_FR].pwm = 25; + motor[TIRE_FL].pwm = 25; + motor[TIRE_BR].pwm = 25; + motor[TIRE_BL].pwm = 25; + } + } + + + filip(); + if(Rt_flagX && !boxslip) + { + filip(); + if(disX < goalAX - 5){ - Color_changeflag(); - } - else if(compA && compB && !invationC)//AB固定C下 - { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = FOR; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - - motor[TIRE_FR].pwm = 55; - motor[TIRE_FL].pwm = startP; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 100; - - Color_changeflag(); - } - else if(compA && compB && !compC && invationC)//AB固定C上 - { - motor[TIRE_FR].dir = BACK; - motor[TIRE_FL].dir = BACK; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - - motor[TIRE_FR].pwm = startP; - motor[TIRE_FL].pwm = 55; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 100; + motor[TIRE_BR].dir = BACK; + motor[TIRE_BL].dir = BACK; + motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8; + motor[TIRE_FL].pwm = SetPWM(RtpwmX); + motor[TIRE_BR].pwm = SetPWM(RtpwmX); + motor[TIRE_BL].pwm = SetPWM(RtpwmX); + } + else if(disX > goalAX - 5){ - Color_changeflag(); - } - else if(!compA && invationA && compC)//C固定A上 - { motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = FOR; - motor[TIRE_BL].dir = FOR; - - motor[TIRE_FR].pwm = 255; - motor[TIRE_FL].pwm = 100; - motor[TIRE_BR].pwm = startP; - motor[TIRE_BL].pwm = 55; - - Color_changeflag(); - } - } - - if(boxslip) - { - for(int i = 0; i<500; i++) - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; @@ -927,52 +1211,7 @@ motor[TIRE_BR].pwm = 255; motor[TIRE_BL].pwm = 255; - Rt_flagX = true; - nopushed = true; - - RtX.reset(); - RtY.reset(); - } - - - /*if(controller->Button.A && !nopushed){ - Rt_flagX = true; - nopushed = true; - - RtX.reset(); - RtY.reset(); - }else if(!controller->Button.A)nopushed = false; - */ - filip(); - - if(Rt_flagX) - { - filip(); - if(disX < goalX - 5){ - 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.8; - motor[TIRE_FL].pwm = SetPWM(RtpwmX); - motor[TIRE_BR].pwm = SetPWM(RtpwmX); - motor[TIRE_BL].pwm = SetPWM(RtpwmX); - } - else if(disX > goalX - 5){ - - for(int i = 0; i<500; i++){ - 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; - } + wait(1); Rt_flagY = true; Rt_flagX = false; @@ -982,8 +1221,7 @@ if(Rt_flagY && !Rt_flagX){ filip(); - if(disY < goalY - 5){ - filip(); + if(disY < goalAY - 5){ motor[TIRE_FR].dir = SetStatus(-RtpwmY); motor[TIRE_FL].dir = SetStatus(RtpwmY); motor[TIRE_BR].dir = SetStatus(-RtpwmY); @@ -993,9 +1231,8 @@ motor[TIRE_BR].pwm = SetPWM(RtpwmY); motor[TIRE_BL].pwm = SetPWM(RtpwmY); } - else if(disY > goalY - 5) + else if(disY > goalAY - 5) { - filip(); motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; @@ -1006,7 +1243,6 @@ motor[TIRE_BL].pwm = 255; } } - } #endif @@ -1139,7 +1375,6 @@ #if USE_PROCESS_NUM>8 //kakudo static void Process8() { - } #endif @@ -1180,8 +1415,22 @@ disX = 48*3.141*rpmX; disY = 48*3.141*rpmY; - RtpwmX = (int)Rt_X.SetPV(disX , goalX); - RtpwmY = (int)Rt_Y.SetPV(disY , goalY); + RtpwmX = (int)Rt_X.SetPV(disX , goalAX); + RtpwmY = (int)Rt_Y.SetPV(disY , goalAY); +} + +void filipB(){ + palseX = RtX.getPulses(); + palseY = RtY.getPulses(); + + rpmXB = (double)palseX/(ROTATE_PER_REVOLUTIONS*4); + rpmYB = (double)palseY/(ROTATE_PER_REVOLUTIONS*4); + + disXB = 48*3.141*rpmXB; + disYB = 48*3.141*rpmYB; + + RtpwmXB = (int)Rt_X.SetPV(disXB , goalBX); + RtpwmYB = (int)Rt_Y.SetPV(disYB , goalBY); } unsigned long ColorIn(int index) @@ -1241,29 +1490,26 @@ Color_D[2] = ColorIn(3); } -void Color_changeflag(){ +void Color_changeflagA(){ ColorDetection(); if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白 { - invationA ^= 1;//start false,over true compA = true;//on true,noon false } - else if(!(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2]))compA = false;//茶 + else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶 if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白 { - invationB ^= 1;//start false,over true compB = true;//on true,noon false } - else if(!(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2]))compB = false;//茶 + else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶 if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白 { - invationC ^= 1;//start false,over true compC = true;//on true,noon false } - else if(!(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2]))compC = false;//茶 + else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[2])compC = false;//茶 /* if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白 { @@ -1272,6 +1518,68 @@ } else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶 */ + + if(compA && !compB) + { + invationA = false; + } + else if(!compA && compB) + { + invationA = true; + } + + if(!invationA && !compA && !compB){ + invationB = false; + } + else if(invationA && !compA && !compB){ + invationB = true; + } +} + +void Color_changeflagB(){ + ColorDetection(); + + if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白 + { + compA = true;//on true,noon false + } + else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶 + + if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白 + { + compB = true;//on true,noon false + } + else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶 + + if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白 + { + compC = true;//on true,noon false + } + else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[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(compB && !compA) + { + invationB = false; + } + else if(!compB && compA) + { + invationB = true; + } + + if(!invationB && !compA && !compB){ + invationA = false; + } + else if(invationB && !compA && !compB){ + invationA = true; + } } void getcolor(){ @@ -1319,4 +1627,8 @@ void TapeLedEms_func() { sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; } + +void LostLed_func(){ + sendLedData.code = sendLedData.code == (uint32_t)Yellow ? (uint32_t)Black : (uint32_t)Yellow; +} #pragma endregion