大季 矢花
da
Dependencies: mbed TrapezoidControl 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(){