![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
uuu
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Diff: System/Process/Process.cpp
- Revision:
- 7:e88c5d47a3be
- Parent:
- 6:10e22bc327ce
- Child:
- 8:6fb3723f7747
--- a/System/Process/Process.cpp Mon Oct 01 14:01:03 2018 +0000 +++ b/System/Process/Process.cpp Mon Oct 01 14:45:50 2018 +0000 @@ -59,13 +59,20 @@ return result; } -#define TIRE_FR 0 //足回り前右 -#define TIRE_FL 1 //足回り前左 +#define TIRE_FR 4 //足回り前右 +#define TIRE_FL 5 //足回り前左 #define TIRE_BR 2 //足回り後右 #define TIRE_BL 3 //足回り後左 -#define Angul_R 4 //角度調節右 -#define Angul_L 5 //角度調節左 +#define Angle_R 0 //角度調節右 +#define Angle_L 1 //角度調節左 + +#define Lim_AR 3 //角度調節右 +#define Lim_AL 4 //角度調節左 +#define Lim_R 0 //センター右 +#define Lim_L 1 //センター左 + +//************メカナム******************** const int mecanum[15][15]= { @@ -100,17 +107,25 @@ else return abs(pwmVal); } +//************メカナム******************** + +//************カラーセンサ******************** + int Color_A[3]; //[赤,緑,青] int Color_B[3]; int Color_C[3]; int Color_D[3]; int intergration = 50; +void ColorDetection(); + +//************カラーセンサ******************** + //************ライントレース変数******************* - int Point[3] = {234, 466, 590};//赤,緑,青 +int Point[3] = {234, 466, 590};//赤,緑,青 - int startP = 35; - int downP = 5; +int startP = 35; +int downP = 5; //************ライントレース変数******************* //ROタコン QEI wheel(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING); @@ -122,22 +137,21 @@ PID startup = PID(0.03, -255, 255, 0.3, 0, 0); -// + + -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; +//************ジャイロ******************* +float Angle; +PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0); +bool Angle_flagX = false; +bool Angle_flagY = false; +bool Angle_flagI = false; +float rotateY; +int AngletargetX = 50; +int AngletargetY = -50; +int Angle_I = -5; +//************ジャイロ******************* -void ColorDetection(); #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE @@ -325,11 +339,40 @@ #if USE_PROCESS_NUM>0 static void Process0() { - ColorDetection(); - pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d \r\n",Color_A[0],Color_A[1],Color_A[2]); - pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d \r\n",Color_B[0],Color_B[1],Color_B[2]); - pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d \r\n",Color_C[0],Color_C[1],Color_C[2]); - pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d \r\n",Color_D[0],Color_D[1],Color_D[2]); + if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){ + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; + }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_L].dir == BACK && motor[Angle_L].dir == FOR){ + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; + } + for(int i = 0;i<20;i++){ + float y = 0; + y = acc[1]*1000; + float rotateY = (y - 305)/2.21 - 90; + Angle += rotateY; + } + Angle = Angle /20; + int gyropwm = gyro.SetPV(Angle,Angle_I); + + if(controller->Button.A){ + Angle_flagI = true; + } + if (Angle_flagI){ + 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 < Angle && Angle < Angle_I + 2){ + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; + Angle_flagI = false; + } + } } #endif @@ -343,14 +386,14 @@ 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]); + 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[0].pwm * 1.3; - motor[TIRE_FL].pwm = motor[1].pwm * 1.3; + motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3; + motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3; } @@ -468,7 +511,7 @@ if(!traceon && yokofla && !boxslip) { - if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4)) + if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; @@ -544,7 +587,7 @@ if(!traceon && !yokofla && boxslip) { - if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4)) + if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = FOR; motor[TIRE_FL].dir = FOR; @@ -556,7 +599,7 @@ motor[TIRE_BR].pwm = startP; motor[TIRE_BL].pwm = startP; } - else if(!LimitSw::IsPressed(3) && !LimitSw::IsPressed(4)) + else if(!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)) { motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; @@ -593,32 +636,32 @@ static void Process3() { if(controller->Button.R){ - motor[Angul_R].dir = FOR; - motor[Angul_L].dir = BACK; - motor[Angul_R].pwm = 150; - motor[Angul_L].pwm = 150; + 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[Angul_R].dir = BACK; - motor[Angul_L].dir = FOR; - motor[Angul_R].pwm = 150; - motor[Angul_L].pwm = 150; + motor[Angle_R].dir = BACK; + motor[Angle_L].dir = FOR; + motor[Angle_R].pwm = 150; + motor[Angle_L].pwm = 150; }else{ - motor[Angul_R].dir = BRAKE; - motor[Angul_L].dir = BRAKE; + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; } - if(LimitSw::IsPressed(0) && motor[4].dir == FOR && motor[5].dir == BACK){ - motor[Angul_R].dir = BRAKE; - motor[Angul_L].dir = BRAKE; + 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[Angul_R].pwm = 255; - motor[Angul_L].pwm = 255; - }else if(LimitSw::IsPressed(1) && motor[4].dir == BACK && motor[5].dir == FOR){ - motor[Angul_R].dir = BRAKE; - motor[Angul_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[Angul_R].pwm = 255; - motor[Angul_L].pwm = 255; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; } } #endif @@ -627,40 +670,94 @@ static void Process4() { + if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){ + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; + }else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){ + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; + motor[Angle_R].pwm = 255; + motor[Angle_L].pwm = 255; + } + for(int i = 0;i<20;i++){ + float y = 0; + y = acc[1]*1000; + float rotateY = (y - 305)/2.21 - 90; + Angle += rotateY; + } + Angle = Angle /20; + int gyropwmX = gyro.SetPV(Angle,AngletargetX); + int gyropwmY = gyro.SetPV(Angle,AngletargetY); + + if(controller->Button.X){ + Angle_flagX = true; + } + if(controller->Button.Y){ + Angle_flagY = true; + } + + 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; + 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; + Angle_flagY = false; + } + } + /*float y = 0; + y = acc[1]*1000; + float rotateY = (y - 305)/2.21 - 90; + int gyropwm = gyro.SetPV(rotateY , Angletarget); + + if(controller->Button.X){ + Angle_flag = true; + } + if (Angle_flag){ + motor[Angle_R].dir = SetStatus(gyropwm); + motor[Angle_L].dir = SetStatus(-gyropwm); + motor[Angle_R].pwm = SetPWM(gyropwm); + motor[Angle_L].pwm = SetPWM(gyropwm); + if(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){ + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; + Angle_flag = false; + } + }*/ + else{ + motor[Angle_R].dir = BRAKE; + motor[Angle_L].dir = BRAKE; + } } #endif #if USE_PROCESS_NUM>5 static void Process5() { - pc.printf("X:1.3% , Y:1.3%f , Z:1.3%f \r\n",acc[0].read(),acc[1].read(),acc[2].read()); - //int rotateX = (acc[0].read()-)/ -90; - //int rotateY = (acc[1].read()-)/ -90; - //pc.printf("X:%d ,Y:%d", rotateX, rotateY); - wait_ms(50); + } #endif #if USE_PROCESS_NUM>6 static void Process6() { - float x = 0, y= 0, z = 0; - - pc.printf("X:%1.3f , Y:%1.3f , Z:%1.3f \r\n",acc[0].read(),acc[1].read(),acc[2].read()); - - 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; - pc.printf("X:%3.1f , Y:%3.1f \r\n" , rotateX , rotateY); - wait_ms(50); - - /*void Anglecontrol(){ - if(rotateX>) && */ + } #endif