MB2019_main_alltimes_1123 - aa

Users » yabahiro » Code » MB2019_main_alltimes_1123

大季矢花 / Mbed 2 deprecated MB2019_main_alltimes_1123

System/Process/Process.cpp@25:b3a9f34b201d, 2019-09-27 (annotated)

Committer:: M_souta
Date:: Fri Sep 27 07:51:32 2019 +0000
Revision:: 25:b3a9f34b201d
Parent:: 24:370616a56815
Child:: 26:4c0ce2f05688

ii

Who changed what in which revision?

User	Revision	Line number	New contents of line
t_yamamoto	0:669ef71cba68	1	#include "mbed.h"
t_yamamoto	0:669ef71cba68	2	#include "Process.h"
t_yamamoto	0:669ef71cba68	3
7ka884	4:ba9df71868df	4	#include "../../CommonLibraries/PID/PID.h"
t_yamamoto	0:669ef71cba68	5	#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
M_souta	21:e3b58d675c1c	6	#include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto	0:669ef71cba68	7	#include "../../Communication/Controller/Controller.h"
t_yamamoto	0:669ef71cba68	8	#include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto	0:669ef71cba68	9	#include "../../Input/Switch/Switch.h"
kishibekairohan	16:3f2c2d89372b	10	#include "../../Input/Encoder/Encoder.h"
M_souta	22:7d93f79a3686	11	#include "../../Input/Ultrasonic/Ultrasonic.h"
t_yamamoto	0:669ef71cba68	12	#include "../../LED/LED.h"
t_yamamoto	0:669ef71cba68	13	#include "../../Safty/Safty.h"
t_yamamoto	0:669ef71cba68	14	#include "../Using.h"
t_yamamoto	0:669ef71cba68	15
t_yamamoto	0:669ef71cba68	16	using namespace SWITCH;
7ka884	4:ba9df71868df	17	using namespace PID_SPACE;
kishibekairohan	16:3f2c2d89372b	18	using namespace ENCODER;
M_souta	22:7d93f79a3686	19	using namespace ULTRASONIC;
M_souta	21:e3b58d675c1c	20	using namespace LINEHUB;
t_yamamoto	0:669ef71cba68	21
M_souta	22:7d93f79a3686	22
t_yamamoto	0:669ef71cba68	23	static CONTROLLER::ControllerData *controller;
t_yamamoto	0:669ef71cba68	24	ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto	0:669ef71cba68	25	ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto	0:669ef71cba68	26
t_yamamoto	0:669ef71cba68	27	static bool lock;
t_yamamoto	0:669ef71cba68	28	static bool processChangeComp;
t_yamamoto	0:669ef71cba68	29	static int current;
t_yamamoto	0:669ef71cba68	30
t_yamamoto	0:669ef71cba68	31	static void AllActuatorReset();
t_yamamoto	0:669ef71cba68	32
t_yamamoto	0:669ef71cba68	33	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	34	static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto	0:669ef71cba68	35	#endif
t_yamamoto	0:669ef71cba68	36
t_yamamoto	0:669ef71cba68	37	#pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto	0:669ef71cba68	38
t_yamamoto	0:669ef71cba68	39	/Replace here with the definition code of your variables./
t_yamamoto	0:669ef71cba68	40
M_souta	22:7d93f79a3686	41	USS ultrasonic[] = {
M_souta	22:7d93f79a3686	42	USS(ECHO_0,TRIG_0,TEMP),
M_souta	22:7d93f79a3686	43	USS(ECHO_1,TRIG_1,TEMP),
M_souta	22:7d93f79a3686	44	};
kishibekairohan	2:c015739085d3	45
M_souta	22:7d93f79a3686	46	Serial pc(USBTX, USBRX);
kishibekairohan	7:e88c5d47a3be	47
kishibekairohan	16:3f2c2d89372b	48	//************Buzzer**************
kishibekairohan	11:028a150943b5	49	//DigitalOut buzzer(BUZZER_PIN);
kishibekairohan	9:f93fc79a49ea	50	void BuzzerTimer_func();
kishibekairohan	9:f93fc79a49ea	51	Ticker BuzzerTimer;
kishibekairohan	16:3f2c2d89372b	52	bool EMGflag = false;
kishibekairohan	16:3f2c2d89372b	53	PwmOut buzzer(BUZZER_PIN);
kishibekairohan	16:3f2c2d89372b	54	//************Buzzer**************
kishibekairohan	11:028a150943b5	55
kishibekairohan	11:028a150943b5	56	//**********TapeLed***************
kishibekairohan	11:028a150943b5	57	void TapeLedEms_func();
kishibekairohan	11:028a150943b5	58	TapeLedData tapeLED;
kishibekairohan	11:028a150943b5	59	TapeLedData sendLedData;
kishibekairohan	11:028a150943b5	60	TapeLED_Mode ledMode = Normal;
kishibekairohan	11:028a150943b5	61	Ticker tapeLedTimer;
kishibekairohan	11:028a150943b5	62	//**********TapaLed***************
kishibekairohan	16:3f2c2d89372b	63
M_souta	22:7d93f79a3686	64	//************* lift *************
M_souta	22:7d93f79a3686	65	#define LOWER 1
M_souta	22:7d93f79a3686	66	#define MIDDLRE 2
M_souta	22:7d93f79a3686	67	#define UPPER 3
M_souta	22:7d93f79a3686	68	uint8_t liftState = LOWER;
M_souta	22:7d93f79a3686	69	bool moving = false;
M_souta	22:7d93f79a3686	70	bool switchFlag_LB = false;
M_souta	22:7d93f79a3686	71	bool switchFlag_RB = false;
M_souta	22:7d93f79a3686	72
M_souta	22:7d93f79a3686	73	//************* lift *************
M_souta	22:7d93f79a3686	74
M_souta	22:7d93f79a3686	75	//***********tire***********
M_souta	22:7d93f79a3686	76	PID rotaconPID[] = {
M_souta	22:7d93f79a3686	77	PID(0.0001,-1,1,0.05,0,0), //LF
M_souta	22:7d93f79a3686	78	PID(0.0001,-1,1,0.05,0,0), //LB
M_souta	22:7d93f79a3686	79	PID(0.0001,-1,1,0.05,0,0), //RB
M_souta	22:7d93f79a3686	80	PID(0.0001,-1,1,0.05,0,0), //RF
M_souta	22:7d93f79a3686	81	};
M_souta	22:7d93f79a3686	82
M_souta	22:7d93f79a3686	83	#define FL 0
M_souta	22:7d93f79a3686	84	#define BL 1
M_souta	22:7d93f79a3686	85	#define BR 2
M_souta	22:7d93f79a3686	86	#define FR 3
M_souta	22:7d93f79a3686	87
M_souta	22:7d93f79a3686	88	#define PI 3.141592
M_souta	22:7d93f79a3686	89
M_souta	22:7d93f79a3686	90	const float tireR = 101.6; //タイヤの半径
M_souta	22:7d93f79a3686	91	const float ucR = 420.0; //中心からのタイヤの距離
M_souta	22:7d93f79a3686	92
M_souta	22:7d93f79a3686	93	typedef struct {
M_souta	22:7d93f79a3686	94	float Vx; //X方向の速度
M_souta	22:7d93f79a3686	95	float Vy; //Y方向の速度
M_souta	22:7d93f79a3686	96	float Va; //角速度
M_souta	22:7d93f79a3686	97	} Vvector;
M_souta	22:7d93f79a3686	98
M_souta	22:7d93f79a3686	99	Vvector move; //進む速度
M_souta	22:7d93f79a3686	100	Vvector correction_LT; //ライントレースの補正速度
M_souta	22:7d93f79a3686	101	Vvector synthetic; //合成速度
M_souta	22:7d93f79a3686	102
M_souta	22:7d93f79a3686	103	float sita = 0;
M_souta	22:7d93f79a3686	104
M_souta	22:7d93f79a3686	105	bool PIDflag = false;
M_souta	22:7d93f79a3686	106
M_souta	22:7d93f79a3686	107	int linePara[8];
M_souta	22:7d93f79a3686	108	int linePara_U;
M_souta	22:7d93f79a3686	109	int linePara_B;
M_souta	22:7d93f79a3686	110	int linePara_L;
M_souta	22:7d93f79a3686	111	int linePara_R;
M_souta	22:7d93f79a3686	112
M_souta	22:7d93f79a3686	113	#define FL 0
M_souta	22:7d93f79a3686	114	#define BL 1
M_souta	22:7d93f79a3686	115	#define BR 2
M_souta	22:7d93f79a3686	116	#define FR 3
M_souta	22:7d93f79a3686	117
M_souta	22:7d93f79a3686	118	float tireProcessRPM[4];
M_souta	22:7d93f79a3686	119	float tireTargetMaxRPM[4];
M_souta	22:7d93f79a3686	120	float tireTargetRPM[4];
M_souta	22:7d93f79a3686	121
M_souta	22:7d93f79a3686	122	float tirePWM[4];
M_souta	22:7d93f79a3686	123
M_souta	22:7d93f79a3686	124	float timePV[4];
M_souta	22:7d93f79a3686	125	float timeCV[4];
M_souta	22:7d93f79a3686	126	float pulsePV[4];
M_souta	22:7d93f79a3686	127	float pulseCV[4];
M_souta	22:7d93f79a3686	128
M_souta	22:7d93f79a3686	129	void tirePID();
M_souta	22:7d93f79a3686	130	int lineCast(char k);
M_souta	22:7d93f79a3686	131
M_souta	22:7d93f79a3686	132	Timer rotaconSampling;
M_souta	22:7d93f79a3686	133	Ticker rotaconPIDtimer;
M_souta	22:7d93f79a3686	134
M_souta	22:7d93f79a3686	135	bool countFlag;
M_souta	22:7d93f79a3686	136	//***********tire************//
M_souta	22:7d93f79a3686	137
M_souta	22:7d93f79a3686	138	// *********** Line ************ //
M_souta	22:7d93f79a3686	139
M_souta	22:7d93f79a3686	140	float pw = 0;
M_souta	22:7d93f79a3686	141	int lineFase = 0;
M_souta	22:7d93f79a3686	142	bool lineCheck = false;
M_souta	25:b3a9f34b201d	143	bool lineFlag = false;
M_souta	25:b3a9f34b201d	144	bool adjAnable = false;
M_souta	25:b3a9f34b201d	145	int adj = 0;
M_souta	25:b3a9f34b201d	146
M_souta	25:b3a9f34b201d	147	int count = 0;
M_souta	25:b3a9f34b201d	148	int lineCount = 0;
M_souta	25:b3a9f34b201d	149	int targetCount = 0;
M_souta	25:b3a9f34b201d	150
M_souta	25:b3a9f34b201d	151	bool startFlag = true;
M_souta	25:b3a9f34b201d	152
M_souta	22:7d93f79a3686	153	int linePWM;
M_souta	22:7d93f79a3686	154	int adj_F;
M_souta	22:7d93f79a3686	155	int adj_B;
M_souta	22:7d93f79a3686	156
M_souta	22:7d93f79a3686	157	int mode = 0;
M_souta	22:7d93f79a3686	158
M_souta	22:7d93f79a3686	159	// *********** Line ************ //
M_souta	22:7d93f79a3686	160
M_souta	21:e3b58d675c1c	161	const int omni[15][15] =
M_souta	20:eae8c84f318c	162	{
M_souta	21:e3b58d675c1c	163	{ 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
M_souta	21:e3b58d675c1c	164	{ -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
M_souta	21:e3b58d675c1c	165	{ -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
M_souta	21:e3b58d675c1c	166	{ -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
M_souta	21:e3b58d675c1c	167	{ -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
M_souta	21:e3b58d675c1c	168	{ -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
M_souta	21:e3b58d675c1c	169	{ -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
M_souta	21:e3b58d675c1c	170	{ -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
M_souta	21:e3b58d675c1c	171	{ -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
M_souta	21:e3b58d675c1c	172	{ -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
M_souta	21:e3b58d675c1c	173	{ -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
M_souta	21:e3b58d675c1c	174	{ -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
M_souta	21:e3b58d675c1c	175	{ -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
M_souta	21:e3b58d675c1c	176	{ -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
M_souta	21:e3b58d675c1c	177	{ -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta	21:e3b58d675c1c	178	};
M_souta	21:e3b58d675c1c	179
M_souta	21:e3b58d675c1c	180	const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta	21:e3b58d675c1c	181
M_souta	21:e3b58d675c1c	182	uint8_t SetStatus(int);
M_souta	21:e3b58d675c1c	183	uint8_t SetStatus(int pwmVal) {
M_souta	21:e3b58d675c1c	184	if (pwmVal < 0) return BACK;
M_souta	21:e3b58d675c1c	185	else if (pwmVal > 0) return FOR;
M_souta	21:e3b58d675c1c	186	else if (pwmVal == 0) return BRAKE;
M_souta	21:e3b58d675c1c	187	else return BRAKE;
M_souta	20:eae8c84f318c	188	}
M_souta	21:e3b58d675c1c	189	uint8_t SetPWM(int);
M_souta	21:e3b58d675c1c	190	uint8_t SetPWM(int pwmVal) {
M_souta	21:e3b58d675c1c	191	if (pwmVal == 0 \|\| pwmVal > 255 \|\| pwmVal < -255) return 255;
M_souta	21:e3b58d675c1c	192	else return abs(pwmVal);
M_souta	21:e3b58d675c1c	193	}
M_souta	18:c694bae76e51	194
t_yamamoto	0:669ef71cba68	195	#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto	0:669ef71cba68	196
t_yamamoto	0:669ef71cba68	197	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	198	#if USE_PROCESS_NUM>0
t_yamamoto	0:669ef71cba68	199	static void Process0(void);
t_yamamoto	0:669ef71cba68	200	#endif
t_yamamoto	0:669ef71cba68	201	#if USE_PROCESS_NUM>1
t_yamamoto	0:669ef71cba68	202	static void Process1(void);
t_yamamoto	0:669ef71cba68	203	#endif
t_yamamoto	0:669ef71cba68	204	#if USE_PROCESS_NUM>2
t_yamamoto	0:669ef71cba68	205	static void Process2(void);
t_yamamoto	0:669ef71cba68	206	#endif
t_yamamoto	0:669ef71cba68	207	#if USE_PROCESS_NUM>3
t_yamamoto	0:669ef71cba68	208	static void Process3(void);
t_yamamoto	0:669ef71cba68	209	#endif
t_yamamoto	0:669ef71cba68	210	#if USE_PROCESS_NUM>4
t_yamamoto	0:669ef71cba68	211	static void Process4(void);
t_yamamoto	0:669ef71cba68	212	#endif
t_yamamoto	0:669ef71cba68	213	#if USE_PROCESS_NUM>5
t_yamamoto	0:669ef71cba68	214	static void Process5(void);
t_yamamoto	0:669ef71cba68	215	#endif
t_yamamoto	0:669ef71cba68	216	#if USE_PROCESS_NUM>6
t_yamamoto	0:669ef71cba68	217	static void Process6(void);
t_yamamoto	0:669ef71cba68	218	#endif
t_yamamoto	0:669ef71cba68	219	#if USE_PROCESS_NUM>7
t_yamamoto	0:669ef71cba68	220	static void Process7(void);
t_yamamoto	0:669ef71cba68	221	#endif
t_yamamoto	0:669ef71cba68	222	#if USE_PROCESS_NUM>8
t_yamamoto	0:669ef71cba68	223	static void Process8(void);
t_yamamoto	0:669ef71cba68	224	#endif
t_yamamoto	0:669ef71cba68	225	#if USE_PROCESS_NUM>9
t_yamamoto	0:669ef71cba68	226	static void Process9(void);
t_yamamoto	0:669ef71cba68	227	#endif
t_yamamoto	0:669ef71cba68	228	#endif
t_yamamoto	0:669ef71cba68	229
t_yamamoto	0:669ef71cba68	230	void SystemProcessInitialize()
t_yamamoto	0:669ef71cba68	231	{
t_yamamoto	0:669ef71cba68	232	#pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan	16:3f2c2d89372b	233	/Replace here with the initialization code of your variables./
M_souta	25:b3a9f34b201d	234	//rotaconPIDtimer.attach(tirePID,0.1);
M_souta	25:b3a9f34b201d	235
M_souta	25:b3a9f34b201d	236	//DigitalOut Air_16(LS_16);
M_souta	25:b3a9f34b201d	237	//DigitalOut Air_17(LS_17);
M_souta	25:b3a9f34b201d	238	//DigitalOut Air_18(LS_18);
M_souta	25:b3a9f34b201d	239	//DigitalOut Air_19(LS_19);
M_souta	21:e3b58d675c1c	240
t_yamamoto	0:669ef71cba68	241	#pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto	0:669ef71cba68	242
t_yamamoto	0:669ef71cba68	243	lock = true;
t_yamamoto	0:669ef71cba68	244	processChangeComp = true;
t_yamamoto	0:669ef71cba68	245	current = DEFAULT_PROCESS;
t_yamamoto	0:669ef71cba68	246
t_yamamoto	0:669ef71cba68	247	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	248	#if USE_PROCESS_NUM>0
t_yamamoto	0:669ef71cba68	249	Process[0] = Process0;
t_yamamoto	0:669ef71cba68	250	#endif
t_yamamoto	0:669ef71cba68	251	#if USE_PROCESS_NUM>1
t_yamamoto	0:669ef71cba68	252	Process[1] = Process1;
t_yamamoto	0:669ef71cba68	253	#endif
t_yamamoto	0:669ef71cba68	254	#if USE_PROCESS_NUM>2
t_yamamoto	0:669ef71cba68	255	Process[2] = Process2;
t_yamamoto	0:669ef71cba68	256	#endif
t_yamamoto	0:669ef71cba68	257	#if USE_PROCESS_NUM>3
t_yamamoto	0:669ef71cba68	258	Process[3] = Process3;
t_yamamoto	0:669ef71cba68	259	#endif
t_yamamoto	0:669ef71cba68	260	#if USE_PROCESS_NUM>4
t_yamamoto	0:669ef71cba68	261	Process[4] = Process4;
t_yamamoto	0:669ef71cba68	262	#endif
t_yamamoto	0:669ef71cba68	263	#if USE_PROCESS_NUM>5
t_yamamoto	0:669ef71cba68	264	Process[5] = Process5;
t_yamamoto	0:669ef71cba68	265	#endif
t_yamamoto	0:669ef71cba68	266	#if USE_PROCESS_NUM>6
t_yamamoto	0:669ef71cba68	267	Process[6] = Process6;
t_yamamoto	0:669ef71cba68	268	#endif
t_yamamoto	0:669ef71cba68	269	#if USE_PROCESS_NUM>7
t_yamamoto	0:669ef71cba68	270	Process[7] = Process7;
t_yamamoto	0:669ef71cba68	271	#endif
t_yamamoto	0:669ef71cba68	272	#if USE_PROCESS_NUM>8
t_yamamoto	0:669ef71cba68	273	Process[8] = Process8;
t_yamamoto	0:669ef71cba68	274	#endif
t_yamamoto	0:669ef71cba68	275	#if USE_PROCESS_NUM>9
t_yamamoto	0:669ef71cba68	276	Process[9] = Process9;
t_yamamoto	0:669ef71cba68	277	#endif
t_yamamoto	0:669ef71cba68	278	#endif
t_yamamoto	0:669ef71cba68	279	}
t_yamamoto	0:669ef71cba68	280
t_yamamoto	0:669ef71cba68	281	static void SystemProcessUpdate()
t_yamamoto	0:669ef71cba68	282	{
t_yamamoto	0:669ef71cba68	283	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	284	if(controller->Button.HOME) lock = false;
t_yamamoto	0:669ef71cba68	285
t_yamamoto	0:669ef71cba68	286	if(controller->Button.START && processChangeComp)
t_yamamoto	0:669ef71cba68	287	{
t_yamamoto	0:669ef71cba68	288	current++;
t_yamamoto	0:669ef71cba68	289	if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto	0:669ef71cba68	290	processChangeComp = false;
t_yamamoto	0:669ef71cba68	291	}
t_yamamoto	0:669ef71cba68	292	else if(controller->Button.SELECT && processChangeComp)
t_yamamoto	0:669ef71cba68	293	{
t_yamamoto	0:669ef71cba68	294	current--;
t_yamamoto	0:669ef71cba68	295	if (current < 0) current = 0;
t_yamamoto	0:669ef71cba68	296	processChangeComp = false;
t_yamamoto	0:669ef71cba68	297	}
t_yamamoto	0:669ef71cba68	298	else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto	0:669ef71cba68	299	#endif
t_yamamoto	0:669ef71cba68	300
t_yamamoto	0:669ef71cba68	301	#ifdef USE_MOTOR
t_yamamoto	0:669ef71cba68	302	ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto	0:669ef71cba68	303	#endif
t_yamamoto	0:669ef71cba68	304
t_yamamoto	0:669ef71cba68	305	#ifdef USE_SOLENOID
t_yamamoto	0:669ef71cba68	306	ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto	0:669ef71cba68	307	#endif
t_yamamoto	0:669ef71cba68	308
t_yamamoto	0:669ef71cba68	309	#ifdef USE_RS485
t_yamamoto	0:669ef71cba68	310	ACTUATORHUB::ActuatorHub::Update();
M_souta	21:e3b58d675c1c	311	//LINEHUB::LineHub::Update();
t_yamamoto	0:669ef71cba68	312	#endif
t_yamamoto	0:669ef71cba68	313
t_yamamoto	0:669ef71cba68	314	}
t_yamamoto	0:669ef71cba68	315
kishibekairohan	2:c015739085d3	316
kishibekairohan	2:c015739085d3	317
t_yamamoto	0:669ef71cba68	318	void SystemProcess()
t_yamamoto	0:669ef71cba68	319	{
t_yamamoto	0:669ef71cba68	320	SystemProcessInitialize();
t_yamamoto	0:669ef71cba68	321
t_yamamoto	0:669ef71cba68	322	while(1)
kishibekairohan	15:dfcec98f5aa9	323	{
M_souta	21:e3b58d675c1c	324	int g[8];
M_souta	21:e3b58d675c1c	325	for(int i = 0; i < 8; i++){
M_souta	22:7d93f79a3686	326	g[i] = lineCast(LineHub::GetPara(i));
M_souta	21:e3b58d675c1c	327	}
M_souta	22:7d93f79a3686	328
M_souta	22:7d93f79a3686	329	pc.printf("1:%d 2:%d 3:%d 4:%d 5:%d 6:%d 7:%d 8:%d\n\r",g[0],g[1],g[2],g[3],g[4],g[5],g[6],g[7]);
M_souta	25:b3a9f34b201d	330
M_souta	25:b3a9f34b201d	331
M_souta	25:b3a9f34b201d	332	if(LimitSw::IsPressed(START_SW) && startFlag == true) {
M_souta	25:b3a9f34b201d	333	startFlag == false;
M_souta	25:b3a9f34b201d	334	lock = false;
M_souta	25:b3a9f34b201d	335	lineFase = 0;
M_souta	25:b3a9f34b201d	336	lineCount = 0;
M_souta	25:b3a9f34b201d	337	lineCheck = false;
M_souta	25:b3a9f34b201d	338	count = 0;
M_souta	25:b3a9f34b201d	339	/*
M_souta	25:b3a9f34b201d	340	if(LimitSw::IsPressed(ISREDBLUE_SW) {
M_souta	25:b3a9f34b201d	341	current = 4;
M_souta	25:b3a9f34b201d	342	} else {
M_souta	25:b3a9f34b201d	343	current = 5;
M_souta	25:b3a9f34b201d	344	}
M_souta	25:b3a9f34b201d	345	*/
M_souta	25:b3a9f34b201d	346	current = 4;
M_souta	25:b3a9f34b201d	347	}
M_souta	20:eae8c84f318c	348
kishibekairohan	17:50dc4b449e69	349	buzzer.period(1.0/800);
kishibekairohan	15:dfcec98f5aa9	350
t_yamamoto	0:669ef71cba68	351	#ifdef USE_MU
t_yamamoto	0:669ef71cba68	352	controller = CONTROLLER::Controller::GetData();
t_yamamoto	0:669ef71cba68	353	#endif
t_yamamoto	0:669ef71cba68	354
t_yamamoto	0:669ef71cba68	355	#ifdef USE_ERRORCHECK
M_souta	25:b3a9f34b201d	356	if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag)
t_yamamoto	0:669ef71cba68	357	{
t_yamamoto	0:669ef71cba68	358	CONTROLLER::Controller::DataReset();
t_yamamoto	0:669ef71cba68	359	AllActuatorReset();
t_yamamoto	0:669ef71cba68	360	lock = true;
t_yamamoto	0:669ef71cba68	361	}
t_yamamoto	0:669ef71cba68	362	else
t_yamamoto	0:669ef71cba68	363	#endif
t_yamamoto	0:669ef71cba68	364	{
t_yamamoto	0:669ef71cba68	365
t_yamamoto	0:669ef71cba68	366	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	367	if(!lock)
t_yamamoto	0:669ef71cba68	368	{
t_yamamoto	0:669ef71cba68	369	Process[current]();
t_yamamoto	0:669ef71cba68	370	}
t_yamamoto	0:669ef71cba68	371	else
t_yamamoto	0:669ef71cba68	372	#endif
t_yamamoto	0:669ef71cba68	373	{
t_yamamoto	0:669ef71cba68	374	//ロック時の処理
t_yamamoto	0:669ef71cba68	375	}
t_yamamoto	0:669ef71cba68	376	}
t_yamamoto	0:669ef71cba68	377
M_souta	22:7d93f79a3686	378
kishibekairohan	16:3f2c2d89372b	379	//Emergency!
M_souta	22:7d93f79a3686	380	/*
kishibekairohan	17:50dc4b449e69	381	if(!EMG_0 && !EMG_1 && !EMGflag){
kishibekairohan	17:50dc4b449e69	382	buzzer = 0;
kishibekairohan	17:50dc4b449e69	383	BuzzerTimer.attach(BuzzerTimer_func, 1);
kishibekairohan	17:50dc4b449e69	384	EMGflag = true;
kishibekairohan	17:50dc4b449e69	385	LED_DEBUG0 = 1;
kishibekairohan	9:f93fc79a49ea	386	}
kishibekairohan	17:50dc4b449e69	387	if(EMG_0 && EMG_1 && EMGflag){
kishibekairohan	17:50dc4b449e69	388	buzzer = 1;
kishibekairohan	9:f93fc79a49ea	389	BuzzerTimer.detach();
kishibekairohan	17:50dc4b449e69	390	EMGflag = false;
kishibekairohan	9:f93fc79a49ea	391	}
M_souta	22:7d93f79a3686	392	*/
t_yamamoto	0:669ef71cba68	393	SystemProcessUpdate();
t_yamamoto	0:669ef71cba68	394	}
t_yamamoto	0:669ef71cba68	395	}
t_yamamoto	0:669ef71cba68	396
kishibekairohan	2:c015739085d3	397
kishibekairohan	2:c015739085d3	398
kishibekairohan	2:c015739085d3	399
t_yamamoto	0:669ef71cba68	400	#pragma region PROCESS
t_yamamoto	0:669ef71cba68	401	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	402	#if USE_PROCESS_NUM>0
kishibekairohan	12:c09b3e08a316	403	static void Process0()
kishibekairohan	10:1295d39fec3a	404	{
M_souta	21:e3b58d675c1c	405	AllActuatorReset();
t_yamamoto	0:669ef71cba68	406	}
t_yamamoto	0:669ef71cba68	407	#endif
t_yamamoto	0:669ef71cba68	408
t_yamamoto	0:669ef71cba68	409	#if USE_PROCESS_NUM>1
kishibekairohan	16:3f2c2d89372b	410	static void Process1()
t_yamamoto	0:669ef71cba68	411	{
M_souta	22:7d93f79a3686	412
M_souta	22:7d93f79a3686	413	PIDflag = false;
M_souta	22:7d93f79a3686	414
M_souta	21:e3b58d675c1c	415	if(controller->Button.UP) {
M_souta	21:e3b58d675c1c	416	motor[LIFT_LB].dir = FOR;
M_souta	23:c853372cf626	417	motor[LIFT_LB].pwm = 190;
M_souta	21:e3b58d675c1c	418	motor[LIFT_RB].dir = BACK;
M_souta	21:e3b58d675c1c	419	motor[LIFT_RB].pwm = 180;
M_souta	21:e3b58d675c1c	420	} else if(controller->Button.DOWN) {
M_souta	21:e3b58d675c1c	421	motor[LIFT_LB].dir = BACK;
M_souta	21:e3b58d675c1c	422	motor[LIFT_LB].pwm = 180;
M_souta	21:e3b58d675c1c	423	motor[LIFT_RB].dir = FOR;
M_souta	23:c853372cf626	424	motor[LIFT_RB].pwm = 190;
M_souta	23:c853372cf626	425	} else if(controller->Button.LEFT) {
M_souta	23:c853372cf626	426	motor[LIFT_LB].dir = FOR;
M_souta	23:c853372cf626	427	motor[LIFT_LB].pwm = 180;
M_souta	23:c853372cf626	428	} else if(controller->Button.RIGHT) {
M_souta	23:c853372cf626	429	motor[LIFT_RB].dir = BACK;
M_souta	21:e3b58d675c1c	430	motor[LIFT_RB].pwm = 180;
M_souta	21:e3b58d675c1c	431	} else {
M_souta	25:b3a9f34b201d	432	motor[LIFT_LB].dir = BRAKE;
M_souta	23:c853372cf626	433	motor[LIFT_LB].pwm = 255;
M_souta	23:c853372cf626	434	motor[LIFT_RB].dir = BACK;
M_souta	25:b3a9f34b201d	435	motor[LIFT_RB].pwm = 20;
M_souta	21:e3b58d675c1c	436	}
M_souta	22:7d93f79a3686	437
7ka884	4:ba9df71868df	438
M_souta	21:e3b58d675c1c	439	if(controller->Button.X) {
M_souta	21:e3b58d675c1c	440	motor[LIFT_U].dir = FOR;
M_souta	21:e3b58d675c1c	441	motor[LIFT_U].pwm = 180;
M_souta	21:e3b58d675c1c	442	} else if(controller->Button.Y) {
M_souta	21:e3b58d675c1c	443	motor[LIFT_U].dir = BACK;
M_souta	25:b3a9f34b201d	444	motor[LIFT_U].pwm = 230;
M_souta	21:e3b58d675c1c	445	} else {
M_souta	21:e3b58d675c1c	446	motor[LIFT_U].dir = BRAKE;
M_souta	25:b3a9f34b201d	447	motor[LIFT_U].pwm = 255;
M_souta	21:e3b58d675c1c	448	}
M_souta	21:e3b58d675c1c	449
M_souta	21:e3b58d675c1c	450	if(!(controller->AnalogL.Y == 7) \|\| !(controller->AnalogL.X == 7)) {
M_souta	25:b3a9f34b201d	451	motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
M_souta	21:e3b58d675c1c	452	motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
M_souta	21:e3b58d675c1c	453	motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta	25:b3a9f34b201d	454	motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta	21:e3b58d675c1c	455
M_souta	25:b3a9f34b201d	456	motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
M_souta	22:7d93f79a3686	457	motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
M_souta	22:7d93f79a3686	458	motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta	25:b3a9f34b201d	459	motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta	21:e3b58d675c1c	460	} else {
M_souta	25:b3a9f34b201d	461	motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	462	motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	463	motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta	25:b3a9f34b201d	464	motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	465
M_souta	21:e3b58d675c1c	466	motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	467	motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	468	motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	469	motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	470	}
M_souta	25:b3a9f34b201d	471
M_souta	25:b3a9f34b201d	472	if(controller->Button.ZR) {
M_souta	25:b3a9f34b201d	473	solenoid.solenoid4 = SOLENOID_ON;
M_souta	25:b3a9f34b201d	474	} else {
M_souta	25:b3a9f34b201d	475	solenoid.solenoid4 = SOLENOID_OFF;
M_souta	25:b3a9f34b201d	476	}
t_yamamoto	0:669ef71cba68	477	}
t_yamamoto	0:669ef71cba68	478	#endif
t_yamamoto	0:669ef71cba68	479
t_yamamoto	0:669ef71cba68	480	#if USE_PROCESS_NUM>2
kishibekairohan	16:3f2c2d89372b	481	static void Process2()
kishibekairohan	13:b6e02d6261d7	482	{
M_souta	25:b3a9f34b201d	483
M_souta	25:b3a9f34b201d	484
M_souta	25:b3a9f34b201d	485
M_souta	25:b3a9f34b201d	486
M_souta	22:7d93f79a3686	487	/*
M_souta	22:7d93f79a3686	488	if(moving) {
M_souta	22:7d93f79a3686	489	if(LimiSw::IsPressed(LSW_LB)) {
M_souta	22:7d93f79a3686	490	if(switchFlag_LB) {
M_souta	22:7d93f79a3686	491	switchFlag_LB = false;
M_souta	22:7d93f79a3686	492	motor[LIFT_LB].dir = BRAKE;
M_souta	22:7d93f79a3686	493	motor[LIFT_LB].pwm = 200;
M_souta	22:7d93f79a3686	494	} else {
M_souta	22:7d93f79a3686	495	seitchFlag_LB = true;
M_souta	22:7d93f79a3686	496	}
M_souta	22:7d93f79a3686	497	}
M_souta	22:7d93f79a3686	498	if(LimiSw::IsPressed(LSW_RB)) {
M_souta	22:7d93f79a3686	499	if(switchFlag_RB) {
M_souta	22:7d93f79a3686	500	switchFlag_RB = false;
M_souta	22:7d93f79a3686	501	motor[LIFT_RB].dir = BRAKE;
M_souta	22:7d93f79a3686	502	motor[LIFT_RB].pwm = 200;
M_souta	22:7d93f79a3686	503	} else {
M_souta	22:7d93f79a3686	504	seitchFlag_RB = true;
M_souta	22:7d93f79a3686	505	}
M_souta	22:7d93f79a3686	506	}
M_souta	22:7d93f79a3686	507	if(motor[LIFT_LB].dir == BRAKE && motor[LIFT_RB].dir == BRAKE) moving = false;
M_souta	22:7d93f79a3686	508	} else {
M_souta	22:7d93f79a3686	509	if(controller->Button.UP) {
M_souta	22:7d93f79a3686	510	if(!(state == UPPER)) {
M_souta	22:7d93f79a3686	511	state++;
M_souta	22:7d93f79a3686	512	motor[LIFT_LB].dir = BACK;
M_souta	22:7d93f79a3686	513	motor[LIFT_RB].dir = FOR;
M_souta	22:7d93f79a3686	514	motor[LIFT_LB].pwm = 200;
M_souta	22:7d93f79a3686	515	motor[LIFT_RB].pwm = 200;
M_souta	22:7d93f79a3686	516	}
M_souta	22:7d93f79a3686	517	} else if(controller->Button.DOWN) {
M_souta	22:7d93f79a3686	518	if(!(state == LOWER)) {
M_souta	22:7d93f79a3686	519	state--;
M_souta	22:7d93f79a3686	520	moving = true;
M_souta	22:7d93f79a3686	521	motor[LIFT_LB].dir = FOR;
M_souta	22:7d93f79a3686	522	motor[LIFT_RB].dir = BACK;
M_souta	22:7d93f79a3686	523	motor[LIFT_LB].pwm = 200;
M_souta	22:7d93f79a3686	524	motor[LIFT_RB].pwm = 200;
M_souta	22:7d93f79a3686	525	}
M_souta	22:7d93f79a3686	526	} else {
M_souta	22:7d93f79a3686	527	motor[LIFT_LB].dir = BRAKE;
M_souta	22:7d93f79a3686	528	motor[LIFT_RB].dir = BRAKE;
M_souta	22:7d93f79a3686	529	motor[LIFT_LB].pwm = 200;
M_souta	22:7d93f79a3686	530	motor[LIFT_RB].pwm = 200;
M_souta	22:7d93f79a3686	531	}
M_souta	22:7d93f79a3686	532	}
M_souta	22:7d93f79a3686	533	*/
kishibekairohan	13:b6e02d6261d7	534	}
kishibekairohan	13:b6e02d6261d7	535	#endif
kishibekairohan	13:b6e02d6261d7	536
kishibekairohan	13:b6e02d6261d7	537	#if USE_PROCESS_NUM>3
kishibekairohan	16:3f2c2d89372b	538	static void Process3()
kishibekairohan	13:b6e02d6261d7	539	{
M_souta	25:b3a9f34b201d	540	startFlag = true;
M_souta	22:7d93f79a3686	541	AllActuatorReset();
M_souta	23:c853372cf626	542	lineFase = 0;
M_souta	25:b3a9f34b201d	543	lineCheck = false;
M_souta	25:b3a9f34b201d	544	lineCount = 0;
M_souta	25:b3a9f34b201d	545	count = 0;
kishibekairohan	13:b6e02d6261d7	546	}
kishibekairohan	13:b6e02d6261d7	547	#endif
kishibekairohan	13:b6e02d6261d7	548
kishibekairohan	13:b6e02d6261d7	549	#if USE_PROCESS_NUM>4
kishibekairohan	13:b6e02d6261d7	550	static void Process4()
M_souta	22:7d93f79a3686	551	{
M_souta	25:b3a9f34b201d	552	LED_DEBUG0 = LED_ON;
M_souta	22:7d93f79a3686	553
M_souta	25:b3a9f34b201d	554	/* ************************************** //
M_souta	25:b3a9f34b201d	555
M_souta	25:b3a9f34b201d	556	赤ゾーン赤ゾーン赤ゾーン
M_souta	25:b3a9f34b201d	557
M_souta	25:b3a9f34b201d	558	// ************************************** */
M_souta	25:b3a9f34b201d	559
M_souta	25:b3a9f34b201d	560
M_souta	25:b3a9f34b201d	561	for(int i = 0; i < 8; i++) {
M_souta	25:b3a9f34b201d	562	linePara[i] = lineCast(LineHub::GetPara(i));
M_souta	25:b3a9f34b201d	563	}
M_souta	22:7d93f79a3686	564
M_souta	25:b3a9f34b201d	565	if(lineFase == 0) {
M_souta	25:b3a9f34b201d	566	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	567	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	568	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	569	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	570	if(linePara[0] != 'N' && linePara[0] != 'A') {
M_souta	25:b3a9f34b201d	571	lineFase = 1;
M_souta	25:b3a9f34b201d	572	}
M_souta	25:b3a9f34b201d	573	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	574	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	575	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	576	motor[TIRE_FR].pwm = 30;
M_souta	22:7d93f79a3686	577
M_souta	25:b3a9f34b201d	578	} else if(lineFase == 1) { // 前ライントレース
M_souta	25:b3a9f34b201d	579	switch(linePara[0]) {
M_souta	22:7d93f79a3686	580	case -2:
M_souta	25:b3a9f34b201d	581	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	582	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	583	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	584	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	585	adjAnable = true;
M_souta	25:b3a9f34b201d	586	break;
M_souta	25:b3a9f34b201d	587	case -3:
M_souta	25:b3a9f34b201d	588	tirePWM[TIRE_FL] = 10;
M_souta	25:b3a9f34b201d	589	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	590	tirePWM[TIRE_BR] = -10;
M_souta	25:b3a9f34b201d	591	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	592	adjAnable = true;
M_souta	25:b3a9f34b201d	593	break;
M_souta	25:b3a9f34b201d	594	case -1:
M_souta	25:b3a9f34b201d	595	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	596	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	597	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	598	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	599	adjAnable = true;
M_souta	25:b3a9f34b201d	600	break;
M_souta	25:b3a9f34b201d	601	case 0:
M_souta	25:b3a9f34b201d	602	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	603	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	604	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	605	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	606	adjAnable = true;
M_souta	25:b3a9f34b201d	607	break;
M_souta	25:b3a9f34b201d	608	case 1:
M_souta	25:b3a9f34b201d	609	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	610	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	611	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	612	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	613	adjAnable = true;
M_souta	25:b3a9f34b201d	614	break;
M_souta	25:b3a9f34b201d	615	case 3:
M_souta	25:b3a9f34b201d	616	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	617	tirePWM[TIRE_BL] = 10;
M_souta	25:b3a9f34b201d	618	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	619	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	620	adjAnable = true;
M_souta	25:b3a9f34b201d	621	break;
M_souta	25:b3a9f34b201d	622	case 2:
M_souta	25:b3a9f34b201d	623	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	624	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	625	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	626	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	627	adjAnable = true;
M_souta	25:b3a9f34b201d	628	break;
M_souta	25:b3a9f34b201d	629	case 'A':
M_souta	25:b3a9f34b201d	630	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	631	lineCheck = true;
M_souta	25:b3a9f34b201d	632	lineCount = 0;
M_souta	25:b3a9f34b201d	633	count++;
M_souta	25:b3a9f34b201d	634	}
M_souta	25:b3a9f34b201d	635	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	636	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	637	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	638	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	639	adjAnable = true;
M_souta	25:b3a9f34b201d	640	break;
M_souta	25:b3a9f34b201d	641	case 'N':
M_souta	25:b3a9f34b201d	642	tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta	25:b3a9f34b201d	643	tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta	25:b3a9f34b201d	644	tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta	25:b3a9f34b201d	645	tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta	25:b3a9f34b201d	646	adjAnable = false;
M_souta	25:b3a9f34b201d	647	break;
M_souta	25:b3a9f34b201d	648	default:
M_souta	25:b3a9f34b201d	649	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	650	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	651	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	652	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	653	adjAnable = false;
M_souta	25:b3a9f34b201d	654	}
M_souta	25:b3a9f34b201d	655
M_souta	25:b3a9f34b201d	656	if(adjAnable){
M_souta	25:b3a9f34b201d	657	if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
M_souta	25:b3a9f34b201d	658	} else {
M_souta	25:b3a9f34b201d	659	adj = 0;
M_souta	25:b3a9f34b201d	660	}
M_souta	25:b3a9f34b201d	661
M_souta	25:b3a9f34b201d	662	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	663	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	664	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	665	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	666	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	667	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	668	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	669	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	670
M_souta	25:b3a9f34b201d	671	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	672	lineCount++;
M_souta	25:b3a9f34b201d	673	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	674	}
M_souta	25:b3a9f34b201d	675	if(count == 3) {
M_souta	25:b3a9f34b201d	676	count = 0;
M_souta	25:b3a9f34b201d	677	lineFase = 2;
M_souta	25:b3a9f34b201d	678	lineCount = 0;
M_souta	25:b3a9f34b201d	679	lineCheck = false;
M_souta	25:b3a9f34b201d	680	}
M_souta	25:b3a9f34b201d	681	} else if(lineFase == 2) { // 前低速
M_souta	25:b3a9f34b201d	682	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	683	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	684	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	685	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	686	if(linePara[4] == 0) {
M_souta	25:b3a9f34b201d	687	lineFase = 3;
M_souta	25:b3a9f34b201d	688	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	689	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	690	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	691	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	692	}
M_souta	25:b3a9f34b201d	693	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	694	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	695	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	696	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	697	} else if(lineFase == 3) { // 右ライントレース
M_souta	25:b3a9f34b201d	698	switch(linePara[4]) {
M_souta	25:b3a9f34b201d	699	case -2:
M_souta	25:b3a9f34b201d	700	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	701	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	702	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	703	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	704	adjAnable = true;
M_souta	22:7d93f79a3686	705	break;
M_souta	22:7d93f79a3686	706	case -3:
M_souta	25:b3a9f34b201d	707	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	708	tirePWM[TIRE_BL] = -10;
M_souta	25:b3a9f34b201d	709	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	710	tirePWM[TIRE_FR] = 10;
M_souta	25:b3a9f34b201d	711	adjAnable = true;
M_souta	22:7d93f79a3686	712	break;
M_souta	22:7d93f79a3686	713	case -1:
M_souta	25:b3a9f34b201d	714	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	715	tirePWM[TIRE_BL] = -20;
M_souta	25:b3a9f34b201d	716	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	717	tirePWM[TIRE_FR] = 20;
M_souta	25:b3a9f34b201d	718	adjAnable = true;
M_souta	22:7d93f79a3686	719	break;
M_souta	22:7d93f79a3686	720	case 0:
M_souta	25:b3a9f34b201d	721	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	722	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	723	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	724	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	725	adjAnable = true;
M_souta	22:7d93f79a3686	726	break;
M_souta	22:7d93f79a3686	727	case 1:
M_souta	25:b3a9f34b201d	728	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	729	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	730	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	731	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	732	adjAnable = true;
M_souta	22:7d93f79a3686	733	break;
M_souta	22:7d93f79a3686	734	case 3:
M_souta	25:b3a9f34b201d	735	tirePWM[TIRE_FL] = 10;
M_souta	25:b3a9f34b201d	736	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	737	tirePWM[TIRE_BR] = -10;
M_souta	25:b3a9f34b201d	738	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	739	adjAnable = true;
M_souta	22:7d93f79a3686	740	break;
M_souta	22:7d93f79a3686	741	case 2:
M_souta	25:b3a9f34b201d	742	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	743	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	744	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	745	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	746	adjAnable = true;
M_souta	22:7d93f79a3686	747	break;
M_souta	22:7d93f79a3686	748	case 'A':
M_souta	25:b3a9f34b201d	749	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	750	lineCheck = true;
M_souta	25:b3a9f34b201d	751	lineCount = 0;
M_souta	25:b3a9f34b201d	752	count++;
M_souta	25:b3a9f34b201d	753	}
M_souta	25:b3a9f34b201d	754	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	755	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	756	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	757	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	758	adjAnable = true;
M_souta	25:b3a9f34b201d	759	break;
M_souta	25:b3a9f34b201d	760	case 'N':
M_souta	25:b3a9f34b201d	761	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	762	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	763	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	764	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	765	adjAnable = false;
M_souta	25:b3a9f34b201d	766	break;
M_souta	25:b3a9f34b201d	767	default:
M_souta	25:b3a9f34b201d	768	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	769	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	770	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	771	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	772	adjAnable = false;
M_souta	25:b3a9f34b201d	773	}
M_souta	25:b3a9f34b201d	774
M_souta	25:b3a9f34b201d	775	if(adjAnable){
M_souta	25:b3a9f34b201d	776	if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
M_souta	25:b3a9f34b201d	777	} else {
M_souta	25:b3a9f34b201d	778	adj = 0;
M_souta	25:b3a9f34b201d	779	}
M_souta	25:b3a9f34b201d	780
M_souta	25:b3a9f34b201d	781	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	782	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	783	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	784	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	785	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	786	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	787	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	788	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	789
M_souta	25:b3a9f34b201d	790	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	791	lineCount++;
M_souta	25:b3a9f34b201d	792	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	793	}
M_souta	25:b3a9f34b201d	794
M_souta	25:b3a9f34b201d	795	if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
M_souta	25:b3a9f34b201d	796	targetCount = 3;
M_souta	25:b3a9f34b201d	797	} else if(LimitSw::IsPressed(TOWEL1_SW)) {
M_souta	25:b3a9f34b201d	798	targetCount = 3;
M_souta	25:b3a9f34b201d	799	} else {
M_souta	25:b3a9f34b201d	800	targetCount = 2;
M_souta	25:b3a9f34b201d	801	}
M_souta	25:b3a9f34b201d	802
M_souta	25:b3a9f34b201d	803	if(count == targetCount) {
M_souta	25:b3a9f34b201d	804	count = 0;
M_souta	25:b3a9f34b201d	805	lineFase = 4;
M_souta	25:b3a9f34b201d	806	lineCount = 0;
M_souta	25:b3a9f34b201d	807	lineCheck = false;
M_souta	25:b3a9f34b201d	808	}
M_souta	25:b3a9f34b201d	809	} else if(lineFase == 4) { // 右低速
M_souta	25:b3a9f34b201d	810	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	811	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	812	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	813	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	814	if (linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	815	lineFase = 6;
M_souta	25:b3a9f34b201d	816	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	817	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	818	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	819	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	820	}
M_souta	25:b3a9f34b201d	821	motor[TIRE_FL].pwm = 14;
M_souta	25:b3a9f34b201d	822	motor[TIRE_BL].pwm = 14;
M_souta	25:b3a9f34b201d	823	motor[TIRE_BR].pwm = 14;
M_souta	25:b3a9f34b201d	824	motor[TIRE_FR].pwm = 14;
M_souta	25:b3a9f34b201d	825	} else if (lineFase == 5) {
M_souta	25:b3a9f34b201d	826	lineFase = 6;
M_souta	25:b3a9f34b201d	827	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	828	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	829	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	830	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	831	} else if(lineFase == 6) { // タオル1 検知
M_souta	25:b3a9f34b201d	832	if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	833	lineFase = 7;
M_souta	25:b3a9f34b201d	834	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	835	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	836	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	837	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	838	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	839	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	840	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	841	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	842	} else if(LimitSw::IsPressed(TOW_1L)) {
M_souta	25:b3a9f34b201d	843	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	844	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	845	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	846	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	847	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	848	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	849	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	850	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	851	} else if(LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	852	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	853	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	854	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	855	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	856	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	857	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	858	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	859	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	860	} else {
M_souta	25:b3a9f34b201d	861	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	862	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	863	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	864	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	865	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	866	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	867	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	868	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	869	}
M_souta	25:b3a9f34b201d	870	} else if(lineFase == 7) { // ライン修正
M_souta	25:b3a9f34b201d	871	if(linePara[LINE_TOW_1] == 'A' \|\| linePara[LINE_TOW_1] == 'N') {
M_souta	25:b3a9f34b201d	872	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	873	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	874	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	875	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	876	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	877	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	878	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	879	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	880	} else if(linePara[LINE_TOW_1] > 0) {
M_souta	25:b3a9f34b201d	881	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	882	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	883	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	884	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	885	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	886	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	887	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	888	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	889	} else if(linePara[LINE_TOW_1] < 0) {
M_souta	25:b3a9f34b201d	890	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	891	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	892	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	893	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	894	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	895	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	896	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	897	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	898	} else if(linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	899	lineFase = 8;
M_souta	25:b3a9f34b201d	900	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	901	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	902	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	903	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	904	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	905	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	906	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	907	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	908	} else {
M_souta	25:b3a9f34b201d	909	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	910	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	911	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	912	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	913	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	914	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	915	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	916	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	917	}
M_souta	25:b3a9f34b201d	918	} else if(lineFase == 8) { // タオル1 解放
M_souta	25:b3a9f34b201d	919	//solenoid.TOWEL1 = SOLENOID_ON;
M_souta	25:b3a9f34b201d	920	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	921	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	922	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	923	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	924	lineFase = 9;
M_souta	25:b3a9f34b201d	925	} else if(lineFase == 9) { // 前
M_souta	25:b3a9f34b201d	926	switch(linePara[LINE_TOW_1]) {
M_souta	25:b3a9f34b201d	927	case 2:
M_souta	25:b3a9f34b201d	928	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	929	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	930	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	931	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	932	adjAnable = true;
M_souta	25:b3a9f34b201d	933	break;
M_souta	25:b3a9f34b201d	934	case 3:
M_souta	25:b3a9f34b201d	935	tirePWM[TIRE_FL] = 10;
M_souta	25:b3a9f34b201d	936	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	937	tirePWM[TIRE_BR] = -10;
M_souta	25:b3a9f34b201d	938	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	939	adjAnable = true;
M_souta	25:b3a9f34b201d	940	break;
M_souta	25:b3a9f34b201d	941	case 1:
M_souta	25:b3a9f34b201d	942	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	943	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	944	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	945	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	946	adjAnable = true;
M_souta	25:b3a9f34b201d	947	break;
M_souta	25:b3a9f34b201d	948	case 0:
M_souta	25:b3a9f34b201d	949	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	950	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	951	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	952	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	953	adjAnable = true;
M_souta	25:b3a9f34b201d	954	break;
M_souta	25:b3a9f34b201d	955	case -1:
M_souta	25:b3a9f34b201d	956	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	957	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	958	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	959	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	960	adjAnable = true;
M_souta	25:b3a9f34b201d	961	break;
M_souta	25:b3a9f34b201d	962	case -3:
M_souta	25:b3a9f34b201d	963	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	964	tirePWM[TIRE_BL] = 10;
M_souta	25:b3a9f34b201d	965	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	966	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	967	adjAnable = true;
M_souta	25:b3a9f34b201d	968	break;
M_souta	25:b3a9f34b201d	969	case -2:
M_souta	25:b3a9f34b201d	970	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	971	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	972	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	973	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	974	adjAnable = true;
M_souta	25:b3a9f34b201d	975	break;
M_souta	25:b3a9f34b201d	976	case 'A':
M_souta	25:b3a9f34b201d	977	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	978	lineCheck = true;
M_souta	25:b3a9f34b201d	979	lineCount = 0;
M_souta	25:b3a9f34b201d	980	count++;
M_souta	25:b3a9f34b201d	981	}
M_souta	25:b3a9f34b201d	982	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	983	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	984	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	985	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	986	adjAnable = true;
M_souta	22:7d93f79a3686	987	break;
M_souta	22:7d93f79a3686	988	case 'N':
M_souta	25:b3a9f34b201d	989	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	990	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	991	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	992	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	993	adjAnable = false;
M_souta	25:b3a9f34b201d	994	break;
M_souta	25:b3a9f34b201d	995	default:
M_souta	25:b3a9f34b201d	996	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	997	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	998	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	999	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1000	adjAnable = false;
M_souta	25:b3a9f34b201d	1001	}
M_souta	25:b3a9f34b201d	1002
M_souta	25:b3a9f34b201d	1003	if(adjAnable){
M_souta	25:b3a9f34b201d	1004	adj = 0;
M_souta	25:b3a9f34b201d	1005	} else {
M_souta	25:b3a9f34b201d	1006	adj = 0;
M_souta	25:b3a9f34b201d	1007	}
M_souta	25:b3a9f34b201d	1008
M_souta	25:b3a9f34b201d	1009	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1010	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1011	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1012	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1013	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1014	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1015	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1016	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1017
M_souta	25:b3a9f34b201d	1018	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1019	lineCount++;
M_souta	25:b3a9f34b201d	1020	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1021	}
M_souta	25:b3a9f34b201d	1022	if(count == 1) {
M_souta	25:b3a9f34b201d	1023	count = 0;
M_souta	25:b3a9f34b201d	1024	lineFase = 10;
M_souta	25:b3a9f34b201d	1025	lineCount = 0;
M_souta	25:b3a9f34b201d	1026	lineCheck = false;
M_souta	25:b3a9f34b201d	1027	}
M_souta	25:b3a9f34b201d	1028	} else if(lineFase == 10) {
M_souta	25:b3a9f34b201d	1029	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1030	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1031	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1032	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1033	if(linePara[4] == 0) {
M_souta	25:b3a9f34b201d	1034	lineFase = 11;
M_souta	25:b3a9f34b201d	1035	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1036	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1037	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1038	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1039	}
M_souta	25:b3a9f34b201d	1040	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1041	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1042	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1043	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1044	} else if(lineFase == 11) {
M_souta	25:b3a9f34b201d	1045	switch(linePara[4]) {
M_souta	25:b3a9f34b201d	1046	case -2:
M_souta	25:b3a9f34b201d	1047	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1048	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1049	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1050	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1051	adjAnable = true;
M_souta	22:7d93f79a3686	1052	break;
M_souta	25:b3a9f34b201d	1053	case -3:
M_souta	25:b3a9f34b201d	1054	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1055	tirePWM[TIRE_BL] = -10;
M_souta	25:b3a9f34b201d	1056	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1057	tirePWM[TIRE_FR] = 10;
M_souta	25:b3a9f34b201d	1058	adjAnable = true;
M_souta	25:b3a9f34b201d	1059	break;
M_souta	25:b3a9f34b201d	1060	case -1:
M_souta	25:b3a9f34b201d	1061	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1062	tirePWM[TIRE_BL] = -20;
M_souta	25:b3a9f34b201d	1063	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1064	tirePWM[TIRE_FR] = 20;
M_souta	25:b3a9f34b201d	1065	adjAnable = true;
M_souta	25:b3a9f34b201d	1066	break;
M_souta	25:b3a9f34b201d	1067	case 0:
M_souta	25:b3a9f34b201d	1068	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1069	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1070	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1071	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1072	adjAnable = true;
M_souta	25:b3a9f34b201d	1073	break;
M_souta	25:b3a9f34b201d	1074	case 1:
M_souta	25:b3a9f34b201d	1075	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	1076	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1077	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	1078	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1079	adjAnable = true;
M_souta	25:b3a9f34b201d	1080	break;
M_souta	25:b3a9f34b201d	1081	case 3:
M_souta	25:b3a9f34b201d	1082	tirePWM[TIRE_FL] = 10;
M_souta	25:b3a9f34b201d	1083	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1084	tirePWM[TIRE_BR] = -10;
M_souta	25:b3a9f34b201d	1085	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1086	adjAnable = true;
M_souta	25:b3a9f34b201d	1087	break;
M_souta	25:b3a9f34b201d	1088	case 2:
M_souta	25:b3a9f34b201d	1089	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1090	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1091	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1092	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1093	adjAnable = true;
M_souta	25:b3a9f34b201d	1094	break;
M_souta	25:b3a9f34b201d	1095	case 'A':
M_souta	25:b3a9f34b201d	1096	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1097	lineCheck = true;
M_souta	25:b3a9f34b201d	1098	lineCount = 0;
M_souta	25:b3a9f34b201d	1099	count++;
M_souta	25:b3a9f34b201d	1100	}
M_souta	25:b3a9f34b201d	1101	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1102	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1103	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1104	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1105	adjAnable = true;
M_souta	25:b3a9f34b201d	1106	break;
M_souta	25:b3a9f34b201d	1107	case 'N':
M_souta	25:b3a9f34b201d	1108	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1109	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1110	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1111	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1112	adjAnable = false;
M_souta	25:b3a9f34b201d	1113	break;
M_souta	22:7d93f79a3686	1114	default:
M_souta	25:b3a9f34b201d	1115	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1116	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1117	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1118	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1119	adjAnable = false;
M_souta	22:7d93f79a3686	1120	}
M_souta	25:b3a9f34b201d	1121
M_souta	25:b3a9f34b201d	1122	if(adjAnable){
M_souta	25:b3a9f34b201d	1123	adj = 0;//linePara[3];
M_souta	25:b3a9f34b201d	1124	} else {
M_souta	25:b3a9f34b201d	1125	adj = 0;
M_souta	25:b3a9f34b201d	1126	}
M_souta	25:b3a9f34b201d	1127
M_souta	25:b3a9f34b201d	1128	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	1129	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1130	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1131	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1132	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	1133	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1134	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1135	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1136
M_souta	25:b3a9f34b201d	1137	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1138	lineCount++;
M_souta	25:b3a9f34b201d	1139	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1140	}
M_souta	25:b3a9f34b201d	1141	if(count == 1) {
M_souta	25:b3a9f34b201d	1142	count = 0;
M_souta	25:b3a9f34b201d	1143	lineFase = 12;
M_souta	25:b3a9f34b201d	1144	lineCount = 0;
M_souta	25:b3a9f34b201d	1145	lineCheck = false;
M_souta	25:b3a9f34b201d	1146	}
M_souta	25:b3a9f34b201d	1147	} else if(lineFase == 12) { // 右低速
M_souta	25:b3a9f34b201d	1148	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1149	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1150	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1151	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1152	if(linePara[LINE_TOW_2] == 0) {
M_souta	25:b3a9f34b201d	1153	lineFase = 13;
M_souta	25:b3a9f34b201d	1154	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1155	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1156	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1157	motor[TIRE_FR].dir = BRAKE;
M_souta	22:7d93f79a3686	1158	}
M_souta	25:b3a9f34b201d	1159	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1160	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1161	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1162	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1163	} else if (lineFase == 13) {
M_souta	25:b3a9f34b201d	1164	lineFase = 14;
M_souta	25:b3a9f34b201d	1165	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1166	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1167	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1168	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1169	} else if(lineFase == 14) { // タオル2 竿検知
M_souta	25:b3a9f34b201d	1170	if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
M_souta	25:b3a9f34b201d	1171	lineFase = 15;
M_souta	25:b3a9f34b201d	1172	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1173	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1174	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1175	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1176	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1177	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1178	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1179	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1180	} else if(LimitSw::IsPressed(TOW_2L)) {
M_souta	25:b3a9f34b201d	1181	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1182	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1183	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1184	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1185	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1186	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1187	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1188	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1189	} else if(LimitSw::IsPressed(TOW_2R)) {
M_souta	25:b3a9f34b201d	1190	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1191	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1192	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1193	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1194	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1195	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1196	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1197	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1198	} else {
M_souta	25:b3a9f34b201d	1199	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1200	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1201	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1202	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1203	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1204	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1205	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1206	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1207	}
M_souta	25:b3a9f34b201d	1208	} else if(lineFase == 15 ){ // ライン修正
M_souta	25:b3a9f34b201d	1209	if(linePara[LINE_TOW_2] == 'A' \|\| linePara[LINE_TOW_2] == 'N') {
M_souta	25:b3a9f34b201d	1210	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1211	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1212	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1213	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1214	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1215	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1216	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1217	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1218	} else if(linePara[LINE_TOW_2] > 0) {
M_souta	25:b3a9f34b201d	1219	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1220	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1221	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1222	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1223	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1224	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1225	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1226	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1227	} else if(linePara[LINE_TOW_2] < 0) {
M_souta	25:b3a9f34b201d	1228	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1229	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1230	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1231	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1232	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1233	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1234	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1235	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1236	} else if(linePara[LINE_TOW_2] == 0) {
M_souta	25:b3a9f34b201d	1237	lineFase = 16;
M_souta	25:b3a9f34b201d	1238	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1239	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1240	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1241	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1242	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1243	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1244	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1245	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1246	} else {
M_souta	25:b3a9f34b201d	1247	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1248	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1249	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1250	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1251	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1252	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1253	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1254	motor[TIRE_FR].pwm = 50;
M_souta	22:7d93f79a3686	1255	}
M_souta	25:b3a9f34b201d	1256	} else if(lineFase == 16) { // タオル2 解放
M_souta	25:b3a9f34b201d	1257	//solenoid.TOWEL2 = SOLENOID_ON;
M_souta	25:b3a9f34b201d	1258	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1259	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1260	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1261	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1262	lineFase = 17;
M_souta	25:b3a9f34b201d	1263	} else if(lineFase == 17) { // 前
M_souta	25:b3a9f34b201d	1264	switch(linePara[LINE_TOW_2]) {
M_souta	25:b3a9f34b201d	1265	case 2:
M_souta	25:b3a9f34b201d	1266	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1267	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1268	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1269	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1270	adjAnable = true;
M_souta	25:b3a9f34b201d	1271	break;
M_souta	25:b3a9f34b201d	1272	case 3:
M_souta	25:b3a9f34b201d	1273	tirePWM[TIRE_FL] = 10;
M_souta	25:b3a9f34b201d	1274	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1275	tirePWM[TIRE_BR] = -10;
M_souta	25:b3a9f34b201d	1276	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1277	adjAnable = true;
M_souta	25:b3a9f34b201d	1278	break;
M_souta	25:b3a9f34b201d	1279	case 1:
M_souta	25:b3a9f34b201d	1280	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	1281	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1282	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	1283	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1284	adjAnable = true;
M_souta	25:b3a9f34b201d	1285	break;
M_souta	25:b3a9f34b201d	1286	case 0:
M_souta	25:b3a9f34b201d	1287	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1288	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1289	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1290	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1291	adjAnable = true;
M_souta	25:b3a9f34b201d	1292	break;
M_souta	25:b3a9f34b201d	1293	case -1:
M_souta	25:b3a9f34b201d	1294	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1295	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	1296	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1297	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	1298	adjAnable = true;
M_souta	25:b3a9f34b201d	1299	break;
M_souta	25:b3a9f34b201d	1300	case -3:
M_souta	25:b3a9f34b201d	1301	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1302	tirePWM[TIRE_BL] = 10;
M_souta	25:b3a9f34b201d	1303	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1304	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	1305	adjAnable = true;
M_souta	25:b3a9f34b201d	1306	break;
M_souta	25:b3a9f34b201d	1307	case -2:
M_souta	25:b3a9f34b201d	1308	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1309	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1310	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1311	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1312	adjAnable = true;
M_souta	25:b3a9f34b201d	1313	break;
M_souta	25:b3a9f34b201d	1314	case 'A':
M_souta	25:b3a9f34b201d	1315	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1316	lineCheck = true;
M_souta	25:b3a9f34b201d	1317	lineCount = 0;
M_souta	25:b3a9f34b201d	1318	count++;
M_souta	25:b3a9f34b201d	1319	}
M_souta	25:b3a9f34b201d	1320	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1321	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1322	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1323	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1324	adjAnable = true;
M_souta	25:b3a9f34b201d	1325	break;
M_souta	25:b3a9f34b201d	1326	case 'N':
M_souta	25:b3a9f34b201d	1327	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1328	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1329	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1330	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1331	adjAnable = false;
M_souta	25:b3a9f34b201d	1332	break;
M_souta	25:b3a9f34b201d	1333	default:
M_souta	25:b3a9f34b201d	1334	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1335	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1336	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1337	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1338	adjAnable = false;
M_souta	25:b3a9f34b201d	1339	}
M_souta	25:b3a9f34b201d	1340
M_souta	25:b3a9f34b201d	1341	if(adjAnable){
M_souta	25:b3a9f34b201d	1342	adj = 0;
M_souta	25:b3a9f34b201d	1343	} else {
M_souta	25:b3a9f34b201d	1344	adj = 0;
M_souta	25:b3a9f34b201d	1345	}
M_souta	25:b3a9f34b201d	1346
M_souta	25:b3a9f34b201d	1347	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1348	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1349	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1350	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1351	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1352	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1353	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1354	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1355
M_souta	25:b3a9f34b201d	1356	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1357	lineCount++;
M_souta	25:b3a9f34b201d	1358	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1359	}
M_souta	25:b3a9f34b201d	1360	if(count == 1) {
M_souta	25:b3a9f34b201d	1361	count = 0;
M_souta	25:b3a9f34b201d	1362	lineFase = 18;
M_souta	25:b3a9f34b201d	1363	lineCount = 0;
M_souta	25:b3a9f34b201d	1364	lineCheck = false;
M_souta	25:b3a9f34b201d	1365	}
M_souta	25:b3a9f34b201d	1366	} else if(lineFase == 18) { // 前低速
M_souta	25:b3a9f34b201d	1367	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1368	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1369	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1370	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1371	if(linePara[4] != 'N') {
M_souta	25:b3a9f34b201d	1372	lineFase = 19;
M_souta	25:b3a9f34b201d	1373	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1374	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1375	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1376	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1377	}
M_souta	25:b3a9f34b201d	1378	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1379	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1380	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1381	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1382	} else if(lineFase == 19) { // 左
M_souta	25:b3a9f34b201d	1383	switch(linePara[3]) {
M_souta	25:b3a9f34b201d	1384	case -2:
M_souta	25:b3a9f34b201d	1385	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1386	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1387	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1388	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1389	adjAnable = true;
M_souta	25:b3a9f34b201d	1390	break;
M_souta	25:b3a9f34b201d	1391	case -3:
M_souta	25:b3a9f34b201d	1392	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1393	tirePWM[TIRE_BL] = 10;
M_souta	25:b3a9f34b201d	1394	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1395	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	1396	adjAnable = true;
M_souta	25:b3a9f34b201d	1397	break;
M_souta	25:b3a9f34b201d	1398	case -1:
M_souta	25:b3a9f34b201d	1399	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1400	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	1401	tirePWM[TIRE_BR] = 30
M_souta	25:b3a9f34b201d	1402	tirePWM[TIRE_FR] = -2;
M_souta	25:b3a9f34b201d	1403	adjAnable = true;
M_souta	25:b3a9f34b201d	1404	break;
M_souta	25:b3a9f34b201d	1405	case 0:
M_souta	25:b3a9f34b201d	1406	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1407	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1408	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1409	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1410	adjAnable = true;
M_souta	25:b3a9f34b201d	1411	break;
M_souta	25:b3a9f34b201d	1412	case 1:
M_souta	25:b3a9f34b201d	1413	tirePWM[TIRE_FL] = -20;
M_souta	25:b3a9f34b201d	1414	tirePWM[TIRE_BL] = 40;
M_souta	25:b3a9f34b201d	1415	tirePWM[TIRE_BR] = 20;
M_souta	25:b3a9f34b201d	1416	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1417	adjAnable = true;
M_souta	25:b3a9f34b201d	1418	break;
M_souta	25:b3a9f34b201d	1419	case 3:
M_souta	25:b3a9f34b201d	1420	tirePWM[TIRE_FL] = -10;
M_souta	25:b3a9f34b201d	1421	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1422	tirePWM[TIRE_BR] = 10;
M_souta	25:b3a9f34b201d	1423	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1424	adjAnable = true;
M_souta	25:b3a9f34b201d	1425	break;
M_souta	25:b3a9f34b201d	1426	case 2:
M_souta	25:b3a9f34b201d	1427	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1428	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1429	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1430	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1431	adjAnable = true;
M_souta	25:b3a9f34b201d	1432	break;
M_souta	25:b3a9f34b201d	1433	case 'A':
M_souta	25:b3a9f34b201d	1434	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1435	lineCheck = true;
M_souta	25:b3a9f34b201d	1436	lineCount = 0;
M_souta	25:b3a9f34b201d	1437	count++;
M_souta	25:b3a9f34b201d	1438	}
M_souta	25:b3a9f34b201d	1439	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1440	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1441	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1442	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1443	adjAnable = true;
M_souta	25:b3a9f34b201d	1444	break;
M_souta	25:b3a9f34b201d	1445	case 'N':
M_souta	25:b3a9f34b201d	1446	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1447	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1448	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1449	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1450	adjAnable = false;
M_souta	25:b3a9f34b201d	1451	break;
M_souta	25:b3a9f34b201d	1452	default:
M_souta	25:b3a9f34b201d	1453	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1454	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1455	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1456	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1457	adjAnable = false;
M_souta	25:b3a9f34b201d	1458	}
M_souta	25:b3a9f34b201d	1459
M_souta	25:b3a9f34b201d	1460	if(adjAnable){
M_souta	25:b3a9f34b201d	1461	if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta	25:b3a9f34b201d	1462	} else {
M_souta	25:b3a9f34b201d	1463	adj = 0;
M_souta	25:b3a9f34b201d	1464	}
M_souta	25:b3a9f34b201d	1465
M_souta	25:b3a9f34b201d	1466	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1467	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1468	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1469	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1470	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1471	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1472	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1473	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1474
M_souta	25:b3a9f34b201d	1475	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1476	lineCount++;
M_souta	25:b3a9f34b201d	1477	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1478	}
M_souta	25:b3a9f34b201d	1479
M_souta	25:b3a9f34b201d	1480	/*
M_souta	25:b3a9f34b201d	1481	if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
M_souta	25:b3a9f34b201d	1482	targetCount = 3;
M_souta	25:b3a9f34b201d	1483	} else if(LimitSw::IsPressed(TOWEL1_SW)) {
M_souta	25:b3a9f34b201d	1484	targetCount = 3;
M_souta	25:b3a9f34b201d	1485	} else {
M_souta	25:b3a9f34b201d	1486	targetCount = 2;
M_souta	25:b3a9f34b201d	1487	}
M_souta	25:b3a9f34b201d	1488	*/
M_souta	25:b3a9f34b201d	1489
M_souta	25:b3a9f34b201d	1490	targetCount = 3;
M_souta	25:b3a9f34b201d	1491
M_souta	25:b3a9f34b201d	1492	if(count == targetCount) {
M_souta	25:b3a9f34b201d	1493	count = 0;
M_souta	25:b3a9f34b201d	1494	lineFase = 20;
M_souta	25:b3a9f34b201d	1495	lineCount = 0;
M_souta	25:b3a9f34b201d	1496	lineCheck = false;
M_souta	25:b3a9f34b201d	1497	}
M_souta	25:b3a9f34b201d	1498	} else if(lineFase == 20) { // 左低速
M_souta	25:b3a9f34b201d	1499	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1500	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1501	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1502	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1503	if (linePara[2] == 'N') {
M_souta	25:b3a9f34b201d	1504	//(LimitSw::IsPressed(SHEETS_SW)) {
M_souta	25:b3a9f34b201d	1505	//ineFase = 20;
M_souta	25:b3a9f34b201d	1506	//else if(LimitSw::IsPressed(TOWEL2_SW) {
M_souta	25:b3a9f34b201d	1507	//ineFase = 14;
M_souta	25:b3a9f34b201d	1508	//else {
M_souta	25:b3a9f34b201d	1509	lineFase = 21;
M_souta	25:b3a9f34b201d	1510	//
M_souta	25:b3a9f34b201d	1511	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1512	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1513	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1514	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1515	}
M_souta	25:b3a9f34b201d	1516	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1517	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1518	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1519	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1520	} else if(lineFase == 21) {
M_souta	25:b3a9f34b201d	1521	switch(linePara[2]) {
M_souta	25:b3a9f34b201d	1522	case -2:
M_souta	25:b3a9f34b201d	1523	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1524	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1525	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1526	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1527	adjAnable = true;
M_souta	25:b3a9f34b201d	1528	break;
M_souta	25:b3a9f34b201d	1529	case -3:
M_souta	25:b3a9f34b201d	1530	tirePWM[TIRE_FL] = -10;
M_souta	25:b3a9f34b201d	1531	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1532	tirePWM[TIRE_BR] = 10;
M_souta	25:b3a9f34b201d	1533	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1534	adjAnable = true;
M_souta	25:b3a9f34b201d	1535	break;
M_souta	25:b3a9f34b201d	1536	case -1:
M_souta	25:b3a9f34b201d	1537	tirePWM[TIRE_FL] = -20;
M_souta	25:b3a9f34b201d	1538	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1539	tirePWM[TIRE_BR] = 20;
M_souta	25:b3a9f34b201d	1540	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1541	adjAnable = true;
M_souta	25:b3a9f34b201d	1542	break;
M_souta	25:b3a9f34b201d	1543	case 0:
M_souta	25:b3a9f34b201d	1544	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1545	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1546	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1547	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1548	adjAnable = true;
M_souta	25:b3a9f34b201d	1549	break;
M_souta	25:b3a9f34b201d	1550	case 1:
M_souta	25:b3a9f34b201d	1551	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1552	tirePWM[TIRE_BL] = -20;
M_souta	25:b3a9f34b201d	1553	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1554	tirePWM[TIRE_FR] = 20;
M_souta	25:b3a9f34b201d	1555	adjAnable = true;
M_souta	25:b3a9f34b201d	1556	break;
M_souta	25:b3a9f34b201d	1557	case 3:
M_souta	25:b3a9f34b201d	1558	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1559	tirePWM[TIRE_BL] = -10;
M_souta	25:b3a9f34b201d	1560	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1561	tirePWM[TIRE_FR] = 10;
M_souta	25:b3a9f34b201d	1562	adjAnable = true;
M_souta	25:b3a9f34b201d	1563	break;
M_souta	25:b3a9f34b201d	1564	case 2:
M_souta	25:b3a9f34b201d	1565	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1566	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1567	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1568	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1569	adjAnable = true;
M_souta	25:b3a9f34b201d	1570	break;
M_souta	25:b3a9f34b201d	1571	case 'A':
M_souta	25:b3a9f34b201d	1572	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1573	lineCheck = true;
M_souta	25:b3a9f34b201d	1574	lineCount = 0;
M_souta	25:b3a9f34b201d	1575	count++;
M_souta	25:b3a9f34b201d	1576	}
M_souta	25:b3a9f34b201d	1577	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1578	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1579	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1580	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1581	adjAnable = true;
M_souta	25:b3a9f34b201d	1582	break;
M_souta	25:b3a9f34b201d	1583	case 'N':
M_souta	25:b3a9f34b201d	1584	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1585	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1586	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1587	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1588	adjAnable = false;
M_souta	25:b3a9f34b201d	1589	break;
M_souta	25:b3a9f34b201d	1590	default:
M_souta	25:b3a9f34b201d	1591	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1592	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1593	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1594	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1595	adjAnable = false;
M_souta	25:b3a9f34b201d	1596	}
M_souta	25:b3a9f34b201d	1597
M_souta	25:b3a9f34b201d	1598	if(adjAnable){
M_souta	25:b3a9f34b201d	1599	if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
M_souta	25:b3a9f34b201d	1600	} else {
M_souta	25:b3a9f34b201d	1601	adj = 0;
M_souta	25:b3a9f34b201d	1602	}
M_souta	25:b3a9f34b201d	1603
M_souta	25:b3a9f34b201d	1604	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	1605	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1606	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1607	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1608	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	1609	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1610	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1611	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1612
M_souta	25:b3a9f34b201d	1613	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1614	lineCount++;
M_souta	25:b3a9f34b201d	1615	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1616	}
M_souta	25:b3a9f34b201d	1617	if(count == 3) {
M_souta	25:b3a9f34b201d	1618	count = 0;
M_souta	25:b3a9f34b201d	1619	lineFase = 22;
M_souta	25:b3a9f34b201d	1620	lineCount = 0;
M_souta	25:b3a9f34b201d	1621	lineCheck = false;
M_souta	25:b3a9f34b201d	1622	}
M_souta	25:b3a9f34b201d	1623	} else if(lineFase == 22) {
M_souta	25:b3a9f34b201d	1624	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1625	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1626	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1627	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1628	if (linePara[2] == 'N') {
M_souta	25:b3a9f34b201d	1629	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1630	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1631	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1632	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1633	}
M_souta	25:b3a9f34b201d	1634	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	1635	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	1636	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	1637	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	1638	} else{
M_souta	25:b3a9f34b201d	1639	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1640	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1641	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1642	motor[TIRE_FR].dir = BRAKE;
M_souta	22:7d93f79a3686	1643	}
M_souta	22:7d93f79a3686	1644
kishibekairohan	13:b6e02d6261d7	1645	}
kishibekairohan	13:b6e02d6261d7	1646	#endif
kishibekairohan	13:b6e02d6261d7	1647
kishibekairohan	13:b6e02d6261d7	1648	#if USE_PROCESS_NUM>5
kishibekairohan	16:3f2c2d89372b	1649	static void Process5()
kishibekairohan	16:3f2c2d89372b	1650	{
M_souta	25:b3a9f34b201d	1651
M_souta	25:b3a9f34b201d	1652	/* ************************************** //
M_souta	25:b3a9f34b201d	1653
M_souta	25:b3a9f34b201d	1654	青ゾーン青ゾーン青ゾーン
M_souta	25:b3a9f34b201d	1655
M_souta	25:b3a9f34b201d	1656	// ************************************** */
M_souta	25:b3a9f34b201d	1657
M_souta	25:b3a9f34b201d	1658	for(int i = 0; i < 8; i++) {
M_souta	25:b3a9f34b201d	1659	linePara[i] = lineCast(LineHub::GetPara(i));
M_souta	25:b3a9f34b201d	1660	}
M_souta	25:b3a9f34b201d	1661
M_souta	25:b3a9f34b201d	1662	if(lineFase == 0) {
M_souta	25:b3a9f34b201d	1663	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1664	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1665	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1666	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1667	if(linePara[0] != 'N' && linePara[0] != 'A') {
M_souta	25:b3a9f34b201d	1668	lineFase = 1;
M_souta	25:b3a9f34b201d	1669	}
M_souta	25:b3a9f34b201d	1670	motor[TIRE_FL].pwm = 20;
M_souta	25:b3a9f34b201d	1671	motor[TIRE_BL].pwm = 20;
M_souta	25:b3a9f34b201d	1672	motor[TIRE_BR].pwm = 20;
M_souta	25:b3a9f34b201d	1673	motor[TIRE_FR].pwm = 20;
M_souta	25:b3a9f34b201d	1674	}else if(lineFase == 1) { // 前ライントレース
M_souta	25:b3a9f34b201d	1675	switch(linePara[0]) {
M_souta	25:b3a9f34b201d	1676	case -2:
M_souta	25:b3a9f34b201d	1677	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1678	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1679	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1680	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1681	adjAnable = true;
M_souta	25:b3a9f34b201d	1682	break;
M_souta	25:b3a9f34b201d	1683	case -3:
M_souta	25:b3a9f34b201d	1684	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	1685	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1686	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1687	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1688	adjAnable = true;
M_souta	25:b3a9f34b201d	1689	break;
M_souta	25:b3a9f34b201d	1690	case -1:
M_souta	25:b3a9f34b201d	1691	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	1692	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1693	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	1694	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1695	adjAnable = true;
M_souta	25:b3a9f34b201d	1696	break;
M_souta	25:b3a9f34b201d	1697	case 0:
M_souta	25:b3a9f34b201d	1698	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	1699	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1700	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	1701	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1702	adjAnable = true;
M_souta	25:b3a9f34b201d	1703	break;
M_souta	25:b3a9f34b201d	1704	case 1:
M_souta	25:b3a9f34b201d	1705	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	1706	tirePWM[TIRE_BL] = 40;
M_souta	25:b3a9f34b201d	1707	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	1708	tirePWM[TIRE_FR] = -40;
M_souta	25:b3a9f34b201d	1709	adjAnable = true;
M_souta	25:b3a9f34b201d	1710	break;
M_souta	25:b3a9f34b201d	1711	case 3:
M_souta	25:b3a9f34b201d	1712	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	1713	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	1714	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	1715	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	1716	adjAnable = true;
M_souta	25:b3a9f34b201d	1717	break;
M_souta	25:b3a9f34b201d	1718	case 2:
M_souta	25:b3a9f34b201d	1719	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	1720	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1721	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	1722	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1723	adjAnable = true;
M_souta	25:b3a9f34b201d	1724	break;
M_souta	25:b3a9f34b201d	1725	case 'A':
M_souta	25:b3a9f34b201d	1726	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1727	lineCheck = true;
M_souta	25:b3a9f34b201d	1728	lineCount = 0;
M_souta	25:b3a9f34b201d	1729	count++;
M_souta	25:b3a9f34b201d	1730	}
M_souta	25:b3a9f34b201d	1731	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	1732	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1733	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	1734	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1735	adjAnable = true;
M_souta	25:b3a9f34b201d	1736	break;
M_souta	25:b3a9f34b201d	1737	case 'N':
M_souta	25:b3a9f34b201d	1738	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1739	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1740	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1741	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1742	adjAnable = false;
M_souta	25:b3a9f34b201d	1743	break;
M_souta	25:b3a9f34b201d	1744	default:
M_souta	25:b3a9f34b201d	1745	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1746	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1747	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1748	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1749	adjAnable = false;
M_souta	25:b3a9f34b201d	1750	}
M_souta	25:b3a9f34b201d	1751
M_souta	25:b3a9f34b201d	1752	if(adjAnable){
M_souta	25:b3a9f34b201d	1753	if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
M_souta	25:b3a9f34b201d	1754	} else {
M_souta	25:b3a9f34b201d	1755	adj = 0;
M_souta	25:b3a9f34b201d	1756	}
M_souta	25:b3a9f34b201d	1757
M_souta	25:b3a9f34b201d	1758	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1759	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1760	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1761	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1762	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]) * 0.8;
M_souta	25:b3a9f34b201d	1763	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj) * 0.8;
M_souta	25:b3a9f34b201d	1764	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj) * 0.8;
M_souta	25:b3a9f34b201d	1765	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]) * 0.8;
M_souta	25:b3a9f34b201d	1766
M_souta	25:b3a9f34b201d	1767	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1768	lineCount++;
M_souta	25:b3a9f34b201d	1769	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1770	}
M_souta	25:b3a9f34b201d	1771	if(count == 3) {
M_souta	25:b3a9f34b201d	1772	count = 0;
M_souta	25:b3a9f34b201d	1773	lineFase = 2;
M_souta	25:b3a9f34b201d	1774	lineCount = 0;
M_souta	25:b3a9f34b201d	1775	lineCheck = false;
M_souta	25:b3a9f34b201d	1776	}
M_souta	25:b3a9f34b201d	1777	} else if(lineFase == 2) { // 前低速
M_souta	25:b3a9f34b201d	1778	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1779	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1780	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1781	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1782	if(linePara[3] != 'N') {
M_souta	25:b3a9f34b201d	1783	lineFase = 3;
M_souta	25:b3a9f34b201d	1784	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1785	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1786	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1787	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1788	}
M_souta	25:b3a9f34b201d	1789	motor[TIRE_FL].pwm = 20;
M_souta	25:b3a9f34b201d	1790	motor[TIRE_BL].pwm = 20;
M_souta	25:b3a9f34b201d	1791	motor[TIRE_BR].pwm = 20;
M_souta	25:b3a9f34b201d	1792	motor[TIRE_FR].pwm = 20;
M_souta	25:b3a9f34b201d	1793	} else if(lineFase == 3) { // 左ライントレース
M_souta	25:b3a9f34b201d	1794	switch(linePara[4]) {
M_souta	25:b3a9f34b201d	1795	case -2:
M_souta	25:b3a9f34b201d	1796	tirePWM[TIRE_FL] = -50;
M_souta	25:b3a9f34b201d	1797	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1798	tirePWM[TIRE_BR] = 50;
M_souta	25:b3a9f34b201d	1799	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1800	adjAnable = true;
M_souta	25:b3a9f34b201d	1801	break;
M_souta	25:b3a9f34b201d	1802	case -3:
M_souta	25:b3a9f34b201d	1803	tirePWM[TIRE_FL] = -50;
M_souta	25:b3a9f34b201d	1804	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	1805	tirePWM[TIRE_BR] = 50;
M_souta	25:b3a9f34b201d	1806	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	1807	adjAnable = true;
M_souta	25:b3a9f34b201d	1808	break;
M_souta	25:b3a9f34b201d	1809	case -1:
M_souta	25:b3a9f34b201d	1810	tirePWM[TIRE_FL] = -50;
M_souta	25:b3a9f34b201d	1811	tirePWM[TIRE_BL] = 40;
M_souta	25:b3a9f34b201d	1812	tirePWM[TIRE_BR] = 50;
M_souta	25:b3a9f34b201d	1813	tirePWM[TIRE_FR] = -40;
M_souta	25:b3a9f34b201d	1814	adjAnable = true;
M_souta	25:b3a9f34b201d	1815	break;
M_souta	25:b3a9f34b201d	1816	case 0:
M_souta	25:b3a9f34b201d	1817	tirePWM[TIRE_FL] = -50;
M_souta	25:b3a9f34b201d	1818	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1819	tirePWM[TIRE_BR] = 50;
M_souta	25:b3a9f34b201d	1820	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1821	adjAnable = true;
M_souta	25:b3a9f34b201d	1822	break;
M_souta	25:b3a9f34b201d	1823	case 1:
M_souta	25:b3a9f34b201d	1824	tirePWM[TIRE_FL] = -40;
M_souta	25:b3a9f34b201d	1825	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1826	tirePWM[TIRE_BR] = 40;
M_souta	25:b3a9f34b201d	1827	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1828	adjAnable = true;
M_souta	25:b3a9f34b201d	1829	break;
M_souta	25:b3a9f34b201d	1830	case 3:
M_souta	25:b3a9f34b201d	1831	tirePWM[TIRE_FL] = -20;
M_souta	25:b3a9f34b201d	1832	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1833	tirePWM[TIRE_BR] = 20;
M_souta	25:b3a9f34b201d	1834	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1835	adjAnable = true;
M_souta	25:b3a9f34b201d	1836	break;
M_souta	25:b3a9f34b201d	1837	case 2:
M_souta	25:b3a9f34b201d	1838	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1839	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1840	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1841	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1842	adjAnable = true;
M_souta	25:b3a9f34b201d	1843	break;
M_souta	25:b3a9f34b201d	1844	case 'A':
M_souta	25:b3a9f34b201d	1845	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1846	lineCheck = true;
M_souta	25:b3a9f34b201d	1847	lineCount = 0;
M_souta	25:b3a9f34b201d	1848	count++;
M_souta	25:b3a9f34b201d	1849	}
M_souta	25:b3a9f34b201d	1850	tirePWM[TIRE_FL] = -50;
M_souta	25:b3a9f34b201d	1851	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	1852	tirePWM[TIRE_BR] = 50;
M_souta	25:b3a9f34b201d	1853	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	1854	adjAnable = true;
M_souta	25:b3a9f34b201d	1855	break;
M_souta	25:b3a9f34b201d	1856	case 'N':
M_souta	25:b3a9f34b201d	1857	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1858	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1859	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1860	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1861	adjAnable = false;
M_souta	25:b3a9f34b201d	1862	break;
M_souta	25:b3a9f34b201d	1863	default:
M_souta	25:b3a9f34b201d	1864	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1865	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1866	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1867	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1868	adjAnable = false;
M_souta	25:b3a9f34b201d	1869	}
M_souta	25:b3a9f34b201d	1870
M_souta	25:b3a9f34b201d	1871	if(adjAnable){
M_souta	25:b3a9f34b201d	1872	if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta	25:b3a9f34b201d	1873	} else {
M_souta	25:b3a9f34b201d	1874	adj = 0;
M_souta	25:b3a9f34b201d	1875	}
M_souta	25:b3a9f34b201d	1876
M_souta	25:b3a9f34b201d	1877	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1878	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1879	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1880	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1881	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1882	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1883	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1884	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1885
M_souta	25:b3a9f34b201d	1886	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1887	lineCount++;
M_souta	25:b3a9f34b201d	1888	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1889	}
M_souta	25:b3a9f34b201d	1890	if(count == 2) {
M_souta	25:b3a9f34b201d	1891	count = 0;
M_souta	25:b3a9f34b201d	1892	lineFase = 4;
M_souta	25:b3a9f34b201d	1893	lineCount = 0;
M_souta	25:b3a9f34b201d	1894	lineCheck = false;
M_souta	25:b3a9f34b201d	1895	}
M_souta	25:b3a9f34b201d	1896	} else if(lineFase == 4) { // 右低速
M_souta	25:b3a9f34b201d	1897	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1898	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1899	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1900	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1901	if(linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	1902	lineFase = 5;
M_souta	25:b3a9f34b201d	1903	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1904	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1905	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1906	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1907	}
M_souta	25:b3a9f34b201d	1908	motor[TIRE_FL].pwm = 20;
M_souta	25:b3a9f34b201d	1909	motor[TIRE_BL].pwm = 20;
M_souta	25:b3a9f34b201d	1910	motor[TIRE_BR].pwm = 20;
M_souta	25:b3a9f34b201d	1911	motor[TIRE_FR].pwm = 20;
M_souta	25:b3a9f34b201d	1912	} else if (lineFase == 5) {
M_souta	25:b3a9f34b201d	1913	lineFase = 6;
M_souta	25:b3a9f34b201d	1914	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1915	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1916	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1917	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1918	} else if(lineFase == 6) { // タオル竿検知
M_souta	25:b3a9f34b201d	1919	if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	1920	lineFase = 7;
M_souta	25:b3a9f34b201d	1921	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1922	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1923	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1924	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1925	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1926	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1927	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1928	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1929	} else if(LimitSw::IsPressed(TOW_1L)) {
M_souta	25:b3a9f34b201d	1930	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1931	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1932	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1933	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1934	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1935	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1936	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1937	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1938	} else if(LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	1939	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1940	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1941	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1942	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1943	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1944	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1945	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1946	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1947	} else {
M_souta	25:b3a9f34b201d	1948	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1949	motor[TIRE_FL].pwm = 20;
M_souta	25:b3a9f34b201d	1950	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1951	motor[TIRE_BL].pwm = 20;
M_souta	25:b3a9f34b201d	1952	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1953	motor[TIRE_BR].pwm = 20;
M_souta	25:b3a9f34b201d	1954	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1955	motor[TIRE_FR].pwm = 20;
M_souta	25:b3a9f34b201d	1956	}
M_souta	25:b3a9f34b201d	1957	} else if(lineFase == 7) { // ライン修正
M_souta	25:b3a9f34b201d	1958	if(linePara[LINE_TOW_1] == 'A' \|\| linePara[LINE_TOW_1] == 'N') {
M_souta	25:b3a9f34b201d	1959	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1960	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1961	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1962	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1963	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1964	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1965	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1966	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1967	} else if(linePara[LINE_TOW_1] > 0) {
M_souta	25:b3a9f34b201d	1968	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1969	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1970	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1971	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1972	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1973	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1974	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1975	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1976	} else if(linePara[LINE_TOW_1] < 0) {
M_souta	25:b3a9f34b201d	1977	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1978	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1979	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1980	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1981	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1982	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1983	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1984	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1985	} else if(linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	1986	lineFase = 8;
M_souta	25:b3a9f34b201d	1987	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1988	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1989	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1990	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1991	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1992	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1993	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1994	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1995	} else {
M_souta	25:b3a9f34b201d	1996	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1997	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	1998	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1999	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	2000	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2001	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	2002	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2003	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	2004	}
M_souta	25:b3a9f34b201d	2005	} else if(lineFase == 8) { // タオル1 解放
M_souta	25:b3a9f34b201d	2006	//solenoid.TOWEL1 = SOLENOID_ON;
M_souta	25:b3a9f34b201d	2007	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2008	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2009	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2010	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2011	lineFase = 9;
M_souta	25:b3a9f34b201d	2012	} else if(lineFase == 9) { // 前
M_souta	25:b3a9f34b201d	2013	switch(linePara[LINE_TOW_1]) {
M_souta	25:b3a9f34b201d	2014	case 2:
M_souta	25:b3a9f34b201d	2015	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2016	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	2017	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	2018	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	2019	adjAnable = true;
M_souta	25:b3a9f34b201d	2020	break;
M_souta	25:b3a9f34b201d	2021	case 3:
M_souta	25:b3a9f34b201d	2022	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	2023	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	2024	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	2025	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	2026	adjAnable = true;
M_souta	25:b3a9f34b201d	2027	break;
M_souta	25:b3a9f34b201d	2028	case 1:
M_souta	25:b3a9f34b201d	2029	tirePWM[TIRE_FL] = 40;
M_souta	25:b3a9f34b201d	2030	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	2031	tirePWM[TIRE_BR] = -40;
M_souta	25:b3a9f34b201d	2032	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	2033	adjAnable = true;
M_souta	25:b3a9f34b201d	2034	break;
M_souta	25:b3a9f34b201d	2035	case 0:
M_souta	25:b3a9f34b201d	2036	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2037	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	2038	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2039	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	2040	adjAnable = true;
M_souta	25:b3a9f34b201d	2041	break;
M_souta	25:b3a9f34b201d	2042	case -1:
M_souta	25:b3a9f34b201d	2043	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2044	tirePWM[TIRE_BL] = 40;
M_souta	25:b3a9f34b201d	2045	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2046	tirePWM[TIRE_FR] = -40;
M_souta	25:b3a9f34b201d	2047	adjAnable = true;
M_souta	25:b3a9f34b201d	2048	break;
M_souta	25:b3a9f34b201d	2049	case -3:
M_souta	25:b3a9f34b201d	2050	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2051	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	2052	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2053	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	2054	adjAnable = true;
M_souta	25:b3a9f34b201d	2055	break;
M_souta	25:b3a9f34b201d	2056	case -2:
M_souta	25:b3a9f34b201d	2057	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2058	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	2059	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2060	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2061	adjAnable = true;
M_souta	25:b3a9f34b201d	2062	break;
M_souta	25:b3a9f34b201d	2063	case 'A':
M_souta	25:b3a9f34b201d	2064	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	2065	lineCheck = true;
M_souta	25:b3a9f34b201d	2066	lineCount = 0;
M_souta	25:b3a9f34b201d	2067	count++;
M_souta	25:b3a9f34b201d	2068	}
M_souta	25:b3a9f34b201d	2069	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2070	tirePWM[TIRE_BL] = 50;
M_souta	25:b3a9f34b201d	2071	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2072	tirePWM[TIRE_FR] = -50;
M_souta	25:b3a9f34b201d	2073	adjAnable = true;
M_souta	25:b3a9f34b201d	2074	break;
M_souta	25:b3a9f34b201d	2075	case 'N':
M_souta	25:b3a9f34b201d	2076	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2077	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	2078	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	2079	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2080	adjAnable = false;
M_souta	25:b3a9f34b201d	2081	break;
M_souta	25:b3a9f34b201d	2082	default:
M_souta	25:b3a9f34b201d	2083	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2084	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	2085	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	2086	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2087	adjAnable = false;
M_souta	25:b3a9f34b201d	2088	}
M_souta	25:b3a9f34b201d	2089
M_souta	25:b3a9f34b201d	2090	if(adjAnable){
M_souta	25:b3a9f34b201d	2091	adj = 0;
M_souta	25:b3a9f34b201d	2092	} else {
M_souta	25:b3a9f34b201d	2093	adj = 0;
M_souta	25:b3a9f34b201d	2094	}
M_souta	25:b3a9f34b201d	2095
M_souta	25:b3a9f34b201d	2096	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	2097	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	2098	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	2099	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	2100	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	2101	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	2102	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	2103	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	2104
M_souta	25:b3a9f34b201d	2105	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	2106	lineCount++;
M_souta	25:b3a9f34b201d	2107	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	2108	}
M_souta	25:b3a9f34b201d	2109	if(count == 1) {
M_souta	25:b3a9f34b201d	2110	count = 0;
M_souta	25:b3a9f34b201d	2111	lineFase = 10;
M_souta	25:b3a9f34b201d	2112	lineCount = 0;
M_souta	25:b3a9f34b201d	2113	lineCheck = false;
M_souta	25:b3a9f34b201d	2114	}
M_souta	25:b3a9f34b201d	2115	} else if(lineFase == 10) {
M_souta	25:b3a9f34b201d	2116	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	2117	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	2118	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	2119	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	2120	if(linePara[4] == 0) {
M_souta	25:b3a9f34b201d	2121	lineFase = 11;
M_souta	25:b3a9f34b201d	2122	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2123	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2124	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2125	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2126	}
M_souta	25:b3a9f34b201d	2127	motor[TIRE_FL].pwm = 20;
M_souta	25:b3a9f34b201d	2128	motor[TIRE_BL].pwm = 20;
M_souta	25:b3a9f34b201d	2129	motor[TIRE_BR].pwm = 20;
M_souta	25:b3a9f34b201d	2130	motor[TIRE_FR].pwm = 20;
M_souta	25:b3a9f34b201d	2131	} else if(lineFase == 11) {
M_souta	25:b3a9f34b201d	2132	switch(linePara[4]) {
M_souta	25:b3a9f34b201d	2133	case -2:
M_souta	25:b3a9f34b201d	2134	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2135	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	2136	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2137	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2138	adjAnable = true;
M_souta	25:b3a9f34b201d	2139	break;
M_souta	25:b3a9f34b201d	2140	case -3:
M_souta	25:b3a9f34b201d	2141	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2142	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	2143	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2144	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	2145	adjAnable = true;
M_souta	25:b3a9f34b201d	2146	break;
M_souta	25:b3a9f34b201d	2147	case -1:
M_souta	25:b3a9f34b201d	2148	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2149	tirePWM[TIRE_BL] = -40;
M_souta	25:b3a9f34b201d	2150	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2151	tirePWM[TIRE_FR] = 40;
M_souta	25:b3a9f34b201d	2152	adjAnable = true;
M_souta	25:b3a9f34b201d	2153	break;
M_souta	25:b3a9f34b201d	2154	case 0:
M_souta	25:b3a9f34b201d	2155	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2156	tirePWM[TIRE_BL] = -50;
M_souta	25:b3a9f34b201d	2157	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2158	tirePWM[TIRE_FR] = 50;
M_souta	25:b3a9f34b201d	2159	adjAnable = true;
M_souta	25:b3a9f34b201d	2160	break;
M_souta	25:b3a9f34b201d	2161	case 1:
M_souta	25:b3a9f34b201d	2162	tirePWM[TIRE_FL] = 40;
M_souta	25:b3a9f34b201d	2163	tirePWM[TIRE_BL] = -50;
M_souta	25:b3a9f34b201d	2164	tirePWM[TIRE_BR] = -40;
M_souta	25:b3a9f34b201d	2165	tirePWM[TIRE_FR] = 50;
M_souta	25:b3a9f34b201d	2166	adjAnable = true;
M_souta	25:b3a9f34b201d	2167	break;
M_souta	25:b3a9f34b201d	2168	case 3:
M_souta	25:b3a9f34b201d	2169	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	2170	tirePWM[TIRE_BL] = -50;
M_souta	25:b3a9f34b201d	2171	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	2172	tirePWM[TIRE_FR] = 50;
M_souta	25:b3a9f34b201d	2173	adjAnable = true;
M_souta	25:b3a9f34b201d	2174	break;
M_souta	25:b3a9f34b201d	2175	case 2:
M_souta	25:b3a9f34b201d	2176	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2177	tirePWM[TIRE_BL] = -50;
M_souta	25:b3a9f34b201d	2178	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	2179	tirePWM[TIRE_FR] = 50;
M_souta	25:b3a9f34b201d	2180	adjAnable = true;
M_souta	25:b3a9f34b201d	2181	break;
M_souta	25:b3a9f34b201d	2182	case 'A':
M_souta	25:b3a9f34b201d	2183	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	2184	lineCheck = true;
M_souta	25:b3a9f34b201d	2185	lineCount = 0;
M_souta	25:b3a9f34b201d	2186	count++;
M_souta	25:b3a9f34b201d	2187	}
M_souta	25:b3a9f34b201d	2188	tirePWM[TIRE_FL] = 50;
M_souta	25:b3a9f34b201d	2189	tirePWM[TIRE_BL] = -50;
M_souta	25:b3a9f34b201d	2190	tirePWM[TIRE_BR] = -50;
M_souta	25:b3a9f34b201d	2191	tirePWM[TIRE_FR] = 50;
M_souta	25:b3a9f34b201d	2192	adjAnable = true;
M_souta	25:b3a9f34b201d	2193	break;
M_souta	25:b3a9f34b201d	2194	case 'N':
M_souta	25:b3a9f34b201d	2195	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2196	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	2197	tirePWM[TIRE_BR] = 0.;
M_souta	25:b3a9f34b201d	2198	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2199	adjAnable = false;
M_souta	25:b3a9f34b201d	2200	break;
M_souta	25:b3a9f34b201d	2201	default:
M_souta	25:b3a9f34b201d	2202	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2203	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	2204	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	2205	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2206	adjAnable = false;
M_souta	25:b3a9f34b201d	2207	}
M_souta	25:b3a9f34b201d	2208
M_souta	25:b3a9f34b201d	2209	if(adjAnable){
M_souta	25:b3a9f34b201d	2210	adj = 0;//linePara[3];
M_souta	25:b3a9f34b201d	2211	} else {
M_souta	25:b3a9f34b201d	2212	adj = 0;
M_souta	25:b3a9f34b201d	2213	}
M_souta	25:b3a9f34b201d	2214
M_souta	25:b3a9f34b201d	2215	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	2216	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	2217	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	2218	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	2219	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	2220	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	2221	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	2222	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	2223
M_souta	25:b3a9f34b201d	2224	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	2225	lineCount++;
M_souta	25:b3a9f34b201d	2226	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	2227	}
M_souta	25:b3a9f34b201d	2228	if(count == 3) {
M_souta	25:b3a9f34b201d	2229	count = 0;
M_souta	25:b3a9f34b201d	2230	lineFase = 12;
M_souta	25:b3a9f34b201d	2231	lineCount = 0;
M_souta	25:b3a9f34b201d	2232	lineCheck = false;
M_souta	25:b3a9f34b201d	2233	}
M_souta	25:b3a9f34b201d	2234	} else if(lineFase == 12) { // 右低速
M_souta	25:b3a9f34b201d	2235	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	2236	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	2237	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	2238	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	2239	if(linePara[LINE_TOW_2] == 0) {
M_souta	25:b3a9f34b201d	2240	lineFase = 13;
M_souta	25:b3a9f34b201d	2241	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2242	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2243	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2244	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2245	}
M_souta	25:b3a9f34b201d	2246	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	2247	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	2248	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	2249	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	2250	} else if (lineFase == 13) {
M_souta	25:b3a9f34b201d	2251	lineFase = 14;
M_souta	25:b3a9f34b201d	2252	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2253	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2254	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2255	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2256	} else {
M_souta	25:b3a9f34b201d	2257	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2258	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2259	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2260	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2261	}
M_souta	25:b3a9f34b201d	2262
kishibekairohan	13:b6e02d6261d7	2263	}
kishibekairohan	13:b6e02d6261d7	2264	#endif
kishibekairohan	13:b6e02d6261d7	2265
kishibekairohan	13:b6e02d6261d7	2266	#if USE_PROCESS_NUM>6
kishibekairohan	13:b6e02d6261d7	2267	static void Process6()
kishibekairohan	13:b6e02d6261d7	2268	{
M_souta	25:b3a9f34b201d	2269	for(int i = 0; i < 8; i++) {
M_souta	22:7d93f79a3686	2270	linePara[i] = lineCast(LineHub::GetPara(i));
M_souta	22:7d93f79a3686	2271	}
M_souta	22:7d93f79a3686	2272
M_souta	25:b3a9f34b201d	2273	if(lineFase == 0) {
M_souta	25:b3a9f34b201d	2274	if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	2275	lineFase = 1;
M_souta	25:b3a9f34b201d	2276	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2277	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	2278	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2279	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	2280	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2281	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	2282	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2283	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	2284	} else if(LimitSw::IsPressed(TOW_1L)) {
M_souta	25:b3a9f34b201d	2285	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	2286	motor[TIRE_FL].pwm = 12;
M_souta	25:b3a9f34b201d	2287	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2288	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	2289	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2290	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	2291	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	2292	motor[TIRE_FR].pwm = 12;
M_souta	25:b3a9f34b201d	2293	} else if(LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	2294	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	2295	motor[TIRE_FL].pwm = 12;
M_souta	25:b3a9f34b201d	2296	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2297	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	2298	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2299	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	2300	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	2301	motor[TIRE_FR].pwm = 12;
M_souta	25:b3a9f34b201d	2302	} else {
M_souta	25:b3a9f34b201d	2303	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	2304	motor[TIRE_FL].pwm = 18;
M_souta	25:b3a9f34b201d	2305	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	2306	motor[TIRE_BL].pwm = 18;
M_souta	25:b3a9f34b201d	2307	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	2308	motor[TIRE_BR].pwm = 18;
M_souta	25:b3a9f34b201d	2309	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	2310	motor[TIRE_FR].pwm = 18;
M_souta	25:b3a9f34b201d	2311	}
M_souta	25:b3a9f34b201d	2312	} else if(lineFase == 1) {
M_souta	25:b3a9f34b201d	2313	if(linePara[LINE_TOW_1] == 'A' \|\| linePara[LINE_TOW_1] == 'N') {
M_souta	25:b3a9f34b201d	2314	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2315	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	2316	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2317	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	2318	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2319	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	2320	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2321	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	2322	} else if(linePara[LINE_TOW_1] > 0) {
M_souta	25:b3a9f34b201d	2323	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	2324	motor[TIRE_FL].pwm = 15;
M_souta	25:b3a9f34b201d	2325	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	2326	motor[TIRE_BL].pwm = 15;
M_souta	25:b3a9f34b201d	2327	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	2328	motor[TIRE_BR].pwm = 15;
M_souta	25:b3a9f34b201d	2329	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	2330	motor[TIRE_FR].pwm = 15;
M_souta	25:b3a9f34b201d	2331	} else if(linePara[LINE_TOW_1] < 0) {
M_souta	25:b3a9f34b201d	2332	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	2333	motor[TIRE_FL].pwm = 15;
M_souta	25:b3a9f34b201d	2334	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	2335	motor[TIRE_BL].pwm = 15;
M_souta	25:b3a9f34b201d	2336	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	2337	motor[TIRE_BR].pwm = 15;
M_souta	25:b3a9f34b201d	2338	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	2339	motor[TIRE_FR].pwm = 15;
M_souta	25:b3a9f34b201d	2340	} else if(linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	2341	lineFase = 2;
M_souta	25:b3a9f34b201d	2342	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2343	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	2344	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2345	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	2346	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2347	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	2348	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2349	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	2350	} else {
M_souta	25:b3a9f34b201d	2351	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2352	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	2353	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2354	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	2355	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2356	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	2357	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2358	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	2359	}
M_souta	25:b3a9f34b201d	2360	} else if(lineFase == 2) {
M_souta	25:b3a9f34b201d	2361	//solenoid::solenoid1 = SOLENOID_ON
M_souta	25:b3a9f34b201d	2362
M_souta	22:7d93f79a3686	2363	}
kishibekairohan	13:b6e02d6261d7	2364	}
kishibekairohan	13:b6e02d6261d7	2365	#endif
kishibekairohan	13:b6e02d6261d7	2366
kishibekairohan	13:b6e02d6261d7	2367	#if USE_PROCESS_NUM>7
kishibekairohan	13:b6e02d6261d7	2368	static void Process7()
kishibekairohan	13:b6e02d6261d7	2369	{
M_souta	24:370616a56815	2370
kishibekairohan	12:c09b3e08a316	2371	}
kishibekairohan	12:c09b3e08a316	2372	#endif
kishibekairohan	12:c09b3e08a316	2373
kishibekairohan	16:3f2c2d89372b	2374	#if USE_PROCESS_NUM>8
kishibekairohan	13:b6e02d6261d7	2375	static void Process8()
kishibekairohan	12:c09b3e08a316	2376	{
M_souta	22:7d93f79a3686	2377	if(controller->Button.A) {
M_souta	22:7d93f79a3686	2378	rotaconSampling.start();
M_souta	22:7d93f79a3686	2379	PIDflag = true;
M_souta	22:7d93f79a3686	2380
M_souta	22:7d93f79a3686	2381	//linePara_U = LineHub::GetPara(0);
M_souta	22:7d93f79a3686	2382	//linePara_B = LineHub::GetPara(3);
M_souta	22:7d93f79a3686	2383
M_souta	22:7d93f79a3686	2384
M_souta	22:7d93f79a3686	2385	pulsePV[FL] = encoder[FL].getPulses();
M_souta	22:7d93f79a3686	2386	pulsePV[BL] = encoder[BL].getPulses();
M_souta	22:7d93f79a3686	2387	pulsePV[BR] = encoder[BR].getPulses();
M_souta	22:7d93f79a3686	2388	pulsePV[FR] = encoder[FR].getPulses();
M_souta	22:7d93f79a3686	2389
M_souta	22:7d93f79a3686	2390
M_souta	22:7d93f79a3686	2391	for (int i = 0; i < 4; i++) {
M_souta	22:7d93f79a3686	2392	timeCV[i] = timePV[i];
M_souta	22:7d93f79a3686	2393	timePV[i] = rotaconSampling.read();
M_souta	22:7d93f79a3686	2394	tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
M_souta	22:7d93f79a3686	2395	pulseCV[i] = pulsePV[i];
M_souta	22:7d93f79a3686	2396	}
M_souta	22:7d93f79a3686	2397
M_souta	22:7d93f79a3686	2398	move.Vx = 0.5;
M_souta	22:7d93f79a3686	2399	move.Vy = 0.5;
M_souta	22:7d93f79a3686	2400	move.Va = 0;
M_souta	22:7d93f79a3686	2401
M_souta	22:7d93f79a3686	2402	correction_LT.Vx = 0; //0.1 * linePara_U;
M_souta	22:7d93f79a3686	2403	correction_LT.Vy = 0;
M_souta	22:7d93f79a3686	2404	correction_LT.Va = 0;
M_souta	22:7d93f79a3686	2405
M_souta	22:7d93f79a3686	2406	synthetic.Vx = move.Vx + correction_LT.Vx;
M_souta	22:7d93f79a3686	2407	synthetic.Vy = move.Vy + correction_LT.Vy;
M_souta	22:7d93f79a3686	2408	synthetic.Va = move.Va + correction_LT.Va;
M_souta	22:7d93f79a3686	2409
M_souta	22:7d93f79a3686	2410	sita = 0;
M_souta	22:7d93f79a3686	2411
M_souta	22:7d93f79a3686	2412	//タイヤの目標速度算出
M_souta	22:7d93f79a3686	2413	float sinR = 0.7071 * (float)sin(sita);
M_souta	22:7d93f79a3686	2414	float cosR = 0.7071 * (float)cos(sita);
M_souta	22:7d93f79a3686	2415	float nv = (60 * 1000) / ( 2.00 * PI * tireR);
M_souta	22:7d93f79a3686	2416	tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta	22:7d93f79a3686	2417	tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta	22:7d93f79a3686	2418	tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta	22:7d93f79a3686	2419	tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta	22:7d93f79a3686	2420
M_souta	22:7d93f79a3686	2421	//pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
M_souta	22:7d93f79a3686	2422
M_souta	22:7d93f79a3686	2423	//PIDによるPWM算出
M_souta	22:7d93f79a3686	2424
M_souta	22:7d93f79a3686	2425	//モータの駆動
M_souta	22:7d93f79a3686	2426	for (int i = 0; i < 4; i++) {
M_souta	22:7d93f79a3686	2427	if (tirePWM[i] > 255){
M_souta	22:7d93f79a3686	2428	tirePWM[i] = 255;
M_souta	22:7d93f79a3686	2429	} else if (tirePWM[i] < -255) {
M_souta	22:7d93f79a3686	2430	tirePWM[i] = -255;
M_souta	22:7d93f79a3686	2431	}
M_souta	22:7d93f79a3686	2432	}
M_souta	22:7d93f79a3686	2433
M_souta	22:7d93f79a3686	2434	for(int i = 0;i < 4;i++){
M_souta	22:7d93f79a3686	2435	motor[i].dir = SetStatus(tirePWM[i]);
M_souta	22:7d93f79a3686	2436	motor[i].pwm = SetPWM(tirePWM[i]);
M_souta	22:7d93f79a3686	2437	}
M_souta	22:7d93f79a3686	2438	} else {
M_souta	22:7d93f79a3686	2439	PIDflag = false;
M_souta	22:7d93f79a3686	2440	rotaconSampling.stop();
M_souta	22:7d93f79a3686	2441	rotaconSampling.reset();
M_souta	22:7d93f79a3686	2442	for(int i = 0;i < 4;i++){
M_souta	22:7d93f79a3686	2443	encoder[i].reset();
M_souta	22:7d93f79a3686	2444	pulsePV[i] = 0;
M_souta	22:7d93f79a3686	2445	pulseCV[i] = 0;
M_souta	22:7d93f79a3686	2446	timePV[i] = 0;
M_souta	22:7d93f79a3686	2447	timeCV[i] = 0;
M_souta	22:7d93f79a3686	2448	tirePWM[i] = 0;
M_souta	22:7d93f79a3686	2449	motor[i].dir = SetStatus(tirePWM[i]);
M_souta	22:7d93f79a3686	2450	motor[i].pwm = SetPWM(tirePWM[i]);
M_souta	22:7d93f79a3686	2451	}
M_souta	22:7d93f79a3686	2452	}
kishibekairohan	12:c09b3e08a316	2453	}
kishibekairohan	12:c09b3e08a316	2454	#endif
kishibekairohan	12:c09b3e08a316	2455
kishibekairohan	13:b6e02d6261d7	2456	#if USE_PROCESS_NUM>9
kishibekairohan	13:b6e02d6261d7	2457	static void Process9()
kishibekairohan	12:c09b3e08a316	2458	{
kishibekairohan	2:c015739085d3	2459
t_yamamoto	0:669ef71cba68	2460	}
t_yamamoto	0:669ef71cba68	2461	#endif
t_yamamoto	0:669ef71cba68	2462	#endif
t_yamamoto	0:669ef71cba68	2463	#pragma endregion PROCESS
t_yamamoto	0:669ef71cba68	2464
t_yamamoto	0:669ef71cba68	2465	static void AllActuatorReset()
t_yamamoto	0:669ef71cba68	2466	{
t_yamamoto	0:669ef71cba68	2467
t_yamamoto	0:669ef71cba68	2468	#ifdef USE_SOLENOID
t_yamamoto	0:669ef71cba68	2469	solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto	0:669ef71cba68	2470	#endif
t_yamamoto	0:669ef71cba68	2471
t_yamamoto	0:669ef71cba68	2472	#ifdef USE_MOTOR
t_yamamoto	0:669ef71cba68	2473	for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto	0:669ef71cba68	2474	{
t_yamamoto	0:669ef71cba68	2475	motor[i].dir = FREE;
t_yamamoto	0:669ef71cba68	2476	motor[i].pwm = 0;
t_yamamoto	0:669ef71cba68	2477	}
t_yamamoto	0:669ef71cba68	2478	#endif
t_yamamoto	0:669ef71cba68	2479	}
t_yamamoto	0:669ef71cba68	2480
kishibekairohan	16:3f2c2d89372b	2481	void BuzzerTimer_func(){
kishibekairohan	16:3f2c2d89372b	2482	buzzer = !buzzer;
kishibekairohan	17:50dc4b449e69	2483	//LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan	11:028a150943b5	2484	}
kishibekairohan	11:028a150943b5	2485
kishibekairohan	11:028a150943b5	2486	void TapeLedEms_func() {
kishibekairohan	16:3f2c2d89372b	2487	sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan	9:f93fc79a49ea	2488	}
kishibekairohan	16:3f2c2d89372b	2489
kishibekairohan	16:3f2c2d89372b	2490	#pragma region USER-DEFINED-FUNCTIONS
M_souta	22:7d93f79a3686	2491	void tirePID() {
M_souta	22:7d93f79a3686	2492	if(PIDflag == true) {
M_souta	22:7d93f79a3686	2493	//加算するPID値の算出
M_souta	22:7d93f79a3686	2494	rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
M_souta	22:7d93f79a3686	2495	rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
M_souta	22:7d93f79a3686	2496	rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
M_souta	22:7d93f79a3686	2497	rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
M_souta	22:7d93f79a3686	2498	//PID値の加算
M_souta	22:7d93f79a3686	2499	tirePWM[FL] += rotaconPID[0].GetMV();
M_souta	22:7d93f79a3686	2500	tirePWM[BL] += rotaconPID[1].GetMV();
M_souta	22:7d93f79a3686	2501	tirePWM[FR] += rotaconPID[2].GetMV();
M_souta	22:7d93f79a3686	2502	tirePWM[BR] += rotaconPID[3].GetMV();
M_souta	22:7d93f79a3686	2503	}
M_souta	22:7d93f79a3686	2504	}
kishibekairohan	16:3f2c2d89372b	2505
M_souta	22:7d93f79a3686	2506	int lineCast(char k) {
M_souta	22:7d93f79a3686	2507	int l;
M_souta	22:7d93f79a3686	2508	switch(k) {
M_souta	22:7d93f79a3686	2509	case 255:
M_souta	22:7d93f79a3686	2510	l = -1;
M_souta	22:7d93f79a3686	2511	break;
M_souta	22:7d93f79a3686	2512	case 254:
M_souta	22:7d93f79a3686	2513	l = -2;
M_souta	22:7d93f79a3686	2514	break;
M_souta	22:7d93f79a3686	2515	case 253:
M_souta	22:7d93f79a3686	2516	l = -3;
M_souta	22:7d93f79a3686	2517	break;
M_souta	22:7d93f79a3686	2518	default:
M_souta	22:7d93f79a3686	2519	l = k;
M_souta	22:7d93f79a3686	2520	}
M_souta	22:7d93f79a3686	2521	return l;
M_souta	22:7d93f79a3686	2522	}
kishibekairohan	16:3f2c2d89372b	2523
t_yamamoto	0:669ef71cba68	2524	#pragma endregion

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	23 Nov 2019
Imports:	0
Forks:	0
Commits:	60
Dependents:	0
Dependencies:	1
Followers:	1

System/Process/Process.cpp@25:b3a9f34b201d, 2019-09-27 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning