Diff: System/Process/Process.cpp

Revision:

15:dfcec98f5aa9

Parent:

13:b6e02d6261d7

--- a/System/Process/Process.cpp	Mon Oct 08 15:51:15 2018 +0000
+++ b/System/Process/Process.cpp	Sun Oct 21 02:14:15 2018 +0000
@@ -145,7 +145,11 @@
 int RtpwmY; 
 double goalX = 900.000;
 double goalY = 700.000; 
+
+//double goalXB = 900.000;
+//double goalYB = 700.000;
 void filip();
+void filipB();
 
 //************ROタコン****************** 
 
@@ -298,13 +302,13 @@
 	SystemProcessInitialize();
 
 	while(1)
-	{ /*
+	{
 		getcolor();
 		pc.printf("R1:%d, G1:%d, B1:%d \r\n",Avecolor_A[0],Avecolor_A[1],Avecolor_A[2]);
 		pc.printf("R2:%d, G2:%d, B2:%d \r\n",Avecolor_B[0],Avecolor_B[1],Avecolor_B[2]);
 		pc.printf("R3:%d, G3:%d, B3:%d \r\n",Avecolor_C[0],Avecolor_C[1],Avecolor_C[2]);
 		pc.printf("R4:%d, G4:%d, B4:%d \r\n",Avecolor_D[0],Avecolor_D[1],Avecolor_D[2]);
-		*/
+		
 		#ifdef USE_MU
 		controller = CONTROLLER::Controller::GetData();
 		#endif
@@ -315,6 +319,8 @@
 			CONTROLLER::Controller::DataReset();
 			AllActuatorReset();
 			lock = true;
+			buzzer = 0.5;
+			BuzzerTimer.attach(BuzzerTimer_func, 0.5);
 		}
 		else
 		#endif
@@ -639,15 +645,80 @@
 #endif
 
 #if USE_PROCESS_NUM>3
-static void Process3() 
+static void Process3() //Blue Zone
 {
-
+	filipB();
+	
+	static bool Rt_flagX = false;
+	static bool Rt_flagY = false;
+	
+	if(Rt_flagX)
+	{
+	  filipB();
+		if(disX < goalX - 5){
+		filipB();
+		
+		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)*0.8;
+		motor[TIRE_FL].pwm = SetPWM(RtpwmX);
+		motor[TIRE_BR].pwm = SetPWM(RtpwmX);
+		motor[TIRE_BL].pwm = SetPWM(RtpwmX);
+	}
+	else if(disX > goalX - 5){
+		
+		for(int i = 0; i<200; i++){
+		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;
+		}
+		
+		Rt_flagY = true;
+		Rt_flagX = false;
+	}
+  }
+	
+if(Rt_flagY && !Rt_flagX){
+	filipB();
+	 if(disY < goalY - 5){
+		filipB();
+		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);
+	}
+	else if(disY > goalY - 5)
+	{
+		filipB();
+		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*0.85;
+		motor[TIRE_FL].pwm = 255;
+		motor[TIRE_BR].pwm = 255;
+		motor[TIRE_BL].pwm = 255;
+	}
+  }
 }
 #endif
 
 #if USE_PROCESS_NUM>4
 static void Process4() 
 {	
+
 }
 #endif
 
@@ -1184,6 +1255,20 @@
 	RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
 }
 
+void filipB(){
+   	palseX = RtX.getPulses();
+   	palseY = RtY.getPulses();
+   	
+   	rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
+   	rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
+   	
+   	disX = abs(48*3.141*rpmX);
+	disY = 48*3.141*rpmY;
+	
+	RtpwmX = (int)Rt_X.SetPV(disX , goalX);
+	RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
+}
+
 unsigned long ColorIn(int index)
 {
 	int result = 0;