大季 矢花
da
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 10:1295d39fec3a
- Parent:
- 9:f93fc79a49ea
- Child:
- 11:028a150943b5
diff -r f93fc79a49ea -r 1295d39fec3a System/Process/Process.cpp
--- a/System/Process/Process.cpp Thu Oct 04 12:14:25 2018 +0000
+++ b/System/Process/Process.cpp Fri Oct 05 12:17:21 2018 +0000
@@ -139,14 +139,12 @@
//************ライントレース変数*******************
-//ROタコン
+//************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);
-bool Rt_flagX = false;
-
double rpmX;
double rpmY;
double disX;
@@ -157,11 +155,10 @@
int RtpwmY;
double goalX = 1200.000;
double goalY = 900.000;
-
void filip();
//PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
-//ROタコン
+//************ROタコン******************
//************ジャイロ*******************
@@ -170,8 +167,8 @@
PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
float rotateY;
-int AngletargetX = 20;
-int AngletargetY = -20;
+int AngletargetX = 18;
+int AngletargetY = -35;
int Angle_I = -5;
//************ジャイロ*******************
@@ -391,8 +388,8 @@
}
}
- if (EMS_0 || EMS_1 && !Emsflag){
- buzzer = 1;
+ if ((EMS_0 || EMS_1) && !Emsflag){
+ buzzer = 1;
BuzzerTimer.attach(BuzzerTimer_func, 1.2);
Emsflag = true;
}
@@ -414,7 +411,10 @@
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0()
-{
+{
+ static bool Xnopush = false;
+ static bool Angle_flagX = 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;
@@ -433,22 +433,28 @@
Angle += rotateY;
}
Angle = Angle /20;
- int gyropwm = gyro.SetPV(Angle,Angle_I);
+ int gyropwmX = gyro.SetPV(Angle,AngletargetX);
+
+ if(controller->Button.X && !Xnopush){
+ Angle_flagX = true;
+ Xnopush = true;
+ }else if(!controller->Button.X)Xnopush = false;
- 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){
+
+ 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_flagI = false;
+ motor[Angle_R].pwm = 255;
+ motor[Angle_L].pwm = 255;
+ Angle_flagX = false;
}
}
+
}
#endif
@@ -724,14 +730,20 @@
}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;
@@ -740,7 +752,7 @@
Angle += rotateY;
}
Angle = Angle /20;
- pc.printf("Y:%f \r\n",Angle);
+ pc.printf("Y:%d \r\n",Angle);
}
#endif
@@ -772,18 +784,19 @@
Angle += rotateY;
}
Angle = Angle /20;
+
int gyropwmX = gyro.SetPV(Angle,AngletargetX);
int gyropwmY = gyro.SetPV(Angle,AngletargetY);
if(controller->Button.X && !Xnopush){
Angle_flagX = true;
Xnopush = true;
- }else{Xnopush = false;}
+ }else if(!controller->Button.X)Xnopush = false;
if(controller->Button.Y && !Ynopush){
Angle_flagY = true;
Ynopush = true;
- }else{Ynopush = false;}
+ }else if(!controller->Button.Y)Ynopush = false;
if (Angle_flagX){
motor[Angle_R].dir = SetStatus(gyropwmX);
@@ -793,18 +806,22 @@
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].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;
}
}
@@ -830,7 +847,10 @@
else{
motor[Angle_R].dir = BRAKE;
motor[Angle_L].dir = BRAKE;
+ motor[Angle_R].pwm = 255;
+ motor[Angle_L].pwm = 255;
}
+ //pc.printf("%d \r\n",gyropwmY);
}
#endif
@@ -852,7 +872,7 @@
if(controller->Button.B && !nopushed){
Rt_flagY = true;
nopushed = true;
- }else{nopushed = false;}
+ }else if(!controller->Button.B)nopushed = false;
if (Rt_flagY && SetPWM(RtpwmY) > 0){
@@ -874,6 +894,11 @@
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
{
@@ -881,14 +906,18 @@
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);
+ //pc.printf("PWM:%d \r\n", RtpwmY);
+ //pc.printf("回転数:%f \r\n" ,rpmY);
+ //pc.printf("距離:%f \r\n", disY);
}
@@ -1080,19 +1109,54 @@
#if USE_PROCESS_NUM>7
static void Process7()
{
- //et_rpm.attach_us(&filip,1000);
+ static bool nopushed = false;
+ static bool Rt_flagX = false;
- disX = 48*3.141*rpmX;
- disY = 48*3.141*rpmY;
+ if(controller->Button.A && !nopushed){
+ Rt_flagX = true;
+ nopushed = true;
+ }else if(!controller->Button.A)nopushed = false;
+
- 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);*/
+ 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);
+ 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
@@ -1150,10 +1214,10 @@
rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
- //disX = 48*3.141592*rpmX;
+ disX = 48*3.141*rpmX;
disY = 48*3.141*rpmY;
- //RtpwmX = (int)Rt_X.SetPV(disX , goalX);
+ RtpwmX = (int)Rt_X.SetPV(disX , goalX);
RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
}