Taiki Maruyama / Mbed 2 deprecated MainBoard2018_Auto_Master_A_final

aaaaaaaaa

Dependencies:   mbed QEI

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