大季 矢花
da
Dependencies: mbed TrapezoidControl QEI
Diff: System/Process/Process.cpp
- Revision:
- 16:3f2c2d89372b
- Parent:
- 15:dfcec98f5aa9
- Child:
- 17:50dc4b449e69
--- a/System/Process/Process.cpp Sun Oct 21 02:14:15 2018 +0000
+++ b/System/Process/Process.cpp Mon Jul 01 13:00:20 2019 +0000
@@ -8,20 +8,15 @@
#include "../../Communication/Controller/Controller.h"
#include "../../Input/ExternalInt/ExternalInt.h"
#include "../../Input/Switch/Switch.h"
-#include "../../Input/ColorSensor/ColorSensor.h"
-#include "../../Input/AccelerationSensor/AccelerationSensor.h"
#include "../../Input/Potentiometer/Potentiometer.h"
-#include "../../Input/Rotaryencoder/Rotaryencoder.h"
+#include "../../Input/Encoder/Encoder.h"
#include "../../LED/LED.h"
#include "../../Safty/Safty.h"
#include "../Using.h"
-
using namespace SWITCH;
-using namespace COLORSENSOR;
-using namespace ACCELERATIONSENSOR;
using namespace PID_SPACE;
-using namespace ROTARYENCODER;
+using namespace ENCODER;
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
@@ -43,134 +38,22 @@
Serial pc(USBTX, USBRX);
-//************メカナム********************
-
-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 Avecolor_A[3];
-int Avecolor_B[3];
-int Avecolor_C[3];
-int Avecolor_D[3];
-
-void ColorIn();
-void ColorDetection();
-void getcolor();
-
-//************カラーセンサ********************
+//**************Encoder***************
+const int PerRev = 256;
+QEI ECD_0(ECD_A_0,ECD_B_0,NC,PerRev,QEI::X4_ENCODING);
+QEI ECD_1(ECD_A_1,ECD_B_1,NC,PerRev,QEI::X4_ENCODING);
+QEI ECD_2(ECD_A_2,ECD_B_2,NC,PerRev,QEI::X4_ENCODING);
+QEI ECD_3(ECD_A_3,ECD_B_3,NC,PerRev,QEI::X4_ENCODING);
+//**************Encoder***************
-//************ライントレース変数*******************
-int PointA[3] = {400, 700, 1000};//赤，緑，青
-int PointB[3] = {400, 700, 1000};//赤，緑，青
-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;
-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.12, 0, 0);
-PID Rt_Y = PID(0.03, -255, 255, 0.1, 0, 0);
-double rpmX;
-double rpmY;
-double disX;
-double disY;
-int palseX;
-int palseY;
-int RtpwmX;
-int RtpwmY;
-double goalX = 900.000;
-double goalY = 700.000;
-
-//double goalXB = 900.000;
-//double goalYB = 700.000;
-void filip();
-void filipB();
-
-//************ROタコン******************
-
-//************ジャイロ*******************
-bool Angle_flagI = false;
-int Angle;
-PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
-float rotateY;
-//初期値 -5
-int AngletargetX = 4;
-int AngletargetY = -12;
-int AngletargetI = -5;
-//************ジャイロ*******************
-
-//************Buzzer******************
+//**************Buzzer****************
//DigitalOut buzzer(BUZZER_PIN);
-PwmOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
-bool Emsflag = false;
-//************Buzzer******************
+bool EMGflag = false;
+PwmOut buzzer(BUZZER_PIN);
+//buzzer.period(1.0/800);
+//**************Buzzer****************
//************TapeLed*****************
void TapeLedEms_func();
@@ -179,6 +62,7 @@
TapeLED_Mode ledMode = Normal;
Ticker tapeLedTimer;
//************TapaLed*****************
+
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
@@ -217,10 +101,7 @@
void SystemProcessInitialize()
{
#pragma region USER-DEFINED_VARIABLE_INIT
- /*Replace here with the initialization code of your variables.*/
- get_rpm.attach_us(&filip,100);
- buzzer.period(1.0/800);
-
+ /*Replace here with the initialization code of your variables.*/
#pragma endregion USER-DEFINED_VARIABLE_INIT
lock = true;
@@ -303,11 +184,9 @@
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]);
+ //LED_DEBUG0 = !LED_DEBUG0;
+ //LED_DEBUG1 = !LED_DEBUG1;
+ //printf("%d\r\n",ECD_0.getPulses());
#ifdef USE_MU
controller = CONTROLLER::Controller::GetData();
@@ -319,7 +198,7 @@
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
- buzzer = 0.5;
+ buzzer = 1;
BuzzerTimer.attach(BuzzerTimer_func, 0.5);
}
else
@@ -338,34 +217,17 @@
}
}
- if ((EMS_0 || EMS_1) && !Emsflag){
- buzzer = 0.5;
+ //Emergency!
+ if(EMG_0 || EMG_1){
+ buzzer = 1;
BuzzerTimer.attach(BuzzerTimer_func, 1.2);
- Emsflag = true;
- ledMode = EMS;
current = 0;
- tapeLedTimer.attach(TapeLedEms_func, 1.2);
- sendLedData.code = (uint32_t)Red;
}
- if(!EMS_0 && !EMS_1) {
+ //Safety
+ if(!EMG_0 && !EMG_1){
buzzer = 0;
BuzzerTimer.detach();
- Emsflag = false;
- if(ledMode == EMS) ledMode = Normal;
- tapeLedTimer.detach();
- }
-
- switch(ledMode)
- {
- case EMS :
- break;
-
- case Normal :
- sendLedData.code = tapeLED.code;
-
- default:
- break;
}
SystemProcessUpdate();
@@ -380,338 +242,28 @@
#if USE_PROCESS_NUM>0
static void Process0()
{
- tapeLED.code = (uint32_t)Green;
- if(RedSW){
- current = 1;
- }
- if(BlueSW){
- current = 2;
- }
+
}
#endif
#if USE_PROCESS_NUM>1
-static void Process1() //手動
+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]);
- motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
-
- motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]) *0.8;
- motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]) *0.8;
- motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
- motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
-
- if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
- motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
- motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
- }
- if(controller->Button.R){
- motor[Angle_R].dir = FOR;
- motor[Angle_L].dir = BACK;
- motor[Angle_R].pwm = 150;
- motor[Angle_L].pwm = 150;
- }else if(controller->Button.L){
- motor[Angle_R].dir = BACK;
- motor[Angle_L].dir = FOR;
- motor[Angle_R].pwm = 150;
- motor[Angle_L].pwm = 150;
- }else{
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- }
-
- 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;
- }
- /*for(int i = 0;i<20;i++){
- float y = 0;
- y = acc[1]*1000;
- float rotateY = (y - 305)/2.21 - 90;
- Angle += rotateY;
- }
- Angle = Angle/20;
- pc.printf("Y:%d \r\n",Angle);*/
-
- //wheel.getPulses()...どちらの方向にどれだけ回ったか
- //pc.printf("Pulses:%07d \r\n",wheel.getPulses());
- //軸が何回転したか
- //pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
}
#endif
#if USE_PROCESS_NUM>2
-static void Process2() //trace
+static void Process2()
{
- 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() //Blue Zone
+static void Process3()
{
- filipB();
-
- static bool Rt_flagX = false;
- static bool Rt_flagY = false;
- if(Rt_flagX)
- {
- filipB();
- if(disX < goalX - 5){
- filipB();
-
- 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){
- filipB();
- if(disY < goalY - 5){
- filipB();
- 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)
- {
- filipB();
- 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
@@ -723,491 +275,27 @@
#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;
+static void Process5()
+{
- 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;
- static bool Inopush = false;
- static bool Angle_flagX = false;
- static bool Angle_flagY = false;
- static bool ANgle_flagI = false;
-
- 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;
- }
- for(int i = 0;i<20;i++){
- float y = 0;
- y = acc[1]*1000;
- float rotateY = (y - 305)/2.21 - 90;
- Angle += rotateY;
- }
- Angle = Angle /20;
-
- int gyropwmX = gyro.SetPV(Angle,AngletargetX);
- int gyropwmY = gyro.SetPV(Angle,AngletargetY);
- int gyropwmI = gyro.SetPV(Angle,AngletargetI);
-
- if(controller->Button.X && !Xnopush){
- Angle_flagX = true;
- Xnopush = true;
- }else if(!controller->Button.X)Xnopush = false;
-
- if(controller->Button.Y && !Ynopush){
- Angle_flagY = true;
- Ynopush = true;
- }else if(!controller->Button.Y)Ynopush = false;
-
- if(controller->Button.A && !Inopush){
- Angle_flagI = true;
- Inopush = true;
- }else if(!controller->Button.A)Inopush = false;
-
- if (Angle_flagX){
- motor[Angle_R].dir = SetStatus(gyropwmX);
- motor[Angle_L].dir = SetStatus(-gyropwmX);
- motor[Angle_R].pwm = SetPWM(gyropwmX);
- motor[Angle_L].pwm = SetPWM(gyropwmX);
- if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- Angle_flagX = false;
- }
- }
-
- if (Angle_flagY){
- motor[Angle_R].dir = SetStatus(-gyropwmY);
- motor[Angle_L].dir = SetStatus(gyropwmY);
- motor[Angle_R].pwm = SetPWM(gyropwmY);
- motor[Angle_L].pwm = SetPWM(gyropwmY);
- if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- Angle_flagY = false;
- }
- }
-
- if (Angle_flagI){
- if(Angle < 0)
- {
- motor[Angle_R].dir = SetStatus(-gyropwmI);
- motor[Angle_L].dir = SetStatus(gyropwmI);
- motor[Angle_R].pwm = SetPWM(gyropwmI);
- motor[Angle_L].pwm = SetPWM(gyropwmI);
-
- if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- Angle_flagI = false;
- }
- }
- else if(Angle > 0)
- {
- motor[Angle_R].dir = FOR;
- motor[Angle_L].dir = BACK;
- motor[Angle_R].pwm = 150;
- motor[Angle_L].pwm = 150;
-
- if(Angle < 0){
- motor[Angle_R].dir = SetStatus(gyropwmI);
- motor[Angle_L].dir = SetStatus(-gyropwmI);
- motor[Angle_R].pwm = SetPWM(gyropwmI);
- motor[Angle_L].pwm = SetPWM(gyropwmI);
-
- if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- Angle_flagI = false;
- }
- }
- }
- }
- else{
- motor[Angle_R].dir = BRAKE;
- motor[Angle_L].dir = BRAKE;
- motor[Angle_R].pwm = 255;
- motor[Angle_L].pwm = 255;
- }
}
#endif
-#if USE_PROCESS_NUM>8 //kakudo
+#if USE_PROCESS_NUM>8
static void Process8()
{
@@ -1239,169 +327,15 @@
#endif
}
-#pragma region USER-DEFINED-FUNCTIONS
-
-void filip(){
- palseX = RtX.getPulses();
- palseY = RtY.getPulses();
-
- rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
- rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
-
- disX = 48*3.141*rpmX;
- disY = 48*3.141*rpmY;
-
- RtpwmX = (int)Rt_X.SetPV(disX , goalX);
- RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
-}
-
-void filipB(){
- palseX = RtX.getPulses();
- palseY = RtY.getPulses();
-
- rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
- rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
-
- disX = abs(48*3.141*rpmX);
- disY = 48*3.141*rpmY;
-
- RtpwmX = (int)Rt_X.SetPV(disX , goalX);
- RtpwmY = (int)Rt_Y.SetPV(disY , goalY);
-}
-
-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;
-}
-
-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); //緑
-
- Color_B[0] = ColorIn(1);
- wait_us(3);
- Color_B[1] = ColorIn(1);
- wait_us(3);
- Color_B[2] = ColorIn(1);
-
- Color_C[0] = ColorIn(2);
- wait_us(3);
- Color_C[1] = ColorIn(2);
- wait_us(3);
- Color_C[2] = ColorIn(2);
-
- Color_D[0] = ColorIn(3);
- wait_us(3);
- Color_D[1] = ColorIn(3);
- wait_us(3);
- 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<10;i++){
- ColorDetection();
-
- Avecolor_A[0] += Color_A[0];
- Avecolor_A[1] += Color_A[1];
- Avecolor_A[2] += Color_A[2];
- Avecolor_B[0] += Color_B[0];
- Avecolor_B[1] += Color_B[1];
- Avecolor_B[2] += Color_B[2];
- Avecolor_C[0] += Color_C[0];
- Avecolor_C[1] += Color_C[1];
- Avecolor_C[2] += Color_C[2];
- Avecolor_D[0] += Color_D[0];
- Avecolor_D[1] += Color_D[1];
- Avecolor_D[2] += Color_D[2];
-}
-
-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;
- }
- else if(buzzer == 0){
- buzzer = 0.5;
- }
+void BuzzerTimer_func(){
+ buzzer = !buzzer;
}
void TapeLedEms_func() {
- sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
+ sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}
+
+#pragma region USER-DEFINED-FUNCTIONS
+
+
#pragma endregion