![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
uuu
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
System/Process/Process.cpp
- Committer:
- kishibekairohan
- Date:
- 2018-10-01
- Revision:
- 5:3ae504b88679
- Parent:
- 3:e10d8736fd22
File content as of revision 5:3ae504b88679:
#include "mbed.h" #include "Process.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" #include "../../Communication/PID/PID.h" using namespace SWITCH; using namespace COLORSENSOR; using namespace ACCELERATIONSENSOR; using namespace PID_SPACE; using namespace ROTARYENCODER; static CONTROLLER::ControllerData *controller; ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM]; ACTUATORHUB::SOLENOID::SolenoidStatus solenoid; static bool lock; static bool processChangeComp; static int current; static void AllActuatorReset(); #ifdef USE_SUBPROCESS static void (*Process[USE_PROCESS_NUM])(void); #endif #pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE /*Replace here with the definition code of your variables.*/ Serial pc(USBTX, USBRX); unsigned long ColorIn(int index) { int result = 0; bool rtn = false; for(int i=0; i<12; i++) { CK[index] = 1; rtn = DOUT[index]; CK[index] = 0; if(rtn) { result|=(1 << i); } } return result; } #define TIRE_FR 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 //センター左 //************メカナム******************** 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 averageR_A; int averageG_A; int averageB_A; int averageR_B; int averageG_B; int averageB_B; int averageR_C; int averageG_C; int averageB_C; int averageR_D; int averageG_D; int averageB_D;*/ void ColorDetection(); /*DigitalIn www(RT11_PIN); DigitalIn mmm(RT12_PIN); int pp = 0; int bb = 0; */ //************カラーセンサ変数******************** //************ライントレース変数******************* int Point[3] = {234, 466, 590};//赤,緑,青 int colorSN; int startP = 35; int downP = 5; bool Goal_flag = false; PID goal = PID(0.03,-255,255,0,0,0); void GoalArrival(); //************ライントレース変数******************* //************ジャイロ******************* //void AngleDetection(); //void AngleControl(); float AngleY; PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0); bool Angle_flag = false; float rotateY; int AngletargetX = 50; int AngletargetY = -50; int Angle_I; //************ジャイロ******************* //************ロタコン******************* int MemoRt; int Rt0; int Rt1; int Rt_A; int Rt_B; int PresentRt; //************ロタコン******************* #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 static void Process0(void); #endif #if USE_PROCESS_NUM>1 static void Process1(void); #endif #if USE_PROCESS_NUM>2 static void Process2(void); #endif #if USE_PROCESS_NUM>3 static void Process3(void); #endif #if USE_PROCESS_NUM>4 static void Process4(void); #endif #if USE_PROCESS_NUM>5 static void Process5(void); #endif #if USE_PROCESS_NUM>6 static void Process6(void); #endif #if USE_PROCESS_NUM>7 static void Process7(void); #endif #if USE_PROCESS_NUM>8 static void Process8(void); #endif #if USE_PROCESS_NUM>9 static void Process9(void); #endif #endif void SystemProcessInitialize() { #pragma region USER-DEFINED_VARIABLE_INIT /*Replace here with the initialization code of your variables.*/ #pragma endregion USER-DEFINED_VARIABLE_INIT lock = true; processChangeComp = true; current = DEFAULT_PROCESS; #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 Process[0] = Process0; #endif #if USE_PROCESS_NUM>1 Process[1] = Process1; #endif #if USE_PROCESS_NUM>2 Process[2] = Process2; #endif #if USE_PROCESS_NUM>3 Process[3] = Process3; #endif #if USE_PROCESS_NUM>4 Process[4] = Process4; #endif #if USE_PROCESS_NUM>5 Process[5] = Process5; #endif #if USE_PROCESS_NUM>6 Process[6] = Process6; #endif #if USE_PROCESS_NUM>7 Process[7] = Process7; #endif #if USE_PROCESS_NUM>8 Process[8] = Process8; #endif #if USE_PROCESS_NUM>9 Process[9] = Process9; #endif #endif } static void SystemProcessUpdate() { #ifdef USE_SUBPROCESS if(controller->Button.HOME) lock = false; if(controller->Button.START && processChangeComp) { current++; if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM; processChangeComp = false; } else if(controller->Button.SELECT && processChangeComp) { current--; if (current < 0) current = 0; processChangeComp = false; } else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true; #endif #ifdef USE_MOTOR ACTUATORHUB::MOTOR::Motor::Update(motor); #endif #ifdef USE_SOLENOID ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid); #endif #ifdef USE_RS485 ACTUATORHUB::ActuatorHub::Update(); #endif } void SystemProcess() { SystemProcessInitialize(); while(1) { #ifdef USE_MU controller = CONTROLLER::Controller::GetData(); #endif #ifdef USE_ERRORCHECK if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost) { CONTROLLER::Controller::DataReset(); AllActuatorReset(); lock = true; } else #endif { #ifdef USE_SUBPROCESS if(!lock) { Process[current](); } else #endif { //ロック時の処理 } } SystemProcessUpdate(); } } #pragma region PROCESS #ifdef USE_SUBPROCESS #if USE_PROCESS_NUM>0 static void Process0() { if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].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[4].dir == BACK && motor[5].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; AngleY += rotateY; } AngleY = AngleY /20; int gyropwm = gyro.SetPV(AngleY,Angle_I); if(controller->Button.A){ Angle_flag = true; }/* if(controller->Button.Y){ 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(Angle_I - 2 < AngleY && AngleY < Angle_I + 2){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; Angle_flag = false; } } } #endif #if USE_PROCESS_NUM>1 static void Process1() { motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]); motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]); motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]); motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]); motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]); motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]); motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]); if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){ motor[TIRE_FR].pwm = motor[0].pwm * 1.3; motor[TIRE_FL].pwm = motor[1].pwm * 1.3; } } #endif #if USE_PROCESS_NUM>2 static void Process2() { static bool color_flag = false; static bool traceon = false; static bool yokofla = 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(controller->Button.B && !color_flag) { traceon ^= 1; color_flag = true; } else if(!controller->Button.B) { color_flag = false; } if(traceon) { 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; motor[0].pwm = 255; motor[1].pwm = 255; motor[2].pwm = 255; motor[3].pwm = 255; wait(5); yokofla = true; } else if(invationA && !compA && invationB && !compB && yokofla){ 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 - 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 && yokofla){ 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(3) && LimitSw::IsPressed(4) && invationA && !compA && !invationB && !compB && yokofla && traceon){ 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; } /*//// 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; }*////// else 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 = 100; motor[TIRE_FL].pwm = 100; motor[TIRE_BR].pwm = 100; motor[TIRE_BL].pwm = 100; }else if(!LimitSw::IsPressed(3) && LimitSw::IsPressed(4)){ motor[TIRE_FR].dir = FOR; motor[TIRE_BR].dir = FOR; motor[TIRE_FR].pwm = 20; motor[TIRE_BR].pwm = 20; }else if(LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)){ motor[TIRE_FL].dir = BACK; motor[TIRE_BL].dir = BACK; motor[TIRE_FL].pwm = 20; motor[TIRE_BL].pwm = 20; } else if(!LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)){ Goal_flag = true; GoalArrival(); }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>3 static void Process3() { if(controller->Button.R){ motor[Angle_R].dir = FOR; motor[Angle_L].dir = BACK; motor[Angle_R].pwm = 150; motor[Angle_L].pwm = 150; }else if(controller->Button.L){ motor[Angle_R].dir = BACK; motor[Angle_L].dir = FOR; motor[Angle_R].pwm = 150; motor[Angle_L].pwm = 150; }else{ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; } if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].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[4].dir == BACK && motor[5].dir == FOR){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; motor[Angle_R].pwm = 255; motor[Angle_L].pwm = 255; } } #endif #if USE_PROCESS_NUM>4 static void Process4() { /* for(int i=0;i<10;i++) { ColorDetection(); averageR_A += Color_A[0]; averageG_A += Color_A[1]; averageB_A += Color_A[2]; averageR_B += Color_B[0]; averageG_B += Color_B[1]; averageB_B += Color_B[2]; averageR_C += Color_C[0]; averageG_C += Color_C[1]; averageB_C += Color_C[2]; averageR_D += Color_D[0]; averageG_D += Color_D[1]; averageB_D += Color_D[2]; } pc.printf("AR_A:%d, AG_A:%d ,AB_A:%d \r\n",averageR_A / 10 ,averageG_A / 10, averageB_A / 10); pc.printf("AR_B:%d, AG_B:%d ,AB_B:%d \r\n",averageR_B / 10 ,averageG_B / 10, averageB_B / 10); pc.printf("AR_C:%d, AG_C:%d ,AB_C:%d \r\n",averageR_C / 10 ,averageG_C / 10, averageB_C / 10); pc.printf("AR_D:%d, AG_D:%d ,AB_D:%d \r\n",averageR_D / 10 ,averageG_D / 10, averageB_D / 10); averageR_A = 0; averageG_A = 0; averageB_A = 0; averageR_B = 0; averageG_B = 0; averageB_B = 0; averageR_C = 0; averageG_C = 0; averageB_C = 0; averageR_D = 0; averageG_D = 0; averageB_D = 0;*/ } #endif #if USE_PROCESS_NUM>5 static void Process5() { if(LimitSw::IsPressed(Lim_AR) && motor[4].dir == FOR && motor[5].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[4].dir == BACK && motor[5].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; AngleY += rotateY; } AngleY = AngleY /20; int gyropwm = gyro.SetPV(AngleY,AngletargetY); if(controller->Button.X){ Angle_flag = true; }/* if(controller->Button.Y){ 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(AngletargetY - 2 < AngleY && AngleY < AngletargetY + 2){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; Angle_flag = false; } } /*float y = 0; y = acc[1]*1000; float rotateY = (y - 305)/2.21 - 90; int gyropwm = gyro.SetPV(rotateY , Angletarget); if(controller->Button.X){ Angle_flag = true; } if (Angle_flag){ motor[Angle_R].dir = SetStatus(gyropwm); motor[Angle_L].dir = SetStatus(-gyropwm); motor[Angle_R].pwm = SetPWM(gyropwm); motor[Angle_L].pwm = SetPWM(gyropwm); if(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; Angle_flag = false; } }*/ else{ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; } } #endif #if USE_PROCESS_NUM>6 static void Process6() { //void Int::Initialize(BoardRtInt[0].fall(int2)); } #endif #if USE_PROCESS_NUM>7 static void Process7() { } #endif #if USE_PROCESS_NUM>8 static void Process8() { } #endif #if USE_PROCESS_NUM>9 static void Process9() { } #endif #endif #pragma endregion PROCESS static void AllActuatorReset() { #ifdef USE_SOLENOID solenoid.all = ALL_SOLENOID_OFF; #endif #ifdef USE_MOTOR for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) { motor[i].dir = FREE; motor[i].pwm = 0; } #endif } #pragma region USER-DEFINED-FUNCTIONS void 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 AngleDetection(){ float x = 0, y= 0, z = 0; //x = acc[0]*1000; y = acc[1]*1000; //z = acc[2]*1000; //pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z); //float rotateX = (x - 306)/2.22 - 90; float rotateY = (y - 305)/2.21 - 90; //float rotateZ = (z - 300)/1.18 - 90; //pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ); }*/ /*void AngleControl(){ int Angletarget = -50; float AnglevalueY = rotateY; int gyropwm = gyro.SetPV(AnglevalueY , Angletarget); 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 < AnglevalueY && AnglevalueY < 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; } //pc.printf("PWM:%d \r\n" , gyropwm); }*/ void GoalArrival(){ int Goaltarget = 0; float Goalvalue = 0; int goalpwm = goal.SetPV(Goalvalue,Goaltarget); if (Goal_flag){ motor[TIRE_FR].dir = SetStatus(goalpwm); motor[TIRE_FL].dir = SetStatus(-goalpwm); motor[TIRE_BR].dir = SetStatus(goalpwm); motor[TIRE_BL].dir = SetStatus(-goalpwm); motor[TIRE_FR].pwm = SetPWM(goalpwm); motor[TIRE_FL].pwm = SetPWM(goalpwm); motor[TIRE_BR].pwm = SetPWM(goalpwm); motor[TIRE_BL].pwm = SetPWM(goalpwm); if(Goaltarget > Goalvalue - 10 && Goaltarget < Goalvalue+ 10){ motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; Goal_flag = false; } }else{ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; } } /* void RotaryD(uint8_t, uint8_t){ uint8_t D; MemoRt = Rt1; Rt0 = Rt0 << 1; Rt0 = Rt0 & 3; Rt1 = Rt1 & 3; D = (Rt0 + Rt1) & 3; //return (D); } void GetRt(){ uint8_t DJ; double rad = 0; PresentRt = 0x00; if(LimitSw::IsPressed(Rt_A)) PresentRt = 0x02; if(LimitSw::IsPressed(Rt_B)) PresentRt = 0x01; if(MemoRt != PresentRt) DJ = RotaryD(MemoRt,PresentRt); if(D>=2)rad = 360.0 /50.0; else rad = -(360.0/50.0); Rt = Rt + rad /360.0*20; }*/ #pragma endregion