![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
uuu
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Diff: System/Process/Process.cpp
- Revision:
- 9:f93fc79a49ea
- Parent:
- 8:6fb3723f7747
- Child:
- 10:1295d39fec3a
--- a/System/Process/Process.cpp Tue Oct 02 12:18:05 2018 +0000 +++ b/System/Process/Process.cpp Thu Oct 04 12:14:25 2018 +0000 @@ -59,19 +59,20 @@ return result; } -#define TIRE_FR 4 //足回り前右 -#define TIRE_FL 5 //足回り前左 +#define TIRE_FR 0 //足回り前右 +#define TIRE_FL 1 //足回り前左 #define TIRE_BR 2 //足回り後右 #define TIRE_BL 3 //足回り後左 -#define Angle_R 0 //角度調節右 -#define Angle_L 1 //角度調節左 - +#define Angle_R 4 //角度調節右 +#define Angle_L 5 //角度調節左 #define Lim_AR 3 //角度調節右 #define Lim_AL 4 //角度調節左 #define Lim_R 0 //センター右 #define Lim_L 1 //センター左 +#define EMS_0 LimitSw::IsPressed(8) +#define EMS_1 LimitSw::IsPressed(9) //************メカナム******************** const int mecanum[15][15]= @@ -136,18 +137,20 @@ Ticker Color_T; //************ライントレース変数******************* + + //ROタコン -QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING); -QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING); +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); bool Rt_flagX = false; -bool Rt_flagY = false; -int rpmX; -int rpmY; -int disX; -int disY; + +double rpmX; +double rpmY; +double disX; +double disY; int palseX; int palseY; int RtpwmX; @@ -161,18 +164,23 @@ //ROタコン //************ジャイロ******************* + +bool Angle_flagI = false; 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 AngletargetX = 20; +int AngletargetY = -20; int Angle_I = -5; //************ジャイロ******************* - +//************Buzzer****************** +DigitalOut buzzer(BUZZER_PIN); +void BuzzerTimer_func(); +Ticker BuzzerTimer; +bool Emsflag = false; +//************Buzzer****************** #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE #ifdef USE_SUBPROCESS @@ -211,6 +219,7 @@ void SystemProcessInitialize() { #pragma region USER-DEFINED_VARIABLE_INIT + get_rpm.attach_us(&filip,100); /*Replace here with the initialization code of your variables.*/ @@ -295,8 +304,8 @@ SystemProcessInitialize(); while(1) - { - get_rpm.attach_us(&filip,1000); + { + /*get_rpm.attach_us(&filip,1000); disX = 48*3.141592*rpmX; disY = 48*3.141592*rpmY; @@ -304,10 +313,10 @@ RtpwmX = Rt_X.SetPV(disX , goalX); RtpwmY = Rt_Y.SetPV(disY , goalY); - //if(controller->Button.B){ + if(controller->Button.B){ Rt_flagX = true; - //} - /*Rt_flagY = true; + } + Rt_flagY = true; if (Rt_flagY){ motor[TIRE_FR].dir = SetStatus(RtpwmY); motor[TIRE_FL].dir = SetStatus(RtpwmY); @@ -318,14 +327,14 @@ motor[TIRE_BR].pwm = SetPWM(RtpwmY); motor[TIRE_BL].pwm = SetPWM(RtpwmY); } - if(goalY - 5 < disY && disY < goalY + 5){ + if(goalY - 15 < disY && disY < goalY + 15){ motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; Rt_flagY = false; Rt_flagX = true; - }*/ + } if(Rt_flagX){ motor[TIRE_FR].dir = SetStatus(RtpwmX); motor[TIRE_FL].dir = SetStatus(-RtpwmX); @@ -336,7 +345,7 @@ motor[TIRE_BR].pwm = SetPWM(RtpwmX); motor[TIRE_BL].pwm = SetPWM(RtpwmX); } - if(goalX - 5 < disX && disX < goalX + 5){ + if(goalX - 15 < disX && disX < goalX + 15){ motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; @@ -348,9 +357,10 @@ motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; } - pc.printf("%d \r\n",RtpwmX); + pc.printf("%f \r\n",RtpwmX); wait_ms(50); + /*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON; else LED_DEBUG0 = LED_OFF;*/ @@ -381,6 +391,18 @@ } } + if (EMS_0 || EMS_1 && !Emsflag){ + buzzer = 1; + BuzzerTimer.attach(BuzzerTimer_func, 1.2); + Emsflag = true; + } + + if(!EMS_0 && !EMS_1) { + buzzer = 0; + BuzzerTimer.detach(); + Emsflag = false; + } + SystemProcessUpdate(); } } @@ -454,7 +476,7 @@ //wheel.getPulses()...どちらの方向にどれだけ回ったか //pc.printf("Pulses:%07d \r\n",wheel.getPulses()); //軸が何回転したか - //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2)); + //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); } #endif @@ -711,12 +733,26 @@ motor[Angle_R].dir = BRAKE; motor[Angle_L].dir = BRAKE; } + 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:%f \r\n",Angle); } #endif #if USE_PROCESS_NUM>4 static void Process4() { + static bool Xnopush = false; + static bool Ynopush = false; + + static bool Angle_flagX = false; + static bool Angle_flagY = false; + if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){ motor[Angle_R].dir = BRAKE; @@ -739,12 +775,15 @@ int gyropwmX = gyro.SetPV(Angle,AngletargetX); int gyropwmY = gyro.SetPV(Angle,AngletargetY); - if(controller->Button.X){ + if(controller->Button.X && !Xnopush){ Angle_flagX = true; - } - if(controller->Button.Y){ + Xnopush = true; + }else{Xnopush = false;} + + if(controller->Button.Y && !Ynopush){ Angle_flagY = true; - } + Ynopush = true; + }else{Ynopush = false;} if (Angle_flagX){ motor[Angle_R].dir = SetStatus(gyropwmX); @@ -798,66 +837,60 @@ #if USE_PROCESS_NUM>5 static void Process5() { - /*wait(0.1); - //RtX.getPulses();...どちらの方向にどれだけ回ったか + 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*2)); + pc.printf("Rotate:%04.3f \r\n",(double)RtX.getPulses()/(ROTATE_PER_REVOLUTIONS*4)); */ - /* - get_rpm.attach_us(&filip,1000); - disX = 48*3.141592*rpmX; - disY = 48*3.141592*rpmY; - RtpwmX = Rt_X.SetPV(disX , goalX); - RtpwmY = Rt_Y.SetPV(disY , goalY); + if(controller->Button.B && !nopushed){ + Rt_flagY = true; + nopushed = true; + }else{nopushed = false;} + - if(controller->Button.B){ - Rt_flagY = true; - } - if (Rt_flagY){ - motor[TIRE_FR].dir = SetStatus(RtpwmY); + 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_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); } - if(goalY - 15 < disY && disY < goalY + 15){ + 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; - Rt_flagY = false; - Rt_flagX = true; } - if(Rt_flagX){ - 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); - motor[TIRE_FL].pwm = SetPWM(RtpwmX); - motor[TIRE_BR].pwm = SetPWM(RtpwmX); - motor[TIRE_BL].pwm = SetPWM(RtpwmX); - } - if(goalX - 15 < disX && disX < goalX + 15){ + else + { motor[TIRE_FR].dir = BRAKE; motor[TIRE_FL].dir = BRAKE; motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; - Rt_flagX = false; - }else{ - motor[TIRE_FR].dir = BRAKE; - motor[TIRE_FL].dir = BRAKE; - motor[TIRE_BR].dir = BRAKE; - motor[TIRE_BL].dir = BRAKE; - }*/ + } + + //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 @@ -1040,7 +1073,6 @@ motor[TIRE_BR].dir = BRAKE; motor[TIRE_BL].dir = BRAKE; } - */ } #endif @@ -1048,7 +1080,19 @@ #if USE_PROCESS_NUM>7 static void Process7() { - + //et_rpm.attach_us(&filip,1000); + + disX = 48*3.141*rpmX; + disY = 48*3.141*rpmY; + + RtpwmX = Rt_X.SetPV(disX , goalX); + RtpwmY = Rt_Y.SetPV(disY , goalY); + + /*pc.printf("disX:%f \r\n", disX); + pc.printf("disY:%f \r\n", disY); + pc.printf("RtpwmX:%f \r\n", RtpwmX); + pc.printf("RtpwmY:%f \r\n", RtpwmY); + wait_ms(50);*/ } #endif @@ -1103,8 +1147,14 @@ palseX = RtX.getPulses(); palseY = RtY.getPulses(); - rpmX = palseX/(ROTATE_PER_REVOLUTIONS*2); - rpmY = palseY/(ROTATE_PER_REVOLUTIONS*2); + rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4); + rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4); + + //disX = 48*3.141592*rpmX; + disY = 48*3.141*rpmY; + + //RtpwmX = (int)Rt_X.SetPV(disX , goalX); + RtpwmY = (int)Rt_Y.SetPV(disY , goalY); } @@ -1147,4 +1197,7 @@ wait_us(3); Color_D[2] = ColorIn(3); } +void BuzzerTimer_func() { + buzzer = !buzzer; +} #pragma endregion