大季 矢花 / Mbed 2 deprecated MainBoard2019_Master_9_16

da

Dependencies:   mbed TrapezoidControl QEI

Diff: System/Process/Process.cpp

Revision:
10:1295d39fec3a
Parent:
9:f93fc79a49ea
Child:
11:028a150943b5
--- 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);
 }