大季 矢花
aa
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 14:93776ca449a4
- Parent:
- 13:b6e02d6261d7
--- a/System/Process/Process.cpp Mon Oct 08 15:51:15 2018 +0000
+++ b/System/Process/Process.cpp Thu Oct 25 09:00:19 2018 +0000
@@ -78,7 +78,7 @@
else return abs(pwmVal);
}
-//************メカナム********************
+//****************************************************************************************************
//************カラーセンサ********************
@@ -97,20 +97,12 @@
void ColorDetection();
void getcolor();
-//************カラーセンサ********************
+//***************************************************************************************************************
//************ライントレース変数*******************
-int PointA[3] = {400, 700, 1000};//赤，緑，青
-int PointB[3] = {400, 700, 1000};//赤，緑，青
+int PointA[3] = {350, 600, 800};//赤，緑，青
+int PointB[3] = {350, 600, 800};//赤，緑，青
int PointC[3] = {1000, 1700, 2400};//赤，緑，青
-
-int startP = 35;
-int downP = 5;
-
-int Asasult = 0;
-int Bsasult = 0;
-int Csasult = 0;
-int Dsasult = 0;
bool compA = false;
bool compB = false;
@@ -125,27 +117,30 @@
Ticker Color_T;
void ColorDetection();
-void Color_changeflag();
-
-//************ライントレース変数*******************
+void Color_changeflagA();
+void Color_changeflagB();
+//*****************************************************************************************************
//************ROタコン******************
QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
Ticker get_rpm;
-PID Rt_X = PID(0.03, -255, 255, 0.12, 0, 0);
-PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0);
-double rpmX;
-double rpmY;
-double disX;
-double disY;
+PID Rt_X = PID(0.03, -255, 255, 0.15, 0, 0);
+PID Rt_Y = PID(0.03, -255, 255, 0.17, 0, 0);
+double rpmX = 0, rpmXB =0;
+double rpmY = 0, rpmYB =0;
+double disX =0,disXB =0;
+double disY =0,disYB =0;
int palseX;
int palseY;
-int RtpwmX;
-int RtpwmY;
-double goalX = 900.000;
-double goalY = 700.000;
+int RtpwmX =0,RtpwmXB =0;
+int RtpwmY =0,RtpwmYB =0;
+double goalAX = 1000.000;
+double goalAY = 850.000;
+double goalBX = -430.000;
+double goalBY = 1000.000;
void filip();
+void filipB();
//************ROタコン******************
@@ -165,6 +160,7 @@
PwmOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
+Ticker LostTimer;
bool Emsflag = false;
//************Buzzer******************
@@ -174,6 +170,8 @@
TapeLedData sendLedData;
TapeLED_Mode ledMode = Normal;
Ticker tapeLedTimer;
+Ticker LostLedTimer;
+//void LostLed_func();
//************TapaLed*****************
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
@@ -298,13 +296,24 @@
SystemProcessInitialize();
while(1)
- { /*
+ {
+ /*
getcolor();
pc.printf("R1:%d, G1:%d, B1:%d \r\n",Avecolor_A[0],Avecolor_A[1],Avecolor_A[2]);
pc.printf("R2:%d, G2:%d, B2:%d \r\n",Avecolor_B[0],Avecolor_B[1],Avecolor_B[2]);
pc.printf("R3:%d, G3:%d, B3:%d \r\n",Avecolor_C[0],Avecolor_C[1],Avecolor_C[2]);
pc.printf("R4:%d, G4:%d, B4:%d \r\n",Avecolor_D[0],Avecolor_D[1],Avecolor_D[2]);
+
+ palseX = RtX.getPulses();
+ palseY = RtY.getPulses();
+
+ pc.printf("X:%d",palseX);
+ pc.printf("Y:%d",palseY);
*/
+ //wheel.getPulses()...どちらの方向にどれだけ回ったか
+ //pc.printf("Pulses:%07d \r\n",wheel.getPulses());
+ //軸が何回転したか
+ //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
#endif
@@ -315,6 +324,11 @@
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
+ //BuzzerTimer.attach(BuzzerTimer_func, 0.5);
+ //LostLedTimer.attach(LostLed_func, 0.5);
+ //sendLedData.code = (uint32_t)Yellow;
+ //buzzer = 0.5;
+ //LostTimer.attach(BuzzerTimer_func, 1.2);
}
else
#endif
@@ -376,10 +390,10 @@
{
tapeLED.code = (uint32_t)Green;
if(RedSW){
- current = 1;
+ current = 6;
}
if(BlueSW){
- current = 2;
+ current = 5;
}
}
#endif
@@ -388,6 +402,7 @@
static void Process1() //手動
{
tapeLED.code = (uint32_t)Orange;
+
motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
@@ -444,49 +459,55 @@
//pc.printf("Pulses:%07d \r\n",wheel.getPulses());
//軸が何回転したか
//pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
+
+ //filip();
+ //pc.printf("AKApwmX:%d , AKApwmY:%d\r\n",RtpwmX,RtpwmY);
+
+ //filipB();
+ //pc.printf("BLUEpwmX:%d , BLUEpwmY:%d \r\n",RtpwmXB,RtpwmYB);
}
#endif
-#if USE_PROCESS_NUM>2
+#if USE_PROCESS_NUM>2 //Blue Zone
static void Process2() //trace
{
- tapeLED.code = (uint32_t)Yellow;
- static bool color_flag = false;
-
+ /*//tapeLED.code = (uint32_t)Blue;
static bool traceon = false;//fase1
static bool yokofla = false;//fase2
static bool boxslip = false;//fase3
- //static bool syu = false;
+ static bool Rt_flagX = false;
+ static bool Rt_flagY = false;
+
ColorDetection();
- Color_changeflag();
+ Color_changeflagA();
- if(controller->Button.B && !color_flag)
+ /*if(controller->Button.B && !color_nopush)
{
traceon = true;
- color_flag = true;
+ color_nopush = true;
}
- else if(!controller->Button.B)color_flag = false;
+ traceon = true;
+
+ //else if(!controller->Button.B)color_nopush = false;
if(traceon)
{
- Color_changeflag();
- if(!invationA && !compA && !invationB && !compB)
+ Color_changeflagA();
+ if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
{
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].dir = BACK;
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
- motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FR].pwm = 42;
motor[TIRE_FL].pwm = startP;
motor[TIRE_BR].pwm = startP;
motor[TIRE_BL].pwm = startP;
-
- Color_changeflag();
}
- else if(invationC && compC && !invationB && !compB)
+ else if(invationB || compB || invationA || compA || invationC || compC)
{
for(int i = 0; i<1000; i++){
motor[TIRE_FR].dir = BRAKE;
@@ -509,8 +530,345 @@
if(yokofla && !traceon)
{
//pointcalculation();
- Color_changeflag();
- if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
+ Color_changeflagA();
+ 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;
+
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+
+
+ boxslip = true;
+ yokofla = false;
+ }
+ else{
+ 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 = 47;
+ }
+}
+
+ if(boxslip)
+ {
+ for(int i = 0; i<500; i++){
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+ }
+
+ RtX.reset();
+ RtY.reset();
+
+ Rt_flagX = true;
+ boxslip = false;
+ }
+
+ filipB();
+ if(Rt_flagX && !boxslip)
+ {
+ filipB();
+ if(disX > -goalAX + 5){
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+ motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
+ motor[TIRE_FL].pwm = SetPWM(RtpwmX);
+ motor[TIRE_BR].pwm = SetPWM(RtpwmX);
+ motor[TIRE_BL].pwm = SetPWM(RtpwmX);
+ }
+ else if(disX < -goalAX + 5){
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+
+ Rt_flagY = true;
+ Rt_flagX = false;
+ }
+ }
+
+
+if(Rt_flagY && !Rt_flagX){
+ filipB();
+ if(disY < goalAY - 5){
+ 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)*1.2;
+ }
+ else if(disY > goalAY - 5)
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+ }
+ }*/
+}
+#endif
+
+#if USE_PROCESS_NUM>3
+static void Process3() //Red Zone
+{
+ /*tapeLED.code = (uint32_t)Red;
+
+ //static bool color_nopush = false;
+
+ static bool traceon = false;//fase1
+ static bool yokofla = false;//fase2
+ static bool boxslip = false;//fase3
+
+ static bool Rt_flagX = false;
+ static bool Rt_flagY = false;
+
+ //static bool syu = false;
+
+ ColorDetection();
+ Color_changeflagA();
+
+ /*if(controller->Button.B && !color_nopush)
+ {
+ traceon = true;
+ color_nopush = true;
+ }
+ //else if(!controller->Button.B)color_nopush = false;
+ traceon = true;
+
+ if(traceon)
+ {
+ Color_changeflagA();
+ if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
+ {
+ 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 || invationC || compC)
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+
+ yokofla = true;
+ traceon = false;
+ }
+ }
+
+
+ if(yokofla && !traceon)
+ {
+ //pointcalculation();
+ Color_changeflagA();
+ 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;
+
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+
+ boxslip = true;
+ yokofla = false;
+ }
+ else{
+ 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;
+ }
+}
+
+
+ /********************************
+ if(boxslip && !yokofla)
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+
+
+ RtX.reset();
+ RtY.reset();
+
+ Rt_flagX = true;
+ boxslip = false;
+ }
+
+ if(Rt_flagX && !boxslip)
+ {
+ filip();
+ if(disX < goalAX - 5){
+
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = FOR;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
+ motor[TIRE_FL].pwm = SetPWM(RtpwmX);
+ motor[TIRE_BR].pwm = SetPWM(RtpwmX);
+ motor[TIRE_BL].pwm = SetPWM(RtpwmX);
+ }
+ else if(disX > goalAX - 5){
+
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+
+ Rt_flagY = true;
+ Rt_flagX = false;
+ }
+ }
+
+
+if(Rt_flagY && !Rt_flagX){
+ filip();
+ if(disY < goalAY - 5){
+ 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);
+ }
+ else if(disY > goalAY - 5)
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+ }
+ }*/
+}
+#endif
+
+#if USE_PROCESS_NUM>4
+static void Process4()
+{
+
+}
+#endif
+
+#if USE_PROCESS_NUM>5
+static void Process5()//traceB
+{
+ static bool traceonB = true;//fase1
+ static bool yokoflaB = false;//fase2
+ static bool boxslipB = false;//fase3
+
+ static bool Rt_flagXB = false;
+ static bool Rt_flagYB = false;
+
+ ColorDetection();
+ Color_changeflagB();
+
+ if(traceonB && !Rt_flagXB && !Rt_flagYB && !boxslipB)
+ {
+ Color_changeflagB();
+ if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
+ {
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+
+ motor[TIRE_FR].pwm = 48;
+ motor[TIRE_FL].pwm = 43;
+ motor[TIRE_BR].pwm = 43;
+ motor[TIRE_BL].pwm = 43;
+ }
+ else if(invationA || compA || invationB || compB || invationC || compC)
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ motor[TIRE_BR].dir = BRAKE;
+ motor[TIRE_BL].dir = BRAKE;
+
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+
+ yokoflaB = true;
+ traceonB = false;
+ }
+ }
+
+
+ if(yokoflaB && !traceonB)
+ {
+ Color_changeflagB();
+ if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
@@ -522,172 +880,113 @@
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
- wait(2);
-
- boxslip = true;
- yokofla = false;
+ boxslipB = true;
+ yokoflaB = false;
}
- else if(compA && compB && compC)
+ else if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L))
{
- motor[TIRE_FR].dir = FOR;
+ 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;
-
- Color_changeflag();
+ motor[TIRE_FR].pwm = 55;
+ motor[TIRE_FL].pwm = 55;
+ motor[TIRE_BR].pwm = 55;
+ motor[TIRE_BL].pwm = 55;
}
- else if(invationA && invationB && invationC)
- {
- motor[TIRE_FR].dir = FREE;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].dir = FREE;
-
- //motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = startP;
- //motor[TIRE_BL].pwm = startP;
- }
- else if(!invationA && !invationB && !invationC)
+ else if(invationA || invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)))
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].dir = FREE;
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;
+ motor[TIRE_FR].pwm = 55;
+ //motor[TIRE_FL].pwm = 30;
+ //motor[TIRE_BR].pwm = 30;
+ motor[TIRE_BL].pwm = 55;
+ }
+ else if(!invationA || !invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)))
+ {
+ motor[TIRE_FR].dir = FREE;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = FREE;
+
+ //motor[TIRE_FR].pwm = 30;
+ motor[TIRE_FL].pwm = 55;
+ motor[TIRE_BR].pwm = 55;
+ //motor[TIRE_BL].pwm = 30;
}
- else if(!invationA && compC && invationC)//C固定A下
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
+ else
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = BACK;
+
+ motor[TIRE_FR].pwm = 50;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_BL].pwm = 50;
+ }
+}
+
+
+ /*********************************/
+ if(boxslipB && !yokoflaB && !traceonB)
+ {
+ Color_changeflagB();
+ if((compA && compB) || compC){
+
+ RtX.reset();
+ RtY.reset();
+
+ Rt_flagXB = true;
+ boxslipB = false;
+ }
+ else if(invationA || invationB){
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = FOR;
motor[TIRE_BR].dir = BACK;
motor[TIRE_BL].dir = BACK;
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 100;
- motor[TIRE_BR].pwm = 55;
- motor[TIRE_BL].pwm = startP;
-
- Color_changeflag();
+ motor[TIRE_FR].pwm = 25;
+ motor[TIRE_FL].pwm = 25;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BL].pwm = 25;
}
- else if(compA && compB && !invationC)//AB固定C下
- {
- motor[TIRE_FR].dir = FOR;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 55;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 100;
-
- Color_changeflag();
- }
- else if(compA && compB && !compC && invationC)//AB固定C上
- {
+ else if(!invationA || !invationB){
motor[TIRE_FR].dir = BACK;
motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = 55;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 100;
-
- Color_changeflag();
- }
- else if(!compA && invationA && compC)//C固定A上
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = FOR;
motor[TIRE_BL].dir = FOR;
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 100;
- motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = 55;
-
- Color_changeflag();
+ motor[TIRE_FR].pwm = 25;
+ motor[TIRE_FL].pwm = 25;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BL].pwm = 25;
}
- }
-
- if(boxslip)
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
}
-}
-#endif
-
-#if USE_PROCESS_NUM>3
-static void Process3()
-{
-
-}
-#endif
-
-#if USE_PROCESS_NUM>4
-static void Process4()
-{
-}
-#endif
-
-#if USE_PROCESS_NUM>5
-static void Process5() //ロタコンXY
-{
- tapeLED.code = (uint32_t)White;
-
- static bool nopushed = false;
- static bool Rt_flagX = false;
- static bool Rt_flagY = false;
-
- if(controller->Button.A && !nopushed){
- Rt_flagX = true;
- nopushed = true;
-
- RtX.reset();
- RtY.reset();
- }else if(!controller->Button.A)nopushed = false;
-
- filip();
-
- if(Rt_flagX)
+
+
+
+ filipB();
+ if(Rt_flagXB && !boxslipB)
{
- filip();
- if(disX < goalX - 5){
- filip();
-
- motor[TIRE_FR].dir = SetStatus(-RtpwmX);
- motor[TIRE_FL].dir = SetStatus(-RtpwmX);
- motor[TIRE_BR].dir = SetStatus(RtpwmX);
- motor[TIRE_BL].dir = SetStatus(RtpwmX);
+ filipB();
+ if(disXB > goalBX + 5){
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
motor[TIRE_FL].pwm = SetPWM(RtpwmX);
motor[TIRE_BR].pwm = SetPWM(RtpwmX);
motor[TIRE_BL].pwm = SetPWM(RtpwmX);
}
- else if(disX > goalX - 5){
-
- for(int i = 0; i<200; i++){
+ else if(disXB < goalBX + 5){
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -697,18 +996,18 @@
motor[TIRE_FL].pwm = 255;
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
- }
+
+ wait(1);
- Rt_flagY = true;
- Rt_flagX = false;
+ Rt_flagYB = true;
+ Rt_flagXB = false;
}
}
-if(Rt_flagY && !Rt_flagX){
- filip();
- if(disY < goalY - 5){
- filip();
+if(Rt_flagYB && !Rt_flagXB){
+ filipB();
+ if(disYB < goalBY - 5){
motor[TIRE_FR].dir = SetStatus(-RtpwmY);
motor[TIRE_FL].dir = SetStatus(RtpwmY);
motor[TIRE_BR].dir = SetStatus(-RtpwmY);
@@ -716,16 +1015,15 @@
motor[TIRE_FR].pwm = SetPWM(RtpwmY);
motor[TIRE_FL].pwm = SetPWM(RtpwmY);
motor[TIRE_BR].pwm = SetPWM(RtpwmY);
- motor[TIRE_BL].pwm = SetPWM(RtpwmY);
+ motor[TIRE_BL].pwm = SetPWM(RtpwmY)*1.2;
}
- else if(disY > goalY - 5)
+ else if(disYB > goalBY - 5)
{
- filip();
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
- motor[TIRE_FR].pwm = 255*0.85;
+ motor[TIRE_FR].pwm = 255;
motor[TIRE_FL].pwm = 255;
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
@@ -734,52 +1032,36 @@
}
#endif
-#if USE_PROCESS_NUM>6
+#if USE_PROCESS_NUM>6//traceA
static void Process6()
-{
- tapeLED.code = (uint32_t)Yellow;
- static bool color_flag = false;
-
- static bool traceon = false;//fase1
+{
+ static bool traceon = true;//fase1
static bool yokofla = false;//fase2
static bool boxslip = false;//fase3
- static bool nopushed = false;
static bool Rt_flagX = false;
static bool Rt_flagY = false;
- //static bool syu = false;
+ ColorDetection();
+ Color_changeflagA();
- ColorDetection();
- Color_changeflag();
-
- if(controller->Button.B && !color_flag)
+ if(traceon && !Rt_flagX && !Rt_flagY && !boxslip )
{
- traceon = true;
- color_flag = true;
- }
- else if(!controller->Button.B)color_flag = false;
-
- if(traceon)
- {
- Color_changeflag();
- if(!invationA && !compA && !invationB && !compB)
+ Color_changeflagA();
+ if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
{
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;
-
- Color_changeflag();
+ motor[TIRE_FR].pwm = 42;
+ motor[TIRE_FL].pwm = 42;
+ motor[TIRE_BR].pwm = 42;
+ motor[TIRE_BL].pwm = 42;
}
- else if(invationC && compC && !invationB && !compB)
+ else if(invationA || compA || invationB || compB || invationC || compC)
{
- for(int i = 0; i<1000; i++){
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -789,7 +1071,6 @@
motor[TIRE_FL].pwm = 255;
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
- }
yokofla = true;
traceon = false;
@@ -799,9 +1080,8 @@
if(yokofla && !traceon)
{
- //pointcalculation();
- Color_changeflag();
- if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
+ Color_changeflagA();
+ if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
{
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
@@ -813,26 +1093,22 @@
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
- wait(2);
-
boxslip = true;
yokofla = false;
}
- else if(compA && compB && compC)
+ else if(LimitSw::IsPressed(Lim_R) || LimitSw::IsPressed(Lim_L))
{
- motor[TIRE_FR].dir = FOR;
+ 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;
-
- Color_changeflag();
+ motor[TIRE_FR].pwm = 55;
+ motor[TIRE_FL].pwm = 55;
+ motor[TIRE_BR].pwm = 55;
+ motor[TIRE_BL].pwm = 55;
}
- else if(invationA && invationB && invationC)
+ else if((invationA || invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
{
motor[TIRE_FR].dir = FREE;
motor[TIRE_FL].dir = BACK;
@@ -840,85 +1116,93 @@
motor[TIRE_BL].dir = FREE;
//motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = startP;
+ motor[TIRE_FL].pwm = 55;
+ motor[TIRE_BR].pwm = 55;
//motor[TIRE_BL].pwm = startP;
}
- else if(!invationA && !invationB && !invationC)
+ else if((!invationA || !invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
{
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FREE;
motor[TIRE_BR].dir = FREE;
motor[TIRE_BL].dir = BACK;
- motor[TIRE_FR].pwm = startP;
+ motor[TIRE_FR].pwm = 55;
//motor[TIRE_FL].pwm = startP;
- //motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = startP;
+ //motor[TIRE_BR].pwm = startP;
+ motor[TIRE_BL].pwm = 55;
+ }
+ else
+ {
+ motor[TIRE_FR].dir = FOR;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = BACK;
+
+ motor[TIRE_FR].pwm = 50;
+ motor[TIRE_FL].pwm = 50;
+ motor[TIRE_BR].pwm = 50;
+ motor[TIRE_BL].pwm = 50;
}
- else if(!invationA && compC && invationC)//C固定A下
- {
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
+}
+
+
+ /*********************************/
+ if(boxslip && !yokofla && !traceon)
+ {
+ Color_changeflagA();
+ if((compA && compB) || compC){
+
+ RtX.reset();
+ RtY.reset();
+
+ Rt_flagX = true;
+ boxslip = false;
+ }
+ else if(invationA || invationB && !compC && !(compA && compB)){
+ motor[TIRE_FR].dir = BACK;
+ motor[TIRE_FL].dir = BACK;
+ motor[TIRE_BR].dir = FOR;
+ motor[TIRE_BL].dir = FOR;
+
+ motor[TIRE_FR].pwm = 25;
+ motor[TIRE_FL].pwm = 25;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BL].pwm = 25;
+ }
+ else if(!invationA || !invationB && !compC && !(compA && 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 = 255;
- motor[TIRE_FL].pwm = 100;
- motor[TIRE_BR].pwm = 55;
- motor[TIRE_BL].pwm = startP;
+ motor[TIRE_FR].pwm = 25;
+ motor[TIRE_FL].pwm = 25;
+ motor[TIRE_BR].pwm = 25;
+ motor[TIRE_BL].pwm = 25;
+ }
+ }
+
+
+ filip();
+ if(Rt_flagX && !boxslip)
+ {
+ filip();
+ if(disX < goalAX - 5){
- Color_changeflag();
- }
- else if(compA && compB && !invationC)//AB固定C下
- {
motor[TIRE_FR].dir = FOR;
motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 55;
- motor[TIRE_FL].pwm = startP;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 100;
-
- Color_changeflag();
- }
- else if(compA && compB && !compC && invationC)//AB固定C上
- {
- motor[TIRE_FR].dir = BACK;
- motor[TIRE_FL].dir = BACK;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = startP;
- motor[TIRE_FL].pwm = 55;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 100;
+ motor[TIRE_BR].dir = BACK;
+ motor[TIRE_BL].dir = BACK;
+ motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
+ motor[TIRE_FL].pwm = SetPWM(RtpwmX);
+ motor[TIRE_BR].pwm = SetPWM(RtpwmX);
+ motor[TIRE_BL].pwm = SetPWM(RtpwmX);
+ }
+ else if(disX > goalAX - 5){
- Color_changeflag();
- }
- else if(!compA && invationA && compC)//C固定A上
- {
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = FOR;
- motor[TIRE_BL].dir = FOR;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 100;
- motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = 55;
-
- Color_changeflag();
- }
- }
-
- if(boxslip)
- {
- for(int i = 0; i<500; i++)
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
motor[TIRE_BL].dir = BRAKE;
@@ -927,52 +1211,7 @@
motor[TIRE_BR].pwm = 255;
motor[TIRE_BL].pwm = 255;
- Rt_flagX = true;
- nopushed = true;
-
- RtX.reset();
- RtY.reset();
- }
-
-
- /*if(controller->Button.A && !nopushed){
- Rt_flagX = true;
- nopushed = true;
-
- RtX.reset();
- RtY.reset();
- }else if(!controller->Button.A)nopushed = false;
- */
- filip();
-
- if(Rt_flagX)
- {
- filip();
- if(disX < goalX - 5){
- filip();
-
- 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)*0.8;
- motor[TIRE_FL].pwm = SetPWM(RtpwmX);
- motor[TIRE_BR].pwm = SetPWM(RtpwmX);
- motor[TIRE_BL].pwm = SetPWM(RtpwmX);
- }
- else if(disX > goalX - 5){
-
- for(int i = 0; i<500; i++){
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
-
- motor[TIRE_FR].pwm = 255;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
+ wait(1);
Rt_flagY = true;
Rt_flagX = false;
@@ -982,8 +1221,7 @@
if(Rt_flagY && !Rt_flagX){
filip();
- if(disY < goalY - 5){
- filip();
+ if(disY < goalAY - 5){
motor[TIRE_FR].dir = SetStatus(-RtpwmY);
motor[TIRE_FL].dir = SetStatus(RtpwmY);
motor[TIRE_BR].dir = SetStatus(-RtpwmY);
@@ -993,9 +1231,8 @@
motor[TIRE_BR].pwm = SetPWM(RtpwmY);
motor[TIRE_BL].pwm = SetPWM(RtpwmY);
}
- else if(disY > goalY - 5)
+ else if(disY > goalAY - 5)
{
- filip();
motor[TIRE_FR].dir = BRAKE;
motor[TIRE_FL].dir = BRAKE;
motor[TIRE_BR].dir = BRAKE;
@@ -1006,7 +1243,6 @@
motor[TIRE_BL].pwm = 255;
}
}
-
}
#endif
@@ -1139,7 +1375,6 @@
#if USE_PROCESS_NUM>8 //kakudo
static void Process8()
{
-
}
#endif
@@ -1180,8 +1415,22 @@
disX = 48*3.141*rpmX;
disY = 48*3.141*rpmY;
- RtpwmX = (int)Rt_X.SetPV(disX , goalX);
- RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
+ RtpwmX = (int)Rt_X.SetPV(disX , goalAX);
+ RtpwmY = (int)Rt_Y.SetPV(disY , goalAY);
+}
+
+void filipB(){
+ palseX = RtX.getPulses();
+ palseY = RtY.getPulses();
+
+ rpmXB = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
+ rpmYB = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
+
+ disXB = 48*3.141*rpmXB;
+ disYB = 48*3.141*rpmYB;
+
+ RtpwmXB = (int)Rt_X.SetPV(disXB , goalBX);
+ RtpwmYB = (int)Rt_Y.SetPV(disYB , goalBY);
}
unsigned long ColorIn(int index)
@@ -1241,29 +1490,26 @@
Color_D[2] = ColorIn(3);
}
-void Color_changeflag(){
+void Color_changeflagA(){
ColorDetection();
if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
{
- invationA ^= 1;//start false,over true
compA = true;//on true,noon false
}
- else if(!(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2]))compA = false;//茶
+ else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶
if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
{
- invationB ^= 1;//start false,over true
compB = true;//on true,noon false
}
- else if(!(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2]))compB = false;//茶
+ else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶
if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
{
- invationC ^= 1;//start false,over true
compC = true;//on true,noon false
}
- else if(!(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2]))compC = false;//茶
+ else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[2])compC = false;//茶
/*
if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
{
@@ -1272,6 +1518,68 @@
}
else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
*/
+
+ if(compA && !compB)
+ {
+ invationA = false;
+ }
+ else if(!compA && compB)
+ {
+ invationA = true;
+ }
+
+ if(!invationA && !compA && !compB){
+ invationB = false;
+ }
+ else if(invationA && !compA && !compB){
+ invationB = true;
+ }
+}
+
+void Color_changeflagB(){
+ ColorDetection();
+
+ if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
+ {
+ compA = true;//on true,noon false
+ }
+ else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶
+
+ if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
+ {
+ compB = true;//on true,noon false
+ }
+ else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶
+
+ if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
+ {
+ compC = true;//on true,noon false
+ }
+ else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[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(compB && !compA)
+ {
+ invationB = false;
+ }
+ else if(!compB && compA)
+ {
+ invationB = true;
+ }
+
+ if(!invationB && !compA && !compB){
+ invationA = false;
+ }
+ else if(invationB && !compA && !compB){
+ invationA = true;
+ }
}
void getcolor(){
@@ -1319,4 +1627,8 @@
void TapeLedEms_func() {
sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}
+
+void LostLed_func(){
+ sendLedData.code = sendLedData.code == (uint32_t)Yellow ? (uint32_t)Black : (uint32_t)Yellow;
+}
#pragma endregion