MainBoard2019_Master_9_23 - aa

Users » yabahiro » Code » MainBoard2019_Master_9_23

大季矢花 / Mbed 2 deprecated MainBoard2019_Master_9_23

Dependencies: mbed TrapezoidControl QEI

System/Process/Process.cpp@14:93776ca449a4, 2018-10-25 (annotated)

Committer:: 7ka884
Date:: Thu Oct 25 09:00:19 2018 +0000
Revision:: 14:93776ca449a4
Parent:: 13:b6e02d6261d7

okok

Who changed what in which revision?

User	Revision	Line number	New contents of line
kishibekairohan	13:b6e02d6261d7	1
t_yamamoto	0:669ef71cba68	2	#include "mbed.h"
t_yamamoto	0:669ef71cba68	3	#include "Process.h"
7ka884	4:ba9df71868df	4	#include "QEI.h"
t_yamamoto	0:669ef71cba68	5
7ka884	4:ba9df71868df	6	#include "../../CommonLibraries/PID/PID.h"
t_yamamoto	0:669ef71cba68	7	#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
t_yamamoto	0:669ef71cba68	8	#include "../../Communication/Controller/Controller.h"
t_yamamoto	0:669ef71cba68	9	#include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto	0:669ef71cba68	10	#include "../../Input/Switch/Switch.h"
7ka884	1:b1219d8ca117	11	#include "../../Input/ColorSensor/ColorSensor.h"
7ka884	1:b1219d8ca117	12	#include "../../Input/AccelerationSensor/AccelerationSensor.h"
t_yamamoto	0:669ef71cba68	13	#include "../../Input/Potentiometer/Potentiometer.h"
7ka884	1:b1219d8ca117	14	#include "../../Input/Rotaryencoder/Rotaryencoder.h"
t_yamamoto	0:669ef71cba68	15	#include "../../LED/LED.h"
t_yamamoto	0:669ef71cba68	16	#include "../../Safty/Safty.h"
t_yamamoto	0:669ef71cba68	17	#include "../Using.h"
t_yamamoto	0:669ef71cba68	18
kishibekairohan	2:c015739085d3	19
t_yamamoto	0:669ef71cba68	20	using namespace SWITCH;
kishibekairohan	2:c015739085d3	21	using namespace COLORSENSOR;
kishibekairohan	2:c015739085d3	22	using namespace ACCELERATIONSENSOR;
7ka884	4:ba9df71868df	23	using namespace PID_SPACE;
7ka884	4:ba9df71868df	24	using namespace ROTARYENCODER;
t_yamamoto	0:669ef71cba68	25
t_yamamoto	0:669ef71cba68	26	static CONTROLLER::ControllerData *controller;
t_yamamoto	0:669ef71cba68	27	ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto	0:669ef71cba68	28	ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto	0:669ef71cba68	29
t_yamamoto	0:669ef71cba68	30	static bool lock;
t_yamamoto	0:669ef71cba68	31	static bool processChangeComp;
t_yamamoto	0:669ef71cba68	32	static int current;
t_yamamoto	0:669ef71cba68	33
t_yamamoto	0:669ef71cba68	34	static void AllActuatorReset();
t_yamamoto	0:669ef71cba68	35
t_yamamoto	0:669ef71cba68	36	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	37	static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto	0:669ef71cba68	38	#endif
t_yamamoto	0:669ef71cba68	39
t_yamamoto	0:669ef71cba68	40	#pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto	0:669ef71cba68	41
t_yamamoto	0:669ef71cba68	42	/Replace here with the definition code of your variables./
t_yamamoto	0:669ef71cba68	43
kishibekairohan	2:c015739085d3	44	Serial pc(USBTX, USBRX);
kishibekairohan	2:c015739085d3	45
kishibekairohan	7:e88c5d47a3be	46	//**********メカナム******************
kishibekairohan	2:c015739085d3	47
kishibekairohan	2:c015739085d3	48	const int mecanum[15][15]=
kishibekairohan	2:c015739085d3	49	{
kishibekairohan	2:c015739085d3	50	{ 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255},
kishibekairohan	2:c015739085d3	51	{ -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255},
kishibekairohan	2:c015739085d3	52	{ -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255},
kishibekairohan	2:c015739085d3	53	{ -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255},
kishibekairohan	2:c015739085d3	54	{ -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255},
kishibekairohan	2:c015739085d3	55	{-130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255},
kishibekairohan	2:c015739085d3	56	{-187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255},
kishibekairohan	2:c015739085d3	57	{-255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255},
kishibekairohan	2:c015739085d3	58	{-255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187},
kishibekairohan	2:c015739085d3	59	{-255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130},
kishibekairohan	2:c015739085d3	60	{-255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83},
kishibekairohan	2:c015739085d3	61	{-255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47},
kishibekairohan	2:c015739085d3	62	{-255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21},
kishibekairohan	2:c015739085d3	63	{-255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5},
kishibekairohan	2:c015739085d3	64	{-255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0}
kishibekairohan	2:c015739085d3	65	};
kishibekairohan	2:c015739085d3	66
kishibekairohan	2:c015739085d3	67	const int curve[15] = {-204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204};
kishibekairohan	2:c015739085d3	68	uint8_t SetStatus(int);
kishibekairohan	2:c015739085d3	69	uint8_t SetStatus(int pwmVal){
kishibekairohan	2:c015739085d3	70	if(pwmVal < 0) return BACK;
kishibekairohan	2:c015739085d3	71	else if(pwmVal > 0) return FOR;
kishibekairohan	2:c015739085d3	72	else if(pwmVal == 0) return BRAKE;
kishibekairohan	2:c015739085d3	73	else return BRAKE;
kishibekairohan	2:c015739085d3	74	}
kishibekairohan	2:c015739085d3	75	uint8_t SetPWM(int);
kishibekairohan	2:c015739085d3	76	uint8_t SetPWM(int pwmVal){
kishibekairohan	2:c015739085d3	77	if(pwmVal == 0 \|\| pwmVal > 255 \|\| pwmVal < -255) return 255;
kishibekairohan	2:c015739085d3	78	else return abs(pwmVal);
kishibekairohan	2:c015739085d3	79	}
kishibekairohan	2:c015739085d3	80
7ka884	14:93776ca449a4	81	//****************************************************************************************************
kishibekairohan	7:e88c5d47a3be	82
kishibekairohan	7:e88c5d47a3be	83	//**********カラーセンサ******************
kishibekairohan	7:e88c5d47a3be	84
kishibekairohan	2:c015739085d3	85	int Color_A[3]; //[赤,緑,青]
kishibekairohan	2:c015739085d3	86	int Color_B[3];
kishibekairohan	2:c015739085d3	87	int Color_C[3];
kishibekairohan	2:c015739085d3	88	int Color_D[3];
kishibekairohan	2:c015739085d3	89	int intergration = 50;
kishibekairohan	2:c015739085d3	90
kishibekairohan	12:c09b3e08a316	91	int Avecolor_A[3];
kishibekairohan	12:c09b3e08a316	92	int Avecolor_B[3];
kishibekairohan	12:c09b3e08a316	93	int Avecolor_C[3];
kishibekairohan	12:c09b3e08a316	94	int Avecolor_D[3];
kishibekairohan	12:c09b3e08a316	95
kishibekairohan	12:c09b3e08a316	96	void ColorIn();
kishibekairohan	7:e88c5d47a3be	97	void ColorDetection();
kishibekairohan	12:c09b3e08a316	98	void getcolor();
kishibekairohan	13:b6e02d6261d7	99
7ka884	14:93776ca449a4	100	//***************************************************************************************************************
kishibekairohan	7:e88c5d47a3be	101
kishibekairohan	2:c015739085d3	102	//**********ライントレース変数*****************
7ka884	14:93776ca449a4	103	int PointA[3] = {350, 600, 800};//赤，緑，青
7ka884	14:93776ca449a4	104	int PointB[3] = {350, 600, 800};//赤，緑，青
kishibekairohan	13:b6e02d6261d7	105	int PointC[3] = {1000, 1700, 2400};//赤，緑，青
kishibekairohan	8:6fb3723f7747	106
kishibekairohan	13:b6e02d6261d7	107	bool compA = false;
kishibekairohan	13:b6e02d6261d7	108	bool compB = false;
kishibekairohan	13:b6e02d6261d7	109	bool compC = false;
kishibekairohan	13:b6e02d6261d7	110	bool compD = false;
kishibekairohan	13:b6e02d6261d7	111
kishibekairohan	13:b6e02d6261d7	112	bool invationA = false;
kishibekairohan	13:b6e02d6261d7	113	bool invationB = false;
kishibekairohan	13:b6e02d6261d7	114	bool invationC = false;
kishibekairohan	13:b6e02d6261d7	115	bool invationD = false;
kishibekairohan	8:6fb3723f7747	116
kishibekairohan	8:6fb3723f7747	117	Ticker Color_T;
kishibekairohan	13:b6e02d6261d7	118
kishibekairohan	13:b6e02d6261d7	119	void ColorDetection();
7ka884	14:93776ca449a4	120	void Color_changeflagA();
7ka884	14:93776ca449a4	121	void Color_changeflagB();
7ka884	14:93776ca449a4	122	//*****************************************************************************************************
kishibekairohan	9:f93fc79a49ea	123
kishibekairohan	10:1295d39fec3a	124	//**********ROタコン****************
kishibekairohan	9:f93fc79a49ea	125	QEI RtX(RT11_PIN, RT12_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
kishibekairohan	9:f93fc79a49ea	126	QEI RtY(RT21_PIN, RT22_PIN, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING);
kishibekairohan	8:6fb3723f7747	127	Ticker get_rpm;
7ka884	14:93776ca449a4	128	PID Rt_X = PID(0.03, -255, 255, 0.15, 0, 0);
7ka884	14:93776ca449a4	129	PID Rt_Y = PID(0.03, -255, 255, 0.17, 0, 0);
7ka884	14:93776ca449a4	130	double rpmX = 0, rpmXB =0;
7ka884	14:93776ca449a4	131	double rpmY = 0, rpmYB =0;
7ka884	14:93776ca449a4	132	double disX =0,disXB =0;
7ka884	14:93776ca449a4	133	double disY =0,disYB =0;
kishibekairohan	8:6fb3723f7747	134	int palseX;
kishibekairohan	8:6fb3723f7747	135	int palseY;
7ka884	14:93776ca449a4	136	int RtpwmX =0,RtpwmXB =0;
7ka884	14:93776ca449a4	137	int RtpwmY =0,RtpwmYB =0;
7ka884	14:93776ca449a4	138	double goalAX = 1000.000;
7ka884	14:93776ca449a4	139	double goalAY = 850.000;
7ka884	14:93776ca449a4	140	double goalBX = -430.000;
7ka884	14:93776ca449a4	141	double goalBY = 1000.000;
kishibekairohan	8:6fb3723f7747	142	void filip();
7ka884	14:93776ca449a4	143	void filipB();
kishibekairohan	12:c09b3e08a316	144
kishibekairohan	10:1295d39fec3a	145	//**********ROタコン****************
kishibekairohan	2:c015739085d3	146
kishibekairohan	7:e88c5d47a3be	147	//**********ジャイロ*****************
kishibekairohan	9:f93fc79a49ea	148	bool Angle_flagI = false;
kishibekairohan	11:028a150943b5	149	int Angle;
kishibekairohan	7:e88c5d47a3be	150	PID gyro = PID(0.03, -150 , 150 , 8 , 0.03, 0);
kishibekairohan	7:e88c5d47a3be	151	float rotateY;
kishibekairohan	11:028a150943b5	152	//初期値 -5
kishibekairohan	11:028a150943b5	153	int AngletargetX = 4;
kishibekairohan	11:028a150943b5	154	int AngletargetY = -12;
kishibekairohan	12:c09b3e08a316	155	int AngletargetI = -5;
kishibekairohan	7:e88c5d47a3be	156	//**********ジャイロ*****************
kishibekairohan	2:c015739085d3	157
kishibekairohan	9:f93fc79a49ea	158	//**********Buzzer****************
kishibekairohan	11:028a150943b5	159	//DigitalOut buzzer(BUZZER_PIN);
kishibekairohan	11:028a150943b5	160	PwmOut buzzer(BUZZER_PIN);
kishibekairohan	9:f93fc79a49ea	161	void BuzzerTimer_func();
kishibekairohan	9:f93fc79a49ea	162	Ticker BuzzerTimer;
7ka884	14:93776ca449a4	163	Ticker LostTimer;
kishibekairohan	9:f93fc79a49ea	164	bool Emsflag = false;
kishibekairohan	9:f93fc79a49ea	165	//**********Buzzer****************
kishibekairohan	11:028a150943b5	166
kishibekairohan	11:028a150943b5	167	//**********TapeLed***************
kishibekairohan	11:028a150943b5	168	void TapeLedEms_func();
kishibekairohan	11:028a150943b5	169	TapeLedData tapeLED;
kishibekairohan	11:028a150943b5	170	TapeLedData sendLedData;
kishibekairohan	11:028a150943b5	171	TapeLED_Mode ledMode = Normal;
kishibekairohan	11:028a150943b5	172	Ticker tapeLedTimer;
7ka884	14:93776ca449a4	173	Ticker LostLedTimer;
7ka884	14:93776ca449a4	174	//void LostLed_func();
kishibekairohan	11:028a150943b5	175	//**********TapaLed***************
t_yamamoto	0:669ef71cba68	176	#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto	0:669ef71cba68	177
t_yamamoto	0:669ef71cba68	178	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	179	#if USE_PROCESS_NUM>0
t_yamamoto	0:669ef71cba68	180	static void Process0(void);
t_yamamoto	0:669ef71cba68	181	#endif
t_yamamoto	0:669ef71cba68	182	#if USE_PROCESS_NUM>1
t_yamamoto	0:669ef71cba68	183	static void Process1(void);
t_yamamoto	0:669ef71cba68	184	#endif
t_yamamoto	0:669ef71cba68	185	#if USE_PROCESS_NUM>2
t_yamamoto	0:669ef71cba68	186	static void Process2(void);
t_yamamoto	0:669ef71cba68	187	#endif
t_yamamoto	0:669ef71cba68	188	#if USE_PROCESS_NUM>3
t_yamamoto	0:669ef71cba68	189	static void Process3(void);
t_yamamoto	0:669ef71cba68	190	#endif
t_yamamoto	0:669ef71cba68	191	#if USE_PROCESS_NUM>4
t_yamamoto	0:669ef71cba68	192	static void Process4(void);
t_yamamoto	0:669ef71cba68	193	#endif
t_yamamoto	0:669ef71cba68	194	#if USE_PROCESS_NUM>5
t_yamamoto	0:669ef71cba68	195	static void Process5(void);
t_yamamoto	0:669ef71cba68	196	#endif
t_yamamoto	0:669ef71cba68	197	#if USE_PROCESS_NUM>6
t_yamamoto	0:669ef71cba68	198	static void Process6(void);
t_yamamoto	0:669ef71cba68	199	#endif
t_yamamoto	0:669ef71cba68	200	#if USE_PROCESS_NUM>7
t_yamamoto	0:669ef71cba68	201	static void Process7(void);
t_yamamoto	0:669ef71cba68	202	#endif
t_yamamoto	0:669ef71cba68	203	#if USE_PROCESS_NUM>8
t_yamamoto	0:669ef71cba68	204	static void Process8(void);
t_yamamoto	0:669ef71cba68	205	#endif
t_yamamoto	0:669ef71cba68	206	#if USE_PROCESS_NUM>9
t_yamamoto	0:669ef71cba68	207	static void Process9(void);
t_yamamoto	0:669ef71cba68	208	#endif
t_yamamoto	0:669ef71cba68	209	#endif
t_yamamoto	0:669ef71cba68	210
t_yamamoto	0:669ef71cba68	211	void SystemProcessInitialize()
t_yamamoto	0:669ef71cba68	212	{
t_yamamoto	0:669ef71cba68	213	#pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan	11:028a150943b5	214	/Replace here with the initialization code of your variables./
kishibekairohan	9:f93fc79a49ea	215	get_rpm.attach_us(&filip,100);
kishibekairohan	11:028a150943b5	216	buzzer.period(1.0/800);
t_yamamoto	0:669ef71cba68	217
t_yamamoto	0:669ef71cba68	218	#pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto	0:669ef71cba68	219
t_yamamoto	0:669ef71cba68	220	lock = true;
t_yamamoto	0:669ef71cba68	221	processChangeComp = true;
t_yamamoto	0:669ef71cba68	222	current = DEFAULT_PROCESS;
t_yamamoto	0:669ef71cba68	223
t_yamamoto	0:669ef71cba68	224	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	225	#if USE_PROCESS_NUM>0
t_yamamoto	0:669ef71cba68	226	Process[0] = Process0;
t_yamamoto	0:669ef71cba68	227	#endif
t_yamamoto	0:669ef71cba68	228	#if USE_PROCESS_NUM>1
t_yamamoto	0:669ef71cba68	229	Process[1] = Process1;
t_yamamoto	0:669ef71cba68	230	#endif
t_yamamoto	0:669ef71cba68	231	#if USE_PROCESS_NUM>2
t_yamamoto	0:669ef71cba68	232	Process[2] = Process2;
t_yamamoto	0:669ef71cba68	233	#endif
t_yamamoto	0:669ef71cba68	234	#if USE_PROCESS_NUM>3
t_yamamoto	0:669ef71cba68	235	Process[3] = Process3;
t_yamamoto	0:669ef71cba68	236	#endif
t_yamamoto	0:669ef71cba68	237	#if USE_PROCESS_NUM>4
t_yamamoto	0:669ef71cba68	238	Process[4] = Process4;
t_yamamoto	0:669ef71cba68	239	#endif
t_yamamoto	0:669ef71cba68	240	#if USE_PROCESS_NUM>5
t_yamamoto	0:669ef71cba68	241	Process[5] = Process5;
t_yamamoto	0:669ef71cba68	242	#endif
t_yamamoto	0:669ef71cba68	243	#if USE_PROCESS_NUM>6
t_yamamoto	0:669ef71cba68	244	Process[6] = Process6;
t_yamamoto	0:669ef71cba68	245	#endif
t_yamamoto	0:669ef71cba68	246	#if USE_PROCESS_NUM>7
t_yamamoto	0:669ef71cba68	247	Process[7] = Process7;
t_yamamoto	0:669ef71cba68	248	#endif
t_yamamoto	0:669ef71cba68	249	#if USE_PROCESS_NUM>8
t_yamamoto	0:669ef71cba68	250	Process[8] = Process8;
t_yamamoto	0:669ef71cba68	251	#endif
t_yamamoto	0:669ef71cba68	252	#if USE_PROCESS_NUM>9
t_yamamoto	0:669ef71cba68	253	Process[9] = Process9;
t_yamamoto	0:669ef71cba68	254	#endif
t_yamamoto	0:669ef71cba68	255	#endif
t_yamamoto	0:669ef71cba68	256	}
t_yamamoto	0:669ef71cba68	257
t_yamamoto	0:669ef71cba68	258	static void SystemProcessUpdate()
t_yamamoto	0:669ef71cba68	259	{
t_yamamoto	0:669ef71cba68	260	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	261	if(controller->Button.HOME) lock = false;
t_yamamoto	0:669ef71cba68	262
t_yamamoto	0:669ef71cba68	263	if(controller->Button.START && processChangeComp)
t_yamamoto	0:669ef71cba68	264	{
t_yamamoto	0:669ef71cba68	265	current++;
t_yamamoto	0:669ef71cba68	266	if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto	0:669ef71cba68	267	processChangeComp = false;
t_yamamoto	0:669ef71cba68	268	}
t_yamamoto	0:669ef71cba68	269	else if(controller->Button.SELECT && processChangeComp)
t_yamamoto	0:669ef71cba68	270	{
t_yamamoto	0:669ef71cba68	271	current--;
t_yamamoto	0:669ef71cba68	272	if (current < 0) current = 0;
t_yamamoto	0:669ef71cba68	273	processChangeComp = false;
t_yamamoto	0:669ef71cba68	274	}
t_yamamoto	0:669ef71cba68	275	else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto	0:669ef71cba68	276	#endif
t_yamamoto	0:669ef71cba68	277
t_yamamoto	0:669ef71cba68	278	#ifdef USE_MOTOR
t_yamamoto	0:669ef71cba68	279	ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto	0:669ef71cba68	280	#endif
t_yamamoto	0:669ef71cba68	281
t_yamamoto	0:669ef71cba68	282	#ifdef USE_SOLENOID
t_yamamoto	0:669ef71cba68	283	ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto	0:669ef71cba68	284	#endif
t_yamamoto	0:669ef71cba68	285
t_yamamoto	0:669ef71cba68	286	#ifdef USE_RS485
t_yamamoto	0:669ef71cba68	287	ACTUATORHUB::ActuatorHub::Update();
t_yamamoto	0:669ef71cba68	288	#endif
t_yamamoto	0:669ef71cba68	289
t_yamamoto	0:669ef71cba68	290	}
t_yamamoto	0:669ef71cba68	291
kishibekairohan	2:c015739085d3	292
kishibekairohan	2:c015739085d3	293
t_yamamoto	0:669ef71cba68	294	void SystemProcess()
t_yamamoto	0:669ef71cba68	295	{
t_yamamoto	0:669ef71cba68	296	SystemProcessInitialize();
t_yamamoto	0:669ef71cba68	297
t_yamamoto	0:669ef71cba68	298	while(1)
7ka884	14:93776ca449a4	299	{
7ka884	14:93776ca449a4	300	/*
kishibekairohan	12:c09b3e08a316	301	getcolor();
kishibekairohan	12:c09b3e08a316	302	pc.printf("R1:%d, G1:%d, B1:%d \r\n",Avecolor_A[0],Avecolor_A[1],Avecolor_A[2]);
kishibekairohan	12:c09b3e08a316	303	pc.printf("R2:%d, G2:%d, B2:%d \r\n",Avecolor_B[0],Avecolor_B[1],Avecolor_B[2]);
kishibekairohan	12:c09b3e08a316	304	pc.printf("R3:%d, G3:%d, B3:%d \r\n",Avecolor_C[0],Avecolor_C[1],Avecolor_C[2]);
kishibekairohan	12:c09b3e08a316	305	pc.printf("R4:%d, G4:%d, B4:%d \r\n",Avecolor_D[0],Avecolor_D[1],Avecolor_D[2]);
7ka884	14:93776ca449a4	306
7ka884	14:93776ca449a4	307	palseX = RtX.getPulses();
7ka884	14:93776ca449a4	308	palseY = RtY.getPulses();
7ka884	14:93776ca449a4	309
7ka884	14:93776ca449a4	310	pc.printf("X:%d",palseX);
7ka884	14:93776ca449a4	311	pc.printf("Y:%d",palseY);
kishibekairohan	13:b6e02d6261d7	312	*/
7ka884	14:93776ca449a4	313	//wheel.getPulses()...どちらの方向にどれだけ回ったか
7ka884	14:93776ca449a4	314	//pc.printf("Pulses:%07d \r\n",wheel.getPulses());
7ka884	14:93776ca449a4	315	//軸が何回転したか
7ka884	14:93776ca449a4	316	//pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
t_yamamoto	0:669ef71cba68	317	#ifdef USE_MU
t_yamamoto	0:669ef71cba68	318	controller = CONTROLLER::Controller::GetData();
t_yamamoto	0:669ef71cba68	319	#endif
t_yamamoto	0:669ef71cba68	320
t_yamamoto	0:669ef71cba68	321	#ifdef USE_ERRORCHECK
t_yamamoto	0:669ef71cba68	322	if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost)
t_yamamoto	0:669ef71cba68	323	{
t_yamamoto	0:669ef71cba68	324	CONTROLLER::Controller::DataReset();
t_yamamoto	0:669ef71cba68	325	AllActuatorReset();
t_yamamoto	0:669ef71cba68	326	lock = true;
7ka884	14:93776ca449a4	327	//BuzzerTimer.attach(BuzzerTimer_func, 0.5);
7ka884	14:93776ca449a4	328	//LostLedTimer.attach(LostLed_func, 0.5);
7ka884	14:93776ca449a4	329	//sendLedData.code = (uint32_t)Yellow;
7ka884	14:93776ca449a4	330	//buzzer = 0.5;
7ka884	14:93776ca449a4	331	//LostTimer.attach(BuzzerTimer_func, 1.2);
t_yamamoto	0:669ef71cba68	332	}
t_yamamoto	0:669ef71cba68	333	else
t_yamamoto	0:669ef71cba68	334	#endif
t_yamamoto	0:669ef71cba68	335	{
t_yamamoto	0:669ef71cba68	336
t_yamamoto	0:669ef71cba68	337	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	338	if(!lock)
t_yamamoto	0:669ef71cba68	339	{
t_yamamoto	0:669ef71cba68	340	Process[current]();
t_yamamoto	0:669ef71cba68	341	}
t_yamamoto	0:669ef71cba68	342	else
t_yamamoto	0:669ef71cba68	343	#endif
t_yamamoto	0:669ef71cba68	344	{
t_yamamoto	0:669ef71cba68	345	//ロック時の処理
t_yamamoto	0:669ef71cba68	346	}
t_yamamoto	0:669ef71cba68	347	}
t_yamamoto	0:669ef71cba68	348
kishibekairohan	10:1295d39fec3a	349	if ((EMS_0 \|\| EMS_1) && !Emsflag){
kishibekairohan	11:028a150943b5	350	buzzer = 0.5;
kishibekairohan	9:f93fc79a49ea	351	BuzzerTimer.attach(BuzzerTimer_func, 1.2);
kishibekairohan	9:f93fc79a49ea	352	Emsflag = true;
kishibekairohan	11:028a150943b5	353	ledMode = EMS;
kishibekairohan	11:028a150943b5	354	current = 0;
kishibekairohan	11:028a150943b5	355	tapeLedTimer.attach(TapeLedEms_func, 1.2);
kishibekairohan	11:028a150943b5	356	sendLedData.code = (uint32_t)Red;
kishibekairohan	9:f93fc79a49ea	357	}
kishibekairohan	9:f93fc79a49ea	358
kishibekairohan	9:f93fc79a49ea	359	if(!EMS_0 && !EMS_1) {
kishibekairohan	9:f93fc79a49ea	360	buzzer = 0;
kishibekairohan	9:f93fc79a49ea	361	BuzzerTimer.detach();
kishibekairohan	9:f93fc79a49ea	362	Emsflag = false;
kishibekairohan	11:028a150943b5	363	if(ledMode == EMS) ledMode = Normal;
kishibekairohan	11:028a150943b5	364	tapeLedTimer.detach();
kishibekairohan	11:028a150943b5	365	}
kishibekairohan	11:028a150943b5	366
kishibekairohan	11:028a150943b5	367	switch(ledMode)
kishibekairohan	11:028a150943b5	368	{
kishibekairohan	11:028a150943b5	369	case EMS :
kishibekairohan	11:028a150943b5	370	break;
kishibekairohan	11:028a150943b5	371
kishibekairohan	11:028a150943b5	372	case Normal :
kishibekairohan	11:028a150943b5	373	sendLedData.code = tapeLED.code;
kishibekairohan	11:028a150943b5	374
kishibekairohan	11:028a150943b5	375	default:
kishibekairohan	11:028a150943b5	376	break;
kishibekairohan	9:f93fc79a49ea	377	}
kishibekairohan	9:f93fc79a49ea	378
t_yamamoto	0:669ef71cba68	379	SystemProcessUpdate();
t_yamamoto	0:669ef71cba68	380	}
t_yamamoto	0:669ef71cba68	381	}
t_yamamoto	0:669ef71cba68	382
kishibekairohan	2:c015739085d3	383
kishibekairohan	2:c015739085d3	384
kishibekairohan	2:c015739085d3	385
t_yamamoto	0:669ef71cba68	386	#pragma region PROCESS
t_yamamoto	0:669ef71cba68	387	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	388	#if USE_PROCESS_NUM>0
kishibekairohan	12:c09b3e08a316	389	static void Process0()
kishibekairohan	10:1295d39fec3a	390	{
kishibekairohan	12:c09b3e08a316	391	tapeLED.code = (uint32_t)Green;
kishibekairohan	13:b6e02d6261d7	392	if(RedSW){
7ka884	14:93776ca449a4	393	current = 6;
kishibekairohan	13:b6e02d6261d7	394	}
kishibekairohan	13:b6e02d6261d7	395	if(BlueSW){
7ka884	14:93776ca449a4	396	current = 5;
kishibekairohan	13:b6e02d6261d7	397	}
t_yamamoto	0:669ef71cba68	398	}
t_yamamoto	0:669ef71cba68	399	#endif
t_yamamoto	0:669ef71cba68	400
t_yamamoto	0:669ef71cba68	401	#if USE_PROCESS_NUM>1
kishibekairohan	12:c09b3e08a316	402	static void Process1() //手動
t_yamamoto	0:669ef71cba68	403	{
kishibekairohan	12:c09b3e08a316	404	tapeLED.code = (uint32_t)Orange;
7ka884	14:93776ca449a4	405
7ka884	4:ba9df71868df	406	motor[TIRE_FR].dir = SetStatus(-mecanum[controller->AnalogL.Y][14-controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884	4:ba9df71868df	407	motor[TIRE_FL].dir = SetStatus(mecanum[controller->AnalogL.Y][controller->AnalogL.X] + curve[controller->AnalogR.X]);
7ka884	4:ba9df71868df	408	motor[TIRE_BR].dir = SetStatus(-mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
7ka884	4:ba9df71868df	409	motor[TIRE_BL].dir = SetStatus(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y] + curve[controller->AnalogR.X]);
kishibekairohan	2:c015739085d3	410
kishibekairohan	7:e88c5d47a3be	411	motor[TIRE_FR].pwm = SetPWM(mecanum[controller->AnalogL.Y][14-controller->AnalogL.X]) *0.8;
kishibekairohan	7:e88c5d47a3be	412	motor[TIRE_FL].pwm = SetPWM(mecanum[controller->AnalogL.Y][controller->AnalogL.X]) *0.8;
kishibekairohan	7:e88c5d47a3be	413	motor[TIRE_BR].pwm = SetPWM(mecanum[14-controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan	7:e88c5d47a3be	414	motor[TIRE_BL].pwm = SetPWM(mecanum[controller->AnalogL.X][14-controller->AnalogL.Y]) *0.8;
kishibekairohan	2:c015739085d3	415
kishibekairohan	2:c015739085d3	416	if (abs(controller->AnalogL.X-7) <= 4 && controller->AnalogL.X!=7 && controller->AnalogL.Y!=7 && controller->AnalogR.X==7){
kishibekairohan	7:e88c5d47a3be	417	motor[TIRE_FR].pwm = motor[TIRE_FR].pwm * 1.3;
kishibekairohan	7:e88c5d47a3be	418	motor[TIRE_FL].pwm = motor[TIRE_FL].pwm * 1.3;
7ka884	4:ba9df71868df	419	}
7ka884	4:ba9df71868df	420
kishibekairohan	12:c09b3e08a316	421	if(controller->Button.R){
kishibekairohan	12:c09b3e08a316	422	motor[Angle_R].dir = FOR;
kishibekairohan	12:c09b3e08a316	423	motor[Angle_L].dir = BACK;
kishibekairohan	12:c09b3e08a316	424	motor[Angle_R].pwm = 150;
kishibekairohan	12:c09b3e08a316	425	motor[Angle_L].pwm = 150;
kishibekairohan	12:c09b3e08a316	426	}else if(controller->Button.L){
kishibekairohan	12:c09b3e08a316	427	motor[Angle_R].dir = BACK;
kishibekairohan	12:c09b3e08a316	428	motor[Angle_L].dir = FOR;
kishibekairohan	12:c09b3e08a316	429	motor[Angle_R].pwm = 150;
kishibekairohan	12:c09b3e08a316	430	motor[Angle_L].pwm = 150;
kishibekairohan	12:c09b3e08a316	431	}else{
kishibekairohan	12:c09b3e08a316	432	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	433	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	434	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	435	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	436	}
kishibekairohan	12:c09b3e08a316	437
kishibekairohan	12:c09b3e08a316	438	if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan	12:c09b3e08a316	439	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	440	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	441	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	442	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	443	}else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan	12:c09b3e08a316	444	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	445	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	446	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	447	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	448	}
kishibekairohan	12:c09b3e08a316	449	/*for(int i = 0;i<20;i++){
kishibekairohan	12:c09b3e08a316	450	float y = 0;
kishibekairohan	12:c09b3e08a316	451	y = acc[1]*1000;
kishibekairohan	12:c09b3e08a316	452	float rotateY = (y - 305)/2.21 - 90;
kishibekairohan	12:c09b3e08a316	453	Angle += rotateY;
kishibekairohan	12:c09b3e08a316	454	}
kishibekairohan	12:c09b3e08a316	455	Angle = Angle/20;
kishibekairohan	12:c09b3e08a316	456	pc.printf("Y:%d \r\n",Angle);*/
kishibekairohan	2:c015739085d3	457
kishibekairohan	12:c09b3e08a316	458	//wheel.getPulses()...どちらの方向にどれだけ回ったか
kishibekairohan	12:c09b3e08a316	459	//pc.printf("Pulses:%07d \r\n",wheel.getPulses());
kishibekairohan	12:c09b3e08a316	460	//軸が何回転したか
kishibekairohan	12:c09b3e08a316	461	//pc.printf("Rotate:%04.3f \r\n",(double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4));
7ka884	14:93776ca449a4	462
7ka884	14:93776ca449a4	463	//filip();
7ka884	14:93776ca449a4	464	//pc.printf("AKApwmX:%d , AKApwmY:%d\r\n",RtpwmX,RtpwmY);
7ka884	14:93776ca449a4	465
7ka884	14:93776ca449a4	466	//filipB();
7ka884	14:93776ca449a4	467	//pc.printf("BLUEpwmX:%d , BLUEpwmY:%d \r\n",RtpwmXB,RtpwmYB);
t_yamamoto	0:669ef71cba68	468	}
t_yamamoto	0:669ef71cba68	469	#endif
t_yamamoto	0:669ef71cba68	470
7ka884	14:93776ca449a4	471	#if USE_PROCESS_NUM>2 //Blue Zone
kishibekairohan	13:b6e02d6261d7	472	static void Process2() //trace
kishibekairohan	13:b6e02d6261d7	473	{
7ka884	14:93776ca449a4	474	/*//tapeLED.code = (uint32_t)Blue;
kishibekairohan	13:b6e02d6261d7	475	static bool traceon = false;//fase1
kishibekairohan	13:b6e02d6261d7	476	static bool yokofla = false;//fase2
kishibekairohan	13:b6e02d6261d7	477	static bool boxslip = false;//fase3
kishibekairohan	13:b6e02d6261d7	478
7ka884	14:93776ca449a4	479	static bool Rt_flagX = false;
7ka884	14:93776ca449a4	480	static bool Rt_flagY = false;
7ka884	14:93776ca449a4	481
kishibekairohan	13:b6e02d6261d7	482
kishibekairohan	13:b6e02d6261d7	483	ColorDetection();
7ka884	14:93776ca449a4	484	Color_changeflagA();
kishibekairohan	13:b6e02d6261d7	485
7ka884	14:93776ca449a4	486	/*if(controller->Button.B && !color_nopush)
kishibekairohan	13:b6e02d6261d7	487	{
kishibekairohan	13:b6e02d6261d7	488	traceon = true;
7ka884	14:93776ca449a4	489	color_nopush = true;
kishibekairohan	13:b6e02d6261d7	490	}
7ka884	14:93776ca449a4	491	traceon = true;
7ka884	14:93776ca449a4	492
7ka884	14:93776ca449a4	493	//else if(!controller->Button.B)color_nopush = false;
kishibekairohan	13:b6e02d6261d7	494
kishibekairohan	13:b6e02d6261d7	495	if(traceon)
kishibekairohan	13:b6e02d6261d7	496	{
7ka884	14:93776ca449a4	497	Color_changeflagA();
7ka884	14:93776ca449a4	498	if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
kishibekairohan	13:b6e02d6261d7	499	{
7ka884	14:93776ca449a4	500	motor[TIRE_FR].dir = BACK;
7ka884	14:93776ca449a4	501	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	502	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	503	motor[TIRE_BL].dir = FOR;
kishibekairohan	13:b6e02d6261d7	504
7ka884	14:93776ca449a4	505	motor[TIRE_FR].pwm = 42;
kishibekairohan	13:b6e02d6261d7	506	motor[TIRE_FL].pwm = startP;
kishibekairohan	13:b6e02d6261d7	507	motor[TIRE_BR].pwm = startP;
kishibekairohan	13:b6e02d6261d7	508	motor[TIRE_BL].pwm = startP;
kishibekairohan	13:b6e02d6261d7	509	}
7ka884	14:93776ca449a4	510	else if(invationB \|\| compB \|\| invationA \|\| compA \|\| invationC \|\| compC)
kishibekairohan	13:b6e02d6261d7	511	{
kishibekairohan	13:b6e02d6261d7	512	for(int i = 0; i<1000; i++){
kishibekairohan	13:b6e02d6261d7	513	motor[TIRE_FR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	514	motor[TIRE_FL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	515	motor[TIRE_BR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	516	motor[TIRE_BL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	517
kishibekairohan	13:b6e02d6261d7	518	motor[TIRE_FR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	519	motor[TIRE_FL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	520	motor[TIRE_BR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	521	motor[TIRE_BL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	522	}
kishibekairohan	13:b6e02d6261d7	523
kishibekairohan	13:b6e02d6261d7	524	yokofla = true;
kishibekairohan	13:b6e02d6261d7	525	traceon = false;
kishibekairohan	13:b6e02d6261d7	526	}
kishibekairohan	13:b6e02d6261d7	527	}
kishibekairohan	13:b6e02d6261d7	528
kishibekairohan	13:b6e02d6261d7	529
kishibekairohan	13:b6e02d6261d7	530	if(yokofla && !traceon)
kishibekairohan	13:b6e02d6261d7	531	{
kishibekairohan	13:b6e02d6261d7	532	//pointcalculation();
7ka884	14:93776ca449a4	533	Color_changeflagA();
7ka884	14:93776ca449a4	534	if(LimitSw::IsPressed(Lim_R) \|\| LimitSw::IsPressed(Lim_L))
7ka884	14:93776ca449a4	535	{
7ka884	14:93776ca449a4	536	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	537	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	538	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	539	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	540
7ka884	14:93776ca449a4	541	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	542	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	543	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	544	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	545
7ka884	14:93776ca449a4	546
7ka884	14:93776ca449a4	547	boxslip = true;
7ka884	14:93776ca449a4	548	yokofla = false;
7ka884	14:93776ca449a4	549	}
7ka884	14:93776ca449a4	550	else{
7ka884	14:93776ca449a4	551	motor[TIRE_FR].dir = FOR;
7ka884	14:93776ca449a4	552	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	553	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	554	motor[TIRE_BL].dir = BACK;
7ka884	14:93776ca449a4	555
7ka884	14:93776ca449a4	556	motor[TIRE_FR].pwm = startP;
7ka884	14:93776ca449a4	557	motor[TIRE_FL].pwm = startP;
7ka884	14:93776ca449a4	558	motor[TIRE_BR].pwm = startP;
7ka884	14:93776ca449a4	559	motor[TIRE_BL].pwm = 47;
7ka884	14:93776ca449a4	560	}
7ka884	14:93776ca449a4	561	}
7ka884	14:93776ca449a4	562
7ka884	14:93776ca449a4	563	if(boxslip)
7ka884	14:93776ca449a4	564	{
7ka884	14:93776ca449a4	565	for(int i = 0; i<500; i++){
7ka884	14:93776ca449a4	566	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	567	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	568	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	569	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	570
7ka884	14:93776ca449a4	571	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	572	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	573	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	574	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	575	}
7ka884	14:93776ca449a4	576
7ka884	14:93776ca449a4	577	RtX.reset();
7ka884	14:93776ca449a4	578	RtY.reset();
7ka884	14:93776ca449a4	579
7ka884	14:93776ca449a4	580	Rt_flagX = true;
7ka884	14:93776ca449a4	581	boxslip = false;
7ka884	14:93776ca449a4	582	}
7ka884	14:93776ca449a4	583
7ka884	14:93776ca449a4	584	filipB();
7ka884	14:93776ca449a4	585	if(Rt_flagX && !boxslip)
7ka884	14:93776ca449a4	586	{
7ka884	14:93776ca449a4	587	filipB();
7ka884	14:93776ca449a4	588	if(disX > -goalAX + 5){
7ka884	14:93776ca449a4	589	motor[TIRE_FR].dir = BACK;
7ka884	14:93776ca449a4	590	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	591	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	592	motor[TIRE_BL].dir = FOR;
7ka884	14:93776ca449a4	593	motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
7ka884	14:93776ca449a4	594	motor[TIRE_FL].pwm = SetPWM(RtpwmX);
7ka884	14:93776ca449a4	595	motor[TIRE_BR].pwm = SetPWM(RtpwmX);
7ka884	14:93776ca449a4	596	motor[TIRE_BL].pwm = SetPWM(RtpwmX);
7ka884	14:93776ca449a4	597	}
7ka884	14:93776ca449a4	598	else if(disX < -goalAX + 5){
7ka884	14:93776ca449a4	599	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	600	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	601	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	602	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	603
7ka884	14:93776ca449a4	604	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	605	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	606	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	607	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	608
7ka884	14:93776ca449a4	609	Rt_flagY = true;
7ka884	14:93776ca449a4	610	Rt_flagX = false;
7ka884	14:93776ca449a4	611	}
7ka884	14:93776ca449a4	612	}
7ka884	14:93776ca449a4	613
7ka884	14:93776ca449a4	614
7ka884	14:93776ca449a4	615	if(Rt_flagY && !Rt_flagX){
7ka884	14:93776ca449a4	616	filipB();
7ka884	14:93776ca449a4	617	if(disY < goalAY - 5){
7ka884	14:93776ca449a4	618	motor[TIRE_FR].dir = SetStatus(-RtpwmY);
7ka884	14:93776ca449a4	619	motor[TIRE_FL].dir = SetStatus(RtpwmY);
7ka884	14:93776ca449a4	620	motor[TIRE_BR].dir = SetStatus(-RtpwmY);
7ka884	14:93776ca449a4	621	motor[TIRE_BL].dir = SetStatus(RtpwmY);
7ka884	14:93776ca449a4	622	motor[TIRE_FR].pwm = SetPWM(RtpwmY);
7ka884	14:93776ca449a4	623	motor[TIRE_FL].pwm = SetPWM(RtpwmY);
7ka884	14:93776ca449a4	624	motor[TIRE_BR].pwm = SetPWM(RtpwmY);
7ka884	14:93776ca449a4	625	motor[TIRE_BL].pwm = SetPWM(RtpwmY)*1.2;
7ka884	14:93776ca449a4	626	}
7ka884	14:93776ca449a4	627	else if(disY > goalAY - 5)
7ka884	14:93776ca449a4	628	{
7ka884	14:93776ca449a4	629	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	630	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	631	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	632	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	633	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	634	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	635	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	636	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	637	}
7ka884	14:93776ca449a4	638	}*/
7ka884	14:93776ca449a4	639	}
7ka884	14:93776ca449a4	640	#endif
7ka884	14:93776ca449a4	641
7ka884	14:93776ca449a4	642	#if USE_PROCESS_NUM>3
7ka884	14:93776ca449a4	643	static void Process3() //Red Zone
7ka884	14:93776ca449a4	644	{
7ka884	14:93776ca449a4	645	/*tapeLED.code = (uint32_t)Red;
7ka884	14:93776ca449a4	646
7ka884	14:93776ca449a4	647	//static bool color_nopush = false;
7ka884	14:93776ca449a4	648
7ka884	14:93776ca449a4	649	static bool traceon = false;//fase1
7ka884	14:93776ca449a4	650	static bool yokofla = false;//fase2
7ka884	14:93776ca449a4	651	static bool boxslip = false;//fase3
7ka884	14:93776ca449a4	652
7ka884	14:93776ca449a4	653	static bool Rt_flagX = false;
7ka884	14:93776ca449a4	654	static bool Rt_flagY = false;
7ka884	14:93776ca449a4	655
7ka884	14:93776ca449a4	656	//static bool syu = false;
7ka884	14:93776ca449a4	657
7ka884	14:93776ca449a4	658	ColorDetection();
7ka884	14:93776ca449a4	659	Color_changeflagA();
7ka884	14:93776ca449a4	660
7ka884	14:93776ca449a4	661	/*if(controller->Button.B && !color_nopush)
7ka884	14:93776ca449a4	662	{
7ka884	14:93776ca449a4	663	traceon = true;
7ka884	14:93776ca449a4	664	color_nopush = true;
7ka884	14:93776ca449a4	665	}
7ka884	14:93776ca449a4	666	//else if(!controller->Button.B)color_nopush = false;
7ka884	14:93776ca449a4	667	traceon = true;
7ka884	14:93776ca449a4	668
7ka884	14:93776ca449a4	669	if(traceon)
7ka884	14:93776ca449a4	670	{
7ka884	14:93776ca449a4	671	Color_changeflagA();
7ka884	14:93776ca449a4	672	if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
7ka884	14:93776ca449a4	673	{
7ka884	14:93776ca449a4	674	motor[TIRE_FR].dir = FOR;
7ka884	14:93776ca449a4	675	motor[TIRE_FL].dir = FOR;
7ka884	14:93776ca449a4	676	motor[TIRE_BR].dir = BACK;
7ka884	14:93776ca449a4	677	motor[TIRE_BL].dir = BACK;
7ka884	14:93776ca449a4	678
7ka884	14:93776ca449a4	679	motor[TIRE_FR].pwm = startP;
7ka884	14:93776ca449a4	680	motor[TIRE_FL].pwm = startP;
7ka884	14:93776ca449a4	681	motor[TIRE_BR].pwm = startP;
7ka884	14:93776ca449a4	682	motor[TIRE_BL].pwm = startP;
7ka884	14:93776ca449a4	683	}
7ka884	14:93776ca449a4	684	else if(invationA \|\| compA \|\| invationB \|\| compB \|\| invationC \|\| compC)
7ka884	14:93776ca449a4	685	{
7ka884	14:93776ca449a4	686	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	687	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	688	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	689	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	690
7ka884	14:93776ca449a4	691	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	692	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	693	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	694	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	695
7ka884	14:93776ca449a4	696	yokofla = true;
7ka884	14:93776ca449a4	697	traceon = false;
7ka884	14:93776ca449a4	698	}
7ka884	14:93776ca449a4	699	}
7ka884	14:93776ca449a4	700
7ka884	14:93776ca449a4	701
7ka884	14:93776ca449a4	702	if(yokofla && !traceon)
7ka884	14:93776ca449a4	703	{
7ka884	14:93776ca449a4	704	//pointcalculation();
7ka884	14:93776ca449a4	705	Color_changeflagA();
7ka884	14:93776ca449a4	706	if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
7ka884	14:93776ca449a4	707	{
7ka884	14:93776ca449a4	708	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	709	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	710	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	711	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	712
7ka884	14:93776ca449a4	713	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	714	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	715	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	716	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	717
7ka884	14:93776ca449a4	718	boxslip = true;
7ka884	14:93776ca449a4	719	yokofla = false;
7ka884	14:93776ca449a4	720	}
7ka884	14:93776ca449a4	721	else{
7ka884	14:93776ca449a4	722	motor[TIRE_FR].dir = FOR;
7ka884	14:93776ca449a4	723	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	724	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	725	motor[TIRE_BL].dir = BACK;
7ka884	14:93776ca449a4	726
7ka884	14:93776ca449a4	727	motor[TIRE_FR].pwm = startP;
7ka884	14:93776ca449a4	728	motor[TIRE_FL].pwm = startP;
7ka884	14:93776ca449a4	729	motor[TIRE_BR].pwm = startP;
7ka884	14:93776ca449a4	730	motor[TIRE_BL].pwm = startP;
7ka884	14:93776ca449a4	731	}
7ka884	14:93776ca449a4	732	}
7ka884	14:93776ca449a4	733
7ka884	14:93776ca449a4	734
7ka884	14:93776ca449a4	735	/********************************
7ka884	14:93776ca449a4	736	if(boxslip && !yokofla)
7ka884	14:93776ca449a4	737	{
7ka884	14:93776ca449a4	738	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	739	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	740	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	741	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	742
7ka884	14:93776ca449a4	743	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	744	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	745	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	746	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	747
7ka884	14:93776ca449a4	748
7ka884	14:93776ca449a4	749	RtX.reset();
7ka884	14:93776ca449a4	750	RtY.reset();
7ka884	14:93776ca449a4	751
7ka884	14:93776ca449a4	752	Rt_flagX = true;
7ka884	14:93776ca449a4	753	boxslip = false;
7ka884	14:93776ca449a4	754	}
7ka884	14:93776ca449a4	755
7ka884	14:93776ca449a4	756	if(Rt_flagX && !boxslip)
7ka884	14:93776ca449a4	757	{
7ka884	14:93776ca449a4	758	filip();
7ka884	14:93776ca449a4	759	if(disX < goalAX - 5){
7ka884	14:93776ca449a4	760
7ka884	14:93776ca449a4	761	motor[TIRE_FR].dir = FOR;
7ka884	14:93776ca449a4	762	motor[TIRE_FL].dir = FOR;
7ka884	14:93776ca449a4	763	motor[TIRE_BR].dir = BACK;
7ka884	14:93776ca449a4	764	motor[TIRE_BL].dir = BACK;
7ka884	14:93776ca449a4	765	motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
7ka884	14:93776ca449a4	766	motor[TIRE_FL].pwm = SetPWM(RtpwmX);
7ka884	14:93776ca449a4	767	motor[TIRE_BR].pwm = SetPWM(RtpwmX);
7ka884	14:93776ca449a4	768	motor[TIRE_BL].pwm = SetPWM(RtpwmX);
7ka884	14:93776ca449a4	769	}
7ka884	14:93776ca449a4	770	else if(disX > goalAX - 5){
7ka884	14:93776ca449a4	771
7ka884	14:93776ca449a4	772	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	773	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	774	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	775	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	776
7ka884	14:93776ca449a4	777	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	778	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	779	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	780	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	781
7ka884	14:93776ca449a4	782	Rt_flagY = true;
7ka884	14:93776ca449a4	783	Rt_flagX = false;
7ka884	14:93776ca449a4	784	}
7ka884	14:93776ca449a4	785	}
7ka884	14:93776ca449a4	786
7ka884	14:93776ca449a4	787
7ka884	14:93776ca449a4	788	if(Rt_flagY && !Rt_flagX){
7ka884	14:93776ca449a4	789	filip();
7ka884	14:93776ca449a4	790	if(disY < goalAY - 5){
7ka884	14:93776ca449a4	791	motor[TIRE_FR].dir = SetStatus(-RtpwmY);
7ka884	14:93776ca449a4	792	motor[TIRE_FL].dir = SetStatus(RtpwmY);
7ka884	14:93776ca449a4	793	motor[TIRE_BR].dir = SetStatus(-RtpwmY);
7ka884	14:93776ca449a4	794	motor[TIRE_BL].dir = SetStatus(RtpwmY);
7ka884	14:93776ca449a4	795	motor[TIRE_FR].pwm = SetPWM(RtpwmY);
7ka884	14:93776ca449a4	796	motor[TIRE_FL].pwm = SetPWM(RtpwmY);
7ka884	14:93776ca449a4	797	motor[TIRE_BR].pwm = SetPWM(RtpwmY);
7ka884	14:93776ca449a4	798	motor[TIRE_BL].pwm = SetPWM(RtpwmY);
7ka884	14:93776ca449a4	799	}
7ka884	14:93776ca449a4	800	else if(disY > goalAY - 5)
7ka884	14:93776ca449a4	801	{
7ka884	14:93776ca449a4	802	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	803	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	804	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	805	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	806	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	807	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	808	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	809	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	810	}
7ka884	14:93776ca449a4	811	}*/
7ka884	14:93776ca449a4	812	}
7ka884	14:93776ca449a4	813	#endif
7ka884	14:93776ca449a4	814
7ka884	14:93776ca449a4	815	#if USE_PROCESS_NUM>4
7ka884	14:93776ca449a4	816	static void Process4()
7ka884	14:93776ca449a4	817	{
7ka884	14:93776ca449a4	818
7ka884	14:93776ca449a4	819	}
7ka884	14:93776ca449a4	820	#endif
7ka884	14:93776ca449a4	821
7ka884	14:93776ca449a4	822	#if USE_PROCESS_NUM>5
7ka884	14:93776ca449a4	823	static void Process5()//traceB
7ka884	14:93776ca449a4	824	{
7ka884	14:93776ca449a4	825	static bool traceonB = true;//fase1
7ka884	14:93776ca449a4	826	static bool yokoflaB = false;//fase2
7ka884	14:93776ca449a4	827	static bool boxslipB = false;//fase3
7ka884	14:93776ca449a4	828
7ka884	14:93776ca449a4	829	static bool Rt_flagXB = false;
7ka884	14:93776ca449a4	830	static bool Rt_flagYB = false;
7ka884	14:93776ca449a4	831
7ka884	14:93776ca449a4	832	ColorDetection();
7ka884	14:93776ca449a4	833	Color_changeflagB();
7ka884	14:93776ca449a4	834
7ka884	14:93776ca449a4	835	if(traceonB && !Rt_flagXB && !Rt_flagYB && !boxslipB)
7ka884	14:93776ca449a4	836	{
7ka884	14:93776ca449a4	837	Color_changeflagB();
7ka884	14:93776ca449a4	838	if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
7ka884	14:93776ca449a4	839	{
7ka884	14:93776ca449a4	840	motor[TIRE_FR].dir = BACK;
7ka884	14:93776ca449a4	841	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	842	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	843	motor[TIRE_BL].dir = FOR;
7ka884	14:93776ca449a4	844
7ka884	14:93776ca449a4	845	motor[TIRE_FR].pwm = 48;
7ka884	14:93776ca449a4	846	motor[TIRE_FL].pwm = 43;
7ka884	14:93776ca449a4	847	motor[TIRE_BR].pwm = 43;
7ka884	14:93776ca449a4	848	motor[TIRE_BL].pwm = 43;
7ka884	14:93776ca449a4	849	}
7ka884	14:93776ca449a4	850	else if(invationA \|\| compA \|\| invationB \|\| compB \|\| invationC \|\| compC)
7ka884	14:93776ca449a4	851	{
7ka884	14:93776ca449a4	852	motor[TIRE_FR].dir = BRAKE;
7ka884	14:93776ca449a4	853	motor[TIRE_FL].dir = BRAKE;
7ka884	14:93776ca449a4	854	motor[TIRE_BR].dir = BRAKE;
7ka884	14:93776ca449a4	855	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	856
7ka884	14:93776ca449a4	857	motor[TIRE_FR].pwm = 255;
7ka884	14:93776ca449a4	858	motor[TIRE_FL].pwm = 255;
7ka884	14:93776ca449a4	859	motor[TIRE_BR].pwm = 255;
7ka884	14:93776ca449a4	860	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	861
7ka884	14:93776ca449a4	862	yokoflaB = true;
7ka884	14:93776ca449a4	863	traceonB = false;
7ka884	14:93776ca449a4	864	}
7ka884	14:93776ca449a4	865	}
7ka884	14:93776ca449a4	866
7ka884	14:93776ca449a4	867
7ka884	14:93776ca449a4	868	if(yokoflaB && !traceonB)
7ka884	14:93776ca449a4	869	{
7ka884	14:93776ca449a4	870	Color_changeflagB();
7ka884	14:93776ca449a4	871	if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
kishibekairohan	13:b6e02d6261d7	872	{
kishibekairohan	13:b6e02d6261d7	873	motor[TIRE_FR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	874	motor[TIRE_FL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	875	motor[TIRE_BR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	876	motor[TIRE_BL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	877
kishibekairohan	13:b6e02d6261d7	878	motor[TIRE_FR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	879	motor[TIRE_FL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	880	motor[TIRE_BR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	881	motor[TIRE_BL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	882
7ka884	14:93776ca449a4	883	boxslipB = true;
7ka884	14:93776ca449a4	884	yokoflaB = false;
kishibekairohan	13:b6e02d6261d7	885	}
7ka884	14:93776ca449a4	886	else if(LimitSw::IsPressed(Lim_R) \|\| LimitSw::IsPressed(Lim_L))
kishibekairohan	13:b6e02d6261d7	887	{
7ka884	14:93776ca449a4	888	motor[TIRE_FR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	889	motor[TIRE_FL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	890	motor[TIRE_BR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	891	motor[TIRE_BL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	892
7ka884	14:93776ca449a4	893	motor[TIRE_FR].pwm = 55;
7ka884	14:93776ca449a4	894	motor[TIRE_FL].pwm = 55;
7ka884	14:93776ca449a4	895	motor[TIRE_BR].pwm = 55;
7ka884	14:93776ca449a4	896	motor[TIRE_BL].pwm = 55;
kishibekairohan	13:b6e02d6261d7	897	}
7ka884	14:93776ca449a4	898	else if(invationA \|\| invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)))
kishibekairohan	13:b6e02d6261d7	899	{
kishibekairohan	13:b6e02d6261d7	900	motor[TIRE_FR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	901	motor[TIRE_FL].dir = FREE;
kishibekairohan	13:b6e02d6261d7	902	motor[TIRE_BR].dir = FREE;
kishibekairohan	13:b6e02d6261d7	903	motor[TIRE_BL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	904
7ka884	14:93776ca449a4	905	motor[TIRE_FR].pwm = 55;
7ka884	14:93776ca449a4	906	//motor[TIRE_FL].pwm = 30;
7ka884	14:93776ca449a4	907	//motor[TIRE_BR].pwm = 30;
7ka884	14:93776ca449a4	908	motor[TIRE_BL].pwm = 55;
7ka884	14:93776ca449a4	909	}
7ka884	14:93776ca449a4	910	else if(!invationA \|\| !invationB && (!LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L)))
7ka884	14:93776ca449a4	911	{
7ka884	14:93776ca449a4	912	motor[TIRE_FR].dir = FREE;
7ka884	14:93776ca449a4	913	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	914	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	915	motor[TIRE_BL].dir = FREE;
7ka884	14:93776ca449a4	916
7ka884	14:93776ca449a4	917	//motor[TIRE_FR].pwm = 30;
7ka884	14:93776ca449a4	918	motor[TIRE_FL].pwm = 55;
7ka884	14:93776ca449a4	919	motor[TIRE_BR].pwm = 55;
7ka884	14:93776ca449a4	920	//motor[TIRE_BL].pwm = 30;
kishibekairohan	13:b6e02d6261d7	921	}
7ka884	14:93776ca449a4	922	else
7ka884	14:93776ca449a4	923	{
7ka884	14:93776ca449a4	924	motor[TIRE_FR].dir = FOR;
7ka884	14:93776ca449a4	925	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	926	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	927	motor[TIRE_BL].dir = BACK;
7ka884	14:93776ca449a4	928
7ka884	14:93776ca449a4	929	motor[TIRE_FR].pwm = 50;
7ka884	14:93776ca449a4	930	motor[TIRE_FL].pwm = 50;
7ka884	14:93776ca449a4	931	motor[TIRE_BR].pwm = 50;
7ka884	14:93776ca449a4	932	motor[TIRE_BL].pwm = 50;
7ka884	14:93776ca449a4	933	}
7ka884	14:93776ca449a4	934	}
7ka884	14:93776ca449a4	935
7ka884	14:93776ca449a4	936
7ka884	14:93776ca449a4	937	/*********************************/
7ka884	14:93776ca449a4	938	if(boxslipB && !yokoflaB && !traceonB)
7ka884	14:93776ca449a4	939	{
7ka884	14:93776ca449a4	940	Color_changeflagB();
7ka884	14:93776ca449a4	941	if((compA && compB) \|\| compC){
7ka884	14:93776ca449a4	942
7ka884	14:93776ca449a4	943	RtX.reset();
7ka884	14:93776ca449a4	944	RtY.reset();
7ka884	14:93776ca449a4	945
7ka884	14:93776ca449a4	946	Rt_flagXB = true;
7ka884	14:93776ca449a4	947	boxslipB = false;
7ka884	14:93776ca449a4	948	}
7ka884	14:93776ca449a4	949	else if(invationA \|\| invationB){
7ka884	14:93776ca449a4	950	motor[TIRE_FR].dir = FOR;
7ka884	14:93776ca449a4	951	motor[TIRE_FL].dir = FOR;
kishibekairohan	13:b6e02d6261d7	952	motor[TIRE_BR].dir = BACK;
kishibekairohan	13:b6e02d6261d7	953	motor[TIRE_BL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	954
7ka884	14:93776ca449a4	955	motor[TIRE_FR].pwm = 25;
7ka884	14:93776ca449a4	956	motor[TIRE_FL].pwm = 25;
7ka884	14:93776ca449a4	957	motor[TIRE_BR].pwm = 25;
7ka884	14:93776ca449a4	958	motor[TIRE_BL].pwm = 25;
kishibekairohan	13:b6e02d6261d7	959	}
7ka884	14:93776ca449a4	960	else if(!invationA \|\| !invationB){
kishibekairohan	13:b6e02d6261d7	961	motor[TIRE_FR].dir = BACK;
kishibekairohan	13:b6e02d6261d7	962	motor[TIRE_FL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	963	motor[TIRE_BR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	964	motor[TIRE_BL].dir = FOR;
kishibekairohan	13:b6e02d6261d7	965
7ka884	14:93776ca449a4	966	motor[TIRE_FR].pwm = 25;
7ka884	14:93776ca449a4	967	motor[TIRE_FL].pwm = 25;
7ka884	14:93776ca449a4	968	motor[TIRE_BR].pwm = 25;
7ka884	14:93776ca449a4	969	motor[TIRE_BL].pwm = 25;
kishibekairohan	13:b6e02d6261d7	970	}
kishibekairohan	13:b6e02d6261d7	971	}
7ka884	14:93776ca449a4	972
7ka884	14:93776ca449a4	973
7ka884	14:93776ca449a4	974
7ka884	14:93776ca449a4	975	filipB();
7ka884	14:93776ca449a4	976	if(Rt_flagXB && !boxslipB)
kishibekairohan	13:b6e02d6261d7	977	{
7ka884	14:93776ca449a4	978	filipB();
7ka884	14:93776ca449a4	979	if(disXB > goalBX + 5){
7ka884	14:93776ca449a4	980	motor[TIRE_FR].dir = BACK;
7ka884	14:93776ca449a4	981	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	982	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	983	motor[TIRE_BL].dir = FOR;
kishibekairohan	13:b6e02d6261d7	984	motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
kishibekairohan	13:b6e02d6261d7	985	motor[TIRE_FL].pwm = SetPWM(RtpwmX);
kishibekairohan	13:b6e02d6261d7	986	motor[TIRE_BR].pwm = SetPWM(RtpwmX);
kishibekairohan	13:b6e02d6261d7	987	motor[TIRE_BL].pwm = SetPWM(RtpwmX);
kishibekairohan	13:b6e02d6261d7	988	}
7ka884	14:93776ca449a4	989	else if(disXB < goalBX + 5){
kishibekairohan	13:b6e02d6261d7	990	motor[TIRE_FR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	991	motor[TIRE_FL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	992	motor[TIRE_BR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	993	motor[TIRE_BL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	994
kishibekairohan	13:b6e02d6261d7	995	motor[TIRE_FR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	996	motor[TIRE_FL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	997	motor[TIRE_BR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	998	motor[TIRE_BL].pwm = 255;
7ka884	14:93776ca449a4	999
7ka884	14:93776ca449a4	1000	wait(1);
kishibekairohan	13:b6e02d6261d7	1001
7ka884	14:93776ca449a4	1002	Rt_flagYB = true;
7ka884	14:93776ca449a4	1003	Rt_flagXB = false;
kishibekairohan	13:b6e02d6261d7	1004	}
kishibekairohan	13:b6e02d6261d7	1005	}
kishibekairohan	13:b6e02d6261d7	1006
kishibekairohan	13:b6e02d6261d7	1007
7ka884	14:93776ca449a4	1008	if(Rt_flagYB && !Rt_flagXB){
7ka884	14:93776ca449a4	1009	filipB();
7ka884	14:93776ca449a4	1010	if(disYB < goalBY - 5){
kishibekairohan	13:b6e02d6261d7	1011	motor[TIRE_FR].dir = SetStatus(-RtpwmY);
kishibekairohan	13:b6e02d6261d7	1012	motor[TIRE_FL].dir = SetStatus(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1013	motor[TIRE_BR].dir = SetStatus(-RtpwmY);
kishibekairohan	13:b6e02d6261d7	1014	motor[TIRE_BL].dir = SetStatus(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1015	motor[TIRE_FR].pwm = SetPWM(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1016	motor[TIRE_FL].pwm = SetPWM(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1017	motor[TIRE_BR].pwm = SetPWM(RtpwmY);
7ka884	14:93776ca449a4	1018	motor[TIRE_BL].pwm = SetPWM(RtpwmY)*1.2;
kishibekairohan	13:b6e02d6261d7	1019	}
7ka884	14:93776ca449a4	1020	else if(disYB > goalBY - 5)
kishibekairohan	13:b6e02d6261d7	1021	{
kishibekairohan	13:b6e02d6261d7	1022	motor[TIRE_FR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1023	motor[TIRE_FL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1024	motor[TIRE_BR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1025	motor[TIRE_BL].dir = BRAKE;
7ka884	14:93776ca449a4	1026	motor[TIRE_FR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1027	motor[TIRE_FL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1028	motor[TIRE_BR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1029	motor[TIRE_BL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1030	}
kishibekairohan	13:b6e02d6261d7	1031	}
kishibekairohan	13:b6e02d6261d7	1032	}
kishibekairohan	13:b6e02d6261d7	1033	#endif
kishibekairohan	13:b6e02d6261d7	1034
7ka884	14:93776ca449a4	1035	#if USE_PROCESS_NUM>6//traceA
kishibekairohan	13:b6e02d6261d7	1036	static void Process6()
7ka884	14:93776ca449a4	1037	{
7ka884	14:93776ca449a4	1038	static bool traceon = true;//fase1
kishibekairohan	13:b6e02d6261d7	1039	static bool yokofla = false;//fase2
kishibekairohan	13:b6e02d6261d7	1040	static bool boxslip = false;//fase3
kishibekairohan	13:b6e02d6261d7	1041
kishibekairohan	13:b6e02d6261d7	1042	static bool Rt_flagX = false;
kishibekairohan	13:b6e02d6261d7	1043	static bool Rt_flagY = false;
kishibekairohan	13:b6e02d6261d7	1044
7ka884	14:93776ca449a4	1045	ColorDetection();
7ka884	14:93776ca449a4	1046	Color_changeflagA();
kishibekairohan	13:b6e02d6261d7	1047
7ka884	14:93776ca449a4	1048	if(traceon && !Rt_flagX && !Rt_flagY && !boxslip )
kishibekairohan	13:b6e02d6261d7	1049	{
7ka884	14:93776ca449a4	1050	Color_changeflagA();
7ka884	14:93776ca449a4	1051	if(!invationA && !compA && !invationB && !compB && !invationC && !compC)
kishibekairohan	13:b6e02d6261d7	1052	{
kishibekairohan	13:b6e02d6261d7	1053	motor[TIRE_FR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	1054	motor[TIRE_FL].dir = FOR;
kishibekairohan	13:b6e02d6261d7	1055	motor[TIRE_BR].dir = BACK;
kishibekairohan	13:b6e02d6261d7	1056	motor[TIRE_BL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	1057
7ka884	14:93776ca449a4	1058	motor[TIRE_FR].pwm = 42;
7ka884	14:93776ca449a4	1059	motor[TIRE_FL].pwm = 42;
7ka884	14:93776ca449a4	1060	motor[TIRE_BR].pwm = 42;
7ka884	14:93776ca449a4	1061	motor[TIRE_BL].pwm = 42;
kishibekairohan	13:b6e02d6261d7	1062	}
7ka884	14:93776ca449a4	1063	else if(invationA \|\| compA \|\| invationB \|\| compB \|\| invationC \|\| compC)
kishibekairohan	13:b6e02d6261d7	1064	{
kishibekairohan	13:b6e02d6261d7	1065	motor[TIRE_FR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1066	motor[TIRE_FL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1067	motor[TIRE_BR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1068	motor[TIRE_BL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1069
kishibekairohan	13:b6e02d6261d7	1070	motor[TIRE_FR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1071	motor[TIRE_FL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1072	motor[TIRE_BR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1073	motor[TIRE_BL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1074
kishibekairohan	13:b6e02d6261d7	1075	yokofla = true;
kishibekairohan	13:b6e02d6261d7	1076	traceon = false;
kishibekairohan	13:b6e02d6261d7	1077	}
kishibekairohan	13:b6e02d6261d7	1078	}
kishibekairohan	13:b6e02d6261d7	1079
kishibekairohan	13:b6e02d6261d7	1080
kishibekairohan	13:b6e02d6261d7	1081	if(yokofla && !traceon)
kishibekairohan	13:b6e02d6261d7	1082	{
7ka884	14:93776ca449a4	1083	Color_changeflagA();
7ka884	14:93776ca449a4	1084	if(LimitSw::IsPressed(Lim_R) && LimitSw::IsPressed(Lim_L))
kishibekairohan	13:b6e02d6261d7	1085	{
kishibekairohan	13:b6e02d6261d7	1086	motor[TIRE_FR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1087	motor[TIRE_FL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1088	motor[TIRE_BR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1089	motor[TIRE_BL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1090
kishibekairohan	13:b6e02d6261d7	1091	motor[TIRE_FR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1092	motor[TIRE_FL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1093	motor[TIRE_BR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1094	motor[TIRE_BL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1095
kishibekairohan	13:b6e02d6261d7	1096	boxslip = true;
kishibekairohan	13:b6e02d6261d7	1097	yokofla = false;
kishibekairohan	13:b6e02d6261d7	1098	}
7ka884	14:93776ca449a4	1099	else if(LimitSw::IsPressed(Lim_R) \|\| LimitSw::IsPressed(Lim_L))
kishibekairohan	13:b6e02d6261d7	1100	{
7ka884	14:93776ca449a4	1101	motor[TIRE_FR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	1102	motor[TIRE_FL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	1103	motor[TIRE_BR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	1104	motor[TIRE_BL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	1105
7ka884	14:93776ca449a4	1106	motor[TIRE_FR].pwm = 55;
7ka884	14:93776ca449a4	1107	motor[TIRE_FL].pwm = 55;
7ka884	14:93776ca449a4	1108	motor[TIRE_BR].pwm = 55;
7ka884	14:93776ca449a4	1109	motor[TIRE_BL].pwm = 55;
kishibekairohan	13:b6e02d6261d7	1110	}
7ka884	14:93776ca449a4	1111	else if((invationA \|\| invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
kishibekairohan	13:b6e02d6261d7	1112	{
kishibekairohan	13:b6e02d6261d7	1113	motor[TIRE_FR].dir = FREE;
kishibekairohan	13:b6e02d6261d7	1114	motor[TIRE_FL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	1115	motor[TIRE_BR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	1116	motor[TIRE_BL].dir = FREE;
kishibekairohan	13:b6e02d6261d7	1117
kishibekairohan	13:b6e02d6261d7	1118	//motor[TIRE_FR].pwm = startP;
7ka884	14:93776ca449a4	1119	motor[TIRE_FL].pwm = 55;
7ka884	14:93776ca449a4	1120	motor[TIRE_BR].pwm = 55;
kishibekairohan	13:b6e02d6261d7	1121	//motor[TIRE_BL].pwm = startP;
kishibekairohan	13:b6e02d6261d7	1122	}
7ka884	14:93776ca449a4	1123	else if((!invationA \|\| !invationB) && !LimitSw::IsPressed(Lim_R) && !LimitSw::IsPressed(Lim_L))
kishibekairohan	13:b6e02d6261d7	1124	{
kishibekairohan	13:b6e02d6261d7	1125	motor[TIRE_FR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	1126	motor[TIRE_FL].dir = FREE;
kishibekairohan	13:b6e02d6261d7	1127	motor[TIRE_BR].dir = FREE;
kishibekairohan	13:b6e02d6261d7	1128	motor[TIRE_BL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	1129
7ka884	14:93776ca449a4	1130	motor[TIRE_FR].pwm = 55;
kishibekairohan	13:b6e02d6261d7	1131	//motor[TIRE_FL].pwm = startP;
7ka884	14:93776ca449a4	1132	//motor[TIRE_BR].pwm = startP;
7ka884	14:93776ca449a4	1133	motor[TIRE_BL].pwm = 55;
7ka884	14:93776ca449a4	1134	}
7ka884	14:93776ca449a4	1135	else
7ka884	14:93776ca449a4	1136	{
7ka884	14:93776ca449a4	1137	motor[TIRE_FR].dir = FOR;
7ka884	14:93776ca449a4	1138	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	1139	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	1140	motor[TIRE_BL].dir = BACK;
7ka884	14:93776ca449a4	1141
7ka884	14:93776ca449a4	1142	motor[TIRE_FR].pwm = 50;
7ka884	14:93776ca449a4	1143	motor[TIRE_FL].pwm = 50;
7ka884	14:93776ca449a4	1144	motor[TIRE_BR].pwm = 50;
7ka884	14:93776ca449a4	1145	motor[TIRE_BL].pwm = 50;
kishibekairohan	13:b6e02d6261d7	1146	}
7ka884	14:93776ca449a4	1147	}
7ka884	14:93776ca449a4	1148
7ka884	14:93776ca449a4	1149
7ka884	14:93776ca449a4	1150	/*********************************/
7ka884	14:93776ca449a4	1151	if(boxslip && !yokofla && !traceon)
7ka884	14:93776ca449a4	1152	{
7ka884	14:93776ca449a4	1153	Color_changeflagA();
7ka884	14:93776ca449a4	1154	if((compA && compB) \|\| compC){
7ka884	14:93776ca449a4	1155
7ka884	14:93776ca449a4	1156	RtX.reset();
7ka884	14:93776ca449a4	1157	RtY.reset();
7ka884	14:93776ca449a4	1158
7ka884	14:93776ca449a4	1159	Rt_flagX = true;
7ka884	14:93776ca449a4	1160	boxslip = false;
7ka884	14:93776ca449a4	1161	}
7ka884	14:93776ca449a4	1162	else if(invationA \|\| invationB && !compC && !(compA && compB)){
7ka884	14:93776ca449a4	1163	motor[TIRE_FR].dir = BACK;
7ka884	14:93776ca449a4	1164	motor[TIRE_FL].dir = BACK;
7ka884	14:93776ca449a4	1165	motor[TIRE_BR].dir = FOR;
7ka884	14:93776ca449a4	1166	motor[TIRE_BL].dir = FOR;
7ka884	14:93776ca449a4	1167
7ka884	14:93776ca449a4	1168	motor[TIRE_FR].pwm = 25;
7ka884	14:93776ca449a4	1169	motor[TIRE_FL].pwm = 25;
7ka884	14:93776ca449a4	1170	motor[TIRE_BR].pwm = 25;
7ka884	14:93776ca449a4	1171	motor[TIRE_BL].pwm = 25;
7ka884	14:93776ca449a4	1172	}
7ka884	14:93776ca449a4	1173	else if(!invationA \|\| !invationB && !compC && !(compA && compB)){
7ka884	14:93776ca449a4	1174	motor[TIRE_FR].dir = FOR;
7ka884	14:93776ca449a4	1175	motor[TIRE_FL].dir = FOR;
kishibekairohan	13:b6e02d6261d7	1176	motor[TIRE_BR].dir = BACK;
kishibekairohan	13:b6e02d6261d7	1177	motor[TIRE_BL].dir = BACK;
kishibekairohan	13:b6e02d6261d7	1178
7ka884	14:93776ca449a4	1179	motor[TIRE_FR].pwm = 25;
7ka884	14:93776ca449a4	1180	motor[TIRE_FL].pwm = 25;
7ka884	14:93776ca449a4	1181	motor[TIRE_BR].pwm = 25;
7ka884	14:93776ca449a4	1182	motor[TIRE_BL].pwm = 25;
7ka884	14:93776ca449a4	1183	}
7ka884	14:93776ca449a4	1184	}
7ka884	14:93776ca449a4	1185
7ka884	14:93776ca449a4	1186
7ka884	14:93776ca449a4	1187	filip();
7ka884	14:93776ca449a4	1188	if(Rt_flagX && !boxslip)
7ka884	14:93776ca449a4	1189	{
7ka884	14:93776ca449a4	1190	filip();
7ka884	14:93776ca449a4	1191	if(disX < goalAX - 5){
kishibekairohan	13:b6e02d6261d7	1192
kishibekairohan	13:b6e02d6261d7	1193	motor[TIRE_FR].dir = FOR;
kishibekairohan	13:b6e02d6261d7	1194	motor[TIRE_FL].dir = FOR;
7ka884	14:93776ca449a4	1195	motor[TIRE_BR].dir = BACK;
7ka884	14:93776ca449a4	1196	motor[TIRE_BL].dir = BACK;
7ka884	14:93776ca449a4	1197	motor[TIRE_FR].pwm = SetPWM(RtpwmX)*0.8;
7ka884	14:93776ca449a4	1198	motor[TIRE_FL].pwm = SetPWM(RtpwmX);
7ka884	14:93776ca449a4	1199	motor[TIRE_BR].pwm = SetPWM(RtpwmX);
7ka884	14:93776ca449a4	1200	motor[TIRE_BL].pwm = SetPWM(RtpwmX);
7ka884	14:93776ca449a4	1201	}
7ka884	14:93776ca449a4	1202	else if(disX > goalAX - 5){
kishibekairohan	13:b6e02d6261d7	1203
kishibekairohan	13:b6e02d6261d7	1204	motor[TIRE_FR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1205	motor[TIRE_FL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1206	motor[TIRE_BR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1207	motor[TIRE_BL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1208
kishibekairohan	13:b6e02d6261d7	1209	motor[TIRE_FR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1210	motor[TIRE_FL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1211	motor[TIRE_BR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1212	motor[TIRE_BL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1213
7ka884	14:93776ca449a4	1214	wait(1);
kishibekairohan	13:b6e02d6261d7	1215
kishibekairohan	13:b6e02d6261d7	1216	Rt_flagY = true;
kishibekairohan	13:b6e02d6261d7	1217	Rt_flagX = false;
kishibekairohan	13:b6e02d6261d7	1218	}
kishibekairohan	13:b6e02d6261d7	1219	}
kishibekairohan	13:b6e02d6261d7	1220
kishibekairohan	13:b6e02d6261d7	1221
kishibekairohan	13:b6e02d6261d7	1222	if(Rt_flagY && !Rt_flagX){
kishibekairohan	13:b6e02d6261d7	1223	filip();
7ka884	14:93776ca449a4	1224	if(disY < goalAY - 5){
kishibekairohan	13:b6e02d6261d7	1225	motor[TIRE_FR].dir = SetStatus(-RtpwmY);
kishibekairohan	13:b6e02d6261d7	1226	motor[TIRE_FL].dir = SetStatus(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1227	motor[TIRE_BR].dir = SetStatus(-RtpwmY);
kishibekairohan	13:b6e02d6261d7	1228	motor[TIRE_BL].dir = SetStatus(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1229	motor[TIRE_FR].pwm = SetPWM(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1230	motor[TIRE_FL].pwm = SetPWM(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1231	motor[TIRE_BR].pwm = SetPWM(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1232	motor[TIRE_BL].pwm = SetPWM(RtpwmY);
kishibekairohan	13:b6e02d6261d7	1233	}
7ka884	14:93776ca449a4	1234	else if(disY > goalAY - 5)
kishibekairohan	13:b6e02d6261d7	1235	{
kishibekairohan	13:b6e02d6261d7	1236	motor[TIRE_FR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1237	motor[TIRE_FL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1238	motor[TIRE_BR].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1239	motor[TIRE_BL].dir = BRAKE;
kishibekairohan	13:b6e02d6261d7	1240	motor[TIRE_FR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1241	motor[TIRE_FL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1242	motor[TIRE_BR].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1243	motor[TIRE_BL].pwm = 255;
kishibekairohan	13:b6e02d6261d7	1244	}
kishibekairohan	13:b6e02d6261d7	1245	}
kishibekairohan	13:b6e02d6261d7	1246	}
kishibekairohan	13:b6e02d6261d7	1247	#endif
kishibekairohan	13:b6e02d6261d7	1248
kishibekairohan	13:b6e02d6261d7	1249	#if USE_PROCESS_NUM>7
kishibekairohan	13:b6e02d6261d7	1250	static void Process7()
kishibekairohan	13:b6e02d6261d7	1251	{
kishibekairohan	12:c09b3e08a316	1252	tapeLED.code = (uint32_t)Hotpink;
kishibekairohan	12:c09b3e08a316	1253	static bool Xnopush = false;
kishibekairohan	12:c09b3e08a316	1254	static bool Ynopush = false;
kishibekairohan	12:c09b3e08a316	1255	static bool Inopush = false;
kishibekairohan	12:c09b3e08a316	1256
kishibekairohan	12:c09b3e08a316	1257	static bool Angle_flagX = false;
kishibekairohan	12:c09b3e08a316	1258	static bool Angle_flagY = false;
kishibekairohan	12:c09b3e08a316	1259	static bool ANgle_flagI = false;
kishibekairohan	12:c09b3e08a316	1260
kishibekairohan	12:c09b3e08a316	1261	if(LimitSw::IsPressed(Lim_AR) && motor[Angle_R].dir == FOR && motor[Angle_L].dir == BACK){
kishibekairohan	12:c09b3e08a316	1262	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1263	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1264	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	1265	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	1266	}else if(LimitSw::IsPressed(Lim_AL) && motor[Angle_R].dir == BACK && motor[Angle_L].dir == FOR){
kishibekairohan	12:c09b3e08a316	1267	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1268	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1269	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	1270	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	1271	}
kishibekairohan	12:c09b3e08a316	1272	for(int i = 0;i<20;i++){
kishibekairohan	12:c09b3e08a316	1273	float y = 0;
kishibekairohan	12:c09b3e08a316	1274	y = acc[1]*1000;
kishibekairohan	12:c09b3e08a316	1275	float rotateY = (y - 305)/2.21 - 90;
kishibekairohan	12:c09b3e08a316	1276	Angle += rotateY;
kishibekairohan	12:c09b3e08a316	1277	}
kishibekairohan	12:c09b3e08a316	1278	Angle = Angle /20;
kishibekairohan	12:c09b3e08a316	1279
kishibekairohan	12:c09b3e08a316	1280	int gyropwmX = gyro.SetPV(Angle,AngletargetX);
kishibekairohan	12:c09b3e08a316	1281	int gyropwmY = gyro.SetPV(Angle,AngletargetY);
kishibekairohan	12:c09b3e08a316	1282	int gyropwmI = gyro.SetPV(Angle,AngletargetI);
kishibekairohan	12:c09b3e08a316	1283
kishibekairohan	12:c09b3e08a316	1284	if(controller->Button.X && !Xnopush){
kishibekairohan	12:c09b3e08a316	1285	Angle_flagX = true;
kishibekairohan	12:c09b3e08a316	1286	Xnopush = true;
kishibekairohan	12:c09b3e08a316	1287	}else if(!controller->Button.X)Xnopush = false;
kishibekairohan	12:c09b3e08a316	1288
kishibekairohan	12:c09b3e08a316	1289	if(controller->Button.Y && !Ynopush){
kishibekairohan	12:c09b3e08a316	1290	Angle_flagY = true;
kishibekairohan	12:c09b3e08a316	1291	Ynopush = true;
kishibekairohan	12:c09b3e08a316	1292	}else if(!controller->Button.Y)Ynopush = false;
kishibekairohan	12:c09b3e08a316	1293
kishibekairohan	12:c09b3e08a316	1294	if(controller->Button.A && !Inopush){
kishibekairohan	12:c09b3e08a316	1295	Angle_flagI = true;
kishibekairohan	12:c09b3e08a316	1296	Inopush = true;
kishibekairohan	12:c09b3e08a316	1297	}else if(!controller->Button.A)Inopush = false;
kishibekairohan	12:c09b3e08a316	1298
kishibekairohan	12:c09b3e08a316	1299	if (Angle_flagX){
kishibekairohan	12:c09b3e08a316	1300	motor[Angle_R].dir = SetStatus(gyropwmX);
kishibekairohan	12:c09b3e08a316	1301	motor[Angle_L].dir = SetStatus(-gyropwmX);
kishibekairohan	12:c09b3e08a316	1302	motor[Angle_R].pwm = SetPWM(gyropwmX);
kishibekairohan	12:c09b3e08a316	1303	motor[Angle_L].pwm = SetPWM(gyropwmX);
kishibekairohan	12:c09b3e08a316	1304	if(AngletargetX - 2 < Angle && Angle < AngletargetX + 2){
kishibekairohan	12:c09b3e08a316	1305	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1306	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1307	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	1308	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	1309	Angle_flagX = false;
kishibekairohan	12:c09b3e08a316	1310	}
kishibekairohan	12:c09b3e08a316	1311	}
kishibekairohan	12:c09b3e08a316	1312
kishibekairohan	12:c09b3e08a316	1313	if (Angle_flagY){
kishibekairohan	12:c09b3e08a316	1314	motor[Angle_R].dir = SetStatus(-gyropwmY);
kishibekairohan	12:c09b3e08a316	1315	motor[Angle_L].dir = SetStatus(gyropwmY);
kishibekairohan	12:c09b3e08a316	1316	motor[Angle_R].pwm = SetPWM(gyropwmY);
kishibekairohan	12:c09b3e08a316	1317	motor[Angle_L].pwm = SetPWM(gyropwmY);
kishibekairohan	12:c09b3e08a316	1318	if(AngletargetY - 2 < Angle && Angle < AngletargetY + 2){
kishibekairohan	12:c09b3e08a316	1319	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1320	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1321	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	1322	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	1323	Angle_flagY = false;
kishibekairohan	12:c09b3e08a316	1324	}
kishibekairohan	12:c09b3e08a316	1325	}
kishibekairohan	12:c09b3e08a316	1326
kishibekairohan	12:c09b3e08a316	1327	if (Angle_flagI){
kishibekairohan	12:c09b3e08a316	1328	if(Angle < 0)
kishibekairohan	12:c09b3e08a316	1329	{
kishibekairohan	12:c09b3e08a316	1330	motor[Angle_R].dir = SetStatus(-gyropwmI);
kishibekairohan	12:c09b3e08a316	1331	motor[Angle_L].dir = SetStatus(gyropwmI);
kishibekairohan	12:c09b3e08a316	1332	motor[Angle_R].pwm = SetPWM(gyropwmI);
kishibekairohan	12:c09b3e08a316	1333	motor[Angle_L].pwm = SetPWM(gyropwmI);
kishibekairohan	12:c09b3e08a316	1334
kishibekairohan	12:c09b3e08a316	1335	if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
kishibekairohan	12:c09b3e08a316	1336	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1337	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1338	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	1339	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	1340	Angle_flagI = false;
kishibekairohan	12:c09b3e08a316	1341	}
kishibekairohan	12:c09b3e08a316	1342	}
kishibekairohan	12:c09b3e08a316	1343	else if(Angle > 0)
kishibekairohan	12:c09b3e08a316	1344	{
kishibekairohan	12:c09b3e08a316	1345	motor[Angle_R].dir = FOR;
kishibekairohan	12:c09b3e08a316	1346	motor[Angle_L].dir = BACK;
kishibekairohan	12:c09b3e08a316	1347	motor[Angle_R].pwm = 150;
kishibekairohan	12:c09b3e08a316	1348	motor[Angle_L].pwm = 150;
kishibekairohan	12:c09b3e08a316	1349
kishibekairohan	12:c09b3e08a316	1350	if(Angle < 0){
kishibekairohan	12:c09b3e08a316	1351	motor[Angle_R].dir = SetStatus(gyropwmI);
kishibekairohan	12:c09b3e08a316	1352	motor[Angle_L].dir = SetStatus(-gyropwmI);
kishibekairohan	12:c09b3e08a316	1353	motor[Angle_R].pwm = SetPWM(gyropwmI);
kishibekairohan	12:c09b3e08a316	1354	motor[Angle_L].pwm = SetPWM(gyropwmI);
kishibekairohan	12:c09b3e08a316	1355
kishibekairohan	12:c09b3e08a316	1356	if(AngletargetI - 2 < Angle && Angle < AngletargetI + 2){
kishibekairohan	12:c09b3e08a316	1357	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1358	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1359	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	1360	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	1361	Angle_flagI = false;
kishibekairohan	12:c09b3e08a316	1362	}
kishibekairohan	12:c09b3e08a316	1363	}
kishibekairohan	12:c09b3e08a316	1364	}
kishibekairohan	12:c09b3e08a316	1365	}
kishibekairohan	12:c09b3e08a316	1366	else{
kishibekairohan	12:c09b3e08a316	1367	motor[Angle_R].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1368	motor[Angle_L].dir = BRAKE;
kishibekairohan	12:c09b3e08a316	1369	motor[Angle_R].pwm = 255;
kishibekairohan	12:c09b3e08a316	1370	motor[Angle_L].pwm = 255;
kishibekairohan	12:c09b3e08a316	1371	}
kishibekairohan	12:c09b3e08a316	1372	}
kishibekairohan	12:c09b3e08a316	1373	#endif
kishibekairohan	12:c09b3e08a316	1374
kishibekairohan	13:b6e02d6261d7	1375	#if USE_PROCESS_NUM>8 //kakudo
kishibekairohan	13:b6e02d6261d7	1376	static void Process8()
kishibekairohan	12:c09b3e08a316	1377	{
kishibekairohan	12:c09b3e08a316	1378	}
kishibekairohan	12:c09b3e08a316	1379	#endif
kishibekairohan	12:c09b3e08a316	1380
kishibekairohan	13:b6e02d6261d7	1381	#if USE_PROCESS_NUM>9
kishibekairohan	13:b6e02d6261d7	1382	static void Process9()
kishibekairohan	12:c09b3e08a316	1383	{
kishibekairohan	2:c015739085d3	1384
t_yamamoto	0:669ef71cba68	1385	}
t_yamamoto	0:669ef71cba68	1386	#endif
t_yamamoto	0:669ef71cba68	1387	#endif
t_yamamoto	0:669ef71cba68	1388	#pragma endregion PROCESS
t_yamamoto	0:669ef71cba68	1389
t_yamamoto	0:669ef71cba68	1390	static void AllActuatorReset()
t_yamamoto	0:669ef71cba68	1391	{
t_yamamoto	0:669ef71cba68	1392
t_yamamoto	0:669ef71cba68	1393	#ifdef USE_SOLENOID
t_yamamoto	0:669ef71cba68	1394	solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto	0:669ef71cba68	1395	#endif
t_yamamoto	0:669ef71cba68	1396
t_yamamoto	0:669ef71cba68	1397	#ifdef USE_MOTOR
t_yamamoto	0:669ef71cba68	1398	for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto	0:669ef71cba68	1399	{
t_yamamoto	0:669ef71cba68	1400	motor[i].dir = FREE;
t_yamamoto	0:669ef71cba68	1401	motor[i].pwm = 0;
t_yamamoto	0:669ef71cba68	1402	}
t_yamamoto	0:669ef71cba68	1403	#endif
t_yamamoto	0:669ef71cba68	1404	}
t_yamamoto	0:669ef71cba68	1405
t_yamamoto	0:669ef71cba68	1406	#pragma region USER-DEFINED-FUNCTIONS
kishibekairohan	8:6fb3723f7747	1407
kishibekairohan	8:6fb3723f7747	1408	void filip(){
kishibekairohan	8:6fb3723f7747	1409	palseX = RtX.getPulses();
kishibekairohan	8:6fb3723f7747	1410	palseY = RtY.getPulses();
7ka884	6:10e22bc327ce	1411
kishibekairohan	9:f93fc79a49ea	1412	rpmX = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan	9:f93fc79a49ea	1413	rpmY = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
kishibekairohan	9:f93fc79a49ea	1414
kishibekairohan	10:1295d39fec3a	1415	disX = 483.141rpmX;
kishibekairohan	9:f93fc79a49ea	1416	disY = 483.141rpmY;
kishibekairohan	9:f93fc79a49ea	1417
7ka884	14:93776ca449a4	1418	RtpwmX = (int)Rt_X.SetPV(disX , goalAX);
7ka884	14:93776ca449a4	1419	RtpwmY = (int)Rt_Y.SetPV(disY , goalAY);
7ka884	14:93776ca449a4	1420	}
7ka884	14:93776ca449a4	1421
7ka884	14:93776ca449a4	1422	void filipB(){
7ka884	14:93776ca449a4	1423	palseX = RtX.getPulses();
7ka884	14:93776ca449a4	1424	palseY = RtY.getPulses();
7ka884	14:93776ca449a4	1425
7ka884	14:93776ca449a4	1426	rpmXB = (double)palseX/(ROTATE_PER_REVOLUTIONS*4);
7ka884	14:93776ca449a4	1427	rpmYB = (double)palseY/(ROTATE_PER_REVOLUTIONS*4);
7ka884	14:93776ca449a4	1428
7ka884	14:93776ca449a4	1429	disXB = 483.141rpmXB;
7ka884	14:93776ca449a4	1430	disYB = 483.141rpmYB;
7ka884	14:93776ca449a4	1431
7ka884	14:93776ca449a4	1432	RtpwmXB = (int)Rt_X.SetPV(disXB , goalBX);
7ka884	14:93776ca449a4	1433	RtpwmYB = (int)Rt_Y.SetPV(disYB , goalBY);
kishibekairohan	8:6fb3723f7747	1434	}
7ka884	6:10e22bc327ce	1435
kishibekairohan	12:c09b3e08a316	1436	unsigned long ColorIn(int index)
kishibekairohan	12:c09b3e08a316	1437	{
kishibekairohan	12:c09b3e08a316	1438	int result = 0;
kishibekairohan	12:c09b3e08a316	1439	bool rtn = false;
kishibekairohan	12:c09b3e08a316	1440	for(int i=0; i<12; i++)
kishibekairohan	12:c09b3e08a316	1441	{
kishibekairohan	12:c09b3e08a316	1442	CK[index] = 1;
kishibekairohan	12:c09b3e08a316	1443	rtn = DOUT[index];
kishibekairohan	12:c09b3e08a316	1444	CK[index] = 0;
kishibekairohan	12:c09b3e08a316	1445	if(rtn)
kishibekairohan	12:c09b3e08a316	1446	{
kishibekairohan	12:c09b3e08a316	1447	result\|=(1 << i);
kishibekairohan	12:c09b3e08a316	1448	}
kishibekairohan	12:c09b3e08a316	1449	}
kishibekairohan	12:c09b3e08a316	1450	return result;
kishibekairohan	12:c09b3e08a316	1451	}
7ka884	6:10e22bc327ce	1452
kishibekairohan	2:c015739085d3	1453	void ColorDetection(){
kishibekairohan	2:c015739085d3	1454	GATE = 0;
kishibekairohan	2:c015739085d3	1455
kishibekairohan	2:c015739085d3	1456	CK[0] = 0;
kishibekairohan	2:c015739085d3	1457	CK[1] = 0;
kishibekairohan	2:c015739085d3	1458	CK[2] = 0;
kishibekairohan	2:c015739085d3	1459	CK[3] = 0;
kishibekairohan	2:c015739085d3	1460
kishibekairohan	2:c015739085d3	1461	RANGE = 1;
kishibekairohan	2:c015739085d3	1462
kishibekairohan	2:c015739085d3	1463	GATE = 1;
kishibekairohan	2:c015739085d3	1464	wait_ms(intergration);
kishibekairohan	2:c015739085d3	1465	GATE = 0;
kishibekairohan	2:c015739085d3	1466	wait_us(4);
kishibekairohan	2:c015739085d3	1467
kishibekairohan	2:c015739085d3	1468	Color_A[0] = ColorIn(0); //赤
kishibekairohan	2:c015739085d3	1469	wait_us(3);
kishibekairohan	2:c015739085d3	1470	Color_A[1] = ColorIn(0); //青
kishibekairohan	2:c015739085d3	1471	wait_us(3);
kishibekairohan	2:c015739085d3	1472	Color_A[2] = ColorIn(0); //緑
kishibekairohan	2:c015739085d3	1473
kishibekairohan	2:c015739085d3	1474	Color_B[0] = ColorIn(1);
kishibekairohan	2:c015739085d3	1475	wait_us(3);
kishibekairohan	2:c015739085d3	1476	Color_B[1] = ColorIn(1);
kishibekairohan	2:c015739085d3	1477	wait_us(3);
kishibekairohan	2:c015739085d3	1478	Color_B[2] = ColorIn(1);
kishibekairohan	2:c015739085d3	1479
kishibekairohan	2:c015739085d3	1480	Color_C[0] = ColorIn(2);
kishibekairohan	2:c015739085d3	1481	wait_us(3);
kishibekairohan	2:c015739085d3	1482	Color_C[1] = ColorIn(2);
kishibekairohan	2:c015739085d3	1483	wait_us(3);
kishibekairohan	2:c015739085d3	1484	Color_C[2] = ColorIn(2);
kishibekairohan	2:c015739085d3	1485
kishibekairohan	2:c015739085d3	1486	Color_D[0] = ColorIn(3);
kishibekairohan	2:c015739085d3	1487	wait_us(3);
kishibekairohan	2:c015739085d3	1488	Color_D[1] = ColorIn(3);
kishibekairohan	2:c015739085d3	1489	wait_us(3);
kishibekairohan	2:c015739085d3	1490	Color_D[2] = ColorIn(3);
kishibekairohan	2:c015739085d3	1491	}
kishibekairohan	12:c09b3e08a316	1492
7ka884	14:93776ca449a4	1493	void Color_changeflagA(){
kishibekairohan	13:b6e02d6261d7	1494	ColorDetection();
kishibekairohan	13:b6e02d6261d7	1495
kishibekairohan	13:b6e02d6261d7	1496	if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
kishibekairohan	13:b6e02d6261d7	1497	{
kishibekairohan	13:b6e02d6261d7	1498	compA = true;//on true,noon false
kishibekairohan	13:b6e02d6261d7	1499	}
7ka884	14:93776ca449a4	1500	else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶
kishibekairohan	13:b6e02d6261d7	1501
kishibekairohan	13:b6e02d6261d7	1502	if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
kishibekairohan	13:b6e02d6261d7	1503	{
kishibekairohan	13:b6e02d6261d7	1504	compB = true;//on true,noon false
kishibekairohan	13:b6e02d6261d7	1505	}
7ka884	14:93776ca449a4	1506	else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶
kishibekairohan	13:b6e02d6261d7	1507
kishibekairohan	13:b6e02d6261d7	1508	if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
kishibekairohan	13:b6e02d6261d7	1509	{
kishibekairohan	13:b6e02d6261d7	1510	compC = true;//on true,noon false
kishibekairohan	13:b6e02d6261d7	1511	}
7ka884	14:93776ca449a4	1512	else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[2])compC = false;//茶
kishibekairohan	13:b6e02d6261d7	1513	/*
kishibekairohan	13:b6e02d6261d7	1514	if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
kishibekairohan	13:b6e02d6261d7	1515	{
kishibekairohan	13:b6e02d6261d7	1516	invationD ^= 1;//start false,over true
kishibekairohan	13:b6e02d6261d7	1517	compD = true;//on true,noon false
kishibekairohan	13:b6e02d6261d7	1518	}
kishibekairohan	13:b6e02d6261d7	1519	else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
kishibekairohan	13:b6e02d6261d7	1520	*/
7ka884	14:93776ca449a4	1521
7ka884	14:93776ca449a4	1522	if(compA && !compB)
7ka884	14:93776ca449a4	1523	{
7ka884	14:93776ca449a4	1524	invationA = false;
7ka884	14:93776ca449a4	1525	}
7ka884	14:93776ca449a4	1526	else if(!compA && compB)
7ka884	14:93776ca449a4	1527	{
7ka884	14:93776ca449a4	1528	invationA = true;
7ka884	14:93776ca449a4	1529	}
7ka884	14:93776ca449a4	1530
7ka884	14:93776ca449a4	1531	if(!invationA && !compA && !compB){
7ka884	14:93776ca449a4	1532	invationB = false;
7ka884	14:93776ca449a4	1533	}
7ka884	14:93776ca449a4	1534	else if(invationA && !compA && !compB){
7ka884	14:93776ca449a4	1535	invationB = true;
7ka884	14:93776ca449a4	1536	}
7ka884	14:93776ca449a4	1537	}
7ka884	14:93776ca449a4	1538
7ka884	14:93776ca449a4	1539	void Color_changeflagB(){
7ka884	14:93776ca449a4	1540	ColorDetection();
7ka884	14:93776ca449a4	1541
7ka884	14:93776ca449a4	1542	if(Color_A[0] > PointA[0] && Color_A[1] > PointA[1] && Color_A[2] > PointA[2] && !compA)//白
7ka884	14:93776ca449a4	1543	{
7ka884	14:93776ca449a4	1544	compA = true;//on true,noon false
7ka884	14:93776ca449a4	1545	}
7ka884	14:93776ca449a4	1546	else if(Color_A[0] < PointA[0] && Color_A[1] < PointA[1] && Color_A[2] < PointA[2])compA = false;//茶
7ka884	14:93776ca449a4	1547
7ka884	14:93776ca449a4	1548	if(Color_B[0] > PointB[0] && Color_B[1] > PointB[1] && Color_B[2] > PointB[2] && !compB)//白
7ka884	14:93776ca449a4	1549	{
7ka884	14:93776ca449a4	1550	compB = true;//on true,noon false
7ka884	14:93776ca449a4	1551	}
7ka884	14:93776ca449a4	1552	else if(Color_B[0] < PointB[0] && Color_B[1] < PointB[1] && Color_B[2] < PointB[2])compB = false;//茶
7ka884	14:93776ca449a4	1553
7ka884	14:93776ca449a4	1554	if(Color_C[0] > PointC[0] && Color_C[1] > PointC[1] && Color_C[2] > PointC[2] && !compC)//白
7ka884	14:93776ca449a4	1555	{
7ka884	14:93776ca449a4	1556	compC = true;//on true,noon false
7ka884	14:93776ca449a4	1557	}
7ka884	14:93776ca449a4	1558	else if(Color_C[0] < PointC[0] && Color_C[1] < PointC[1] && Color_C[2] < PointC[2])compC = false;//茶
7ka884	14:93776ca449a4	1559	/*
7ka884	14:93776ca449a4	1560	if(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2] && !compD)//白
7ka884	14:93776ca449a4	1561	{
7ka884	14:93776ca449a4	1562	invationD ^= 1;//start false,over true
7ka884	14:93776ca449a4	1563	compD = true;//on true,noon false
7ka884	14:93776ca449a4	1564	}
7ka884	14:93776ca449a4	1565	else if(!(Color_D[0] > Point[0] && Color_D[1] > Point[1] && Color_D[2] > Point[2]))compD = false;//茶
7ka884	14:93776ca449a4	1566	*/
7ka884	14:93776ca449a4	1567
7ka884	14:93776ca449a4	1568	if(compB && !compA)
7ka884	14:93776ca449a4	1569	{
7ka884	14:93776ca449a4	1570	invationB = false;
7ka884	14:93776ca449a4	1571	}
7ka884	14:93776ca449a4	1572	else if(!compB && compA)
7ka884	14:93776ca449a4	1573	{
7ka884	14:93776ca449a4	1574	invationB = true;
7ka884	14:93776ca449a4	1575	}
7ka884	14:93776ca449a4	1576
7ka884	14:93776ca449a4	1577	if(!invationB && !compA && !compB){
7ka884	14:93776ca449a4	1578	invationA = false;
7ka884	14:93776ca449a4	1579	}
7ka884	14:93776ca449a4	1580	else if(invationB && !compA && !compB){
7ka884	14:93776ca449a4	1581	invationA = true;
7ka884	14:93776ca449a4	1582	}
kishibekairohan	13:b6e02d6261d7	1583	}
kishibekairohan	13:b6e02d6261d7	1584
kishibekairohan	12:c09b3e08a316	1585	void getcolor(){
kishibekairohan	13:b6e02d6261d7	1586	for(int i=0;i<10;i++){
kishibekairohan	12:c09b3e08a316	1587	ColorDetection();
kishibekairohan	12:c09b3e08a316	1588
kishibekairohan	12:c09b3e08a316	1589	Avecolor_A[0] += Color_A[0];
kishibekairohan	12:c09b3e08a316	1590	Avecolor_A[1] += Color_A[1];
kishibekairohan	12:c09b3e08a316	1591	Avecolor_A[2] += Color_A[2];
kishibekairohan	12:c09b3e08a316	1592	Avecolor_B[0] += Color_B[0];
kishibekairohan	12:c09b3e08a316	1593	Avecolor_B[1] += Color_B[1];
kishibekairohan	12:c09b3e08a316	1594	Avecolor_B[2] += Color_B[2];
kishibekairohan	12:c09b3e08a316	1595	Avecolor_C[0] += Color_C[0];
kishibekairohan	12:c09b3e08a316	1596	Avecolor_C[1] += Color_C[1];
kishibekairohan	12:c09b3e08a316	1597	Avecolor_C[2] += Color_C[2];
kishibekairohan	12:c09b3e08a316	1598	Avecolor_D[0] += Color_D[0];
kishibekairohan	12:c09b3e08a316	1599	Avecolor_D[1] += Color_D[1];
kishibekairohan	12:c09b3e08a316	1600	Avecolor_D[2] += Color_D[2];
kishibekairohan	12:c09b3e08a316	1601	}
kishibekairohan	12:c09b3e08a316	1602
kishibekairohan	13:b6e02d6261d7	1603	Avecolor_A[0] = Avecolor_A[0]/10;
kishibekairohan	13:b6e02d6261d7	1604	Avecolor_A[1] = Avecolor_A[1]/10;
kishibekairohan	13:b6e02d6261d7	1605	Avecolor_A[2] = Avecolor_A[2]/10;
kishibekairohan	13:b6e02d6261d7	1606	Avecolor_B[0] = Avecolor_B[0]/10;
kishibekairohan	13:b6e02d6261d7	1607	Avecolor_B[1] = Avecolor_B[1]/10;
kishibekairohan	13:b6e02d6261d7	1608	Avecolor_B[2] = Avecolor_B[2]/10;
kishibekairohan	13:b6e02d6261d7	1609	Avecolor_C[0] = Avecolor_C[0]/10;
kishibekairohan	13:b6e02d6261d7	1610	Avecolor_C[1] = Avecolor_C[1]/10;
kishibekairohan	13:b6e02d6261d7	1611	Avecolor_C[2] = Avecolor_C[2]/10;
kishibekairohan	13:b6e02d6261d7	1612	Avecolor_D[0] = Avecolor_D[0]/10;
kishibekairohan	13:b6e02d6261d7	1613	Avecolor_D[1] = Avecolor_D[1]/10;
kishibekairohan	13:b6e02d6261d7	1614	Avecolor_D[2] = Avecolor_D[2]/10;
kishibekairohan	13:b6e02d6261d7	1615
kishibekairohan	12:c09b3e08a316	1616	}
kishibekairohan	12:c09b3e08a316	1617
kishibekairohan	9:f93fc79a49ea	1618	void BuzzerTimer_func() {
kishibekairohan	11:028a150943b5	1619	if(buzzer == 0.5){
kishibekairohan	11:028a150943b5	1620	buzzer = 0;
kishibekairohan	11:028a150943b5	1621	}
kishibekairohan	11:028a150943b5	1622	else if(buzzer == 0){
kishibekairohan	11:028a150943b5	1623	buzzer = 0.5;
kishibekairohan	11:028a150943b5	1624	}
kishibekairohan	11:028a150943b5	1625	}
kishibekairohan	11:028a150943b5	1626
kishibekairohan	11:028a150943b5	1627	void TapeLedEms_func() {
kishibekairohan	11:028a150943b5	1628	sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan	9:f93fc79a49ea	1629	}
7ka884	14:93776ca449a4	1630
7ka884	14:93776ca449a4	1631	void LostLed_func(){
7ka884	14:93776ca449a4	1632	sendLedData.code = sendLedData.code == (uint32_t)Yellow ? (uint32_t)Black : (uint32_t)Yellow;
7ka884	14:93776ca449a4	1633	}
t_yamamoto	0:669ef71cba68	1634	#pragma endregion

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	23 Sep 2019
Imports:	1
Forks:	0
Commits:	36
Dependents:	0
Dependencies:	3
Followers:	0

System/Process/Process.cpp@14:93776ca449a4, 2018-10-25 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning