![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
uuu
Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ
Diff: System/Process/Process.cpp
- 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