Taiki Maruyama
aaaaaaaaa
Fork of
MainBoard2018_Auto_Master_A_new
by 
Akihiro Nakabayashi
System/Process/Process.cpp
- Committer:
- kishibekairohan
- Date:
- 2018-10-21
- Revision:
- 14:dfcec98f5aa9
- Parent:
- 13:b6e02d6261d7
File content as of revision 14:dfcec98f5aa9:
#include "mbed.h"
#include "Process.h"
#include "QEI.h"
#include "../../CommonLibraries/PID/PID.h"
#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
#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 "../../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;
static CONTROLLER::ControllerData *controller;
ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
static bool lock;
static bool processChangeComp;
static int current;
static void AllActuatorReset();
#ifdef USE_SUBPROCESS
static void (*Process[USE_PROCESS_NUM])(void);
#endif
#pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
/*Replace here with the definition code of your variables.*/
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();
//************カラーセンサ********************
//************ライントレース変数*******************
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******************
//DigitalOut buzzer(BUZZER_PIN);
PwmOut buzzer(BUZZER_PIN);
void BuzzerTimer_func();
Ticker BuzzerTimer;
bool Emsflag = false;
//************Buzzer******************
//************TapeLed*****************
void TapeLedEms_func();
TapeLedData tapeLED;
TapeLedData sendLedData;
TapeLED_Mode ledMode = Normal;
Ticker tapeLedTimer;
//************TapaLed*****************
#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
static void Process0(void);
#endif
#if USE_PROCESS_NUM>1
static void Process1(void);
#endif
#if USE_PROCESS_NUM>2
static void Process2(void);
#endif
#if USE_PROCESS_NUM>3
static void Process3(void);
#endif
#if USE_PROCESS_NUM>4
static void Process4(void);
#endif
#if USE_PROCESS_NUM>5
static void Process5(void);
#endif
#if USE_PROCESS_NUM>6
static void Process6(void);
#endif
#if USE_PROCESS_NUM>7
static void Process7(void);
#endif
#if USE_PROCESS_NUM>8
static void Process8(void);
#endif
#if USE_PROCESS_NUM>9
static void Process9(void);
#endif
#endif
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);
#pragma endregion USER-DEFINED_VARIABLE_INIT
lock = true;
processChangeComp = true;
current = DEFAULT_PROCESS;
#ifdef USE_SUBPROCESS
#if USE_PROCESS_NUM>0
Process[0] = Process0;
#endif
#if USE_PROCESS_NUM>1
Process[1] = Process1;
#endif
#if USE_PROCESS_NUM>2
Process[2] = Process2;
#endif
#if USE_PROCESS_NUM>3
Process[3] = Process3;
#endif
#if USE_PROCESS_NUM>4
Process[4] = Process4;
#endif
#if USE_PROCESS_NUM>5
Process[5] = Process5;
#endif
#if USE_PROCESS_NUM>6
Process[6] = Process6;
#endif
#if USE_PROCESS_NUM>7
Process[7] = Process7;
#endif
#if USE_PROCESS_NUM>8
Process[8] = Process8;
#endif
#if USE_PROCESS_NUM>9
Process[9] = Process9;
#endif
#endif
}
static void SystemProcessUpdate()
{
#ifdef USE_SUBPROCESS
if(controller->Button.HOME) lock = false;
if(controller->Button.START && processChangeComp)
{
current++;
if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
processChangeComp = false;
}
else if(controller->Button.SELECT && processChangeComp)
{
current--;
if (current < 0) current = 0;
processChangeComp = false;
}
else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
#endif
#ifdef USE_MOTOR
ACTUATORHUB::MOTOR::Motor::Update(motor);
#endif
#ifdef USE_SOLENOID
ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
#endif
#ifdef USE_RS485
ACTUATORHUB::ActuatorHub::Update();
#endif
}
void SystemProcess()
{
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
#ifdef USE_ERRORCHECK
if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
{
CONTROLLER::Controller::DataReset();
AllActuatorReset();
lock = true;
buzzer = 0.5;
BuzzerTimer.attach(BuzzerTimer_func, 0.5);
}
else
#endif
{
#ifdef USE_SUBPROCESS
if(!lock)
{
Process[current]();
}
else
#endif
{
//ロック時の処理
}
}
if ((EMS_0 || EMS_1) && !Emsflag){
buzzer = 0.5;
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) {
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();
}
}
#pragma region PROCESS
#ifdef USE_SUBPROCESS
#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() //手動
{
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
{
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
{
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
#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;
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
static void Process8()
{
}
#endif
#if USE_PROCESS_NUM>9
static void Process9()
{
}
#endif
#endif
#pragma endregion PROCESS
static void AllActuatorReset()
{
#ifdef USE_SOLENOID
solenoid.all = ALL_SOLENOID_OFF;
#endif
#ifdef USE_MOTOR
for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
{
motor[i].dir = FREE;
motor[i].pwm = 0;
}
#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 TapeLedEms_func() {
sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
}
#pragma endregion