![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ttt
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
System/Process/Process.cpp
- Committer:
- kishibekairohan
- Date:
- 2018-10-07
- Revision:
- 12:c09b3e08a316
- Parent:
- 11:028a150943b5
- Child:
- 13:b6e02d6261d7
File content as of revision 12:c09b3e08a316:
#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); //************メカナム******************** 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(); //************カラーセンサ******************** //************ライントレース変数******************* int Point[3] = {234, 466, 590};//赤,緑,青 int startP = 35; int downP = 5; int Asasult = 0; int Bsasult = 0; int Csasult = 0; int Dsasult = 0; void pointcalculation(); 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); Ticker get_rpm; PID Rt_X = PID(0.03, -255, 255, 0.1, 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 = 1200.000; double goalY = 900.000; void filip(); //************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****************** //DigitalOut buzzer(BUZZER_PIN); PwmOut buzzer(BUZZER_PIN); void BuzzerTimer_func(); Ticker BuzzerTimer; bool Emsflag = false; //************Buzzer****************** //************TapeLed***************** void TapeLedEms_func(); TapeLedData tapeLED; TapeLedData sendLedData; TapeLED_Mode ledMode = Normal; Ticker tapeLedTimer; //************TapaLed***************** #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.*/ get_rpm.attach_us(&filip,100); buzzer.period(1.0/800); #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) { 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 #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 { //ロック時の処理 } } if ((EMS_0 || EMS_1) && !Emsflag){ buzzer = 0.5; 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) { 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(); } } #pragma region PROCESS #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 static void Process0() { tapeLED.code = (uint32_t)Green; } #endif #if USE_PROCESS_NUM>1 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() //自動角度調節 { 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 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 #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 pointcalculation(){ 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;//茶*/ for(int i=0;i<3;i++){Asasult += Color_A[i]-Point[i];} for(int i=0;i<3;i++){Bsasult += Color_B[i]-Point[i];} for(int i=0;i<3;i++){Csasult += Color_A[i]-Point[i];} for(int i=0;i<3;i++){Dsasult += Color_B[i]-Point[i];} } 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); } 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 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; } else if(buzzer == 0){ buzzer = 0.5; } } void TapeLedEms_func() { sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red; } #pragma endregion