Diff: System/Process/Process.cpp

Revision:

8:6fb3723f7747

Parent:

7:e88c5d47a3be

Child:

9:f93fc79a49ea

--- a/System/Process/Process.cpp	Mon Oct 01 14:45:50 2018 +0000
+++ b/System/Process/Process.cpp	Tue Oct 02 12:18:05 2018 +0000
@@ -67,11 +67,11 @@
 #define Angle_R 0 //角度調節右
 #define Angle_L 1 //角度調節左
 
+
 #define Lim_AR 3 //角度調節右
 #define Lim_AL 4 //角度調節左
 #define Lim_R  0 //センター右
 #define Lim_L  1 //センター左
-
 //************メカナム********************
 
 const int mecanum[15][15]=
@@ -126,19 +126,39 @@
 	
 int startP = 35;
 int downP = 5;
+
+int Asasult = 0;
+int Bsasult = 0;
+int Csasult = 0;
+int Dsasult = 0;
+
+void pointcalculation();
+
+Ticker Color_T;
 //************ライントレース変数*******************
 //ROタコン
- QEI wheel(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING);
+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);
+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;
+int palseX;
+int palseY;
+int RtpwmX;
+int RtpwmY; 
+double goalX = 1200.000;
+double goalY = 900.000; 
+
+void filip();
+//PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
  
- Ticker get_rpm;
- int rpm;
- int palse;
- double goalpoint = 3000.0000;
- 
- PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
- 
-
-
+//ROタコン 
 
 //************ジャイロ*******************
 float Angle;
@@ -275,28 +295,62 @@
 	SystemProcessInitialize();
 
 	while(1)
-	{     
-	   /*wait(0.1);
-	   //wheel.getPulses()...どちらの方向にどれだけ回ったか
-	   pc.printf("Pulses:%07d \r\n",wheel.getPulses());
-	   //軸が何回転したか
-	   pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*2));
-	   */
-	   
-	   /*	float x = 0, y= 0, z = 0;
-	    
-	    x = acc[0]*1000;
-	    y = acc[1]*1000;
-	    z = acc[2]*1000;
-	    
-	    pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n",x,y,z);
-	    
-	    float rotateX = (x - 306)/2.22 - 90;
-	    float rotateY = (y - 305)/2.21 - 90;
-	    float rotateZ = (z - 300)/1.18 - 90;
-	    pc.printf("X:%3.1f , Y:%3.1f , Z:%3.1f \r\n" , rotateX , rotateY , rotateZ);
-		wait_ms(50);
-		*/
+	{    
+	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 - 5 < disY && disY < goalY + 5){
+		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 - 5 < disX && disX < goalX + 5){
+		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("%d \r\n",RtpwmX);
+	wait_ms(50);
+	
 		/*if(LimitSw::IsPressed(9)) LED_DEBUG0 = LED_ON;
 		else LED_DEBUG0 = LED_OFF;*/
 
@@ -398,9 +452,9 @@
     
     
 	   //wheel.getPulses()...どちらの方向にどれだけ回ったか
-	   pc.printf("Pulses:%07d \r\n",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*2));
 }
 #endif
 
@@ -438,7 +492,7 @@
 	compB = true;//on true,noon false
 	}	
 	else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
-	
+	/*
 	if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
 	{
 	invationC ^= 1;//start false,over true
@@ -452,7 +506,7 @@
 	compD = true;//on true,noon false
 	}	
 	else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
-
+    */
 
 	//
 	
@@ -653,15 +707,9 @@
 	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;
 	}
 }
 #endif
@@ -750,14 +798,250 @@
 #if USE_PROCESS_NUM>5
 static void Process5()
 {
+	   /*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));
+	   */
+	/* 
+	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_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;
+	}*/
 }
 #endif
 
 #if USE_PROCESS_NUM>6
 static void Process6()
 {
+	/*static bool color_flag = false;
+	
+	static bool traceon = false;//fase1
+	static bool yokofla = false;//fase2
+	static bool boxslip = false;//fase3
+	
+	static bool syu = false;
+	
+	static bool compA = false;
+	static bool compB = false;
+	static bool compC = false;
+	static bool compD = false;
+	
+	static bool invationA = false;
+	static bool invationB = false;
+	static bool invationC = false;
+	static bool invationD = false;
+	
+	
+	ColorDetection();
+    
+	//
+	if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
+	{
+	invationA ^= 1;//start false,over true
+	compA = true;//on true,noon false
+	}	
+	else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶
+	
+	if(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2] && !compB)//白
+	{
+	invationB ^= 1;//start false,over true
+	compB = true;//on true,noon false
+	}	
+	else if(!(Color_B[0] > Point[0] && Color_B[1] > Point[1] && Color_B[2] > Point[2]))compB = false;//茶
+	/*
+	if(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2] && !compC)//白
+	{
+	invationC ^= 1;//start false,over true
+	compC = true;//on true,noon false
+	}	
+	else if(!(Color_C[0] > Point[0] && Color_C[1] > Point[1] && Color_C[2] > Point[2]))compC = false;//茶
+	
+	if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
+	{
+	invationD ^= 1;//start false,over true
+	compD = true;//on true,noon false
+	}	
+	else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
+    */
 
+	//
+	/*
+	if(controller->Button.B && !color_flag)
+	{
+		traceon ^= 1;
+		color_flag = true;
+	}
+	else if(!controller->Button.B)color_flag = false;
+	
+	if(traceon && !yokofla && !boxslip && !syu)
+	{
+		if(!invationA && !compA && !invationB && !compB)
+		{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+		}
+	    else if(invationA && compA && !invationB && !compB)
+	    {
+	    motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		wait(2);
+		
+		syu = true;
+		yokofla = false;
+		traceon = false;
+		}
+		else{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		}
+	}
+	
+	pointcalculation();
+	
+	if(syu && !traceon && !yokofla && !boxslip)
+	{
+		if(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10)
+		{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		wait(2);
+		
+		yokofla = true;
+		traceon = false;
+		syu = false;
+		}
+		else if(Asasult < Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10))
+		{
+		motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = FOR;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = FOR;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+		}
+		else if(Asasult > Bsasult && !(Asasult < Bsasult +10 && Asasult > Bsasult -10 || Bsasult < Asasult +10 && Bsasult > Asasult -10))
+		{
+	    motor[TIRE_FR].dir = BACK;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = BACK;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+		}
+		else
+		{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		}
+	}
+	
+	if(!syu && !traceon && yokofla && !boxslip)
+	{
+	    motor[TIRE_FR].dir = FOR;
+		motor[TIRE_FL].dir = BACK;
+		motor[TIRE_BR].dir = FOR;
+		motor[TIRE_BL].dir = BACK;
+		
+		motor[TIRE_FR].pwm = startP;
+		motor[TIRE_FL].pwm = startP;
+		motor[TIRE_BR].pwm = startP;
+		motor[TIRE_BL].pwm = startP;
+	}
+	else if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
+		{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+		
+		wait(2);
+		
+		boxslip = true;
+		yokofla = false;
+	}
+	else
+	{
+		motor[TIRE_FR].dir = BRAKE;
+		motor[TIRE_FL].dir = BRAKE;
+		motor[TIRE_BR].dir = BRAKE;
+		motor[TIRE_BL].dir = BRAKE;
+	}
+	
+	*/
 }
 #endif
 
@@ -801,10 +1085,27 @@
 }
 
 #pragma region USER-DEFINED-FUNCTIONS
-void rotconpulex()
-   {
+void pointcalculation(){
+	ColorDetection();
+	/*if(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2] && !compA)//白
+	{
+	invationA ^= 1;//start false,over true
+	compA = true;//on true,noon false
+	}	
+	else if(!(Color_A[0] > Point[0] && Color_A[1] > Point[1] && Color_A[2] > Point[2]))compA = false;//茶*/
+	for(int i=0;i<3;i++){Asasult += Color_A[i]-Point[i];}
+	for(int i=0;i<3;i++){Bsasult += Color_B[i]-Point[i];}
+	for(int i=0;i<3;i++){Csasult += Color_A[i]-Point[i];}
+	for(int i=0;i<3;i++){Dsasult += Color_B[i]-Point[i];}
+}
+
+void filip(){
+   	palseX = RtX.getPulses();
+   	palseY = RtY.getPulses();
    	
-   }
+   	rpmX = palseX/(ROTATE_PER_REVOLUTIONS*2);
+   	rpmY = palseY/(ROTATE_PER_REVOLUTIONS*2);
+}
 
 
 void ColorDetection(){