大季 矢花
da
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 13:b6e02d6261d7
- Parent:
- 12:c09b3e08a316
- Child:
- 14:93776ca449a4
- Child:
- 15:dfcec98f5aa9
diff -r c09b3e08a316 -r b6e02d6261d7 System/Process/Process.cpp
--- a/System/Process/Process.cpp Sun Oct 07 09:08:18 2018 +0000
+++ b/System/Process/Process.cpp Mon Oct 08 15:51:15 2018 +0000
@@ -1,3 +1,4 @@
+
#include "mbed.h"
#include "Process.h"
#include "QEI.h"
@@ -95,10 +96,13 @@
void ColorIn();
void ColorDetection();
void getcolor();
+
//************カラーセンサ********************
//************ライントレース変数*******************
-int Point[3] = {234, 466, 590};//赤，緑，青
+int PointA[3] = {400, 700, 1000};//赤，緑，青
+int PointB[3] = {400, 700, 1000};//赤，緑，青
+int PointC[3] = {1000, 1700, 2400};//赤，緑，青
int startP = 35;
int downP = 5;
@@ -108,16 +112,28 @@
int Csasult = 0;
int Dsasult = 0;
-void pointcalculation();
+bool compA = false;
+bool compB = false;
+bool compC = false;
+bool compD = false;
+
+bool invationA = false;
+bool invationB = false;
+bool invationC = false;
+bool invationD = false;
Ticker Color_T;
+
+void ColorDetection();
+void Color_changeflag();
+
//************ライントレース変数*******************
//************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.1, 0, 0);
+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;
@@ -127,8 +143,8 @@
int palseY;
int RtpwmX;
int RtpwmY;
-double goalX = 1200.000;
-double goalY = 900.000;
+double goalX = 900.000;
+double goalY = 700.000;
void filip();
//************ROタコン******************
@@ -282,13 +298,13 @@
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]);
-
+ */
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
#endif
@@ -359,6 +375,12 @@
static void Process0()
{
tapeLED.code = (uint32_t)Green;
+ if(RedSW){
+ current = 1;
+ }
+ if(BlueSW){
+ current = 2;
+ }
}
#endif
@@ -426,8 +448,571 @@
#endif
#if USE_PROCESS_NUM>2
-static void Process2() //自動角度調節
+static void Process2() //trace
+{
+ tapeLED.code = (uint32_t)Yellow;
+ static bool color_flag = false;
+
+ static bool traceon = false;//fase1
+ static bool yokofla = false;//fase2
+ static bool boxslip = false;//fase3
+
+ //static bool syu = false;
+
+ ColorDetection();
+ Color_changeflag();
+
+ if(controller->Button.B && !color_flag)
+ {
+ traceon = true;
+ color_flag = true;
+ }
+ else if(!controller->Button.B)color_flag = false;
+
+ if(traceon)
+ {
+ Color_changeflag();
+ 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;
+
+ Color_changeflag();
+ }
+ else if(invationC && compC && !invationB && !compB)
+ {
+ for(int i = 0; i<1000; 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;
+ }
+
+ yokofla = true;
+ traceon = false;
+ }
+ }
+
+
+ if(yokofla && !traceon)
+ {
+ //pointcalculation();
+ Color_changeflag();
+ 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;
+
+ wait(2);
+
+ boxslip = true;
+ yokofla = false;
+ }
+ else if(compA && compB && compC)
+ {
+ 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();
+ }
+ 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)
+ {
+ 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;
+ }
+ else if(!invationA && compC && invationC)//C固定A下
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ 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();
+ }
+ 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;
+
+ 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)
+ {
+ 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)
+ {
+ 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<200; 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;
+ }
+
+ Rt_flagY = true;
+ Rt_flagX = false;
+ }
+ }
+
+
+if(Rt_flagY && !Rt_flagX){
+ filip();
+ if(disY < goalY - 5){
+ filip();
+ 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 > goalY - 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_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+ }
+ }
+}
+#endif
+
+#if USE_PROCESS_NUM>6
+static void Process6()
+{
+ tapeLED.code = (uint32_t)Yellow;
+ static bool color_flag = false;
+
+ static bool traceon = false;//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_changeflag();
+
+ if(controller->Button.B && !color_flag)
+ {
+ traceon = true;
+ color_flag = true;
+ }
+ else if(!controller->Button.B)color_flag = false;
+
+ if(traceon)
+ {
+ Color_changeflag();
+ 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;
+
+ Color_changeflag();
+ }
+ else if(invationC && compC && !invationB && !compB)
+ {
+ for(int i = 0; i<1000; 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;
+ }
+
+ yokofla = true;
+ traceon = false;
+ }
+ }
+
+
+ if(yokofla && !traceon)
+ {
+ //pointcalculation();
+ Color_changeflag();
+ 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;
+
+ wait(2);
+
+ boxslip = true;
+ yokofla = false;
+ }
+ else if(compA && compB && compC)
+ {
+ 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();
+ }
+ 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)
+ {
+ 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;
+ }
+ else if(!invationA && compC && invationC)//C固定A下
+ {
+ motor[TIRE_FR].dir = BRAKE;
+ motor[TIRE_FL].dir = BRAKE;
+ 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();
+ }
+ 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;
+
+ 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;
+
+ motor[TIRE_FR].pwm = 255;
+ motor[TIRE_FL].pwm = 255;
+ 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;
+ }
+
+ Rt_flagY = true;
+ Rt_flagX = false;
+ }
+ }
+
+
+if(Rt_flagY && !Rt_flagX){
+ filip();
+ if(disY < goalY - 5){
+ filip();
+ 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 > goalY - 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;
+ motor[TIRE_FL].pwm = 255;
+ motor[TIRE_BR].pwm = 255;
+ motor[TIRE_BL].pwm = 255;
+ }
+ }
+
+}
+#endif
+
+#if USE_PROCESS_NUM>7
+static void Process7()
+{
tapeLED.code = (uint32_t)Hotpink;
static bool Xnopush = false;
static bool Ynopush = false;
@@ -548,463 +1133,20 @@
motor[Angle_R].pwm = 255;
motor[Angle_L].pwm = 255;
}
-
-}
-#endif
-
-#if USE_PROCESS_NUM>3
-static void Process3() // ロタコンX
-{
- tapeLED.code = (uint32_t)Blue;
- static bool nopushed = false;
- static bool Rt_flagX = false;
-
- if(controller->Button.A && !nopushed){
- Rt_flagX = true;
- nopushed = true;
- }else if(!controller->Button.A)nopushed = false;
-
-
- if (Rt_flagX && SetPWM(RtpwmX) > 0){
- 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.72;
- motor[TIRE_FL].pwm = SetPWM(RtpwmX);
- motor[TIRE_BR].pwm = SetPWM(RtpwmX);
- motor[TIRE_BL].pwm = SetPWM(RtpwmX);
- }
- else if(Rt_flagX && SetPWM(RtpwmX) < 0)
- {
- 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;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
-
- }
- else
- {
- 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;
- }
- //pc.printf("X:%d \r\n",RtpwmX);
}
#endif
-#if USE_PROCESS_NUM>4
-static void Process4() //ロタコンY
-{
- tapeLED.code = (uint32_t)Violet;
- static bool nopushed = false;
- static bool Rt_flagY = false;
- /* 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*4));
- */
-
-
- if(controller->Button.B && !nopushed){
- Rt_flagY = true;
- nopushed = true;
- }else if(!controller->Button.B)nopushed = false;
-
-
- if (Rt_flagY && SetPWM(RtpwmY) > 0){
- filip();
-
- 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(Rt_flagY && SetPWM(RtpwmY) < 0)
- {
- 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;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
-
- }
- else
- {
- 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;
- }
-
- //pc.printf("RtpwmX:%f \r\n", RtpwmX);
-
-
- //pc.printf("PWM:%d \r\n", RtpwmY);
- //pc.printf("回転数:%f \r\n" ,rpmY);
- //pc.printf("距離:%f \r\n", disY);
-}
-#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;
- }else if(!controller->Button.A)nopushed = false;
-
- if (Rt_flagX && SetPWM(RtpwmX) > 0){
- 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.72;
- motor[TIRE_FL].pwm = SetPWM(RtpwmX);
- motor[TIRE_BR].pwm = SetPWM(RtpwmX);
- motor[TIRE_BL].pwm = SetPWM(RtpwmX);
- }
- else if(Rt_flagX && SetPWM(RtpwmX) < 0){
- 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;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
- }
-
- if(SetPWM(RtpwmX)== 0 && Rt_flagX){
- wait(2);
-
- Rt_flagX = false;
- Rt_flagY = true;
- }
-
- if (Rt_flagY && SetPWM(RtpwmY) > 0){
- filip();
-
- 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(Rt_flagY && SetPWM(RtpwmY) < 0)
- {
- 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;
- motor[TIRE_FL].pwm = 255;
- motor[TIRE_BR].pwm = 255;
- motor[TIRE_BL].pwm = 255;
-
- }
- else
- {
- 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>6
-static void Process6()
-{
-
-}
-#endif
-
-#if USE_PROCESS_NUM>7
-static void Process7()
+#if USE_PROCESS_NUM>8 //kakudo
+static void Process8()
{
}
#endif
-#if USE_PROCESS_NUM>8
-static void Process8()
+#if USE_PROCESS_NUM>9
+static void Process9()
{
-
-}
-#endif
-
-#if USE_PROCESS_NUM>9
- static void Process9()
- {
- static bool color_flag = false;
-
- static bool traceon = false;//fase1
- static bool yokofla = false;//fase2
- static bool boxslip = false;//fase3
-
- 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)
- {
- 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 = FOR;
- motor[TIRE_FL].dir = FOR;
- motor[TIRE_BR].dir = BACK;
- motor[TIRE_BL].dir = BACK;
-
- motor[TIRE_FR].pwm = startP - downP;
- motor[TIRE_FL].pwm = startP - downP;
- motor[TIRE_BR].pwm = startP - downP;
- motor[TIRE_BL].pwm = startP - downP;
- }
- 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);
-
- yokofla = true;
- traceon = false;
- }
- else{
- motor[TIRE_FR].dir = BRAKE;
- motor[TIRE_FL].dir = BRAKE;
- motor[TIRE_BR].dir = BRAKE;
- motor[TIRE_BL].dir = BRAKE;
- }
- }
-
- if(!traceon && yokofla && !boxslip)
- {
- 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 if(invationA && !compA && invationB)
- {
- 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_FL].pwm = startP;
- motor[TIRE_BR].pwm = startP;
- motor[TIRE_BL].pwm = startP;
- }
- else if(!invationA && !compB && !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 = 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 = 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(compB && invationB)
- {
- 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
- {
- 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(!traceon && !yokofla && boxslip)
- {
- if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
- {
- 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(!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[0].dir = BACK;
- motor[1].dir = BACK;
- motor[2].dir = FOR;
- motor[3].dir = FOR;
-
- motor[0].pwm = startP;
- motor[1].pwm = startP;
- motor[2].pwm = startP;
- motor[3].pwm = startP;
- else if()
- {
- motor[0].dir = BRAKE;
- motor[1].dir = BRAKE;
- motor[2].dir = BRAKE;
- motor[3].dir = BRAKE;
-
- motor[0].pwm = 255;
- motor[1].pwm = 255;
- motor[2].pwm = 255;
- motor[3].pwm = 255;
- }*///////
}
#endif
#endif
@@ -1027,19 +1169,6 @@
}
#pragma region USER-DEFINED-FUNCTIONS
-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();
@@ -1112,8 +1241,41 @@
Color_D[2] = ColorIn(3);
}
+void Color_changeflag(){
+ 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;//茶
+
+ 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;//茶
+
+ 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;//茶
+ /*
+ 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;//茶
+ */
+}
+
void getcolor(){
- for(int i=0;i<20;i++){
+ for(int i=0;i<10;i++){
ColorDetection();
Avecolor_A[0] += Color_A[0];
@@ -1130,22 +1292,21 @@
Avecolor_D[2] += Color_D[2];
}
-Avecolor_A[0] = Avecolor_A[0]/20;
-Avecolor_A[1] = Avecolor_A[1]/20;
-Avecolor_A[2] = Avecolor_A[2]/20;
-Avecolor_B[0] = Avecolor_B[0]/20;
-Avecolor_B[1] = Avecolor_B[1]/20;
-Avecolor_B[2] = Avecolor_B[2]/20;
-Avecolor_C[0] = Avecolor_C[0]/20;
-Avecolor_C[1] = Avecolor_C[1]/20;
-Avecolor_C[2] = Avecolor_C[2]/20;
-Avecolor_D[0] = Avecolor_D[0]/20;
-Avecolor_D[1] = Avecolor_D[1]/20;
-Avecolor_D[2] = Avecolor_D[2]/20;
+Avecolor_A[0] = Avecolor_A[0]/10;
+Avecolor_A[1] = Avecolor_A[1]/10;
+Avecolor_A[2] = Avecolor_A[2]/10;
+Avecolor_B[0] = Avecolor_B[0]/10;
+Avecolor_B[1] = Avecolor_B[1]/10;
+Avecolor_B[2] = Avecolor_B[2]/10;
+Avecolor_C[0] = Avecolor_C[0]/10;
+Avecolor_C[1] = Avecolor_C[1]/10;
+Avecolor_C[2] = Avecolor_C[2]/10;
+Avecolor_D[0] = Avecolor_D[0]/10;
+Avecolor_D[1] = Avecolor_D[1]/10;
+Avecolor_D[2] = Avecolor_D[2]/10;
+
}
-
-
void BuzzerTimer_func() {
if(buzzer == 0.5){
buzzer = 0;