Taiki Maruyama / Mbed 2 deprecated MainBoard2018_Auto_Master_A_final

aaaaaaaaa

Dependencies:   mbed QEI

Diff: System/Process/Process.cpp

Revision:
7:e88c5d47a3be
Parent:
6:10e22bc327ce
Child:
8:6fb3723f7747
--- a/System/Process/Process.cpp	Mon Oct 01 14:01:03 2018 +0000
+++ b/System/Process/Process.cpp	Mon Oct 01 14:45:50 2018 +0000
@@ -59,13 +59,20 @@
     return result;
 }
 
-#define TIRE_FR 0 //足回り前右
-#define TIRE_FL 1 //足回り前左
+#define TIRE_FR 4 //足回り前右
+#define TIRE_FL 5 //足回り前左
 #define TIRE_BR 2 //足回り後右
 #define TIRE_BL 3 //足回り後左
 
-#define Angul_R 4 //角度調節右
-#define Angul_L 5 //角度調節左
+#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]=
 {
@@ -100,17 +107,25 @@
 	else return abs(pwmVal);
 }
 
+//************メカナム********************
+
+//************カラーセンサ********************
+
 int Color_A[3]; //[赤,緑,青]
 int Color_B[3];
 int Color_C[3];
 int Color_D[3];         
 int intergration = 50;
 
+void ColorDetection();
+
+//************カラーセンサ********************
+
 //************ライントレース変数*******************
-	int Point[3] = {234, 466, 590};//赤，緑，青
+int Point[3] = {234, 466, 590};//赤，緑，青
 	
-	int startP = 35;
-	int downP = 5;
+int startP = 35;
+int downP = 5;
 //************ライントレース変数*******************
 //ROタコン
  QEI wheel(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X2_ENCODING);
@@ -122,22 +137,21 @@
  
  PID startup = PID(0.03, -255, 255, 0.3, 0, 0);
  
-//
+
+
 
-int averageR_A;
-int averageG_A;
-int averageB_A;
-int averageR_B;
-int averageG_B;
-int averageB_B;
-int averageR_C;
-int averageG_C;
-int averageB_C;
-int averageR_D;
-int averageG_D;
-int averageB_D;
+//************ジャイロ*******************
+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 Angle_I = -5;
+//************ジャイロ*******************
 
-void ColorDetection();
 
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
@@ -325,11 +339,40 @@
 #if USE_PROCESS_NUM>0
 static void Process0()
 {
-	ColorDetection();
-	pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d \r\n",Color_A[0],Color_A[1],Color_A[2]);
-	pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d \r\n",Color_B[0],Color_B[1],Color_B[2]);
-	pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d \r\n",Color_C[0],Color_C[1],Color_C[2]);
-	pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d \r\n",Color_D[0],Color_D[1],Color_D[2]);
+	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_L].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;
+		y = acc[1]*1000;
+		float rotateY = (y - 305)/2.21 - 90;
+		Angle += rotateY;
+	}
+	Angle = Angle /20;
+	int gyropwm = gyro.SetPV(Angle,Angle_I);
+	
+	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){
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+		Angle_flagI = false;
+		}
+	}
 }
 #endif
 
@@ -343,14 +386,14 @@
     motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]); 
     motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]);
          
-    motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
-   	motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
-    motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
-    motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
+    motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X])    *0.8;
+   	motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X])       *0.8;
+    motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
+    motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y])    *0.8;
                   
     if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
-    motor[TIRE_FR].pwm = motor[0].pwm * 1.3;
-    motor[TIRE_FL].pwm = motor[1].pwm * 1.3;
+    motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
+    motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
     }
     
     
@@ -468,7 +511,7 @@
 		
 	if(!traceon && yokofla && !boxslip)
 	 {
-		if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4))
+		if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
 		{
 		motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
@@ -544,7 +587,7 @@
 		
 	if(!traceon && !yokofla && boxslip)
 	 {
-	 	if(LimitSw::IsPressed(3) && LimitSw::IsPressed(4))
+	 	if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
 	 	{
 	 	motor[TIRE_FR].dir = FOR;
 		motor[TIRE_FL].dir = FOR;
@@ -556,7 +599,7 @@
 		motor[TIRE_BR].pwm = startP;
 		motor[TIRE_BL].pwm = startP;
 	 	}
-	 	else if(!LimitSw::IsPressed(3) && !LimitSw::IsPressed(4))
+	 	else if(!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
 	 	{
 	 	motor[TIRE_FR].dir = BRAKE;
 		motor[TIRE_FL].dir = BRAKE;
@@ -593,32 +636,32 @@
 static void Process3()
 {
 	if(controller->Button.R){
-	    motor[Angul_R].dir = FOR;
-	    motor[Angul_L].dir = BACK;
-	    motor[Angul_R].pwm = 150;
-	    motor[Angul_L].pwm = 150;
+	    motor[Angle_R].dir = FOR;
+	    motor[Angle_L].dir = BACK;
+	    motor[Angle_R].pwm = 150;
+	    motor[Angle_L].pwm = 150;
 	}else if(controller->Button.L){
-	    motor[Angul_R].dir = BACK;
-	    motor[Angul_L].dir = FOR;
-	    motor[Angul_R].pwm = 150;
-	    motor[Angul_L].pwm = 150;
+	    motor[Angle_R].dir = BACK;
+	    motor[Angle_L].dir = FOR;
+	    motor[Angle_R].pwm = 150;
+	    motor[Angle_L].pwm = 150;
 	}else{
-		motor[Angul_R].dir = BRAKE;
-	    motor[Angul_L].dir = BRAKE;
+		motor[Angle_R].dir = BRAKE;
+	    motor[Angle_L].dir = BRAKE;
 	}
 	
-	if(LimitSw::IsPressed(0) && motor[4].dir == FOR && motor[5].dir == BACK){
-	    motor[Angul_R].dir = BRAKE;
-	    motor[Angul_L].dir = BRAKE;
+	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[Angul_R].pwm = 255;
-	    motor[Angul_L].pwm = 255;
-	}else if(LimitSw::IsPressed(1) && motor[4].dir == BACK && motor[5].dir == FOR){
-	    motor[Angul_R].dir = BRAKE;
-	    motor[Angul_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[Angul_R].pwm = 255;
-	    motor[Angul_L].pwm = 255;
+	    motor[Angle_R].pwm = 255;
+	    motor[Angle_L].pwm = 255;
 	}
 }
 #endif
@@ -627,40 +670,94 @@
 static void Process4()
 {
 	
+	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;
+		y = acc[1]*1000;
+		float rotateY = (y - 305)/2.21 - 90;
+		Angle += rotateY;
+	}
+	Angle = Angle /20;
+	int gyropwmX = gyro.SetPV(Angle,AngletargetX);
+	int gyropwmY = gyro.SetPV(Angle,AngletargetY);
+	
+	if(controller->Button.X){
+		Angle_flagX = true;
+	}
+	if(controller->Button.Y){
+		Angle_flagY = true;
+	}
+	
+	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_flagX = false;
+		}
+	}
+	
+	if (Angle_flagY){
+		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;
+		Angle_flagY = false;
+		}
+	}
+	/*float y = 0;
+    y = acc[1]*1000;
+	float rotateY = (y - 305)/2.21 - 90;	
+	int gyropwm = gyro.SetPV(rotateY , Angletarget);
+	
+	if(controller->Button.X){
+		Angle_flag = true;
+	}
+	if (Angle_flag){
+		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(Angletarget - 2 < rotateY && rotateY < Angletarget + 2){
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+		Angle_flag = false;
+		}
+	}*/
+	else{
+		motor[Angle_R].dir = BRAKE;
+		motor[Angle_L].dir = BRAKE;
+	}
 }
 #endif
 
 #if USE_PROCESS_NUM>5
 static void Process5()
 {
-	pc.printf("X:1.3% , Y:1.3%f , Z:1.3%f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
-	//int rotateX = (acc[0].read()-)/ -90;
-	//int rotateY = (acc[1].read()-)/ -90;
-	//pc.printf("X:%d ,Y:%d", rotateX, rotateY);
-	wait_ms(50);
+	
 }
 #endif
 
 #if USE_PROCESS_NUM>6
 static void Process6()
 {
-	float x = 0, y= 0, z = 0;
-	   	
-	pc.printf("X:%1.3f , Y:%1.3f , Z:%1.3f \r\n",acc[0].read(),acc[1].read(),acc[2].read());
-	    
-	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;
-	pc.printf("X:%3.1f , Y:%3.1f \r\n" , rotateX , rotateY);
-	wait_ms(50);
-	
-	/*void Anglecontrol(){
-		if(rotateX>) && */
+
 }
 #endif