![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Diff: System/Process/Process.cpp
- Revision:
- 16:3f2c2d89372b
- Parent:
- 15:dfcec98f5aa9
- Child:
- 17:50dc4b449e69
--- a/System/Process/Process.cpp Sun Oct 21 02:14:15 2018 +0000 +++ b/System/Process/Process.cpp Mon Jul 01 13:00:20 2019 +0000 @@ -8,20 +8,15 @@ #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 "../../Input/Encoder/Encoder.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; +using namespace ENCODER; static CONTROLLER::ControllerData *controller; ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; @@ -43,134 +38,22 @@ Serial pc(USBTX, USBRX); -//************メカナム******************** - -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; - -int Avecolor_A[3]; -int Avecolor_B[3]; -int Avecolor_C[3]; -int Avecolor_D[3]; - -void ColorIn(); -void ColorDetection(); -void getcolor(); - -//************カラーセンサ******************** +//**************Encoder*************** +const int PerRev = 256; +QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING); +QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING); +QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING); +QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING); +//**************Encoder*************** -//************ライントレース変数******************* -int PointA[3] = {400, 700, 1000};//赤,緑,青 -int PointB[3] = {400, 700, 1000};//赤,緑,青 -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; -bool compC = false; -bool compD = false; - -bool invationA = false; -bool invationB = false; -bool invationC = false; -bool invationD = false; - -Ticker Color_T; - -void ColorDetection(); -void Color_changeflag(); - -//************ライントレース変数******************* - -//************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; -int palseX; -int palseY; -int RtpwmX; -int RtpwmY; -double goalX = 900.000; -double goalY = 700.000; - -//double goalXB = 900.000; -//double goalYB = 700.000; -void filip(); -void filipB(); - -//************ROタコン****************** - -//************ジャイロ******************* -bool Angle_flagI = false; -int Angle; -PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0); -float rotateY; -//初期値 -5 -int AngletargetX = 4; -int AngletargetY = -12; -int AngletargetI = -5; -//************ジャイロ******************* - -//************Buzzer****************** +//**************Buzzer**************** //DigitalOut buzzer(BUZZER_PIN); -PwmOut buzzer(BUZZER_PIN); void BuzzerTimer_func(); Ticker BuzzerTimer; -bool Emsflag = false; -//************Buzzer****************** +bool EMGflag = false; +PwmOut buzzer(BUZZER_PIN); +//buzzer.period(1.0/800); +//**************Buzzer**************** //************TapeLed***************** void TapeLedEms_func(); @@ -179,6 +62,7 @@ TapeLED_Mode ledMode = Normal; Ticker tapeLedTimer; //************TapaLed***************** + #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE #ifdef USE_SUBPROCESS @@ -217,10 +101,7 @@ void SystemProcessInitialize() { #pragma region USER-DEFINED_VARIABLE_INIT - /*Replace here with the initialization code of your variables.*/ - get_rpm.attach_us(&filip,100); - buzzer.period(1.0/800); - + /*Replace here with the initialization code of your variables.*/ #pragma endregion USER-DEFINED_VARIABLE_INIT lock = true; @@ -303,11 +184,9 @@ 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]); + //LED_DEBUG0 = !LED_DEBUG0; + //LED_DEBUG1 = !LED_DEBUG1; + //printf("%d\r\n",ECD_0.getPulses()); #ifdef USE_MU controller = CONTROLLER::Controller::GetData(); @@ -319,7 +198,7 @@ CONTROLLER::Controller::DataReset(); AllActuatorReset(); lock = true; - buzzer = 0.5; + buzzer = 1; BuzzerTimer.attach(BuzzerTimer_func, 0.5); } else @@ -338,34 +217,17 @@ } } - if ((EMS_0 || EMS_1) && !Emsflag){ - buzzer = 0.5; + //Emergency! + if(EMG_0 || EMG_1){ + buzzer = 1; BuzzerTimer.attach(BuzzerTimer_func, 1.2); - Emsflag = true; - ledMode = EMS; current = 0; - tapeLedTimer.attach(TapeLedEms_func, 1.2); - sendLedData.code = (uint32_t)Red; } - if(!EMS_0 && !EMS_1) { + //Safety + if(!EMG_0 && !EMG_1){ buzzer = 0; BuzzerTimer.detach(); - Emsflag = false; - if(ledMode == EMS) ledMode = Normal; - tapeLedTimer.detach(); - } - - switch(ledMode) - { - case EMS : - break; - - case Normal : - sendLedData.code = tapeLED.code; - - default: - break; } SystemProcessUpdate(); @@ -380,338 +242,28 @@ #if USE_PROCESS_NUM>0 static void Process0() { - tapeLED.code = (uint32_t)Green; - if(RedSW){ - current = 1; - } - if(BlueSW){ - current = 2; - } + } #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]); - 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; - } - 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)); } #endif #if USE_PROCESS_NUM>2 -static void Process2() //trace +static void Process2() { - tapeLED.code = (uint32_t)Yellow; - static bool color_flag = false; - - static bool traceon = false;//fase1 - static bool yokofla = false;//fase2 - static bool boxslip = false;//fase3 - - //static bool syu = false; - - ColorDetection(); - Color_changeflag(); - - if(controller->Button.B && !color_flag) - { - traceon = true; - color_flag = true; - } - else if(!controller->Button.B)color_flag = false; - - if(traceon) - { - Color_changeflag(); - 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; - - Color_changeflag(); - } - else if(invationC && compC && !invationB && !compB) - { - for(int i = 0; i<1000; 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; - } - - yokofla = true; - traceon = false; - } - } - - if(yokofla && !traceon) - { - //pointcalculation(); - Color_changeflag(); - 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; - - wait(2); - - boxslip = true; - yokofla = false; - } - else if(compA && compB && compC) - { - 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(); - } - 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) - { - 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; - } - else if(!invationA && compC && invationC)//C固定A下 - { - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - 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(); - } - 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; - - 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) - { - 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() //Blue Zone +static void Process3() { - filipB(); - - static bool Rt_flagX = false; - static bool Rt_flagY = false; - if(Rt_flagX) - { - filipB(); - if(disX < goalX - 5){ - filipB(); - - 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<200; 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; - } - - Rt_flagY = true; - Rt_flagX = false; - } - } - -if(Rt_flagY && !Rt_flagX){ - filipB(); - if(disY < goalY - 5){ - filipB(); - 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 > goalY - 5) - { - filipB(); - 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_FL].pwm = 255; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 255; - } - } } #endif @@ -723,491 +275,27 @@ #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; +static void Process5() +{ - 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<200; 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; - } - - Rt_flagY = true; - Rt_flagX = false; - } - } - - -if(Rt_flagY && !Rt_flagX){ - filip(); - if(disY < goalY - 5){ - 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(disY > goalY - 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_FL].pwm = 255; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 255; - } - } } #endif #if USE_PROCESS_NUM>6 static void Process6() { - tapeLED.code = (uint32_t)Yellow; - static bool color_flag = false; - static bool traceon = false;//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_changeflag(); - - if(controller->Button.B && !color_flag) - { - traceon = true; - color_flag = true; - } - else if(!controller->Button.B)color_flag = false; - - if(traceon) - { - Color_changeflag(); - 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; - - Color_changeflag(); - } - else if(invationC && compC && !invationB && !compB) - { - for(int i = 0; i<1000; 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; - } - - yokofla = true; - traceon = false; - } - } - - - if(yokofla && !traceon) - { - //pointcalculation(); - Color_changeflag(); - 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; - - wait(2); - - boxslip = true; - yokofla = false; - } - else if(compA && compB && compC) - { - 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(); - } - 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) - { - 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; - } - else if(!invationA && compC && invationC)//C固定A下 - { - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - 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(); - } - 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; - - 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; - - motor[TIRE_FR].pwm = 255; - motor[TIRE_FL].pwm = 255; - 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; - } - - Rt_flagY = true; - Rt_flagX = false; - } - } - - -if(Rt_flagY && !Rt_flagX){ - filip(); - if(disY < goalY - 5){ - 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(disY > goalY - 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; - motor[TIRE_FL].pwm = 255; - motor[TIRE_BR].pwm = 255; - motor[TIRE_BL].pwm = 255; - } - } - } #endif #if USE_PROCESS_NUM>7 static void Process7() { - 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>8 //kakudo +#if USE_PROCESS_NUM>8 static void Process8() { @@ -1239,169 +327,15 @@ #endif } -#pragma region USER-DEFINED-FUNCTIONS - -void filip(){ - palseX = RtX.getPulses(); - palseY = RtY.getPulses(); - - rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4); - rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4); - - disX = 48*3.141*rpmX; - disY = 48*3.141*rpmY; - - RtpwmX = (int)Rt_X.SetPV(disX , goalX); - RtpwmY = (int)Rt_Y.SetPV(disY , goalY); -} - -void filipB(){ - palseX = RtX.getPulses(); - palseY = RtY.getPulses(); - - rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4); - rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4); - - disX = abs(48*3.141*rpmX); - disY = 48*3.141*rpmY; - - RtpwmX = (int)Rt_X.SetPV(disX , goalX); - 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; - - 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); -} - -void Color_changeflag(){ - 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;//茶 - - 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;//茶 - - 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;//茶 - /* - 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;//茶 - */ -} - -void getcolor(){ - for(int i=0;i<10;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]/10; -Avecolor_A[1] = Avecolor_A[1]/10; -Avecolor_A[2] = Avecolor_A[2]/10; -Avecolor_B[0] = Avecolor_B[0]/10; -Avecolor_B[1] = Avecolor_B[1]/10; -Avecolor_B[2] = Avecolor_B[2]/10; -Avecolor_C[0] = Avecolor_C[0]/10; -Avecolor_C[1] = Avecolor_C[1]/10; -Avecolor_C[2] = Avecolor_C[2]/10; -Avecolor_D[0] = Avecolor_D[0]/10; -Avecolor_D[1] = Avecolor_D[1]/10; -Avecolor_D[2] = Avecolor_D[2]/10; - -} - -void BuzzerTimer_func() { - if(buzzer == 0.5){ - buzzer = 0; - } - else if(buzzer == 0){ - buzzer = 0.5; - } +void BuzzerTimer_func(){ + buzzer = !buzzer; } void TapeLedEms_func() { - sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; + sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; } + +#pragma region USER-DEFINED-FUNCTIONS + + #pragma endregion