Diff: System/Process/Process.cpp

Revision:

11:028a150943b5

Parent:

10:1295d39fec3a

Child:

12:c09b3e08a316

--- a/System/Process/Process.cpp	Fri Oct 05 12:17:21 2018 +0000
+++ b/System/Process/Process.cpp	Sat Oct 06 08:30:58 2018 +0000
@@ -42,23 +42,6 @@
 
 Serial pc(USBTX, USBRX);
 
-unsigned long ColorIn(int index)
-{
-	int result = 0;
-	bool rtn = false;
-    for(int i=0; i<12; i++)
-    {
-        CK[index] = 1;
-        rtn = DOUT[index];
-        CK[index] = 0;
-        if(rtn)
-        {
-           result|=(1 << i);
-        }
-    }
-    return result;
-}
-
 #define TIRE_FR 0 //足回り前右
 #define TIRE_FL 1 //足回り前左
 #define TIRE_BR 2 //足回り後右
@@ -73,6 +56,8 @@
 #define Lim_L  1 //センター左
 #define EMS_0  LimitSw::IsPressed(8) 
 #define EMS_1  LimitSw::IsPressed(9)
+#define LS     LimitSw::IsPressed(7) //赤ゾーン用スイッチ
+#define BS     LimitSw::IsPressed(6) //青ゾーン用スイッチ
 //************メカナム********************
 
 const int mecanum[15][15]=
@@ -118,6 +103,22 @@
 int Color_D[3];         
 int intergration = 50;
 
+unsigned long ColorIn(int index)
+{
+	int result = 0;
+	bool rtn = false;
+    for(int i=0; i<12; i++)
+    {
+        CK[index] = 1;
+        rtn = DOUT[index];
+        CK[index] = 0;
+        if(rtn)
+        {
+           result|=(1 << i);
+        }
+    }
+    return result;
+}
 void ColorDetection();
 
 //************カラーセンサ********************
@@ -163,21 +164,30 @@
 //************ジャイロ*******************
 
 bool Angle_flagI = false;
-float Angle;
+int Angle;
 PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
-
 float rotateY;
-int AngletargetX = 18;
-int AngletargetY = -35;
+//初期値 -5
+int AngletargetX = 4; 
+int AngletargetY = -12;
 int Angle_I = -5;
 //************ジャイロ*******************
 
 //************Buzzer******************
-DigitalOut buzzer(BUZZER_PIN);
+//DigitalOut buzzer(BUZZER_PIN);
+PwmOut buzzer(BUZZER_PIN);
 void BuzzerTimer_func();
 Ticker BuzzerTimer;
 bool Emsflag = false;
 //************Buzzer******************
+
+//************TapeLed*****************
+void TapeLedEms_func();
+TapeLedData tapeLED;
+TapeLedData sendLedData;
+TapeLED_Mode ledMode = Normal;
+Ticker tapeLedTimer;
+//************TapaLed*****************
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
 #ifdef USE_SUBPROCESS
@@ -216,10 +226,10 @@
 void SystemProcessInitialize()
 {
 	#pragma region USER-DEFINED_VARIABLE_INIT
+	/*Replace here with the initialization code of your variables.*/
 	get_rpm.attach_us(&filip,100);
+	buzzer.period(1.0/800);
 	
-	/*Replace here with the initialization code of your variables.*/
-
 	#pragma endregion USER-DEFINED_VARIABLE_INIT
 
 	lock = true;
@@ -302,65 +312,6 @@
 
 	while(1)
 	{ 
-	/*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){
-	Rt_flagX = true;
-	}
-	Rt_flagY = true;
-	if (Rt_flagY){
-		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_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){
-		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){
-		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("%f \r\n",RtpwmX);
-	wait_ms(50);
-	
-	
-		/*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON;
-		else LED_DEBUG0 = LED_OFF;*/
-
 		#ifdef USE_MU
 		controller = CONTROLLER::Controller::GetData();
 		#endif
@@ -389,15 +340,33 @@
 		}
 		
 		if ((EMS_0 || EMS_1) && !Emsflag){
-			buzzer = 1;
+			buzzer = 0.5;
 			BuzzerTimer.attach(BuzzerTimer_func, 1.2);
 			Emsflag = true;
+			ledMode = EMS;
+			current = 0;
+			tapeLedTimer.attach(TapeLedEms_func, 1.2);
+			sendLedData.code = (uint32_t)Red;
 		}
 		
 		if(!EMS_0 && !EMS_1) {
 			buzzer = 0;
 			BuzzerTimer.detach();
 			Emsflag = false;
+			if(ledMode == EMS) ledMode = Normal;
+			tapeLedTimer.detach();
+		}
+		
+		switch(ledMode)
+		{
+			case EMS :
+				break;
+
+			case Normal :
+				sendLedData.code = tapeLED.code;
+
+			default:
+				break;
 		}
 		
 		SystemProcessUpdate();
@@ -455,6 +424,12 @@
 		}
 	}
 	
+	else{
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+		motor[Angle_R].pwm = 255;
+	    motor[Angle_L].pwm = 255;
+	}
 }
 #endif
 
@@ -751,7 +726,7 @@
 		float rotateY = (y - 305)/2.21 - 90;
 		Angle += rotateY;
 	}
-	Angle = Angle /20;
+	Angle = Angle/20;
 	pc.printf("Y:%d \r\n",Angle);
 }
 #endif
@@ -1156,7 +1131,7 @@
 		motor[TIRE_BR].pwm = 255;
 		motor[TIRE_BL].pwm = 255;
 	}
-	pc.printf("X:%d \r\n",RtpwmX);
+	//pc.printf("X:%d \r\n",RtpwmX);
 }
 #endif
 
@@ -1262,6 +1237,15 @@
     Color_D[2] = ColorIn(3);
 }
 void BuzzerTimer_func() {
-	buzzer = !buzzer;
+	if(buzzer == 0.5){
+		buzzer = 0;
+ 	}
+ 	else if(buzzer == 0){
+ 		buzzer = 0.5;
+ 	}
+}
+
+void TapeLedEms_func() {
+	sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
 }
 #pragma endregion