The last version programs
Dependencies: mbed TrapezoidControl Pulse QEI
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(){