lknds

Dependencies:   mbed TrapezoidControl Pulse QEI

System/Process/Process.cpp

Committer:
kishibekairohan
Date:
2018-10-08
Revision:
13:b6e02d6261d7
Parent:
12:c09b3e08a316
Child:
14:93776ca449a4
Child:
15:dfcec98f5aa9

File content as of revision 13:b6e02d6261d7:


#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; 
void filip();

//************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;
		}
		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() 
{

}
#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);
}

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