The last version programs

Dependencies:   mbed TrapezoidControl Pulse QEI

Revision:
2:c015739085d3
Parent:
1:b1219d8ca117
Child:
3:e10d8736fd22
--- a/System/Process/Process.cpp	Mon Sep 10 01:39:15 2018 +0000
+++ b/System/Process/Process.cpp	Sat Sep 22 10:55:22 2018 +0000
@@ -13,7 +13,10 @@
 #include "../../Safty/Safty.h"
 #include "../Using.h"
 
+
 using namespace SWITCH;
+using namespace COLORSENSOR;
+using namespace ACCELERATIONSENSOR;
 
 static CONTROLLER::ControllerData *controller;
 ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
@@ -33,6 +36,104 @@
 
 /*Replace here with the definition code of your variables.*/
 
+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 TILE_FR 0 //足回り前右
+#define TILE_FL 1 //足回り前左
+#define TILE_BR 2 //足回り後右
+#define TILE_BL 3 //足回り後左
+
+#define Anguladjust_R 4 //角度調節右
+#define Anguladjust_L 5 //角度調節左
+
+const int mecanum[15][15]=
+{
+	{   0,    5,     21,     47,     83,    130,    187,    255,    255,    255,    255,    255,    255,    255,    255},
+	{  -5,    0,      5,     21,     47,     83,    130,    187,    193,    208,    234,    255,    255,    255,    255},
+	{ -21,   -5,      0,      5,     21,     47,     83,    130,    135,    151,    177,    213,    255,    255,    255},
+	{ -47,   -21,     5,      0,      5,     21,     47,     83,     88,    104,    130,    167,    213,    255,    255},
+	{ -83,   -47,    -21,     5,      0,      5,     21,     47,     52,     68,     94,    130,    177,    234,    255},
+	{-130,   -83,    -47,    -21,     5,      0,      5,     21,     26,     42,     68,    104,    151,    208,    255},
+	{-187,  -130,    -83,    -47,    -21,    -5,      0,      5,     10,     26,     52,     88,    135,    193,    255},
+	{-255,  -187,   -130,    -83,    -47,    -21,    -5,      0,      5,     21,     47,     83,    130,    187,    255},
+	{-255,  -193,   -135,    -88,    -52,    -26,    -10,    -5,      0,      5,     21,     47,     83,    130,    187},
+	{-255,  -208,   -151,   -104,    -68,    -42,    -26,    -21,    -5,      0,      5,     21,     47,     83,    130},
+	{-255,  -234,   -177,   -130,    -94,    -68,    -52,    -47,    -21,    -7,      0,      7,     21,     47,     83},
+	{-255,  -255,   -213,   -167,   -130,   -104,    -88,    -83,    -47,    -21,    -5,      0,      5,     21,     47},
+	{-255,  -255,   -255,   -213,   -177,   -151,   -135,   -130,    -83,    -47,    -21,    -5,      0,      5,     21},
+	{-255,  -255,   -255,   -255,   -234,   -208,   -193,   -187,   -130,    -83,    -47,    -21,    -5,      0,      5},
+	{-255,  -255,   -255,   -255,   -255,   -255,   -255,   -255,   -187,   -130,    -83,    -47,   -21,     -5,      0}
+};
+
+const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
+uint8_t SetStatus(int);
+uint8_t SetStatus(int pwmVal){
+	if(pwmVal < 0) return BACK;
+	else if(pwmVal > 0) return FOR;
+	else if(pwmVal == 0) return BRAKE;
+	else return BRAKE;
+}
+uint8_t SetPWM(int);
+uint8_t SetPWM(int pwmVal){
+	if(pwmVal == 0 || pwmVal >  255 || pwmVal < -255) return 255;
+	else return abs(pwmVal);
+}
+
+int Color_A[3]; //[赤,緑,青]
+int Color_B[3];
+int Color_C[3];
+int Color_D[3];         
+int intergration = 50;
+
+//************ライントレース変数*******************
+	int Point[3] = {234, 466, 590};//赤,緑,青
+	
+	int startP = 150;
+	int downP = 70;
+	
+	bool compA = false;
+	bool compB = false;
+	bool compC = false;
+	bool compD = false;
+	
+	bool invationA = false;
+	bool invationB = false;
+	bool invationC = false;
+	bool invationD = false;
+//************ライントレース変数*******************
+
+int averageR_0;
+int averageG_0;
+int averageB_0;
+int averageR_1;
+int averageG_1;
+int averageB_1;
+int averageR_2;
+int averageG_2;
+int averageB_2;
+int averageR_3;
+int averageG_3;
+int averageB_3;
+
+void ColorDetection();
+
 #pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
 
 #ifdef USE_SUBPROCESS
@@ -148,12 +249,29 @@
 	
 }
 
+
+
 void SystemProcess()
 {
 	SystemProcessInitialize();
 
 	while(1)
-	{
+	{        	
+	   	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);
+
 		#ifdef USE_MU
 		controller = CONTROLLER::Controller::GetData();
 		#endif
@@ -185,47 +303,147 @@
 	}
 }
 
+
+	
+
 #pragma region PROCESS
 #ifdef USE_SUBPROCESS
 #if USE_PROCESS_NUM>0
 static void Process0()
 {
-
+	ColorDetection();
 }
 #endif
 
 #if USE_PROCESS_NUM>1
 static void Process1()
 {
-
+    motor[0].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]     + curve[controller->AnalogR.X]) * 0.8;
+    motor[1].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X]         + curve[controller->AnalogR.X]) * 0.8;
+    motor[2].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]  + curve[controller->AnalogR.X]) * 0.8; 
+    motor[3].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]      + curve[controller->AnalogR.X]) * 0.8;
+         
+    motor[0].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]);
+   	motor[1].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]);
+    motor[2].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]);
+    motor[3].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]);
+                  
+    if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
+    motor[0].pwm = motor[0].pwm * 1.3;
+    motor[1].pwm = motor[1].pwm * 1.3;
+    
+    }
 }
 #endif
 
+bool buttoncomp = false;
 #if USE_PROCESS_NUM>2
 static void Process2()
 {
-
+	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(controller->Button.A && buttoncomp = false)
+	{
+		motor[0].dir = dir;
+		motor[0].pwm = startP;
+	}
+	
+	if(invationA)
+	{
+	   motor[0].PWM = startP
+		
+	}
+	
+	
+	
+	
 }
 #endif
 
 #if USE_PROCESS_NUM>3
 static void Process3()
 {
-
+	if(controller->Button.R){
+	    motor[4].dir = FOR;
+	    motor[5].dir = BACK;
+	    motor[4].pwm = 150;
+	    motor[5].pwm = 150;
+	}else if(controller->Button.L){
+	    motor[4].dir = BACK;
+	    motor[5].dir = FOR;
+	    motor[4].pwm = 150;
+	    motor[5].pwm = 150;
+	}else{
+	    motor[4].dir = BRAKE;
+	    motor[5].dir = BRAKE;
+	}  
+	if(LimitSw::IsPressed(0)){
+	    motor[4].dir = BRAKE;
+	    motor[5].dir = BRAKE;
+	}else if(LimitSw::IsPressed(1)){
+	    motor[4].dir = BRAKE;
+	    motor[5].dir = BRAKE;
+	} 
 }
 #endif
 
 #if USE_PROCESS_NUM>4
 static void Process4()
 {
-
+	//ColorDetection();
+	
+    for(int i=0;i<=10;i++)
+    {
+    	ColorDetection();
+    	
+    	averageR_0 += Color_A[0];
+    	averageG_0 += Color_A[1];
+    	averageB_0 += Color_A[2];
+    	averageR_1 += Color_B[0];
+    	averageG_1 += Color_B[1];
+    	averageB_1 += Color_B[2];
+    	averageR_2 += Color_C[0];
+    	averageG_2 += Color_C[1];
+    	averageB_2 += Color_C[2];
+    	averageR_3 += Color_D[0];
+    	averageG_3 += Color_D[1];
+    	averageB_3 += Color_D[2];
+    }
+    pc.printf("AR_0:%d, AG_0:%d ,AB_0:%d \r\n",averageR_0 / 10 ,averageG_0 / 10, averageB_0 / 10);
+    pc.printf("AR_1:%d, AG_1:%d ,AB_1:%d \r\n",averageR_1 / 10 ,averageG_1 / 10, averageB_1 / 10);
+    pc.printf("AR_2:%d, AG_2:%d ,AB_2:%d \r\n",averageR_2 / 10 ,averageG_2 / 10, averageB_2 / 10);
+    pc.printf("AR_3:%d, AG_3:%d ,AB_3:%d \r\n",averageR_3 / 10 ,averageG_3 / 10, averageB_3 / 10);
+	
+	averageR_0 = 0;
+    averageG_0 = 0;
+	averageB_0 = 0;
+	averageR_1 = 0;
+    averageG_1 = 0;
+	averageB_1 = 0;
+	averageR_2 = 0;
+    averageG_2 = 0;
+	averageB_2 = 0;
+	averageR_3 = 0;
+    averageG_3 = 0;
+	averageB_3 = 0;
 }
 #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
 
@@ -276,5 +494,55 @@
 }
 
 #pragma region USER-DEFINED-FUNCTIONS
-
+void ColorDetection(){
+	GATE = 0;
+		   
+	CK[0] = 0;
+	CK[1] = 0;
+	CK[2] = 0;
+	CK[3] = 0;
+        
+    RANGE = 1;
+        
+    GATE = 1;
+    wait_ms(intergration);
+    GATE = 0;
+    wait_us(4);
+        
+	Color_A[0] = ColorIn(0); //赤
+    wait_us(3);
+	Color_A[1] = ColorIn(0); //青
+    wait_us(3);
+    Color_A[2] = ColorIn(0); //緑
+        
+    //pc.printf("Red_0=%d , Green_0=%d , Blue_0=%d",Color_A[0],Color_A[1],Color_A[2]);
+    //pc.printf("\r\n");
+        
+    Color_B[0] = ColorIn(1);
+    wait_us(3);
+    Color_B[1] = ColorIn(1);
+    wait_us(3);
+    Color_B[2] = ColorIn(1);
+        
+    //pc.printf("Red_1=%d , Green_1=%d , Blue_1=%d",Color_B[0],Color_B[1],Color_B[2]);
+    //pc.printf("\r\n");
+         
+    Color_C[0] = ColorIn(2);
+    wait_us(3);
+    Color_C[1] = ColorIn(2);
+    wait_us(3);
+    Color_C[2] = ColorIn(2);
+        
+    /*pc.printf("Red_2=%d , Green_2=%d , Blue_2=%d",Color_C[0],Color_C[1],Color_C[2]);
+    pc.printf("\r\n");*/
+         
+    Color_D[0] = ColorIn(3);
+    wait_us(3);
+    Color_D[1] = ColorIn(3);
+    wait_us(3);
+    Color_D[2] = ColorIn(3);
+        
+    /*pc.printf("Red_3=%d , Green_3=%d , Blue_3=%d",Color_D[0],Color_D[1],Color_D[2]);
+    pc.printf("\r\n");*/
+}
 #pragma endregion