MainBoard2019_Auto_B_3 - ttt

Users » M_souta » Code » MainBoard2019_Auto_B_3

三浦颯太 / Mbed 2 deprecated MainBoard2019_Auto_B_3

ttt

Dependencies: mbed TrapezoidControl QEI Pulse LM61CIZ

System/Process/Process.cpp@26:4c0ce2f05688, 2019-09-28 (annotated)

Committer:: M_souta
Date:: Sat Sep 28 23:02:17 2019 +0000
Revision:: 26:4c0ce2f05688
Parent:: 25:b3a9f34b201d
Child:: 27:dd6c9feb7493

lll; ;

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	26:4c0ce2f05688	140	Timer tow_stop;
M_souta	26:4c0ce2f05688	141
M_souta	22:7d93f79a3686	142	float pw = 0;
M_souta	22:7d93f79a3686	143	int lineFase = 0;
M_souta	22:7d93f79a3686	144	bool lineCheck = false;
M_souta	25:b3a9f34b201d	145	bool lineFlag = false;
M_souta	25:b3a9f34b201d	146	bool adjAnable = false;
M_souta	25:b3a9f34b201d	147	int adj = 0;
M_souta	25:b3a9f34b201d	148
M_souta	26:4c0ce2f05688	149	int countW = 0;
M_souta	25:b3a9f34b201d	150	int lineCount = 0;
M_souta	25:b3a9f34b201d	151	int targetCount = 0;
M_souta	25:b3a9f34b201d	152
M_souta	25:b3a9f34b201d	153	bool startFlag = true;
M_souta	25:b3a9f34b201d	154
M_souta	22:7d93f79a3686	155	int linePWM;
M_souta	22:7d93f79a3686	156	int adj_F;
M_souta	22:7d93f79a3686	157	int adj_B;
M_souta	22:7d93f79a3686	158
M_souta	22:7d93f79a3686	159	int mode = 0;
M_souta	22:7d93f79a3686	160
M_souta	22:7d93f79a3686	161	// *********** Line ************ //
M_souta	22:7d93f79a3686	162
M_souta	21:e3b58d675c1c	163	const int omni[15][15] =
M_souta	20:eae8c84f318c	164	{
M_souta	21:e3b58d675c1c	165	{ 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
M_souta	21:e3b58d675c1c	166	{ -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
M_souta	21:e3b58d675c1c	167	{ -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
M_souta	21:e3b58d675c1c	168	{ -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
M_souta	21:e3b58d675c1c	169	{ -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
M_souta	21:e3b58d675c1c	170	{ -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
M_souta	21:e3b58d675c1c	171	{ -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
M_souta	21:e3b58d675c1c	172	{ -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
M_souta	21:e3b58d675c1c	173	{ -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
M_souta	21:e3b58d675c1c	174	{ -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
M_souta	21:e3b58d675c1c	175	{ -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
M_souta	21:e3b58d675c1c	176	{ -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
M_souta	21:e3b58d675c1c	177	{ -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
M_souta	21:e3b58d675c1c	178	{ -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
M_souta	21:e3b58d675c1c	179	{ -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta	21:e3b58d675c1c	180	};
M_souta	21:e3b58d675c1c	181
M_souta	21:e3b58d675c1c	182	const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta	21:e3b58d675c1c	183
M_souta	21:e3b58d675c1c	184	uint8_t SetStatus(int);
M_souta	21:e3b58d675c1c	185	uint8_t SetStatus(int pwmVal) {
M_souta	21:e3b58d675c1c	186	if (pwmVal < 0) return BACK;
M_souta	21:e3b58d675c1c	187	else if (pwmVal > 0) return FOR;
M_souta	21:e3b58d675c1c	188	else if (pwmVal == 0) return BRAKE;
M_souta	21:e3b58d675c1c	189	else return BRAKE;
M_souta	20:eae8c84f318c	190	}
M_souta	21:e3b58d675c1c	191	uint8_t SetPWM(int);
M_souta	21:e3b58d675c1c	192	uint8_t SetPWM(int pwmVal) {
M_souta	21:e3b58d675c1c	193	if (pwmVal == 0 \|\| pwmVal > 255 \|\| pwmVal < -255) return 255;
M_souta	21:e3b58d675c1c	194	else return abs(pwmVal);
M_souta	21:e3b58d675c1c	195	}
M_souta	18:c694bae76e51	196
t_yamamoto	0:669ef71cba68	197	#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto	0:669ef71cba68	198
t_yamamoto	0:669ef71cba68	199	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	200	#if USE_PROCESS_NUM>0
t_yamamoto	0:669ef71cba68	201	static void Process0(void);
t_yamamoto	0:669ef71cba68	202	#endif
t_yamamoto	0:669ef71cba68	203	#if USE_PROCESS_NUM>1
t_yamamoto	0:669ef71cba68	204	static void Process1(void);
t_yamamoto	0:669ef71cba68	205	#endif
t_yamamoto	0:669ef71cba68	206	#if USE_PROCESS_NUM>2
t_yamamoto	0:669ef71cba68	207	static void Process2(void);
t_yamamoto	0:669ef71cba68	208	#endif
t_yamamoto	0:669ef71cba68	209	#if USE_PROCESS_NUM>3
t_yamamoto	0:669ef71cba68	210	static void Process3(void);
t_yamamoto	0:669ef71cba68	211	#endif
t_yamamoto	0:669ef71cba68	212	#if USE_PROCESS_NUM>4
t_yamamoto	0:669ef71cba68	213	static void Process4(void);
t_yamamoto	0:669ef71cba68	214	#endif
t_yamamoto	0:669ef71cba68	215	#if USE_PROCESS_NUM>5
t_yamamoto	0:669ef71cba68	216	static void Process5(void);
t_yamamoto	0:669ef71cba68	217	#endif
t_yamamoto	0:669ef71cba68	218	#if USE_PROCESS_NUM>6
t_yamamoto	0:669ef71cba68	219	static void Process6(void);
t_yamamoto	0:669ef71cba68	220	#endif
t_yamamoto	0:669ef71cba68	221	#if USE_PROCESS_NUM>7
t_yamamoto	0:669ef71cba68	222	static void Process7(void);
t_yamamoto	0:669ef71cba68	223	#endif
t_yamamoto	0:669ef71cba68	224	#if USE_PROCESS_NUM>8
t_yamamoto	0:669ef71cba68	225	static void Process8(void);
t_yamamoto	0:669ef71cba68	226	#endif
t_yamamoto	0:669ef71cba68	227	#if USE_PROCESS_NUM>9
t_yamamoto	0:669ef71cba68	228	static void Process9(void);
t_yamamoto	0:669ef71cba68	229	#endif
t_yamamoto	0:669ef71cba68	230	#endif
t_yamamoto	0:669ef71cba68	231
t_yamamoto	0:669ef71cba68	232	void SystemProcessInitialize()
t_yamamoto	0:669ef71cba68	233	{
t_yamamoto	0:669ef71cba68	234	#pragma region USER-DEFINED_VARIABLE_INIT
kishibekairohan	16:3f2c2d89372b	235	/Replace here with the initialization code of your variables./
M_souta	25:b3a9f34b201d	236	//rotaconPIDtimer.attach(tirePID,0.1);
M_souta	25:b3a9f34b201d	237
M_souta	25:b3a9f34b201d	238	//DigitalOut Air_16(LS_16);
M_souta	25:b3a9f34b201d	239	//DigitalOut Air_17(LS_17);
M_souta	25:b3a9f34b201d	240	//DigitalOut Air_18(LS_18);
M_souta	25:b3a9f34b201d	241	//DigitalOut Air_19(LS_19);
M_souta	21:e3b58d675c1c	242
t_yamamoto	0:669ef71cba68	243	#pragma endregion USER-DEFINED_VARIABLE_INIT
t_yamamoto	0:669ef71cba68	244
t_yamamoto	0:669ef71cba68	245	lock = true;
t_yamamoto	0:669ef71cba68	246	processChangeComp = true;
t_yamamoto	0:669ef71cba68	247	current = DEFAULT_PROCESS;
t_yamamoto	0:669ef71cba68	248
t_yamamoto	0:669ef71cba68	249	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	250	#if USE_PROCESS_NUM>0
t_yamamoto	0:669ef71cba68	251	Process[0] = Process0;
t_yamamoto	0:669ef71cba68	252	#endif
t_yamamoto	0:669ef71cba68	253	#if USE_PROCESS_NUM>1
t_yamamoto	0:669ef71cba68	254	Process[1] = Process1;
t_yamamoto	0:669ef71cba68	255	#endif
t_yamamoto	0:669ef71cba68	256	#if USE_PROCESS_NUM>2
t_yamamoto	0:669ef71cba68	257	Process[2] = Process2;
t_yamamoto	0:669ef71cba68	258	#endif
t_yamamoto	0:669ef71cba68	259	#if USE_PROCESS_NUM>3
t_yamamoto	0:669ef71cba68	260	Process[3] = Process3;
t_yamamoto	0:669ef71cba68	261	#endif
t_yamamoto	0:669ef71cba68	262	#if USE_PROCESS_NUM>4
t_yamamoto	0:669ef71cba68	263	Process[4] = Process4;
t_yamamoto	0:669ef71cba68	264	#endif
t_yamamoto	0:669ef71cba68	265	#if USE_PROCESS_NUM>5
t_yamamoto	0:669ef71cba68	266	Process[5] = Process5;
t_yamamoto	0:669ef71cba68	267	#endif
t_yamamoto	0:669ef71cba68	268	#if USE_PROCESS_NUM>6
t_yamamoto	0:669ef71cba68	269	Process[6] = Process6;
t_yamamoto	0:669ef71cba68	270	#endif
t_yamamoto	0:669ef71cba68	271	#if USE_PROCESS_NUM>7
t_yamamoto	0:669ef71cba68	272	Process[7] = Process7;
t_yamamoto	0:669ef71cba68	273	#endif
t_yamamoto	0:669ef71cba68	274	#if USE_PROCESS_NUM>8
t_yamamoto	0:669ef71cba68	275	Process[8] = Process8;
t_yamamoto	0:669ef71cba68	276	#endif
t_yamamoto	0:669ef71cba68	277	#if USE_PROCESS_NUM>9
t_yamamoto	0:669ef71cba68	278	Process[9] = Process9;
t_yamamoto	0:669ef71cba68	279	#endif
t_yamamoto	0:669ef71cba68	280	#endif
t_yamamoto	0:669ef71cba68	281	}
t_yamamoto	0:669ef71cba68	282
t_yamamoto	0:669ef71cba68	283	static void SystemProcessUpdate()
t_yamamoto	0:669ef71cba68	284	{
t_yamamoto	0:669ef71cba68	285	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	286	if(controller->Button.HOME) lock = false;
t_yamamoto	0:669ef71cba68	287
t_yamamoto	0:669ef71cba68	288	if(controller->Button.START && processChangeComp)
t_yamamoto	0:669ef71cba68	289	{
t_yamamoto	0:669ef71cba68	290	current++;
t_yamamoto	0:669ef71cba68	291	if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
t_yamamoto	0:669ef71cba68	292	processChangeComp = false;
t_yamamoto	0:669ef71cba68	293	}
t_yamamoto	0:669ef71cba68	294	else if(controller->Button.SELECT && processChangeComp)
t_yamamoto	0:669ef71cba68	295	{
t_yamamoto	0:669ef71cba68	296	current--;
t_yamamoto	0:669ef71cba68	297	if (current < 0) current = 0;
t_yamamoto	0:669ef71cba68	298	processChangeComp = false;
t_yamamoto	0:669ef71cba68	299	}
t_yamamoto	0:669ef71cba68	300	else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
t_yamamoto	0:669ef71cba68	301	#endif
t_yamamoto	0:669ef71cba68	302
t_yamamoto	0:669ef71cba68	303	#ifdef USE_MOTOR
t_yamamoto	0:669ef71cba68	304	ACTUATORHUB::MOTOR::Motor::Update(motor);
t_yamamoto	0:669ef71cba68	305	#endif
t_yamamoto	0:669ef71cba68	306
t_yamamoto	0:669ef71cba68	307	#ifdef USE_SOLENOID
t_yamamoto	0:669ef71cba68	308	ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
t_yamamoto	0:669ef71cba68	309	#endif
t_yamamoto	0:669ef71cba68	310
t_yamamoto	0:669ef71cba68	311	#ifdef USE_RS485
t_yamamoto	0:669ef71cba68	312	ACTUATORHUB::ActuatorHub::Update();
M_souta	21:e3b58d675c1c	313	//LINEHUB::LineHub::Update();
t_yamamoto	0:669ef71cba68	314	#endif
t_yamamoto	0:669ef71cba68	315
t_yamamoto	0:669ef71cba68	316	}
t_yamamoto	0:669ef71cba68	317
kishibekairohan	2:c015739085d3	318
kishibekairohan	2:c015739085d3	319
t_yamamoto	0:669ef71cba68	320	void SystemProcess()
t_yamamoto	0:669ef71cba68	321	{
t_yamamoto	0:669ef71cba68	322	SystemProcessInitialize();
t_yamamoto	0:669ef71cba68	323
t_yamamoto	0:669ef71cba68	324	while(1)
kishibekairohan	15:dfcec98f5aa9	325	{
M_souta	21:e3b58d675c1c	326	int g[8];
M_souta	21:e3b58d675c1c	327	for(int i = 0; i < 8; i++){
M_souta	22:7d93f79a3686	328	g[i] = lineCast(LineHub::GetPara(i));
M_souta	21:e3b58d675c1c	329	}
M_souta	22:7d93f79a3686	330
M_souta	22:7d93f79a3686	331	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	332
M_souta	25:b3a9f34b201d	333
M_souta	25:b3a9f34b201d	334	if(LimitSw::IsPressed(START_SW) && startFlag == true) {
M_souta	25:b3a9f34b201d	335	startFlag == false;
M_souta	25:b3a9f34b201d	336	lock = false;
M_souta	25:b3a9f34b201d	337	lineFase = 0;
M_souta	25:b3a9f34b201d	338	lineCount = 0;
M_souta	25:b3a9f34b201d	339	lineCheck = false;
M_souta	26:4c0ce2f05688	340	countW = 0;
M_souta	26:4c0ce2f05688	341	if(LimitSw::IsPressed(REDBLUE_SW)) {
M_souta	25:b3a9f34b201d	342	current = 4;
M_souta	25:b3a9f34b201d	343	} else {
M_souta	25:b3a9f34b201d	344	current = 5;
M_souta	25:b3a9f34b201d	345	}
M_souta	25:b3a9f34b201d	346	}
M_souta	20:eae8c84f318c	347
kishibekairohan	17:50dc4b449e69	348	buzzer.period(1.0/800);
kishibekairohan	15:dfcec98f5aa9	349
t_yamamoto	0:669ef71cba68	350	#ifdef USE_MU
t_yamamoto	0:669ef71cba68	351	controller = CONTROLLER::Controller::GetData();
t_yamamoto	0:669ef71cba68	352	#endif
t_yamamoto	0:669ef71cba68	353
t_yamamoto	0:669ef71cba68	354	#ifdef USE_ERRORCHECK
M_souta	25:b3a9f34b201d	355	if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag)
t_yamamoto	0:669ef71cba68	356	{
t_yamamoto	0:669ef71cba68	357	CONTROLLER::Controller::DataReset();
t_yamamoto	0:669ef71cba68	358	AllActuatorReset();
t_yamamoto	0:669ef71cba68	359	lock = true;
t_yamamoto	0:669ef71cba68	360	}
t_yamamoto	0:669ef71cba68	361	else
t_yamamoto	0:669ef71cba68	362	#endif
t_yamamoto	0:669ef71cba68	363	{
t_yamamoto	0:669ef71cba68	364
t_yamamoto	0:669ef71cba68	365	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	366	if(!lock)
t_yamamoto	0:669ef71cba68	367	{
t_yamamoto	0:669ef71cba68	368	Process[current]();
t_yamamoto	0:669ef71cba68	369	}
t_yamamoto	0:669ef71cba68	370	else
t_yamamoto	0:669ef71cba68	371	#endif
t_yamamoto	0:669ef71cba68	372	{
t_yamamoto	0:669ef71cba68	373	//ロック時の処理
t_yamamoto	0:669ef71cba68	374	}
t_yamamoto	0:669ef71cba68	375	}
t_yamamoto	0:669ef71cba68	376
M_souta	22:7d93f79a3686	377
kishibekairohan	16:3f2c2d89372b	378	//Emergency!
M_souta	22:7d93f79a3686	379	/*
kishibekairohan	17:50dc4b449e69	380	if(!EMG_0 && !EMG_1 && !EMGflag){
kishibekairohan	17:50dc4b449e69	381	buzzer = 0;
kishibekairohan	17:50dc4b449e69	382	BuzzerTimer.attach(BuzzerTimer_func, 1);
kishibekairohan	17:50dc4b449e69	383	EMGflag = true;
kishibekairohan	17:50dc4b449e69	384	LED_DEBUG0 = 1;
kishibekairohan	9:f93fc79a49ea	385	}
kishibekairohan	17:50dc4b449e69	386	if(EMG_0 && EMG_1 && EMGflag){
kishibekairohan	17:50dc4b449e69	387	buzzer = 1;
kishibekairohan	9:f93fc79a49ea	388	BuzzerTimer.detach();
kishibekairohan	17:50dc4b449e69	389	EMGflag = false;
kishibekairohan	9:f93fc79a49ea	390	}
M_souta	22:7d93f79a3686	391	*/
t_yamamoto	0:669ef71cba68	392	SystemProcessUpdate();
t_yamamoto	0:669ef71cba68	393	}
t_yamamoto	0:669ef71cba68	394	}
t_yamamoto	0:669ef71cba68	395
kishibekairohan	2:c015739085d3	396
kishibekairohan	2:c015739085d3	397
kishibekairohan	2:c015739085d3	398
t_yamamoto	0:669ef71cba68	399	#pragma region PROCESS
t_yamamoto	0:669ef71cba68	400	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	401	#if USE_PROCESS_NUM>0
kishibekairohan	12:c09b3e08a316	402	static void Process0()
kishibekairohan	10:1295d39fec3a	403	{
M_souta	21:e3b58d675c1c	404	AllActuatorReset();
t_yamamoto	0:669ef71cba68	405	}
t_yamamoto	0:669ef71cba68	406	#endif
t_yamamoto	0:669ef71cba68	407
t_yamamoto	0:669ef71cba68	408	#if USE_PROCESS_NUM>1
kishibekairohan	16:3f2c2d89372b	409	static void Process1()
t_yamamoto	0:669ef71cba68	410	{
M_souta	22:7d93f79a3686	411
M_souta	22:7d93f79a3686	412	PIDflag = false;
M_souta	22:7d93f79a3686	413
M_souta	21:e3b58d675c1c	414	if(controller->Button.UP) {
M_souta	21:e3b58d675c1c	415	motor[LIFT_LB].dir = FOR;
M_souta	26:4c0ce2f05688	416	motor[LIFT_LB].pwm = 180;
M_souta	21:e3b58d675c1c	417	motor[LIFT_RB].dir = BACK;
M_souta	26:4c0ce2f05688	418	motor[LIFT_RB].pwm = 200;
M_souta	21:e3b58d675c1c	419	} else if(controller->Button.DOWN) {
M_souta	21:e3b58d675c1c	420	motor[LIFT_LB].dir = BACK;
M_souta	21:e3b58d675c1c	421	motor[LIFT_LB].pwm = 180;
M_souta	21:e3b58d675c1c	422	motor[LIFT_RB].dir = FOR;
M_souta	26:4c0ce2f05688	423	motor[LIFT_RB].pwm = 200;
M_souta	23:c853372cf626	424	} else if(controller->Button.LEFT) {
M_souta	23:c853372cf626	425	motor[LIFT_LB].dir = FOR;
M_souta	23:c853372cf626	426	motor[LIFT_LB].pwm = 180;
M_souta	23:c853372cf626	427	} else if(controller->Button.RIGHT) {
M_souta	23:c853372cf626	428	motor[LIFT_RB].dir = BACK;
M_souta	21:e3b58d675c1c	429	motor[LIFT_RB].pwm = 180;
M_souta	21:e3b58d675c1c	430	} else {
M_souta	25:b3a9f34b201d	431	motor[LIFT_LB].dir = BRAKE;
M_souta	23:c853372cf626	432	motor[LIFT_LB].pwm = 255;
M_souta	26:4c0ce2f05688	433	motor[LIFT_RB].dir = BRAKE;
M_souta	26:4c0ce2f05688	434	motor[LIFT_RB].pwm = 255;
M_souta	21:e3b58d675c1c	435	}
M_souta	22:7d93f79a3686	436
7ka884	4:ba9df71868df	437
M_souta	21:e3b58d675c1c	438	if(controller->Button.X) {
M_souta	21:e3b58d675c1c	439	motor[LIFT_U].dir = FOR;
M_souta	21:e3b58d675c1c	440	motor[LIFT_U].pwm = 180;
M_souta	21:e3b58d675c1c	441	} else if(controller->Button.Y) {
M_souta	21:e3b58d675c1c	442	motor[LIFT_U].dir = BACK;
M_souta	25:b3a9f34b201d	443	motor[LIFT_U].pwm = 230;
M_souta	21:e3b58d675c1c	444	} else {
M_souta	21:e3b58d675c1c	445	motor[LIFT_U].dir = BRAKE;
M_souta	25:b3a9f34b201d	446	motor[LIFT_U].pwm = 255;
M_souta	21:e3b58d675c1c	447	}
M_souta	21:e3b58d675c1c	448
M_souta	21:e3b58d675c1c	449	if(!(controller->AnalogL.Y == 7) \|\| !(controller->AnalogL.X == 7)) {
M_souta	25:b3a9f34b201d	450	motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
M_souta	21:e3b58d675c1c	451	motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
M_souta	21:e3b58d675c1c	452	motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta	25:b3a9f34b201d	453	motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
M_souta	21:e3b58d675c1c	454
M_souta	25:b3a9f34b201d	455	motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
M_souta	22:7d93f79a3686	456	motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
M_souta	22:7d93f79a3686	457	motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta	25:b3a9f34b201d	458	motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
M_souta	21:e3b58d675c1c	459	} else {
M_souta	25:b3a9f34b201d	460	motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	461	motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	462	motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta	25:b3a9f34b201d	463	motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	464
M_souta	21:e3b58d675c1c	465	motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	466	motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	467	motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	468	motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
M_souta	21:e3b58d675c1c	469	}
M_souta	25:b3a9f34b201d	470
M_souta	25:b3a9f34b201d	471	if(controller->Button.ZR) {
M_souta	25:b3a9f34b201d	472	solenoid.solenoid4 = SOLENOID_ON;
M_souta	26:4c0ce2f05688	473	}
M_souta	26:4c0ce2f05688	474	if(controller->Button.ZL) {
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	26:4c0ce2f05688	489	if(LimitSw::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	26:4c0ce2f05688	495	switchFlag_LB = true;
M_souta	22:7d93f79a3686	496	}
M_souta	22:7d93f79a3686	497	}
M_souta	26:4c0ce2f05688	498	if(LimitSw::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	26:4c0ce2f05688	504	switchFlag_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	26:4c0ce2f05688	510	if(!liftSstate == UPPER)) {
M_souta	26:4c0ce2f05688	511	liftState++;
M_souta	26:4c0ce2f05688	512	motor[LIFT_LB].dir = FOR;
M_souta	26:4c0ce2f05688	513	motor[LIFT_RB].dir = BACK;
M_souta	26:4c0ce2f05688	514	motor[LIFT_LB].pwm = 185;
M_souta	26:4c0ce2f05688	515	motor[LIFT_RB].pwm = 180;
M_souta	22:7d93f79a3686	516	}
M_souta	22:7d93f79a3686	517	} else if(controller->Button.DOWN) {
M_souta	26:4c0ce2f05688	518	if(!(liftstate == LOWER)) {
M_souta	26:4c0ce2f05688	519	liftState--;
M_souta	22:7d93f79a3686	520	moving = true;
M_souta	26:4c0ce2f05688	521	motor[LIFT_LB].dir = BACK;
M_souta	26:4c0ce2f05688	522	motor[LIFT_RB].dir = FOR;
M_souta	26:4c0ce2f05688	523	motor[LIFT_LB].pwm = 180;
M_souta	26:4c0ce2f05688	524	motor[LIFT_RB].pwm = 185;
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	*/
M_souta	26:4c0ce2f05688	534
kishibekairohan	13:b6e02d6261d7	535	}
kishibekairohan	13:b6e02d6261d7	536	#endif
kishibekairohan	13:b6e02d6261d7	537
kishibekairohan	13:b6e02d6261d7	538	#if USE_PROCESS_NUM>3
kishibekairohan	16:3f2c2d89372b	539	static void Process3()
kishibekairohan	13:b6e02d6261d7	540	{
M_souta	25:b3a9f34b201d	541	startFlag = true;
M_souta	22:7d93f79a3686	542	AllActuatorReset();
M_souta	23:c853372cf626	543	lineFase = 0;
M_souta	25:b3a9f34b201d	544	lineCheck = false;
M_souta	25:b3a9f34b201d	545	lineCount = 0;
M_souta	26:4c0ce2f05688	546	countW = 0;
kishibekairohan	13:b6e02d6261d7	547	}
kishibekairohan	13:b6e02d6261d7	548	#endif
kishibekairohan	13:b6e02d6261d7	549
kishibekairohan	13:b6e02d6261d7	550	#if USE_PROCESS_NUM>4
kishibekairohan	13:b6e02d6261d7	551	static void Process4()
M_souta	22:7d93f79a3686	552	{
M_souta	25:b3a9f34b201d	553	LED_DEBUG0 = LED_ON;
M_souta	22:7d93f79a3686	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
M_souta	25:b3a9f34b201d	562	for(int i = 0; i < 8; i++) {
M_souta	25:b3a9f34b201d	563	linePara[i] = lineCast(LineHub::GetPara(i));
M_souta	25:b3a9f34b201d	564	}
M_souta	22:7d93f79a3686	565
M_souta	25:b3a9f34b201d	566	if(lineFase == 0) {
M_souta	25:b3a9f34b201d	567	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	568	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	569	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	570	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	571	if(linePara[0] != 'N' && linePara[0] != 'A') {
M_souta	25:b3a9f34b201d	572	lineFase = 1;
M_souta	25:b3a9f34b201d	573	}
M_souta	25:b3a9f34b201d	574	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	575	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	576	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	577	motor[TIRE_FR].pwm = 30;
M_souta	22:7d93f79a3686	578
M_souta	25:b3a9f34b201d	579	} else if(lineFase == 1) { // 前ライントレース
M_souta	25:b3a9f34b201d	580	switch(linePara[0]) {
M_souta	22:7d93f79a3686	581	case -2:
M_souta	25:b3a9f34b201d	582	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	583	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	584	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	585	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	586	adjAnable = true;
M_souta	25:b3a9f34b201d	587	break;
M_souta	25:b3a9f34b201d	588	case -3:
M_souta	25:b3a9f34b201d	589	tirePWM[TIRE_FL] = 10;
M_souta	25:b3a9f34b201d	590	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	591	tirePWM[TIRE_BR] = -10;
M_souta	25:b3a9f34b201d	592	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	593	adjAnable = true;
M_souta	25:b3a9f34b201d	594	break;
M_souta	25:b3a9f34b201d	595	case -1:
M_souta	25:b3a9f34b201d	596	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	597	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	598	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	599	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	600	adjAnable = true;
M_souta	25:b3a9f34b201d	601	break;
M_souta	25:b3a9f34b201d	602	case 0:
M_souta	25:b3a9f34b201d	603	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	604	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	605	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	606	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	607	adjAnable = true;
M_souta	25:b3a9f34b201d	608	break;
M_souta	25:b3a9f34b201d	609	case 1:
M_souta	25:b3a9f34b201d	610	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	611	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	612	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	613	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	614	adjAnable = true;
M_souta	25:b3a9f34b201d	615	break;
M_souta	25:b3a9f34b201d	616	case 3:
M_souta	25:b3a9f34b201d	617	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	618	tirePWM[TIRE_BL] = 10;
M_souta	25:b3a9f34b201d	619	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	620	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	621	adjAnable = true;
M_souta	25:b3a9f34b201d	622	break;
M_souta	25:b3a9f34b201d	623	case 2:
M_souta	25:b3a9f34b201d	624	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	625	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	626	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	627	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	628	adjAnable = true;
M_souta	25:b3a9f34b201d	629	break;
M_souta	25:b3a9f34b201d	630	case 'A':
M_souta	25:b3a9f34b201d	631	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	632	lineCheck = true;
M_souta	25:b3a9f34b201d	633	lineCount = 0;
M_souta	26:4c0ce2f05688	634	countW++;
M_souta	25:b3a9f34b201d	635	}
M_souta	25:b3a9f34b201d	636	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	637	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	638	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	639	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	640	adjAnable = true;
M_souta	25:b3a9f34b201d	641	break;
M_souta	25:b3a9f34b201d	642	case 'N':
M_souta	25:b3a9f34b201d	643	tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
M_souta	25:b3a9f34b201d	644	tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
M_souta	25:b3a9f34b201d	645	tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
M_souta	25:b3a9f34b201d	646	tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
M_souta	25:b3a9f34b201d	647	adjAnable = false;
M_souta	25:b3a9f34b201d	648	break;
M_souta	25:b3a9f34b201d	649	default:
M_souta	25:b3a9f34b201d	650	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	651	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	652	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	653	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	654	adjAnable = false;
M_souta	25:b3a9f34b201d	655	}
M_souta	25:b3a9f34b201d	656
M_souta	25:b3a9f34b201d	657	if(adjAnable){
M_souta	25:b3a9f34b201d	658	if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
M_souta	25:b3a9f34b201d	659	} else {
M_souta	25:b3a9f34b201d	660	adj = 0;
M_souta	25:b3a9f34b201d	661	}
M_souta	25:b3a9f34b201d	662
M_souta	25:b3a9f34b201d	663	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	664	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	665	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	666	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	667	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	668	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	669	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	670	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	671
M_souta	25:b3a9f34b201d	672	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	673	lineCount++;
M_souta	25:b3a9f34b201d	674	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	675	}
M_souta	26:4c0ce2f05688	676	if(countW == 3) {
M_souta	26:4c0ce2f05688	677	countW = 0;
M_souta	25:b3a9f34b201d	678	lineFase = 2;
M_souta	25:b3a9f34b201d	679	lineCount = 0;
M_souta	25:b3a9f34b201d	680	lineCheck = false;
M_souta	25:b3a9f34b201d	681	}
M_souta	25:b3a9f34b201d	682	} else if(lineFase == 2) { // 前低速
M_souta	25:b3a9f34b201d	683	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	684	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	685	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	686	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	687	if(linePara[4] == 0) {
M_souta	25:b3a9f34b201d	688	lineFase = 3;
M_souta	25:b3a9f34b201d	689	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	690	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	691	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	692	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	693	}
M_souta	25:b3a9f34b201d	694	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	695	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	696	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	697	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	698	} else if(lineFase == 3) { // 右ライントレース
M_souta	25:b3a9f34b201d	699	switch(linePara[4]) {
M_souta	25:b3a9f34b201d	700	case -2:
M_souta	25:b3a9f34b201d	701	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	702	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	703	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	704	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	705	adjAnable = true;
M_souta	22:7d93f79a3686	706	break;
M_souta	22:7d93f79a3686	707	case -3:
M_souta	25:b3a9f34b201d	708	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	709	tirePWM[TIRE_BL] = -10;
M_souta	25:b3a9f34b201d	710	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	711	tirePWM[TIRE_FR] = 10;
M_souta	25:b3a9f34b201d	712	adjAnable = true;
M_souta	22:7d93f79a3686	713	break;
M_souta	22:7d93f79a3686	714	case -1:
M_souta	25:b3a9f34b201d	715	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	716	tirePWM[TIRE_BL] = -20;
M_souta	25:b3a9f34b201d	717	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	718	tirePWM[TIRE_FR] = 20;
M_souta	25:b3a9f34b201d	719	adjAnable = true;
M_souta	22:7d93f79a3686	720	break;
M_souta	22:7d93f79a3686	721	case 0:
M_souta	25:b3a9f34b201d	722	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	723	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	724	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	725	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	726	adjAnable = true;
M_souta	22:7d93f79a3686	727	break;
M_souta	22:7d93f79a3686	728	case 1:
M_souta	25:b3a9f34b201d	729	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	730	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	731	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	732	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	733	adjAnable = true;
M_souta	22:7d93f79a3686	734	break;
M_souta	22:7d93f79a3686	735	case 3:
M_souta	25:b3a9f34b201d	736	tirePWM[TIRE_FL] = 10;
M_souta	25:b3a9f34b201d	737	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	738	tirePWM[TIRE_BR] = -10;
M_souta	25:b3a9f34b201d	739	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	740	adjAnable = true;
M_souta	22:7d93f79a3686	741	break;
M_souta	22:7d93f79a3686	742	case 2:
M_souta	25:b3a9f34b201d	743	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	744	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	745	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	746	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	747	adjAnable = true;
M_souta	22:7d93f79a3686	748	break;
M_souta	22:7d93f79a3686	749	case 'A':
M_souta	25:b3a9f34b201d	750	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	751	lineCheck = true;
M_souta	25:b3a9f34b201d	752	lineCount = 0;
M_souta	26:4c0ce2f05688	753	countW++;
M_souta	25:b3a9f34b201d	754	}
M_souta	25:b3a9f34b201d	755	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	756	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	757	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	758	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	759	adjAnable = true;
M_souta	25:b3a9f34b201d	760	break;
M_souta	25:b3a9f34b201d	761	case 'N':
M_souta	25:b3a9f34b201d	762	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	763	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	764	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	765	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	766	adjAnable = false;
M_souta	25:b3a9f34b201d	767	break;
M_souta	25:b3a9f34b201d	768	default:
M_souta	25:b3a9f34b201d	769	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	770	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	771	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	772	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	773	adjAnable = false;
M_souta	25:b3a9f34b201d	774	}
M_souta	25:b3a9f34b201d	775
M_souta	25:b3a9f34b201d	776	if(adjAnable){
M_souta	25:b3a9f34b201d	777	if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
M_souta	25:b3a9f34b201d	778	} else {
M_souta	25:b3a9f34b201d	779	adj = 0;
M_souta	25:b3a9f34b201d	780	}
M_souta	25:b3a9f34b201d	781
M_souta	25:b3a9f34b201d	782	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	783	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	784	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	785	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	786	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	787	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	788	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	789	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	790
M_souta	25:b3a9f34b201d	791	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	792	lineCount++;
M_souta	25:b3a9f34b201d	793	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	794	}
M_souta	25:b3a9f34b201d	795
M_souta	25:b3a9f34b201d	796	if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
M_souta	25:b3a9f34b201d	797	targetCount = 3;
M_souta	25:b3a9f34b201d	798	} else if(LimitSw::IsPressed(TOWEL1_SW)) {
M_souta	25:b3a9f34b201d	799	targetCount = 3;
M_souta	25:b3a9f34b201d	800	} else {
M_souta	25:b3a9f34b201d	801	targetCount = 2;
M_souta	25:b3a9f34b201d	802	}
M_souta	25:b3a9f34b201d	803
M_souta	26:4c0ce2f05688	804	if(countW == targetCount) {
M_souta	26:4c0ce2f05688	805	countW = 0;
M_souta	25:b3a9f34b201d	806	lineFase = 4;
M_souta	25:b3a9f34b201d	807	lineCount = 0;
M_souta	25:b3a9f34b201d	808	lineCheck = false;
M_souta	25:b3a9f34b201d	809	}
M_souta	25:b3a9f34b201d	810	} else if(lineFase == 4) { // 右低速
M_souta	25:b3a9f34b201d	811	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	812	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	813	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	814	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	815	if (linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	816	lineFase = 6;
M_souta	25:b3a9f34b201d	817	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	818	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	819	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	820	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	821	}
M_souta	25:b3a9f34b201d	822	motor[TIRE_FL].pwm = 14;
M_souta	25:b3a9f34b201d	823	motor[TIRE_BL].pwm = 14;
M_souta	25:b3a9f34b201d	824	motor[TIRE_BR].pwm = 14;
M_souta	25:b3a9f34b201d	825	motor[TIRE_FR].pwm = 14;
M_souta	25:b3a9f34b201d	826	} else if (lineFase == 5) {
M_souta	25:b3a9f34b201d	827	lineFase = 6;
M_souta	25:b3a9f34b201d	828	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	829	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	830	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	831	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	832	} else if(lineFase == 6) { // タオル1 検知
M_souta	25:b3a9f34b201d	833	if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	834	lineFase = 7;
M_souta	25:b3a9f34b201d	835	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	836	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	837	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	838	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	839	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	840	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	841	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	842	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	843	} else if(LimitSw::IsPressed(TOW_1L)) {
M_souta	25:b3a9f34b201d	844	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	845	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	846	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	847	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	848	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	849	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	850	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	851	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	852	} else if(LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	853	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	854	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	855	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	856	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	857	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	858	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	859	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	860	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	861	} else {
M_souta	25:b3a9f34b201d	862	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	863	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	864	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	865	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	866	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	867	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	868	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	869	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	870	}
M_souta	25:b3a9f34b201d	871	} else if(lineFase == 7) { // ライン修正
M_souta	25:b3a9f34b201d	872	if(linePara[LINE_TOW_1] == 'A' \|\| linePara[LINE_TOW_1] == 'N') {
M_souta	25:b3a9f34b201d	873	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	874	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	875	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	876	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	877	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	878	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	879	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	880	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	881	} else if(linePara[LINE_TOW_1] > 0) {
M_souta	25:b3a9f34b201d	882	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	883	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	884	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	885	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	886	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	887	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	888	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	889	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	890	} else if(linePara[LINE_TOW_1] < 0) {
M_souta	25:b3a9f34b201d	891	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	892	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	893	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	894	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	895	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	896	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	897	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	898	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	899	} else if(linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	900	lineFase = 8;
M_souta	25:b3a9f34b201d	901	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	902	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	903	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	904	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	905	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	906	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	907	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	908	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	909	} else {
M_souta	25:b3a9f34b201d	910	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	911	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	912	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	913	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	914	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	915	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	916	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	917	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	918	}
M_souta	25:b3a9f34b201d	919	} else if(lineFase == 8) { // タオル1 解放
M_souta	25:b3a9f34b201d	920	//solenoid.TOWEL1 = SOLENOID_ON;
M_souta	25:b3a9f34b201d	921	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	922	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	923	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	924	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	925	lineFase = 9;
M_souta	25:b3a9f34b201d	926	} else if(lineFase == 9) { // 前
M_souta	25:b3a9f34b201d	927	switch(linePara[LINE_TOW_1]) {
M_souta	25:b3a9f34b201d	928	case 2:
M_souta	26:4c0ce2f05688	929	tirePWM[TIRE_FL] = 5;
M_souta	26:4c0ce2f05688	930	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	931	tirePWM[TIRE_BR] = 5;
M_souta	26:4c0ce2f05688	932	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	933	adjAnable = true;
M_souta	25:b3a9f34b201d	934	break;
M_souta	25:b3a9f34b201d	935	case 3:
M_souta	25:b3a9f34b201d	936	tirePWM[TIRE_FL] = 10;
M_souta	26:4c0ce2f05688	937	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	938	tirePWM[TIRE_BR] = -10;
M_souta	26:4c0ce2f05688	939	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	940	adjAnable = true;
M_souta	25:b3a9f34b201d	941	break;
M_souta	25:b3a9f34b201d	942	case 1:
M_souta	26:4c0ce2f05688	943	tirePWM[TIRE_FL] = 15;
M_souta	26:4c0ce2f05688	944	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	945	tirePWM[TIRE_BR] = -15;
M_souta	26:4c0ce2f05688	946	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	947	adjAnable = true;
M_souta	25:b3a9f34b201d	948	break;
M_souta	25:b3a9f34b201d	949	case 0:
M_souta	26:4c0ce2f05688	950	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	951	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	952	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	953	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	954	adjAnable = true;
M_souta	25:b3a9f34b201d	955	break;
M_souta	25:b3a9f34b201d	956	case -1:
M_souta	26:4c0ce2f05688	957	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	958	tirePWM[TIRE_BL] = 15;
M_souta	26:4c0ce2f05688	959	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	960	tirePWM[TIRE_FR] = -15;
M_souta	25:b3a9f34b201d	961	adjAnable = true;
M_souta	25:b3a9f34b201d	962	break;
M_souta	25:b3a9f34b201d	963	case -3:
M_souta	26:4c0ce2f05688	964	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	965	tirePWM[TIRE_BL] = 10;
M_souta	26:4c0ce2f05688	966	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	967	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	968	adjAnable = true;
M_souta	25:b3a9f34b201d	969	break;
M_souta	25:b3a9f34b201d	970	case -2:
M_souta	26:4c0ce2f05688	971	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	972	tirePWM[TIRE_BL] = 5;
M_souta	26:4c0ce2f05688	973	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	974	tirePWM[TIRE_FR] = -5;
M_souta	25:b3a9f34b201d	975	adjAnable = true;
M_souta	25:b3a9f34b201d	976	break;
M_souta	25:b3a9f34b201d	977	case 'A':
M_souta	25:b3a9f34b201d	978	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	979	lineCheck = true;
M_souta	25:b3a9f34b201d	980	lineCount = 0;
M_souta	26:4c0ce2f05688	981	countW++;
M_souta	25:b3a9f34b201d	982	}
M_souta	26:4c0ce2f05688	983	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	984	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	985	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	986	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	987	adjAnable = true;
M_souta	22:7d93f79a3686	988	break;
M_souta	22:7d93f79a3686	989	case 'N':
M_souta	25:b3a9f34b201d	990	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	991	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	992	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	993	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	994	adjAnable = false;
M_souta	25:b3a9f34b201d	995	break;
M_souta	25:b3a9f34b201d	996	default:
M_souta	25:b3a9f34b201d	997	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	998	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	999	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1000	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1001	adjAnable = false;
M_souta	25:b3a9f34b201d	1002	}
M_souta	25:b3a9f34b201d	1003
M_souta	25:b3a9f34b201d	1004	if(adjAnable){
M_souta	25:b3a9f34b201d	1005	adj = 0;
M_souta	25:b3a9f34b201d	1006	} else {
M_souta	25:b3a9f34b201d	1007	adj = 0;
M_souta	25:b3a9f34b201d	1008	}
M_souta	25:b3a9f34b201d	1009
M_souta	25:b3a9f34b201d	1010	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1011	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1012	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1013	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1014	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1015	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1016	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1017	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1018
M_souta	25:b3a9f34b201d	1019	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1020	lineCount++;
M_souta	25:b3a9f34b201d	1021	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1022	}
M_souta	26:4c0ce2f05688	1023	if(countW == 1) {
M_souta	26:4c0ce2f05688	1024	countW = 0;
M_souta	25:b3a9f34b201d	1025	lineFase = 10;
M_souta	25:b3a9f34b201d	1026	lineCount = 0;
M_souta	25:b3a9f34b201d	1027	lineCheck = false;
M_souta	25:b3a9f34b201d	1028	}
M_souta	25:b3a9f34b201d	1029	} else if(lineFase == 10) {
M_souta	25:b3a9f34b201d	1030	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1031	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1032	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1033	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1034	if(linePara[4] == 0) {
M_souta	25:b3a9f34b201d	1035	lineFase = 11;
M_souta	25:b3a9f34b201d	1036	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1037	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1038	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1039	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1040	}
M_souta	25:b3a9f34b201d	1041	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1042	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1043	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1044	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1045	} else if(lineFase == 11) {
M_souta	25:b3a9f34b201d	1046	switch(linePara[4]) {
M_souta	25:b3a9f34b201d	1047	case -2:
M_souta	26:4c0ce2f05688	1048	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1049	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	1050	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1051	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1052	adjAnable = true;
M_souta	22:7d93f79a3686	1053	break;
M_souta	25:b3a9f34b201d	1054	case -3:
M_souta	26:4c0ce2f05688	1055	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	1056	tirePWM[TIRE_BL] = 10;
M_souta	26:4c0ce2f05688	1057	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	1058	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	1059	adjAnable = true;
M_souta	25:b3a9f34b201d	1060	break;
M_souta	25:b3a9f34b201d	1061	case -1:
M_souta	26:4c0ce2f05688	1062	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	1063	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	1064	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	1065	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	1066	adjAnable = true;
M_souta	25:b3a9f34b201d	1067	break;
M_souta	25:b3a9f34b201d	1068	case 0:
M_souta	26:4c0ce2f05688	1069	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	1070	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1071	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	1072	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1073	adjAnable = true;
M_souta	25:b3a9f34b201d	1074	break;
M_souta	25:b3a9f34b201d	1075	case 1:
M_souta	26:4c0ce2f05688	1076	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	1077	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1078	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	1079	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1080	adjAnable = true;
M_souta	25:b3a9f34b201d	1081	break;
M_souta	25:b3a9f34b201d	1082	case 3:
M_souta	26:4c0ce2f05688	1083	tirePWM[TIRE_FL] = -10;
M_souta	26:4c0ce2f05688	1084	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1085	tirePWM[TIRE_BR] = 10;
M_souta	26:4c0ce2f05688	1086	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1087	adjAnable = true;
M_souta	25:b3a9f34b201d	1088	break;
M_souta	25:b3a9f34b201d	1089	case 2:
M_souta	25:b3a9f34b201d	1090	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	1091	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1092	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	1093	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1094	adjAnable = true;
M_souta	25:b3a9f34b201d	1095	break;
M_souta	25:b3a9f34b201d	1096	case 'A':
M_souta	25:b3a9f34b201d	1097	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1098	lineCheck = true;
M_souta	25:b3a9f34b201d	1099	lineCount = 0;
M_souta	26:4c0ce2f05688	1100	countW++;
M_souta	25:b3a9f34b201d	1101	}
M_souta	26:4c0ce2f05688	1102	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	1103	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1104	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	1105	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1106	adjAnable = true;
M_souta	25:b3a9f34b201d	1107	break;
M_souta	25:b3a9f34b201d	1108	case 'N':
M_souta	25:b3a9f34b201d	1109	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1110	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1111	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1112	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1113	adjAnable = false;
M_souta	25:b3a9f34b201d	1114	break;
M_souta	22:7d93f79a3686	1115	default:
M_souta	25:b3a9f34b201d	1116	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1117	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1118	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1119	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1120	adjAnable = false;
M_souta	22:7d93f79a3686	1121	}
M_souta	25:b3a9f34b201d	1122
M_souta	25:b3a9f34b201d	1123	if(adjAnable){
M_souta	25:b3a9f34b201d	1124	adj = 0;//linePara[3];
M_souta	25:b3a9f34b201d	1125	} else {
M_souta	25:b3a9f34b201d	1126	adj = 0;
M_souta	25:b3a9f34b201d	1127	}
M_souta	25:b3a9f34b201d	1128
M_souta	25:b3a9f34b201d	1129	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	1130	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1131	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1132	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	26:4c0ce2f05688	1133	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	26:4c0ce2f05688	1134	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1135	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	26:4c0ce2f05688	1136	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1137
M_souta	25:b3a9f34b201d	1138	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1139	lineCount++;
M_souta	25:b3a9f34b201d	1140	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1141	}
M_souta	26:4c0ce2f05688	1142	if(countW == 2) {
M_souta	26:4c0ce2f05688	1143	countW = 0;
M_souta	25:b3a9f34b201d	1144	lineFase = 12;
M_souta	25:b3a9f34b201d	1145	lineCount = 0;
M_souta	25:b3a9f34b201d	1146	lineCheck = false;
M_souta	25:b3a9f34b201d	1147	}
M_souta	25:b3a9f34b201d	1148	} else if(lineFase == 12) { // 右低速
M_souta	25:b3a9f34b201d	1149	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1150	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1151	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1152	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1153	if(linePara[LINE_TOW_2] == 0) {
M_souta	25:b3a9f34b201d	1154	lineFase = 13;
M_souta	25:b3a9f34b201d	1155	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1156	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1157	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1158	motor[TIRE_FR].dir = BRAKE;
M_souta	22:7d93f79a3686	1159	}
M_souta	25:b3a9f34b201d	1160	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1161	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1162	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1163	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1164	} else if (lineFase == 13) {
M_souta	25:b3a9f34b201d	1165	lineFase = 14;
M_souta	25:b3a9f34b201d	1166	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1167	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1168	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1169	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1170	} else if(lineFase == 14) { // タオル2 竿検知
M_souta	25:b3a9f34b201d	1171	if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
M_souta	25:b3a9f34b201d	1172	lineFase = 15;
M_souta	25:b3a9f34b201d	1173	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1174	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1175	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1176	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1177	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1178	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1179	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1180	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1181	} else if(LimitSw::IsPressed(TOW_2L)) {
M_souta	25:b3a9f34b201d	1182	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1183	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1184	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1185	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1186	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1187	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1188	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1189	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1190	} else if(LimitSw::IsPressed(TOW_2R)) {
M_souta	25:b3a9f34b201d	1191	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1192	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1193	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1194	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1195	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1196	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1197	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1198	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1199	} else {
M_souta	25:b3a9f34b201d	1200	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1201	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1202	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1203	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1204	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1205	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1206	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1207	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1208	}
M_souta	25:b3a9f34b201d	1209	} else if(lineFase == 15 ){ // ライン修正
M_souta	25:b3a9f34b201d	1210	if(linePara[LINE_TOW_2] == 'A' \|\| linePara[LINE_TOW_2] == 'N') {
M_souta	25:b3a9f34b201d	1211	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1212	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1213	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1214	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1215	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1216	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1217	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1218	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1219	} else if(linePara[LINE_TOW_2] > 0) {
M_souta	25:b3a9f34b201d	1220	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1221	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1222	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1223	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1224	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1225	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1226	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1227	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1228	} else if(linePara[LINE_TOW_2] < 0) {
M_souta	25:b3a9f34b201d	1229	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1230	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1231	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1232	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1233	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1234	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1235	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1236	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1237	} else if(linePara[LINE_TOW_2] == 0) {
M_souta	25:b3a9f34b201d	1238	lineFase = 16;
M_souta	25:b3a9f34b201d	1239	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1240	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1241	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1242	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1243	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1244	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1245	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1246	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1247	} else {
M_souta	25:b3a9f34b201d	1248	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1249	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1250	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1251	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1252	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1253	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1254	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1255	motor[TIRE_FR].pwm = 50;
M_souta	22:7d93f79a3686	1256	}
M_souta	25:b3a9f34b201d	1257	} else if(lineFase == 16) { // タオル2 解放
M_souta	25:b3a9f34b201d	1258	//solenoid.TOWEL2 = SOLENOID_ON;
M_souta	25:b3a9f34b201d	1259	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1260	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1261	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1262	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1263	lineFase = 17;
M_souta	25:b3a9f34b201d	1264	} else if(lineFase == 17) { // 前
M_souta	25:b3a9f34b201d	1265	switch(linePara[LINE_TOW_2]) {
M_souta	25:b3a9f34b201d	1266	case 2:
M_souta	25:b3a9f34b201d	1267	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1268	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1269	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1270	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1271	adjAnable = true;
M_souta	25:b3a9f34b201d	1272	break;
M_souta	25:b3a9f34b201d	1273	case 3:
M_souta	25:b3a9f34b201d	1274	tirePWM[TIRE_FL] = 10;
M_souta	25:b3a9f34b201d	1275	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1276	tirePWM[TIRE_BR] = -10;
M_souta	25:b3a9f34b201d	1277	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1278	adjAnable = true;
M_souta	25:b3a9f34b201d	1279	break;
M_souta	25:b3a9f34b201d	1280	case 1:
M_souta	25:b3a9f34b201d	1281	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	1282	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1283	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	1284	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1285	adjAnable = true;
M_souta	25:b3a9f34b201d	1286	break;
M_souta	25:b3a9f34b201d	1287	case 0:
M_souta	25:b3a9f34b201d	1288	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1289	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1290	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1291	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1292	adjAnable = true;
M_souta	25:b3a9f34b201d	1293	break;
M_souta	25:b3a9f34b201d	1294	case -1:
M_souta	25:b3a9f34b201d	1295	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1296	tirePWM[TIRE_BL] = 20;
M_souta	25:b3a9f34b201d	1297	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1298	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	1299	adjAnable = true;
M_souta	25:b3a9f34b201d	1300	break;
M_souta	25:b3a9f34b201d	1301	case -3:
M_souta	25:b3a9f34b201d	1302	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1303	tirePWM[TIRE_BL] = 10;
M_souta	25:b3a9f34b201d	1304	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1305	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	1306	adjAnable = true;
M_souta	25:b3a9f34b201d	1307	break;
M_souta	25:b3a9f34b201d	1308	case -2:
M_souta	25:b3a9f34b201d	1309	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1310	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1311	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1312	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1313	adjAnable = true;
M_souta	25:b3a9f34b201d	1314	break;
M_souta	25:b3a9f34b201d	1315	case 'A':
M_souta	25:b3a9f34b201d	1316	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1317	lineCheck = true;
M_souta	25:b3a9f34b201d	1318	lineCount = 0;
M_souta	26:4c0ce2f05688	1319	countW++;
M_souta	25:b3a9f34b201d	1320	}
M_souta	25:b3a9f34b201d	1321	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1322	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1323	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1324	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1325	adjAnable = true;
M_souta	25:b3a9f34b201d	1326	break;
M_souta	25:b3a9f34b201d	1327	case 'N':
M_souta	25:b3a9f34b201d	1328	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1329	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1330	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1331	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1332	adjAnable = false;
M_souta	25:b3a9f34b201d	1333	break;
M_souta	25:b3a9f34b201d	1334	default:
M_souta	25:b3a9f34b201d	1335	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1336	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1337	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1338	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1339	adjAnable = false;
M_souta	25:b3a9f34b201d	1340	}
M_souta	25:b3a9f34b201d	1341
M_souta	25:b3a9f34b201d	1342	if(adjAnable){
M_souta	25:b3a9f34b201d	1343	adj = 0;
M_souta	25:b3a9f34b201d	1344	} else {
M_souta	25:b3a9f34b201d	1345	adj = 0;
M_souta	25:b3a9f34b201d	1346	}
M_souta	25:b3a9f34b201d	1347
M_souta	25:b3a9f34b201d	1348	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1349	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1350	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1351	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1352	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1353	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1354	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1355	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1356
M_souta	25:b3a9f34b201d	1357	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1358	lineCount++;
M_souta	25:b3a9f34b201d	1359	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1360	}
M_souta	26:4c0ce2f05688	1361	if(countW == 1) {
M_souta	26:4c0ce2f05688	1362	countW = 0;
M_souta	25:b3a9f34b201d	1363	lineFase = 18;
M_souta	25:b3a9f34b201d	1364	lineCount = 0;
M_souta	25:b3a9f34b201d	1365	lineCheck = false;
M_souta	25:b3a9f34b201d	1366	}
M_souta	25:b3a9f34b201d	1367	} else if(lineFase == 18) { // 前低速
M_souta	25:b3a9f34b201d	1368	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1369	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1370	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1371	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1372	if(linePara[4] != 'N') {
M_souta	25:b3a9f34b201d	1373	lineFase = 19;
M_souta	25:b3a9f34b201d	1374	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1375	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1376	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1377	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1378	}
M_souta	25:b3a9f34b201d	1379	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1380	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1381	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1382	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1383	} else if(lineFase == 19) { // 左
M_souta	25:b3a9f34b201d	1384	switch(linePara[3]) {
M_souta	25:b3a9f34b201d	1385	case -2:
M_souta	25:b3a9f34b201d	1386	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1387	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1388	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1389	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1390	adjAnable = true;
M_souta	25:b3a9f34b201d	1391	break;
M_souta	25:b3a9f34b201d	1392	case -3:
M_souta	25:b3a9f34b201d	1393	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1394	tirePWM[TIRE_BL] = 10;
M_souta	25:b3a9f34b201d	1395	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1396	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	1397	adjAnable = true;
M_souta	25:b3a9f34b201d	1398	break;
M_souta	25:b3a9f34b201d	1399	case -1:
M_souta	25:b3a9f34b201d	1400	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1401	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	1402	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	1403	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	1404	adjAnable = true;
M_souta	25:b3a9f34b201d	1405	break;
M_souta	25:b3a9f34b201d	1406	case 0:
M_souta	25:b3a9f34b201d	1407	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1408	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1409	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1410	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1411	adjAnable = true;
M_souta	25:b3a9f34b201d	1412	break;
M_souta	25:b3a9f34b201d	1413	case 1:
M_souta	25:b3a9f34b201d	1414	tirePWM[TIRE_FL] = -20;
M_souta	25:b3a9f34b201d	1415	tirePWM[TIRE_BL] = 40;
M_souta	25:b3a9f34b201d	1416	tirePWM[TIRE_BR] = 20;
M_souta	25:b3a9f34b201d	1417	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1418	adjAnable = true;
M_souta	25:b3a9f34b201d	1419	break;
M_souta	25:b3a9f34b201d	1420	case 3:
M_souta	25:b3a9f34b201d	1421	tirePWM[TIRE_FL] = -10;
M_souta	25:b3a9f34b201d	1422	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1423	tirePWM[TIRE_BR] = 10;
M_souta	25:b3a9f34b201d	1424	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1425	adjAnable = true;
M_souta	25:b3a9f34b201d	1426	break;
M_souta	25:b3a9f34b201d	1427	case 2:
M_souta	25:b3a9f34b201d	1428	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1429	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1430	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1431	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1432	adjAnable = true;
M_souta	25:b3a9f34b201d	1433	break;
M_souta	25:b3a9f34b201d	1434	case 'A':
M_souta	25:b3a9f34b201d	1435	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1436	lineCheck = true;
M_souta	25:b3a9f34b201d	1437	lineCount = 0;
M_souta	26:4c0ce2f05688	1438	countW++;
M_souta	25:b3a9f34b201d	1439	}
M_souta	25:b3a9f34b201d	1440	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1441	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1442	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1443	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1444	adjAnable = true;
M_souta	25:b3a9f34b201d	1445	break;
M_souta	25:b3a9f34b201d	1446	case 'N':
M_souta	25:b3a9f34b201d	1447	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1448	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1449	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1450	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1451	adjAnable = false;
M_souta	25:b3a9f34b201d	1452	break;
M_souta	25:b3a9f34b201d	1453	default:
M_souta	25:b3a9f34b201d	1454	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1455	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1456	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1457	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1458	adjAnable = false;
M_souta	25:b3a9f34b201d	1459	}
M_souta	25:b3a9f34b201d	1460
M_souta	25:b3a9f34b201d	1461	if(adjAnable){
M_souta	25:b3a9f34b201d	1462	if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta	25:b3a9f34b201d	1463	} else {
M_souta	25:b3a9f34b201d	1464	adj = 0;
M_souta	25:b3a9f34b201d	1465	}
M_souta	25:b3a9f34b201d	1466
M_souta	25:b3a9f34b201d	1467	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1468	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1469	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1470	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1471	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1472	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1473	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1474	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1475
M_souta	25:b3a9f34b201d	1476	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1477	lineCount++;
M_souta	25:b3a9f34b201d	1478	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1479	}
M_souta	25:b3a9f34b201d	1480
M_souta	25:b3a9f34b201d	1481	/*
M_souta	25:b3a9f34b201d	1482	if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
M_souta	25:b3a9f34b201d	1483	targetCount = 3;
M_souta	25:b3a9f34b201d	1484	} else if(LimitSw::IsPressed(TOWEL1_SW)) {
M_souta	25:b3a9f34b201d	1485	targetCount = 3;
M_souta	25:b3a9f34b201d	1486	} else {
M_souta	25:b3a9f34b201d	1487	targetCount = 2;
M_souta	25:b3a9f34b201d	1488	}
M_souta	25:b3a9f34b201d	1489	*/
M_souta	25:b3a9f34b201d	1490
M_souta	25:b3a9f34b201d	1491	targetCount = 3;
M_souta	25:b3a9f34b201d	1492
M_souta	26:4c0ce2f05688	1493	if(countW == targetCount) {
M_souta	26:4c0ce2f05688	1494	countW = 0;
M_souta	25:b3a9f34b201d	1495	lineFase = 20;
M_souta	25:b3a9f34b201d	1496	lineCount = 0;
M_souta	25:b3a9f34b201d	1497	lineCheck = false;
M_souta	25:b3a9f34b201d	1498	}
M_souta	25:b3a9f34b201d	1499	} else if(lineFase == 20) { // 左低速
M_souta	25:b3a9f34b201d	1500	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1501	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1502	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1503	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1504	if (linePara[2] == 'N') {
M_souta	25:b3a9f34b201d	1505	//(LimitSw::IsPressed(SHEETS_SW)) {
M_souta	25:b3a9f34b201d	1506	//ineFase = 20;
M_souta	25:b3a9f34b201d	1507	//else if(LimitSw::IsPressed(TOWEL2_SW) {
M_souta	25:b3a9f34b201d	1508	//ineFase = 14;
M_souta	25:b3a9f34b201d	1509	//else {
M_souta	25:b3a9f34b201d	1510	lineFase = 21;
M_souta	25:b3a9f34b201d	1511	//
M_souta	25:b3a9f34b201d	1512	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1513	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1514	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1515	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1516	}
M_souta	25:b3a9f34b201d	1517	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1518	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1519	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1520	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1521	} else if(lineFase == 21) {
M_souta	25:b3a9f34b201d	1522	switch(linePara[2]) {
M_souta	25:b3a9f34b201d	1523	case -2:
M_souta	25:b3a9f34b201d	1524	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1525	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1526	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1527	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1528	adjAnable = true;
M_souta	25:b3a9f34b201d	1529	break;
M_souta	25:b3a9f34b201d	1530	case -3:
M_souta	25:b3a9f34b201d	1531	tirePWM[TIRE_FL] = -10;
M_souta	25:b3a9f34b201d	1532	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1533	tirePWM[TIRE_BR] = 10;
M_souta	25:b3a9f34b201d	1534	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1535	adjAnable = true;
M_souta	25:b3a9f34b201d	1536	break;
M_souta	25:b3a9f34b201d	1537	case -1:
M_souta	25:b3a9f34b201d	1538	tirePWM[TIRE_FL] = -20;
M_souta	25:b3a9f34b201d	1539	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1540	tirePWM[TIRE_BR] = 20;
M_souta	25:b3a9f34b201d	1541	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1542	adjAnable = true;
M_souta	25:b3a9f34b201d	1543	break;
M_souta	25:b3a9f34b201d	1544	case 0:
M_souta	25:b3a9f34b201d	1545	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1546	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1547	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1548	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1549	adjAnable = true;
M_souta	25:b3a9f34b201d	1550	break;
M_souta	25:b3a9f34b201d	1551	case 1:
M_souta	25:b3a9f34b201d	1552	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1553	tirePWM[TIRE_BL] = -20;
M_souta	25:b3a9f34b201d	1554	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1555	tirePWM[TIRE_FR] = 20;
M_souta	25:b3a9f34b201d	1556	adjAnable = true;
M_souta	25:b3a9f34b201d	1557	break;
M_souta	25:b3a9f34b201d	1558	case 3:
M_souta	25:b3a9f34b201d	1559	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1560	tirePWM[TIRE_BL] = -10;
M_souta	25:b3a9f34b201d	1561	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1562	tirePWM[TIRE_FR] = 10;
M_souta	25:b3a9f34b201d	1563	adjAnable = true;
M_souta	25:b3a9f34b201d	1564	break;
M_souta	25:b3a9f34b201d	1565	case 2:
M_souta	25:b3a9f34b201d	1566	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1567	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1568	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1569	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1570	adjAnable = true;
M_souta	25:b3a9f34b201d	1571	break;
M_souta	25:b3a9f34b201d	1572	case 'A':
M_souta	25:b3a9f34b201d	1573	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1574	lineCheck = true;
M_souta	25:b3a9f34b201d	1575	lineCount = 0;
M_souta	26:4c0ce2f05688	1576	countW++;
M_souta	25:b3a9f34b201d	1577	}
M_souta	25:b3a9f34b201d	1578	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1579	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1580	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1581	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1582	adjAnable = true;
M_souta	25:b3a9f34b201d	1583	break;
M_souta	25:b3a9f34b201d	1584	case 'N':
M_souta	25:b3a9f34b201d	1585	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1586	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1587	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1588	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1589	adjAnable = false;
M_souta	25:b3a9f34b201d	1590	break;
M_souta	25:b3a9f34b201d	1591	default:
M_souta	25:b3a9f34b201d	1592	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1593	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1594	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1595	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1596	adjAnable = false;
M_souta	25:b3a9f34b201d	1597	}
M_souta	25:b3a9f34b201d	1598
M_souta	25:b3a9f34b201d	1599	if(adjAnable){
M_souta	25:b3a9f34b201d	1600	if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
M_souta	25:b3a9f34b201d	1601	} else {
M_souta	25:b3a9f34b201d	1602	adj = 0;
M_souta	25:b3a9f34b201d	1603	}
M_souta	25:b3a9f34b201d	1604
M_souta	25:b3a9f34b201d	1605	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	1606	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1607	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1608	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1609	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	1610	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1611	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1612	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1613
M_souta	25:b3a9f34b201d	1614	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1615	lineCount++;
M_souta	25:b3a9f34b201d	1616	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1617	}
M_souta	26:4c0ce2f05688	1618	if(countW == 3) {
M_souta	26:4c0ce2f05688	1619	countW = 0;
M_souta	25:b3a9f34b201d	1620	lineFase = 22;
M_souta	25:b3a9f34b201d	1621	lineCount = 0;
M_souta	25:b3a9f34b201d	1622	lineCheck = false;
M_souta	25:b3a9f34b201d	1623	}
M_souta	25:b3a9f34b201d	1624	} else if(lineFase == 22) {
M_souta	25:b3a9f34b201d	1625	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1626	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1627	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1628	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1629	if (linePara[2] == 'N') {
M_souta	25:b3a9f34b201d	1630	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1631	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1632	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1633	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1634	}
M_souta	25:b3a9f34b201d	1635	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	1636	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	1637	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	1638	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	1639	} else{
M_souta	25:b3a9f34b201d	1640	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1641	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1642	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1643	motor[TIRE_FR].dir = BRAKE;
M_souta	22:7d93f79a3686	1644	}
M_souta	22:7d93f79a3686	1645
kishibekairohan	13:b6e02d6261d7	1646	}
kishibekairohan	13:b6e02d6261d7	1647	#endif
kishibekairohan	13:b6e02d6261d7	1648
kishibekairohan	13:b6e02d6261d7	1649	#if USE_PROCESS_NUM>5
kishibekairohan	16:3f2c2d89372b	1650	static void Process5()
kishibekairohan	16:3f2c2d89372b	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
M_souta	25:b3a9f34b201d	1659	for(int i = 0; i < 8; i++) {
M_souta	25:b3a9f34b201d	1660	linePara[i] = lineCast(LineHub::GetPara(i));
M_souta	25:b3a9f34b201d	1661	}
M_souta	25:b3a9f34b201d	1662
M_souta	25:b3a9f34b201d	1663	if(lineFase == 0) {
M_souta	26:4c0ce2f05688	1664	motor[TIRE_FL].dir = FOR;
M_souta	26:4c0ce2f05688	1665	motor[TIRE_BL].dir = FOR;
M_souta	26:4c0ce2f05688	1666	motor[TIRE_BR].dir = BACK;
M_souta	26:4c0ce2f05688	1667	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1668	if(linePara[0] != 'N' && linePara[0] != 'A') {
M_souta	25:b3a9f34b201d	1669	lineFase = 1;
M_souta	25:b3a9f34b201d	1670	}
M_souta	26:4c0ce2f05688	1671	motor[TIRE_FL].pwm = 30;
M_souta	26:4c0ce2f05688	1672	motor[TIRE_BL].pwm = 30;
M_souta	26:4c0ce2f05688	1673	motor[TIRE_BR].pwm = 30;
M_souta	26:4c0ce2f05688	1674	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	1675	}else if(lineFase == 1) { // 前ライントレース
M_souta	25:b3a9f34b201d	1676	switch(linePara[0]) {
M_souta	25:b3a9f34b201d	1677	case -2:
M_souta	25:b3a9f34b201d	1678	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	1679	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1680	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	1681	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1682	adjAnable = true;
M_souta	25:b3a9f34b201d	1683	break;
M_souta	25:b3a9f34b201d	1684	case -3:
M_souta	26:4c0ce2f05688	1685	tirePWM[TIRE_FL] = 10;
M_souta	26:4c0ce2f05688	1686	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1687	tirePWM[TIRE_BR] = -10;
M_souta	26:4c0ce2f05688	1688	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1689	adjAnable = true;
M_souta	25:b3a9f34b201d	1690	break;
M_souta	25:b3a9f34b201d	1691	case -1:
M_souta	25:b3a9f34b201d	1692	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	1693	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1694	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	1695	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1696	adjAnable = true;
M_souta	25:b3a9f34b201d	1697	break;
M_souta	25:b3a9f34b201d	1698	case 0:
M_souta	26:4c0ce2f05688	1699	tirePWM[TIRE_FL] = 30;
M_souta	26:4c0ce2f05688	1700	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1701	tirePWM[TIRE_BR] = -30;
M_souta	26:4c0ce2f05688	1702	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1703	adjAnable = true;
M_souta	25:b3a9f34b201d	1704	break;
M_souta	25:b3a9f34b201d	1705	case 1:
M_souta	26:4c0ce2f05688	1706	tirePWM[TIRE_FL] = 30;
M_souta	26:4c0ce2f05688	1707	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	1708	tirePWM[TIRE_BR] = -30;
M_souta	26:4c0ce2f05688	1709	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	1710	adjAnable = true;
M_souta	25:b3a9f34b201d	1711	break;
M_souta	25:b3a9f34b201d	1712	case 3:
M_souta	26:4c0ce2f05688	1713	tirePWM[TIRE_FL] = 30;
M_souta	26:4c0ce2f05688	1714	tirePWM[TIRE_BL] = 10;
M_souta	26:4c0ce2f05688	1715	tirePWM[TIRE_BR] = -30;
M_souta	26:4c0ce2f05688	1716	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	1717	adjAnable = true;
M_souta	25:b3a9f34b201d	1718	break;
M_souta	25:b3a9f34b201d	1719	case 2:
M_souta	26:4c0ce2f05688	1720	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1721	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	1722	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1723	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1724	adjAnable = true;
M_souta	25:b3a9f34b201d	1725	break;
M_souta	25:b3a9f34b201d	1726	case 'A':
M_souta	25:b3a9f34b201d	1727	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1728	lineCheck = true;
M_souta	25:b3a9f34b201d	1729	lineCount = 0;
M_souta	26:4c0ce2f05688	1730	countW++;
M_souta	25:b3a9f34b201d	1731	}
M_souta	26:4c0ce2f05688	1732	tirePWM[TIRE_FL] = 30;
M_souta	26:4c0ce2f05688	1733	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1734	tirePWM[TIRE_BR] = -30;
M_souta	26:4c0ce2f05688	1735	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1736	adjAnable = true;
M_souta	25:b3a9f34b201d	1737	break;
M_souta	25:b3a9f34b201d	1738	case 'N':
M_souta	25:b3a9f34b201d	1739	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1740	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1741	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1742	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1743	adjAnable = false;
M_souta	25:b3a9f34b201d	1744	break;
M_souta	25:b3a9f34b201d	1745	default:
M_souta	25:b3a9f34b201d	1746	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1747	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1748	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1749	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1750	adjAnable = false;
M_souta	25:b3a9f34b201d	1751	}
M_souta	25:b3a9f34b201d	1752
M_souta	25:b3a9f34b201d	1753	if(adjAnable){
M_souta	25:b3a9f34b201d	1754	if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
M_souta	25:b3a9f34b201d	1755	} else {
M_souta	25:b3a9f34b201d	1756	adj = 0;
M_souta	25:b3a9f34b201d	1757	}
M_souta	25:b3a9f34b201d	1758
M_souta	25:b3a9f34b201d	1759	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1760	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1761	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1762	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	26:4c0ce2f05688	1763	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	26:4c0ce2f05688	1764	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	26:4c0ce2f05688	1765	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	26:4c0ce2f05688	1766	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1767
M_souta	25:b3a9f34b201d	1768	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1769	lineCount++;
M_souta	25:b3a9f34b201d	1770	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1771	}
M_souta	26:4c0ce2f05688	1772	if(countW == 3) {
M_souta	26:4c0ce2f05688	1773	countW = 0;
M_souta	25:b3a9f34b201d	1774	lineFase = 2;
M_souta	25:b3a9f34b201d	1775	lineCount = 0;
M_souta	25:b3a9f34b201d	1776	lineCheck = false;
M_souta	25:b3a9f34b201d	1777	}
M_souta	25:b3a9f34b201d	1778	} else if(lineFase == 2) { // 前低速
M_souta	26:4c0ce2f05688	1779	motor[TIRE_FL].dir = FOR;
M_souta	26:4c0ce2f05688	1780	motor[TIRE_BL].dir = FOR;
M_souta	26:4c0ce2f05688	1781	motor[TIRE_BR].dir = BACK;
M_souta	26:4c0ce2f05688	1782	motor[TIRE_FR].dir = BACK;
M_souta	26:4c0ce2f05688	1783	if(linePara[3] == 0) {
M_souta	25:b3a9f34b201d	1784	lineFase = 3;
M_souta	25:b3a9f34b201d	1785	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1786	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1787	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1788	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1789	}
M_souta	26:4c0ce2f05688	1790	motor[TIRE_FL].pwm = 16;
M_souta	26:4c0ce2f05688	1791	motor[TIRE_BL].pwm = 16;
M_souta	26:4c0ce2f05688	1792	motor[TIRE_BR].pwm = 16;
M_souta	26:4c0ce2f05688	1793	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1794	} else if(lineFase == 3) { // 左ライントレース
M_souta	26:4c0ce2f05688	1795	switch(linePara[3]) {
M_souta	25:b3a9f34b201d	1796	case -2:
M_souta	26:4c0ce2f05688	1797	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	1798	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	1799	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	1800	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1801	adjAnable = true;
M_souta	25:b3a9f34b201d	1802	break;
M_souta	25:b3a9f34b201d	1803	case -3:
M_souta	26:4c0ce2f05688	1804	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	1805	tirePWM[TIRE_BL] = 10;
M_souta	26:4c0ce2f05688	1806	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	1807	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	1808	adjAnable = true;
M_souta	25:b3a9f34b201d	1809	break;
M_souta	25:b3a9f34b201d	1810	case -1:
M_souta	26:4c0ce2f05688	1811	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	1812	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	1813	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	1814	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	1815	adjAnable = true;
M_souta	25:b3a9f34b201d	1816	break;
M_souta	25:b3a9f34b201d	1817	case 0:
M_souta	26:4c0ce2f05688	1818	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	1819	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1820	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	1821	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1822	adjAnable = true;
M_souta	25:b3a9f34b201d	1823	break;
M_souta	25:b3a9f34b201d	1824	case 1:
M_souta	26:4c0ce2f05688	1825	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	1826	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1827	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	1828	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1829	adjAnable = true;
M_souta	25:b3a9f34b201d	1830	break;
M_souta	25:b3a9f34b201d	1831	case 3:
M_souta	26:4c0ce2f05688	1832	tirePWM[TIRE_FL] = -10;
M_souta	26:4c0ce2f05688	1833	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1834	tirePWM[TIRE_BR] = 10;
M_souta	26:4c0ce2f05688	1835	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1836	adjAnable = true;
M_souta	25:b3a9f34b201d	1837	break;
M_souta	25:b3a9f34b201d	1838	case 2:
M_souta	25:b3a9f34b201d	1839	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	1840	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	1841	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	1842	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1843	adjAnable = true;
M_souta	25:b3a9f34b201d	1844	break;
M_souta	25:b3a9f34b201d	1845	case 'A':
M_souta	25:b3a9f34b201d	1846	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1847	lineCheck = true;
M_souta	25:b3a9f34b201d	1848	lineCount = 0;
M_souta	26:4c0ce2f05688	1849	countW++;
M_souta	25:b3a9f34b201d	1850	}
M_souta	26:4c0ce2f05688	1851	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	1852	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	1853	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	1854	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	1855	adjAnable = true;
M_souta	25:b3a9f34b201d	1856	break;
M_souta	25:b3a9f34b201d	1857	case 'N':
M_souta	25:b3a9f34b201d	1858	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1859	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1860	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1861	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1862	adjAnable = false;
M_souta	25:b3a9f34b201d	1863	break;
M_souta	25:b3a9f34b201d	1864	default:
M_souta	25:b3a9f34b201d	1865	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1866	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1867	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1868	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1869	adjAnable = false;
M_souta	25:b3a9f34b201d	1870	}
M_souta	25:b3a9f34b201d	1871
M_souta	25:b3a9f34b201d	1872	if(adjAnable){
M_souta	25:b3a9f34b201d	1873	if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta	25:b3a9f34b201d	1874	} else {
M_souta	25:b3a9f34b201d	1875	adj = 0;
M_souta	25:b3a9f34b201d	1876	}
M_souta	25:b3a9f34b201d	1877
M_souta	25:b3a9f34b201d	1878	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1879	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1880	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1881	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1882	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	1883	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	25:b3a9f34b201d	1884	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	1885	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	1886
M_souta	25:b3a9f34b201d	1887	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1888	lineCount++;
M_souta	25:b3a9f34b201d	1889	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1890	}
M_souta	26:4c0ce2f05688	1891	if(countW == 1) {
M_souta	26:4c0ce2f05688	1892	countW = 0;
M_souta	25:b3a9f34b201d	1893	lineFase = 4;
M_souta	25:b3a9f34b201d	1894	lineCount = 0;
M_souta	25:b3a9f34b201d	1895	lineCheck = false;
M_souta	25:b3a9f34b201d	1896	}
M_souta	25:b3a9f34b201d	1897	} else if(lineFase == 4) { // 右低速
M_souta	26:4c0ce2f05688	1898	motor[TIRE_FL].dir = BACK;
M_souta	26:4c0ce2f05688	1899	motor[TIRE_BL].dir = FOR;
M_souta	26:4c0ce2f05688	1900	motor[TIRE_BR].dir = FOR;
M_souta	26:4c0ce2f05688	1901	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1902	if(linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	1903	lineFase = 5;
M_souta	25:b3a9f34b201d	1904	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1905	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1906	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1907	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1908	}
M_souta	26:4c0ce2f05688	1909	motor[TIRE_FL].pwm = 16;
M_souta	26:4c0ce2f05688	1910	motor[TIRE_BL].pwm = 16;
M_souta	26:4c0ce2f05688	1911	motor[TIRE_BR].pwm = 16;
M_souta	26:4c0ce2f05688	1912	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1913	} else if (lineFase == 5) {
M_souta	25:b3a9f34b201d	1914	lineFase = 6;
M_souta	25:b3a9f34b201d	1915	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1916	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1917	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1918	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	1919	} else if(lineFase == 6) { // タオル1 竿検知
M_souta	25:b3a9f34b201d	1920	if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	1921	lineFase = 7;
M_souta	25:b3a9f34b201d	1922	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1923	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1924	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1925	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1926	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1927	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1928	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1929	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1930	} else if(LimitSw::IsPressed(TOW_1L)) {
M_souta	25:b3a9f34b201d	1931	motor[TIRE_FL].dir = FOR;
M_souta	26:4c0ce2f05688	1932	motor[TIRE_FL].pwm = 20;
M_souta	25:b3a9f34b201d	1933	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1934	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1935	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1936	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1937	motor[TIRE_FR].dir = FOR;
M_souta	26:4c0ce2f05688	1938	motor[TIRE_FR].pwm = 20;
M_souta	25:b3a9f34b201d	1939	} else if(LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	1940	motor[TIRE_FL].dir = BACK;
M_souta	26:4c0ce2f05688	1941	motor[TIRE_FL].pwm = 20;
M_souta	25:b3a9f34b201d	1942	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1943	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1944	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1945	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1946	motor[TIRE_FR].dir = BACK;
M_souta	26:4c0ce2f05688	1947	motor[TIRE_FR].pwm = 20;
M_souta	25:b3a9f34b201d	1948	} else {
M_souta	26:4c0ce2f05688	1949	switch(linePara[LINE_TOW_1]) {
M_souta	26:4c0ce2f05688	1950	case -2:
M_souta	26:4c0ce2f05688	1951	tirePWM[TIRE_FL] = -10;
M_souta	26:4c0ce2f05688	1952	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	1953	tirePWM[TIRE_BR] = 10;
M_souta	26:4c0ce2f05688	1954	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	1955	adjAnable = true;
M_souta	26:4c0ce2f05688	1956	break;
M_souta	26:4c0ce2f05688	1957	case -3:
M_souta	26:4c0ce2f05688	1958	tirePWM[TIRE_FL] = -14;
M_souta	26:4c0ce2f05688	1959	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	1960	tirePWM[TIRE_BR] = 14;
M_souta	26:4c0ce2f05688	1961	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	1962	adjAnable = true;
M_souta	26:4c0ce2f05688	1963	break;
M_souta	26:4c0ce2f05688	1964	case -1:
M_souta	26:4c0ce2f05688	1965	tirePWM[TIRE_FL] = -17;
M_souta	26:4c0ce2f05688	1966	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	1967	tirePWM[TIRE_BR] = 17;
M_souta	26:4c0ce2f05688	1968	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	1969	adjAnable = true;
M_souta	26:4c0ce2f05688	1970	break;
M_souta	26:4c0ce2f05688	1971	case 0:
M_souta	26:4c0ce2f05688	1972	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	1973	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	1974	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	1975	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	1976	adjAnable = true;
M_souta	26:4c0ce2f05688	1977	break;
M_souta	26:4c0ce2f05688	1978	case 1:
M_souta	26:4c0ce2f05688	1979	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	1980	tirePWM[TIRE_BL] = -17;
M_souta	26:4c0ce2f05688	1981	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	1982	tirePWM[TIRE_FR] = 17;
M_souta	26:4c0ce2f05688	1983	adjAnable = true;
M_souta	26:4c0ce2f05688	1984	break;
M_souta	26:4c0ce2f05688	1985	case 3:
M_souta	26:4c0ce2f05688	1986	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	1987	tirePWM[TIRE_BL] = -14;
M_souta	26:4c0ce2f05688	1988	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	1989	tirePWM[TIRE_FR] = 14;
M_souta	26:4c0ce2f05688	1990	adjAnable = true;
M_souta	26:4c0ce2f05688	1991	break;
M_souta	26:4c0ce2f05688	1992	case 2:
M_souta	26:4c0ce2f05688	1993	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	1994	tirePWM[TIRE_BL] = -10;
M_souta	26:4c0ce2f05688	1995	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	1996	tirePWM[TIRE_FR] = 10;
M_souta	26:4c0ce2f05688	1997	adjAnable = true;
M_souta	26:4c0ce2f05688	1998	break;
M_souta	26:4c0ce2f05688	1999	case 'A':
M_souta	26:4c0ce2f05688	2000	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	2001	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	2002	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	2003	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	2004	adjAnable = true;
M_souta	26:4c0ce2f05688	2005	break;
M_souta	26:4c0ce2f05688	2006	case 'N':
M_souta	26:4c0ce2f05688	2007	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2008	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2009	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2010	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2011	adjAnable = false;
M_souta	26:4c0ce2f05688	2012	break;
M_souta	26:4c0ce2f05688	2013	default:
M_souta	26:4c0ce2f05688	2014	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2015	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2016	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2017	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2018	adjAnable = false;
M_souta	26:4c0ce2f05688	2019	}
M_souta	26:4c0ce2f05688	2020
M_souta	26:4c0ce2f05688	2021	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	26:4c0ce2f05688	2022	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	26:4c0ce2f05688	2023	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	26:4c0ce2f05688	2024	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	26:4c0ce2f05688	2025	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	26:4c0ce2f05688	2026	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	26:4c0ce2f05688	2027	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	26:4c0ce2f05688	2028	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	2029	}
M_souta	25:b3a9f34b201d	2030	} else if(lineFase == 7) { // ライン修正
M_souta	25:b3a9f34b201d	2031	if(linePara[LINE_TOW_1] == 'A' \|\| linePara[LINE_TOW_1] == 'N') {
M_souta	25:b3a9f34b201d	2032	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2033	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	2034	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2035	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	2036	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2037	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	2038	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2039	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	2040	} else if(linePara[LINE_TOW_1] > 0) {
M_souta	25:b3a9f34b201d	2041	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	2042	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	2043	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	2044	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	2045	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	2046	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	2047	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	2048	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	2049	} else if(linePara[LINE_TOW_1] < 0) {
M_souta	25:b3a9f34b201d	2050	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	2051	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	2052	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	2053	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	2054	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	2055	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	2056	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	2057	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	2058	} else if(linePara[LINE_TOW_1] == 0) {
M_souta	26:4c0ce2f05688	2059	//tow_stop.reset();
M_souta	26:4c0ce2f05688	2060	//tow_stop.start();
M_souta	25:b3a9f34b201d	2061	lineFase = 8;
M_souta	25:b3a9f34b201d	2062	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2063	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	2064	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2065	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	2066	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2067	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	2068	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2069	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	2070	} else {
M_souta	25:b3a9f34b201d	2071	motor[TIRE_FL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2072	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	2073	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2074	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	2075	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2076	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	2077	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2078	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	2079	}
M_souta	25:b3a9f34b201d	2080	} else if(lineFase == 8) { // タオル1 解放
M_souta	25:b3a9f34b201d	2081	//solenoid.TOWEL1 = SOLENOID_ON;
M_souta	25:b3a9f34b201d	2082	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2083	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2084	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2085	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2086	lineFase = 9;
M_souta	25:b3a9f34b201d	2087	} else if(lineFase == 9) { // 前
M_souta	25:b3a9f34b201d	2088	switch(linePara[LINE_TOW_1]) {
M_souta	25:b3a9f34b201d	2089	case 2:
M_souta	26:4c0ce2f05688	2090	tirePWM[TIRE_FL] = 10;
M_souta	26:4c0ce2f05688	2091	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2092	tirePWM[TIRE_BR] = -10;
M_souta	26:4c0ce2f05688	2093	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	2094	adjAnable = true;
M_souta	25:b3a9f34b201d	2095	break;
M_souta	25:b3a9f34b201d	2096	case 3:
M_souta	26:4c0ce2f05688	2097	tirePWM[TIRE_FL] = 14;
M_souta	26:4c0ce2f05688	2098	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2099	tirePWM[TIRE_BR] = -14;
M_souta	26:4c0ce2f05688	2100	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	2101	adjAnable = true;
M_souta	25:b3a9f34b201d	2102	break;
M_souta	25:b3a9f34b201d	2103	case 1:
M_souta	26:4c0ce2f05688	2104	tirePWM[TIRE_FL] = 17;
M_souta	26:4c0ce2f05688	2105	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2106	tirePWM[TIRE_BR] = -17;
M_souta	26:4c0ce2f05688	2107	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	2108	adjAnable = true;
M_souta	25:b3a9f34b201d	2109	break;
M_souta	25:b3a9f34b201d	2110	case 0:
M_souta	26:4c0ce2f05688	2111	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2112	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2113	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2114	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	2115	adjAnable = true;
M_souta	25:b3a9f34b201d	2116	break;
M_souta	25:b3a9f34b201d	2117	case -1:
M_souta	26:4c0ce2f05688	2118	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2119	tirePWM[TIRE_BL] = 17;
M_souta	26:4c0ce2f05688	2120	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2121	tirePWM[TIRE_FR] = -17;
M_souta	25:b3a9f34b201d	2122	adjAnable = true;
M_souta	25:b3a9f34b201d	2123	break;
M_souta	25:b3a9f34b201d	2124	case -3:
M_souta	26:4c0ce2f05688	2125	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2126	tirePWM[TIRE_BL] = 14;
M_souta	26:4c0ce2f05688	2127	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2128	tirePWM[TIRE_FR] = -14;
M_souta	25:b3a9f34b201d	2129	adjAnable = true;
M_souta	25:b3a9f34b201d	2130	break;
M_souta	25:b3a9f34b201d	2131	case -2:
M_souta	26:4c0ce2f05688	2132	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2133	tirePWM[TIRE_BL] = 10;
M_souta	26:4c0ce2f05688	2134	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2135	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	2136	adjAnable = true;
M_souta	25:b3a9f34b201d	2137	break;
M_souta	25:b3a9f34b201d	2138	case 'A':
M_souta	25:b3a9f34b201d	2139	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	2140	lineCheck = true;
M_souta	25:b3a9f34b201d	2141	lineCount = 0;
M_souta	26:4c0ce2f05688	2142	countW++;
M_souta	25:b3a9f34b201d	2143	}
M_souta	26:4c0ce2f05688	2144	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2145	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2146	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2147	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	2148	adjAnable = true;
M_souta	25:b3a9f34b201d	2149	break;
M_souta	25:b3a9f34b201d	2150	case 'N':
M_souta	25:b3a9f34b201d	2151	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2152	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	2153	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	2154	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2155	adjAnable = false;
M_souta	25:b3a9f34b201d	2156	break;
M_souta	25:b3a9f34b201d	2157	default:
M_souta	25:b3a9f34b201d	2158	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2159	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	2160	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	2161	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2162	adjAnable = false;
M_souta	25:b3a9f34b201d	2163	}
M_souta	25:b3a9f34b201d	2164
M_souta	25:b3a9f34b201d	2165	if(adjAnable){
M_souta	25:b3a9f34b201d	2166	adj = 0;
M_souta	25:b3a9f34b201d	2167	} else {
M_souta	25:b3a9f34b201d	2168	adj = 0;
M_souta	25:b3a9f34b201d	2169	}
M_souta	25:b3a9f34b201d	2170
M_souta	25:b3a9f34b201d	2171	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	2172	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	2173	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	2174	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	2175	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	25:b3a9f34b201d	2176	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	2177	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	25:b3a9f34b201d	2178	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	2179
M_souta	25:b3a9f34b201d	2180	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	2181	lineCount++;
M_souta	25:b3a9f34b201d	2182	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	2183	}
M_souta	26:4c0ce2f05688	2184	if(countW == 1) {
M_souta	26:4c0ce2f05688	2185	countW = 0;
M_souta	25:b3a9f34b201d	2186	lineFase = 10;
M_souta	25:b3a9f34b201d	2187	lineCount = 0;
M_souta	25:b3a9f34b201d	2188	lineCheck = false;
M_souta	25:b3a9f34b201d	2189	}
M_souta	26:4c0ce2f05688	2190	} else if(lineFase == 10) { // 前低速
M_souta	25:b3a9f34b201d	2191	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	2192	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	2193	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	2194	motor[TIRE_FR].dir = BACK;
M_souta	26:4c0ce2f05688	2195	if(linePara[3] == 0) {
M_souta	25:b3a9f34b201d	2196	lineFase = 11;
M_souta	25:b3a9f34b201d	2197	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2198	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2199	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2200	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2201	}
M_souta	26:4c0ce2f05688	2202	motor[TIRE_FL].pwm = 16;
M_souta	26:4c0ce2f05688	2203	motor[TIRE_BL].pwm = 16;
M_souta	26:4c0ce2f05688	2204	motor[TIRE_BR].pwm = 16;
M_souta	26:4c0ce2f05688	2205	motor[TIRE_FR].pwm = 16;
M_souta	26:4c0ce2f05688	2206	} else if(lineFase == 11) { // 左
M_souta	26:4c0ce2f05688	2207	switch(linePara[3]) {
M_souta	25:b3a9f34b201d	2208	case -2:
M_souta	26:4c0ce2f05688	2209	tirePWM[TIRE_FL] = -30;
M_souta	25:b3a9f34b201d	2210	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2211	tirePWM[TIRE_BR] = 30;
M_souta	25:b3a9f34b201d	2212	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2213	adjAnable = true;
M_souta	25:b3a9f34b201d	2214	break;
M_souta	25:b3a9f34b201d	2215	case -3:
M_souta	26:4c0ce2f05688	2216	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	2217	tirePWM[TIRE_BL] = 10;
M_souta	26:4c0ce2f05688	2218	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	2219	tirePWM[TIRE_FR] = -10;
M_souta	25:b3a9f34b201d	2220	adjAnable = true;
M_souta	25:b3a9f34b201d	2221	break;
M_souta	25:b3a9f34b201d	2222	case -1:
M_souta	26:4c0ce2f05688	2223	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	2224	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2225	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	2226	tirePWM[TIRE_FR] = -20;
M_souta	25:b3a9f34b201d	2227	adjAnable = true;
M_souta	25:b3a9f34b201d	2228	break;
M_souta	25:b3a9f34b201d	2229	case 0:
M_souta	26:4c0ce2f05688	2230	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	2231	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	2232	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	2233	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	2234	adjAnable = true;
M_souta	25:b3a9f34b201d	2235	break;
M_souta	25:b3a9f34b201d	2236	case 1:
M_souta	26:4c0ce2f05688	2237	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	2238	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	2239	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	2240	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	2241	adjAnable = true;
M_souta	25:b3a9f34b201d	2242	break;
M_souta	25:b3a9f34b201d	2243	case 3:
M_souta	26:4c0ce2f05688	2244	tirePWM[TIRE_FL] = -10;
M_souta	26:4c0ce2f05688	2245	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	2246	tirePWM[TIRE_BR] = 10;
M_souta	26:4c0ce2f05688	2247	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	2248	adjAnable = true;
M_souta	25:b3a9f34b201d	2249	break;
M_souta	25:b3a9f34b201d	2250	case 2:
M_souta	25:b3a9f34b201d	2251	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2252	tirePWM[TIRE_BL] = 30;
M_souta	25:b3a9f34b201d	2253	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2254	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	2255	adjAnable = true;
M_souta	25:b3a9f34b201d	2256	break;
M_souta	25:b3a9f34b201d	2257	case 'A':
M_souta	25:b3a9f34b201d	2258	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	2259	lineCheck = true;
M_souta	25:b3a9f34b201d	2260	lineCount = 0;
M_souta	26:4c0ce2f05688	2261	countW++;
M_souta	25:b3a9f34b201d	2262	}
M_souta	26:4c0ce2f05688	2263	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	2264	tirePWM[TIRE_BL] = 30;
M_souta	26:4c0ce2f05688	2265	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	2266	tirePWM[TIRE_FR] = -30;
M_souta	25:b3a9f34b201d	2267	adjAnable = true;
M_souta	25:b3a9f34b201d	2268	break;
M_souta	25:b3a9f34b201d	2269	case 'N':
M_souta	25:b3a9f34b201d	2270	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2271	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2272	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	2273	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2274	adjAnable = false;
M_souta	25:b3a9f34b201d	2275	break;
M_souta	25:b3a9f34b201d	2276	default:
M_souta	25:b3a9f34b201d	2277	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	2278	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	2279	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	2280	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	2281	adjAnable = false;
M_souta	25:b3a9f34b201d	2282	}
M_souta	25:b3a9f34b201d	2283
M_souta	25:b3a9f34b201d	2284	if(adjAnable){
M_souta	26:4c0ce2f05688	2285	if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
M_souta	25:b3a9f34b201d	2286	} else {
M_souta	25:b3a9f34b201d	2287	adj = 0;
M_souta	25:b3a9f34b201d	2288	}
M_souta	25:b3a9f34b201d	2289
M_souta	26:4c0ce2f05688	2290	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	26:4c0ce2f05688	2291	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	26:4c0ce2f05688	2292	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	26:4c0ce2f05688	2293	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta	26:4c0ce2f05688	2294	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	26:4c0ce2f05688	2295	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	26:4c0ce2f05688	2296	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	26:4c0ce2f05688	2297	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta	25:b3a9f34b201d	2298	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	2299	lineCount++;
M_souta	25:b3a9f34b201d	2300	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	2301	}
M_souta	26:4c0ce2f05688	2302	if(countW == 2) {
M_souta	26:4c0ce2f05688	2303	countW = 0;
M_souta	25:b3a9f34b201d	2304	lineFase = 12;
M_souta	25:b3a9f34b201d	2305	lineCount = 0;
M_souta	25:b3a9f34b201d	2306	lineCheck = false;
M_souta	25:b3a9f34b201d	2307	}
M_souta	26:4c0ce2f05688	2308	} else if(lineFase == 12) { // 左低速
M_souta	26:4c0ce2f05688	2309	motor[TIRE_FL].dir = BACK;
M_souta	26:4c0ce2f05688	2310	motor[TIRE_BL].dir = FOR;
M_souta	26:4c0ce2f05688	2311	motor[TIRE_BR].dir = FOR;
M_souta	26:4c0ce2f05688	2312	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	2313	if(linePara[LINE_TOW_2] == 0) {
M_souta	25:b3a9f34b201d	2314	lineFase = 13;
M_souta	25:b3a9f34b201d	2315	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2316	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2317	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2318	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2319	}
M_souta	25:b3a9f34b201d	2320	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	2321	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	2322	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	2323	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	2324	} else if (lineFase == 13) {
M_souta	25:b3a9f34b201d	2325	lineFase = 14;
M_souta	25:b3a9f34b201d	2326	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2327	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2328	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2329	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2330	} else if(lineFase == 14) { // タオル2 竿検知
M_souta	26:4c0ce2f05688	2331	if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
M_souta	26:4c0ce2f05688	2332	lineFase = 15;
M_souta	26:4c0ce2f05688	2333	motor[TIRE_FL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2334	motor[TIRE_FL].pwm = 50;
M_souta	26:4c0ce2f05688	2335	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2336	motor[TIRE_BL].pwm = 50;
M_souta	26:4c0ce2f05688	2337	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2338	motor[TIRE_BR].pwm = 50;
M_souta	26:4c0ce2f05688	2339	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2340	motor[TIRE_FR].pwm = 50;
M_souta	26:4c0ce2f05688	2341	} else if(LimitSw::IsPressed(TOW_2L)) {
M_souta	26:4c0ce2f05688	2342	motor[TIRE_FL].dir = FOR;
M_souta	26:4c0ce2f05688	2343	motor[TIRE_FL].pwm = 20;
M_souta	26:4c0ce2f05688	2344	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2345	motor[TIRE_BL].pwm = 50;
M_souta	26:4c0ce2f05688	2346	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2347	motor[TIRE_BR].pwm = 50;
M_souta	26:4c0ce2f05688	2348	motor[TIRE_FR].dir = FOR;
M_souta	26:4c0ce2f05688	2349	motor[TIRE_FR].pwm = 20;
M_souta	26:4c0ce2f05688	2350	} else if(LimitSw::IsPressed(TOW_2R)) {
M_souta	26:4c0ce2f05688	2351	motor[TIRE_FL].dir = BACK;
M_souta	26:4c0ce2f05688	2352	motor[TIRE_FL].pwm = 20;
M_souta	26:4c0ce2f05688	2353	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2354	motor[TIRE_BL].pwm = 50;
M_souta	26:4c0ce2f05688	2355	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2356	motor[TIRE_BR].pwm = 50;
M_souta	26:4c0ce2f05688	2357	motor[TIRE_FR].dir = BACK;
M_souta	26:4c0ce2f05688	2358	motor[TIRE_FR].pwm = 20;
M_souta	26:4c0ce2f05688	2359	} else {
M_souta	26:4c0ce2f05688	2360	switch(linePara[LINE_TOW_2]) {
M_souta	26:4c0ce2f05688	2361	case -2:
M_souta	26:4c0ce2f05688	2362	tirePWM[TIRE_FL] = -10;
M_souta	26:4c0ce2f05688	2363	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	2364	tirePWM[TIRE_BR] = 10;
M_souta	26:4c0ce2f05688	2365	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	2366	break;
M_souta	26:4c0ce2f05688	2367	case -3:
M_souta	26:4c0ce2f05688	2368	tirePWM[TIRE_FL] = -14;
M_souta	26:4c0ce2f05688	2369	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	2370	tirePWM[TIRE_BR] = 14;
M_souta	26:4c0ce2f05688	2371	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	2372	break;
M_souta	26:4c0ce2f05688	2373	case -1:
M_souta	26:4c0ce2f05688	2374	tirePWM[TIRE_FL] = -17;
M_souta	26:4c0ce2f05688	2375	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	2376	tirePWM[TIRE_BR] = 17;
M_souta	26:4c0ce2f05688	2377	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	2378	break;
M_souta	26:4c0ce2f05688	2379	case 0:
M_souta	26:4c0ce2f05688	2380	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	2381	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	2382	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	2383	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	2384	break;
M_souta	26:4c0ce2f05688	2385	case 1:
M_souta	26:4c0ce2f05688	2386	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	2387	tirePWM[TIRE_BL] = -17;
M_souta	26:4c0ce2f05688	2388	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	2389	tirePWM[TIRE_FR] = 17;
M_souta	26:4c0ce2f05688	2390	break;
M_souta	26:4c0ce2f05688	2391	case 3:
M_souta	26:4c0ce2f05688	2392	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	2393	tirePWM[TIRE_BL] = -14;
M_souta	26:4c0ce2f05688	2394	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	2395	tirePWM[TIRE_FR] = 14;
M_souta	26:4c0ce2f05688	2396	break;
M_souta	26:4c0ce2f05688	2397	case 2:
M_souta	26:4c0ce2f05688	2398	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	2399	tirePWM[TIRE_BL] = -10;
M_souta	26:4c0ce2f05688	2400	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	2401	tirePWM[TIRE_FR] = 10;
M_souta	26:4c0ce2f05688	2402	break;
M_souta	26:4c0ce2f05688	2403	case 'A':
M_souta	26:4c0ce2f05688	2404	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	2405	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	2406	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	2407	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	2408	break;
M_souta	26:4c0ce2f05688	2409	case 'N':
M_souta	26:4c0ce2f05688	2410	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2411	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2412	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2413	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2414	break;
M_souta	26:4c0ce2f05688	2415	default:
M_souta	26:4c0ce2f05688	2416	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2417	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2418	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2419	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2420	}
M_souta	26:4c0ce2f05688	2421
M_souta	26:4c0ce2f05688	2422	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	26:4c0ce2f05688	2423	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	26:4c0ce2f05688	2424	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	26:4c0ce2f05688	2425	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	26:4c0ce2f05688	2426	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	26:4c0ce2f05688	2427	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	26:4c0ce2f05688	2428	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	26:4c0ce2f05688	2429	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	26:4c0ce2f05688	2430	}
M_souta	26:4c0ce2f05688	2431	} else if(lineFase == 15 ){ // ライン修正
M_souta	26:4c0ce2f05688	2432	if(linePara[LINE_TOW_2] == 'A' \|\| linePara[LINE_TOW_2] == 'N') {
M_souta	26:4c0ce2f05688	2433	motor[TIRE_FL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2434	motor[TIRE_FL].pwm = 50;
M_souta	26:4c0ce2f05688	2435	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2436	motor[TIRE_BL].pwm = 50;
M_souta	26:4c0ce2f05688	2437	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2438	motor[TIRE_BR].pwm = 50;
M_souta	26:4c0ce2f05688	2439	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2440	motor[TIRE_FR].pwm = 50;
M_souta	26:4c0ce2f05688	2441	} else if(linePara[LINE_TOW_2] > 0) {
M_souta	26:4c0ce2f05688	2442	motor[TIRE_FL].dir = BACK;
M_souta	26:4c0ce2f05688	2443	motor[TIRE_FL].pwm = 16;
M_souta	26:4c0ce2f05688	2444	motor[TIRE_BL].dir = FOR;
M_souta	26:4c0ce2f05688	2445	motor[TIRE_BL].pwm = 16;
M_souta	26:4c0ce2f05688	2446	motor[TIRE_BR].dir = FOR;
M_souta	26:4c0ce2f05688	2447	motor[TIRE_BR].pwm = 16;
M_souta	26:4c0ce2f05688	2448	motor[TIRE_FR].dir = BACK;
M_souta	26:4c0ce2f05688	2449	motor[TIRE_FR].pwm = 16;
M_souta	26:4c0ce2f05688	2450	} else if(linePara[LINE_TOW_2] < 0) {
M_souta	26:4c0ce2f05688	2451	motor[TIRE_FL].dir = FOR;
M_souta	26:4c0ce2f05688	2452	motor[TIRE_FL].pwm = 16;
M_souta	26:4c0ce2f05688	2453	motor[TIRE_BL].dir = BACK;
M_souta	26:4c0ce2f05688	2454	motor[TIRE_BL].pwm = 16;
M_souta	26:4c0ce2f05688	2455	motor[TIRE_BR].dir = BACK;
M_souta	26:4c0ce2f05688	2456	motor[TIRE_BR].pwm = 16;
M_souta	26:4c0ce2f05688	2457	motor[TIRE_FR].dir = FOR;
M_souta	26:4c0ce2f05688	2458	motor[TIRE_FR].pwm = 16;
M_souta	26:4c0ce2f05688	2459	} else if(linePara[LINE_TOW_2] == 0) {
M_souta	26:4c0ce2f05688	2460	lineFase = 16;
M_souta	26:4c0ce2f05688	2461	motor[TIRE_FL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2462	motor[TIRE_FL].pwm = 50;
M_souta	26:4c0ce2f05688	2463	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2464	motor[TIRE_BL].pwm = 50;
M_souta	26:4c0ce2f05688	2465	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2466	motor[TIRE_BR].pwm = 50;
M_souta	26:4c0ce2f05688	2467	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2468	motor[TIRE_FR].pwm = 50;
M_souta	26:4c0ce2f05688	2469	} else {
M_souta	26:4c0ce2f05688	2470	motor[TIRE_FL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2471	motor[TIRE_FL].pwm = 50;
M_souta	26:4c0ce2f05688	2472	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2473	motor[TIRE_BL].pwm = 50;
M_souta	26:4c0ce2f05688	2474	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2475	motor[TIRE_BR].pwm = 50;
M_souta	26:4c0ce2f05688	2476	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2477	motor[TIRE_FR].pwm = 50;
M_souta	26:4c0ce2f05688	2478	}
M_souta	26:4c0ce2f05688	2479	} else if(lineFase == 16) { // タオル2 解放
M_souta	26:4c0ce2f05688	2480	//solenoid.TOWEL2 = SOLENOID_ON;
M_souta	26:4c0ce2f05688	2481	motor[TIRE_FL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2482	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2483	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2484	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2485	lineFase = 17;
M_souta	26:4c0ce2f05688	2486	} else if(lineFase == 17) { // 前
M_souta	26:4c0ce2f05688	2487	switch(linePara[LINE_TOW_2]) {
M_souta	26:4c0ce2f05688	2488	case 2:
M_souta	26:4c0ce2f05688	2489	tirePWM[TIRE_FL] = 10;
M_souta	26:4c0ce2f05688	2490	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2491	tirePWM[TIRE_BR] = -10;
M_souta	26:4c0ce2f05688	2492	tirePWM[TIRE_FR] = -20;
M_souta	26:4c0ce2f05688	2493	adjAnable = true;
M_souta	26:4c0ce2f05688	2494	break;
M_souta	26:4c0ce2f05688	2495	case 3:
M_souta	26:4c0ce2f05688	2496	tirePWM[TIRE_FL] = 14;
M_souta	26:4c0ce2f05688	2497	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2498	tirePWM[TIRE_BR] = -14;
M_souta	26:4c0ce2f05688	2499	tirePWM[TIRE_FR] = -20;
M_souta	26:4c0ce2f05688	2500	adjAnable = true;
M_souta	26:4c0ce2f05688	2501	break;
M_souta	26:4c0ce2f05688	2502	case 1:
M_souta	26:4c0ce2f05688	2503	tirePWM[TIRE_FL] = 17;
M_souta	26:4c0ce2f05688	2504	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2505	tirePWM[TIRE_BR] = -17;
M_souta	26:4c0ce2f05688	2506	tirePWM[TIRE_FR] = -20;
M_souta	26:4c0ce2f05688	2507	adjAnable = true;
M_souta	26:4c0ce2f05688	2508	break;
M_souta	26:4c0ce2f05688	2509	case 0:
M_souta	26:4c0ce2f05688	2510	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2511	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2512	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2513	tirePWM[TIRE_FR] = -20;
M_souta	26:4c0ce2f05688	2514	adjAnable = true;
M_souta	26:4c0ce2f05688	2515	break;
M_souta	26:4c0ce2f05688	2516	case -1:
M_souta	26:4c0ce2f05688	2517	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2518	tirePWM[TIRE_BL] = 17;
M_souta	26:4c0ce2f05688	2519	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2520	tirePWM[TIRE_FR] = -17;
M_souta	26:4c0ce2f05688	2521	adjAnable = true;
M_souta	26:4c0ce2f05688	2522	break;
M_souta	26:4c0ce2f05688	2523	case -3:
M_souta	26:4c0ce2f05688	2524	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2525	tirePWM[TIRE_BL] = 14;
M_souta	26:4c0ce2f05688	2526	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2527	tirePWM[TIRE_FR] = -14;
M_souta	26:4c0ce2f05688	2528	adjAnable = true;
M_souta	26:4c0ce2f05688	2529	break;
M_souta	26:4c0ce2f05688	2530	case -2:
M_souta	26:4c0ce2f05688	2531	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2532	tirePWM[TIRE_BL] = 10;
M_souta	26:4c0ce2f05688	2533	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2534	tirePWM[TIRE_FR] = 10;
M_souta	26:4c0ce2f05688	2535	adjAnable = true;
M_souta	26:4c0ce2f05688	2536	break;
M_souta	26:4c0ce2f05688	2537	case 'A':
M_souta	26:4c0ce2f05688	2538	if(lineCheck == false) {
M_souta	26:4c0ce2f05688	2539	lineCheck = true;
M_souta	26:4c0ce2f05688	2540	lineCount = 0;
M_souta	26:4c0ce2f05688	2541	countW++;
M_souta	26:4c0ce2f05688	2542	}
M_souta	26:4c0ce2f05688	2543	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2544	tirePWM[TIRE_BL] = 20;
M_souta	26:4c0ce2f05688	2545	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2546	tirePWM[TIRE_FR] = -20;
M_souta	26:4c0ce2f05688	2547	adjAnable = true;
M_souta	26:4c0ce2f05688	2548	break;
M_souta	26:4c0ce2f05688	2549	case 'N':
M_souta	26:4c0ce2f05688	2550	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2551	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2552	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2553	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2554	adjAnable = false;
M_souta	26:4c0ce2f05688	2555	break;
M_souta	26:4c0ce2f05688	2556	default:
M_souta	26:4c0ce2f05688	2557	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2558	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2559	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2560	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2561	adjAnable = false;
M_souta	26:4c0ce2f05688	2562	}
M_souta	26:4c0ce2f05688	2563
M_souta	26:4c0ce2f05688	2564	if(adjAnable){
M_souta	26:4c0ce2f05688	2565	adj = 0;
M_souta	26:4c0ce2f05688	2566	} else {
M_souta	26:4c0ce2f05688	2567	adj = 0;
M_souta	26:4c0ce2f05688	2568	}
M_souta	26:4c0ce2f05688	2569
M_souta	26:4c0ce2f05688	2570	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
M_souta	26:4c0ce2f05688	2571	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	26:4c0ce2f05688	2572	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
M_souta	26:4c0ce2f05688	2573	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	26:4c0ce2f05688	2574	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
M_souta	26:4c0ce2f05688	2575	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	26:4c0ce2f05688	2576	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
M_souta	26:4c0ce2f05688	2577	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	26:4c0ce2f05688	2578
M_souta	26:4c0ce2f05688	2579	if(lineCheck == true) {
M_souta	26:4c0ce2f05688	2580	lineCount++;
M_souta	26:4c0ce2f05688	2581	if(lineCount > 20) lineCheck = false;
M_souta	26:4c0ce2f05688	2582	}
M_souta	26:4c0ce2f05688	2583	if(countW == 1) {
M_souta	26:4c0ce2f05688	2584	countW = 0;
M_souta	26:4c0ce2f05688	2585	lineFase = 18;
M_souta	26:4c0ce2f05688	2586	lineCount = 0;
M_souta	26:4c0ce2f05688	2587	lineCheck = false;
M_souta	26:4c0ce2f05688	2588	}
M_souta	26:4c0ce2f05688	2589	} else if(lineFase == 18) { // 前低速
M_souta	26:4c0ce2f05688	2590	motor[TIRE_FL].dir = FOR;
M_souta	26:4c0ce2f05688	2591	motor[TIRE_BL].dir = FOR;
M_souta	26:4c0ce2f05688	2592	motor[TIRE_BR].dir = BACK;
M_souta	26:4c0ce2f05688	2593	motor[TIRE_FR].dir = BACK;
M_souta	26:4c0ce2f05688	2594	if(linePara[4] == 0) {
M_souta	26:4c0ce2f05688	2595	lineFase = 19;
M_souta	26:4c0ce2f05688	2596	motor[TIRE_FL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2597	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2598	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2599	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2600	}
M_souta	26:4c0ce2f05688	2601	motor[TIRE_FL].pwm = 16;
M_souta	26:4c0ce2f05688	2602	motor[TIRE_BL].pwm = 16;
M_souta	26:4c0ce2f05688	2603	motor[TIRE_BR].pwm = 16;
M_souta	26:4c0ce2f05688	2604	motor[TIRE_FR].pwm = 16;
M_souta	26:4c0ce2f05688	2605	} else if(lineFase == 19) { // 右
M_souta	26:4c0ce2f05688	2606	switch(linePara[4]) {
M_souta	26:4c0ce2f05688	2607	case -2:
M_souta	26:4c0ce2f05688	2608	tirePWM[TIRE_FL] = 30;
M_souta	26:4c0ce2f05688	2609	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2610	tirePWM[TIRE_BR] = -30;
M_souta	26:4c0ce2f05688	2611	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2612	adjAnable = true;
M_souta	26:4c0ce2f05688	2613	break;
M_souta	26:4c0ce2f05688	2614	case -3:
M_souta	26:4c0ce2f05688	2615	tirePWM[TIRE_FL] = 30;
M_souta	26:4c0ce2f05688	2616	tirePWM[TIRE_BL] = -10;
M_souta	26:4c0ce2f05688	2617	tirePWM[TIRE_BR] = -30;
M_souta	26:4c0ce2f05688	2618	tirePWM[TIRE_FR] = 10;
M_souta	26:4c0ce2f05688	2619	adjAnable = true;
M_souta	26:4c0ce2f05688	2620	break;
M_souta	26:4c0ce2f05688	2621	case -1:
M_souta	26:4c0ce2f05688	2622	tirePWM[TIRE_FL] = 30;
M_souta	26:4c0ce2f05688	2623	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	2624	tirePWM[TIRE_BR] = -30;
M_souta	26:4c0ce2f05688	2625	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	2626	adjAnable = true;
M_souta	26:4c0ce2f05688	2627	break;
M_souta	26:4c0ce2f05688	2628	case 0:
M_souta	26:4c0ce2f05688	2629	tirePWM[TIRE_FL] = 30;
M_souta	26:4c0ce2f05688	2630	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2631	tirePWM[TIRE_BR] = -30;
M_souta	26:4c0ce2f05688	2632	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2633	adjAnable = true;
M_souta	26:4c0ce2f05688	2634	break;
M_souta	26:4c0ce2f05688	2635	case 1:
M_souta	26:4c0ce2f05688	2636	tirePWM[TIRE_FL] = 20;
M_souta	26:4c0ce2f05688	2637	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2638	tirePWM[TIRE_BR] = -20;
M_souta	26:4c0ce2f05688	2639	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2640	adjAnable = true;
M_souta	26:4c0ce2f05688	2641	break;
M_souta	26:4c0ce2f05688	2642	case 3:
M_souta	26:4c0ce2f05688	2643	tirePWM[TIRE_FL] = 10;
M_souta	26:4c0ce2f05688	2644	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2645	tirePWM[TIRE_BR] = -10;
M_souta	26:4c0ce2f05688	2646	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2647	adjAnable = true;
M_souta	26:4c0ce2f05688	2648	break;
M_souta	26:4c0ce2f05688	2649	case 2:
M_souta	26:4c0ce2f05688	2650	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2651	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2652	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2653	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2654	adjAnable = true;
M_souta	26:4c0ce2f05688	2655	break;
M_souta	26:4c0ce2f05688	2656	case 'A':
M_souta	26:4c0ce2f05688	2657	if(lineCheck == false) {
M_souta	26:4c0ce2f05688	2658	lineCheck = true;
M_souta	26:4c0ce2f05688	2659	lineCount = 0;
M_souta	26:4c0ce2f05688	2660	countW++;
M_souta	26:4c0ce2f05688	2661	}
M_souta	26:4c0ce2f05688	2662	tirePWM[TIRE_FL] = 30;
M_souta	26:4c0ce2f05688	2663	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2664	tirePWM[TIRE_BR] = -30;
M_souta	26:4c0ce2f05688	2665	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2666	adjAnable = true;
M_souta	26:4c0ce2f05688	2667	break;
M_souta	26:4c0ce2f05688	2668	case 'N':
M_souta	26:4c0ce2f05688	2669	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2670	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2671	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2672	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2673	adjAnable = false;
M_souta	26:4c0ce2f05688	2674	break;
M_souta	26:4c0ce2f05688	2675	default:
M_souta	26:4c0ce2f05688	2676	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2677	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2678	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2679	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2680	adjAnable = false;
M_souta	26:4c0ce2f05688	2681	}
M_souta	26:4c0ce2f05688	2682
M_souta	26:4c0ce2f05688	2683	if(adjAnable){
M_souta	26:4c0ce2f05688	2684	if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
M_souta	26:4c0ce2f05688	2685	} else {
M_souta	26:4c0ce2f05688	2686	adj = 0;
M_souta	26:4c0ce2f05688	2687	}
M_souta	26:4c0ce2f05688	2688
M_souta	26:4c0ce2f05688	2689	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	26:4c0ce2f05688	2690	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	26:4c0ce2f05688	2691	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	26:4c0ce2f05688	2692	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	26:4c0ce2f05688	2693	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	26:4c0ce2f05688	2694	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	26:4c0ce2f05688	2695	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	26:4c0ce2f05688	2696	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	26:4c0ce2f05688	2697
M_souta	26:4c0ce2f05688	2698	if(lineCheck == true) {
M_souta	26:4c0ce2f05688	2699	lineCount++;
M_souta	26:4c0ce2f05688	2700	if(lineCount > 20) lineCheck = false;
M_souta	26:4c0ce2f05688	2701	}
M_souta	26:4c0ce2f05688	2702
M_souta	26:4c0ce2f05688	2703	/*
M_souta	26:4c0ce2f05688	2704	if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
M_souta	26:4c0ce2f05688	2705	targetCount = 3;
M_souta	26:4c0ce2f05688	2706	} else if(LimitSw::IsPressed(TOWEL1_SW)) {
M_souta	26:4c0ce2f05688	2707	targetCount = 3;
M_souta	26:4c0ce2f05688	2708	} else {
M_souta	26:4c0ce2f05688	2709	targetCount = 2;
M_souta	26:4c0ce2f05688	2710	}
M_souta	26:4c0ce2f05688	2711	*/
M_souta	26:4c0ce2f05688	2712
M_souta	26:4c0ce2f05688	2713	targetCount = 3;
M_souta	26:4c0ce2f05688	2714
M_souta	26:4c0ce2f05688	2715	if(countW == targetCount) {
M_souta	26:4c0ce2f05688	2716	countW = 0;
M_souta	26:4c0ce2f05688	2717	lineFase = 20;
M_souta	26:4c0ce2f05688	2718	lineCount = 0;
M_souta	26:4c0ce2f05688	2719	lineCheck = false;
M_souta	26:4c0ce2f05688	2720	}
M_souta	26:4c0ce2f05688	2721	} else if(lineFase == 20) { // 右低速
M_souta	26:4c0ce2f05688	2722	motor[TIRE_FL].dir = FOR;
M_souta	26:4c0ce2f05688	2723	motor[TIRE_BL].dir = BACK;
M_souta	26:4c0ce2f05688	2724	motor[TIRE_BR].dir = BACK;
M_souta	26:4c0ce2f05688	2725	motor[TIRE_FR].dir = FOR;
M_souta	26:4c0ce2f05688	2726	if (linePara[2] == 0) {
M_souta	26:4c0ce2f05688	2727	//(LimitSw::IsPressed(SHEETS_SW)) {
M_souta	26:4c0ce2f05688	2728	//ineFase = 20;
M_souta	26:4c0ce2f05688	2729	//else if(LimitSw::IsPressed(TOWEL2_SW) {
M_souta	26:4c0ce2f05688	2730	//ineFase = 14;
M_souta	26:4c0ce2f05688	2731	//else {
M_souta	26:4c0ce2f05688	2732	lineFase = 21;
M_souta	26:4c0ce2f05688	2733	//
M_souta	26:4c0ce2f05688	2734	motor[TIRE_FL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2735	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2736	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2737	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2738	}
M_souta	26:4c0ce2f05688	2739	motor[TIRE_FL].pwm = 16;
M_souta	26:4c0ce2f05688	2740	motor[TIRE_BL].pwm = 16;
M_souta	26:4c0ce2f05688	2741	motor[TIRE_BR].pwm = 16;
M_souta	26:4c0ce2f05688	2742	motor[TIRE_FR].pwm = 16;
M_souta	26:4c0ce2f05688	2743	} else if(lineFase == 21) {
M_souta	26:4c0ce2f05688	2744	switch(linePara[2]) {
M_souta	26:4c0ce2f05688	2745	case -2:
M_souta	26:4c0ce2f05688	2746	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2747	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2748	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2749	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2750	adjAnable = true;
M_souta	26:4c0ce2f05688	2751	break;
M_souta	26:4c0ce2f05688	2752	case -3:
M_souta	26:4c0ce2f05688	2753	tirePWM[TIRE_FL] = -10;
M_souta	26:4c0ce2f05688	2754	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2755	tirePWM[TIRE_BR] = 10;
M_souta	26:4c0ce2f05688	2756	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2757	adjAnable = true;
M_souta	26:4c0ce2f05688	2758	break;
M_souta	26:4c0ce2f05688	2759	case -1:
M_souta	26:4c0ce2f05688	2760	tirePWM[TIRE_FL] = -20;
M_souta	26:4c0ce2f05688	2761	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2762	tirePWM[TIRE_BR] = 20;
M_souta	26:4c0ce2f05688	2763	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2764	adjAnable = true;
M_souta	26:4c0ce2f05688	2765	break;
M_souta	26:4c0ce2f05688	2766	case 0:
M_souta	26:4c0ce2f05688	2767	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	2768	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2769	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	2770	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2771	adjAnable = true;
M_souta	26:4c0ce2f05688	2772	break;
M_souta	26:4c0ce2f05688	2773	case 1:
M_souta	26:4c0ce2f05688	2774	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	2775	tirePWM[TIRE_BL] = -20;
M_souta	26:4c0ce2f05688	2776	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	2777	tirePWM[TIRE_FR] = 20;
M_souta	26:4c0ce2f05688	2778	adjAnable = true;
M_souta	26:4c0ce2f05688	2779	break;
M_souta	26:4c0ce2f05688	2780	case 3:
M_souta	26:4c0ce2f05688	2781	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	2782	tirePWM[TIRE_BL] = -10;
M_souta	26:4c0ce2f05688	2783	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	2784	tirePWM[TIRE_FR] = 10;
M_souta	26:4c0ce2f05688	2785	adjAnable = true;
M_souta	26:4c0ce2f05688	2786	break;
M_souta	26:4c0ce2f05688	2787	case 2:
M_souta	26:4c0ce2f05688	2788	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	2789	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2790	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	2791	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2792	adjAnable = true;
M_souta	26:4c0ce2f05688	2793	break;
M_souta	26:4c0ce2f05688	2794	case 'A':
M_souta	26:4c0ce2f05688	2795	if(lineCheck == false) {
M_souta	26:4c0ce2f05688	2796	lineCheck = true;
M_souta	26:4c0ce2f05688	2797	lineCount = 0;
M_souta	26:4c0ce2f05688	2798	countW++;
M_souta	26:4c0ce2f05688	2799	}
M_souta	26:4c0ce2f05688	2800	tirePWM[TIRE_FL] = -30;
M_souta	26:4c0ce2f05688	2801	tirePWM[TIRE_BL] = -30;
M_souta	26:4c0ce2f05688	2802	tirePWM[TIRE_BR] = 30;
M_souta	26:4c0ce2f05688	2803	tirePWM[TIRE_FR] = 30;
M_souta	26:4c0ce2f05688	2804	adjAnable = true;
M_souta	26:4c0ce2f05688	2805	break;
M_souta	26:4c0ce2f05688	2806	case 'N':
M_souta	26:4c0ce2f05688	2807	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2808	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2809	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2810	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2811	adjAnable = false;
M_souta	26:4c0ce2f05688	2812	break;
M_souta	26:4c0ce2f05688	2813	default:
M_souta	26:4c0ce2f05688	2814	tirePWM[TIRE_FL] = 0;
M_souta	26:4c0ce2f05688	2815	tirePWM[TIRE_BL] = 0;
M_souta	26:4c0ce2f05688	2816	tirePWM[TIRE_BR] = 0;
M_souta	26:4c0ce2f05688	2817	tirePWM[TIRE_FR] = 0;
M_souta	26:4c0ce2f05688	2818	adjAnable = false;
M_souta	26:4c0ce2f05688	2819	}
M_souta	26:4c0ce2f05688	2820
M_souta	26:4c0ce2f05688	2821	if(adjAnable){
M_souta	26:4c0ce2f05688	2822	if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
M_souta	26:4c0ce2f05688	2823	} else {
M_souta	26:4c0ce2f05688	2824	adj = 0;
M_souta	26:4c0ce2f05688	2825	}
M_souta	26:4c0ce2f05688	2826
M_souta	26:4c0ce2f05688	2827	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	26:4c0ce2f05688	2828	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
M_souta	26:4c0ce2f05688	2829	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	26:4c0ce2f05688	2830	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
M_souta	26:4c0ce2f05688	2831	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	26:4c0ce2f05688	2832	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
M_souta	26:4c0ce2f05688	2833	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	26:4c0ce2f05688	2834	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
M_souta	26:4c0ce2f05688	2835
M_souta	26:4c0ce2f05688	2836	if(lineCheck == true) {
M_souta	26:4c0ce2f05688	2837	lineCount++;
M_souta	26:4c0ce2f05688	2838	if(lineCount > 20) lineCheck = false;
M_souta	26:4c0ce2f05688	2839	}
M_souta	26:4c0ce2f05688	2840	if(countW == 2) {
M_souta	26:4c0ce2f05688	2841	countW = 0;
M_souta	26:4c0ce2f05688	2842	lineFase = 22;
M_souta	26:4c0ce2f05688	2843	lineCount = 0;
M_souta	26:4c0ce2f05688	2844	lineCheck = false;
M_souta	26:4c0ce2f05688	2845	}
M_souta	26:4c0ce2f05688	2846	} else if(lineFase == 22) {
M_souta	26:4c0ce2f05688	2847	motor[TIRE_FL].dir = BACK;
M_souta	26:4c0ce2f05688	2848	motor[TIRE_BL].dir = BACK;
M_souta	26:4c0ce2f05688	2849	motor[TIRE_BR].dir = FOR;
M_souta	26:4c0ce2f05688	2850	motor[TIRE_FR].dir = FOR;
M_souta	26:4c0ce2f05688	2851	if (linePara[0] == 'N') {
M_souta	26:4c0ce2f05688	2852	motor[TIRE_FL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2853	motor[TIRE_BL].dir = BRAKE;
M_souta	26:4c0ce2f05688	2854	motor[TIRE_BR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2855	motor[TIRE_FR].dir = BRAKE;
M_souta	26:4c0ce2f05688	2856	}
M_souta	26:4c0ce2f05688	2857	motor[TIRE_FL].pwm = 30;
M_souta	26:4c0ce2f05688	2858	motor[TIRE_BL].pwm = 30;
M_souta	26:4c0ce2f05688	2859	motor[TIRE_BR].pwm = 30;
M_souta	26:4c0ce2f05688	2860	motor[TIRE_FR].pwm = 30;
M_souta	26:4c0ce2f05688	2861	} else{
M_souta	25:b3a9f34b201d	2862	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2863	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2864	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2865	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2866	}
M_souta	25:b3a9f34b201d	2867
kishibekairohan	13:b6e02d6261d7	2868	}
kishibekairohan	13:b6e02d6261d7	2869	#endif
kishibekairohan	13:b6e02d6261d7	2870
kishibekairohan	13:b6e02d6261d7	2871	#if USE_PROCESS_NUM>6
kishibekairohan	13:b6e02d6261d7	2872	static void Process6()
kishibekairohan	13:b6e02d6261d7	2873	{
M_souta	25:b3a9f34b201d	2874	for(int i = 0; i < 8; i++) {
M_souta	22:7d93f79a3686	2875	linePara[i] = lineCast(LineHub::GetPara(i));
M_souta	22:7d93f79a3686	2876	}
M_souta	22:7d93f79a3686	2877
M_souta	25:b3a9f34b201d	2878	if(lineFase == 0) {
M_souta	25:b3a9f34b201d	2879	if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	2880	lineFase = 1;
M_souta	25:b3a9f34b201d	2881	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2882	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	2883	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2884	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	2885	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2886	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	2887	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2888	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	2889	} else if(LimitSw::IsPressed(TOW_1L)) {
M_souta	25:b3a9f34b201d	2890	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	2891	motor[TIRE_FL].pwm = 12;
M_souta	25:b3a9f34b201d	2892	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2893	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	2894	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2895	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	2896	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	2897	motor[TIRE_FR].pwm = 12;
M_souta	25:b3a9f34b201d	2898	} else if(LimitSw::IsPressed(TOW_1R)) {
M_souta	25:b3a9f34b201d	2899	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	2900	motor[TIRE_FL].pwm = 12;
M_souta	25:b3a9f34b201d	2901	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2902	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	2903	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2904	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	2905	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	2906	motor[TIRE_FR].pwm = 12;
M_souta	25:b3a9f34b201d	2907	} else {
M_souta	25:b3a9f34b201d	2908	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	2909	motor[TIRE_FL].pwm = 18;
M_souta	25:b3a9f34b201d	2910	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	2911	motor[TIRE_BL].pwm = 18;
M_souta	25:b3a9f34b201d	2912	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	2913	motor[TIRE_BR].pwm = 18;
M_souta	25:b3a9f34b201d	2914	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	2915	motor[TIRE_FR].pwm = 18;
M_souta	25:b3a9f34b201d	2916	}
M_souta	25:b3a9f34b201d	2917	} else if(lineFase == 1) {
M_souta	25:b3a9f34b201d	2918	if(linePara[LINE_TOW_1] == 'A' \|\| linePara[LINE_TOW_1] == 'N') {
M_souta	25:b3a9f34b201d	2919	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2920	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	2921	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2922	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	2923	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2924	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	2925	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2926	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	2927	} else if(linePara[LINE_TOW_1] > 0) {
M_souta	25:b3a9f34b201d	2928	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	2929	motor[TIRE_FL].pwm = 15;
M_souta	25:b3a9f34b201d	2930	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	2931	motor[TIRE_BL].pwm = 15;
M_souta	25:b3a9f34b201d	2932	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	2933	motor[TIRE_BR].pwm = 15;
M_souta	25:b3a9f34b201d	2934	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	2935	motor[TIRE_FR].pwm = 15;
M_souta	25:b3a9f34b201d	2936	} else if(linePara[LINE_TOW_1] < 0) {
M_souta	25:b3a9f34b201d	2937	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	2938	motor[TIRE_FL].pwm = 15;
M_souta	25:b3a9f34b201d	2939	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	2940	motor[TIRE_BL].pwm = 15;
M_souta	25:b3a9f34b201d	2941	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	2942	motor[TIRE_BR].pwm = 15;
M_souta	25:b3a9f34b201d	2943	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	2944	motor[TIRE_FR].pwm = 15;
M_souta	25:b3a9f34b201d	2945	} else if(linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	2946	lineFase = 2;
M_souta	25:b3a9f34b201d	2947	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2948	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	2949	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2950	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	2951	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2952	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	2953	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2954	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	2955	} else {
M_souta	25:b3a9f34b201d	2956	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2957	motor[TIRE_FL].pwm = 30;
M_souta	25:b3a9f34b201d	2958	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	2959	motor[TIRE_BL].pwm = 30;
M_souta	25:b3a9f34b201d	2960	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2961	motor[TIRE_BR].pwm = 30;
M_souta	25:b3a9f34b201d	2962	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	2963	motor[TIRE_FR].pwm = 30;
M_souta	25:b3a9f34b201d	2964	}
M_souta	25:b3a9f34b201d	2965	} else if(lineFase == 2) {
M_souta	25:b3a9f34b201d	2966	//solenoid::solenoid1 = SOLENOID_ON
M_souta	25:b3a9f34b201d	2967
M_souta	22:7d93f79a3686	2968	}
kishibekairohan	13:b6e02d6261d7	2969	}
kishibekairohan	13:b6e02d6261d7	2970	#endif
kishibekairohan	13:b6e02d6261d7	2971
kishibekairohan	13:b6e02d6261d7	2972	#if USE_PROCESS_NUM>7
kishibekairohan	13:b6e02d6261d7	2973	static void Process7()
kishibekairohan	13:b6e02d6261d7	2974	{
M_souta	24:370616a56815	2975
kishibekairohan	12:c09b3e08a316	2976	}
kishibekairohan	12:c09b3e08a316	2977	#endif
kishibekairohan	12:c09b3e08a316	2978
kishibekairohan	16:3f2c2d89372b	2979	#if USE_PROCESS_NUM>8
kishibekairohan	13:b6e02d6261d7	2980	static void Process8()
kishibekairohan	12:c09b3e08a316	2981	{
M_souta	22:7d93f79a3686	2982	if(controller->Button.A) {
M_souta	22:7d93f79a3686	2983	rotaconSampling.start();
M_souta	22:7d93f79a3686	2984	PIDflag = true;
M_souta	22:7d93f79a3686	2985
M_souta	22:7d93f79a3686	2986	//linePara_U = LineHub::GetPara(0);
M_souta	22:7d93f79a3686	2987	//linePara_B = LineHub::GetPara(3);
M_souta	22:7d93f79a3686	2988
M_souta	22:7d93f79a3686	2989
M_souta	22:7d93f79a3686	2990	pulsePV[FL] = encoder[FL].getPulses();
M_souta	22:7d93f79a3686	2991	pulsePV[BL] = encoder[BL].getPulses();
M_souta	22:7d93f79a3686	2992	pulsePV[BR] = encoder[BR].getPulses();
M_souta	22:7d93f79a3686	2993	pulsePV[FR] = encoder[FR].getPulses();
M_souta	22:7d93f79a3686	2994
M_souta	22:7d93f79a3686	2995
M_souta	22:7d93f79a3686	2996	for (int i = 0; i < 4; i++) {
M_souta	22:7d93f79a3686	2997	timeCV[i] = timePV[i];
M_souta	22:7d93f79a3686	2998	timePV[i] = rotaconSampling.read();
M_souta	22:7d93f79a3686	2999	tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
M_souta	22:7d93f79a3686	3000	pulseCV[i] = pulsePV[i];
M_souta	22:7d93f79a3686	3001	}
M_souta	22:7d93f79a3686	3002
M_souta	22:7d93f79a3686	3003	move.Vx = 0.5;
M_souta	22:7d93f79a3686	3004	move.Vy = 0.5;
M_souta	22:7d93f79a3686	3005	move.Va = 0;
M_souta	22:7d93f79a3686	3006
M_souta	22:7d93f79a3686	3007	correction_LT.Vx = 0; //0.1 * linePara_U;
M_souta	22:7d93f79a3686	3008	correction_LT.Vy = 0;
M_souta	22:7d93f79a3686	3009	correction_LT.Va = 0;
M_souta	22:7d93f79a3686	3010
M_souta	22:7d93f79a3686	3011	synthetic.Vx = move.Vx + correction_LT.Vx;
M_souta	22:7d93f79a3686	3012	synthetic.Vy = move.Vy + correction_LT.Vy;
M_souta	22:7d93f79a3686	3013	synthetic.Va = move.Va + correction_LT.Va;
M_souta	22:7d93f79a3686	3014
M_souta	22:7d93f79a3686	3015	sita = 0;
M_souta	22:7d93f79a3686	3016
M_souta	22:7d93f79a3686	3017	//タイヤの目標速度算出
M_souta	22:7d93f79a3686	3018	float sinR = 0.7071 * (float)sin(sita);
M_souta	22:7d93f79a3686	3019	float cosR = 0.7071 * (float)cos(sita);
M_souta	22:7d93f79a3686	3020	float nv = (60 * 1000) / ( 2.00 * PI * tireR);
M_souta	22:7d93f79a3686	3021	tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta	22:7d93f79a3686	3022	tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta	22:7d93f79a3686	3023	tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
M_souta	22:7d93f79a3686	3024	tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
M_souta	22:7d93f79a3686	3025
M_souta	22:7d93f79a3686	3026	//pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
M_souta	22:7d93f79a3686	3027
M_souta	22:7d93f79a3686	3028	//PIDによるPWM算出
M_souta	22:7d93f79a3686	3029
M_souta	22:7d93f79a3686	3030	//モータの駆動
M_souta	22:7d93f79a3686	3031	for (int i = 0; i < 4; i++) {
M_souta	22:7d93f79a3686	3032	if (tirePWM[i] > 255){
M_souta	22:7d93f79a3686	3033	tirePWM[i] = 255;
M_souta	22:7d93f79a3686	3034	} else if (tirePWM[i] < -255) {
M_souta	22:7d93f79a3686	3035	tirePWM[i] = -255;
M_souta	22:7d93f79a3686	3036	}
M_souta	22:7d93f79a3686	3037	}
M_souta	22:7d93f79a3686	3038
M_souta	22:7d93f79a3686	3039	for(int i = 0;i < 4;i++){
M_souta	22:7d93f79a3686	3040	motor[i].dir = SetStatus(tirePWM[i]);
M_souta	22:7d93f79a3686	3041	motor[i].pwm = SetPWM(tirePWM[i]);
M_souta	22:7d93f79a3686	3042	}
M_souta	22:7d93f79a3686	3043	} else {
M_souta	22:7d93f79a3686	3044	PIDflag = false;
M_souta	22:7d93f79a3686	3045	rotaconSampling.stop();
M_souta	22:7d93f79a3686	3046	rotaconSampling.reset();
M_souta	22:7d93f79a3686	3047	for(int i = 0;i < 4;i++){
M_souta	22:7d93f79a3686	3048	encoder[i].reset();
M_souta	22:7d93f79a3686	3049	pulsePV[i] = 0;
M_souta	22:7d93f79a3686	3050	pulseCV[i] = 0;
M_souta	22:7d93f79a3686	3051	timePV[i] = 0;
M_souta	22:7d93f79a3686	3052	timeCV[i] = 0;
M_souta	22:7d93f79a3686	3053	tirePWM[i] = 0;
M_souta	22:7d93f79a3686	3054	motor[i].dir = SetStatus(tirePWM[i]);
M_souta	22:7d93f79a3686	3055	motor[i].pwm = SetPWM(tirePWM[i]);
M_souta	22:7d93f79a3686	3056	}
M_souta	22:7d93f79a3686	3057	}
kishibekairohan	12:c09b3e08a316	3058	}
kishibekairohan	12:c09b3e08a316	3059	#endif
kishibekairohan	12:c09b3e08a316	3060
kishibekairohan	13:b6e02d6261d7	3061	#if USE_PROCESS_NUM>9
kishibekairohan	13:b6e02d6261d7	3062	static void Process9()
kishibekairohan	12:c09b3e08a316	3063	{
kishibekairohan	2:c015739085d3	3064
t_yamamoto	0:669ef71cba68	3065	}
t_yamamoto	0:669ef71cba68	3066	#endif
t_yamamoto	0:669ef71cba68	3067	#endif
t_yamamoto	0:669ef71cba68	3068	#pragma endregion PROCESS
t_yamamoto	0:669ef71cba68	3069
t_yamamoto	0:669ef71cba68	3070	static void AllActuatorReset()
t_yamamoto	0:669ef71cba68	3071	{
t_yamamoto	0:669ef71cba68	3072
t_yamamoto	0:669ef71cba68	3073	#ifdef USE_SOLENOID
t_yamamoto	0:669ef71cba68	3074	solenoid.all = ALL_SOLENOID_OFF;
t_yamamoto	0:669ef71cba68	3075	#endif
t_yamamoto	0:669ef71cba68	3076
t_yamamoto	0:669ef71cba68	3077	#ifdef USE_MOTOR
t_yamamoto	0:669ef71cba68	3078	for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++)
t_yamamoto	0:669ef71cba68	3079	{
t_yamamoto	0:669ef71cba68	3080	motor[i].dir = FREE;
t_yamamoto	0:669ef71cba68	3081	motor[i].pwm = 0;
t_yamamoto	0:669ef71cba68	3082	}
t_yamamoto	0:669ef71cba68	3083	#endif
t_yamamoto	0:669ef71cba68	3084	}
t_yamamoto	0:669ef71cba68	3085
kishibekairohan	16:3f2c2d89372b	3086	void BuzzerTimer_func(){
kishibekairohan	16:3f2c2d89372b	3087	buzzer = !buzzer;
kishibekairohan	17:50dc4b449e69	3088	//LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan	11:028a150943b5	3089	}
kishibekairohan	11:028a150943b5	3090
kishibekairohan	11:028a150943b5	3091	void TapeLedEms_func() {
kishibekairohan	16:3f2c2d89372b	3092	sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan	9:f93fc79a49ea	3093	}
kishibekairohan	16:3f2c2d89372b	3094
kishibekairohan	16:3f2c2d89372b	3095	#pragma region USER-DEFINED-FUNCTIONS
M_souta	22:7d93f79a3686	3096	void tirePID() {
M_souta	22:7d93f79a3686	3097	if(PIDflag == true) {
M_souta	22:7d93f79a3686	3098	//加算するPID値の算出
M_souta	22:7d93f79a3686	3099	rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
M_souta	22:7d93f79a3686	3100	rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
M_souta	22:7d93f79a3686	3101	rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
M_souta	22:7d93f79a3686	3102	rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
M_souta	22:7d93f79a3686	3103	//PID値の加算
M_souta	22:7d93f79a3686	3104	tirePWM[FL] += rotaconPID[0].GetMV();
M_souta	22:7d93f79a3686	3105	tirePWM[BL] += rotaconPID[1].GetMV();
M_souta	22:7d93f79a3686	3106	tirePWM[FR] += rotaconPID[2].GetMV();
M_souta	22:7d93f79a3686	3107	tirePWM[BR] += rotaconPID[3].GetMV();
M_souta	22:7d93f79a3686	3108	}
M_souta	22:7d93f79a3686	3109	}
kishibekairohan	16:3f2c2d89372b	3110
M_souta	22:7d93f79a3686	3111	int lineCast(char k) {
M_souta	22:7d93f79a3686	3112	int l;
M_souta	22:7d93f79a3686	3113	switch(k) {
M_souta	22:7d93f79a3686	3114	case 255:
M_souta	22:7d93f79a3686	3115	l = -1;
M_souta	22:7d93f79a3686	3116	break;
M_souta	22:7d93f79a3686	3117	case 254:
M_souta	22:7d93f79a3686	3118	l = -2;
M_souta	22:7d93f79a3686	3119	break;
M_souta	22:7d93f79a3686	3120	case 253:
M_souta	22:7d93f79a3686	3121	l = -3;
M_souta	22:7d93f79a3686	3122	break;
M_souta	22:7d93f79a3686	3123	default:
M_souta	22:7d93f79a3686	3124	l = k;
M_souta	22:7d93f79a3686	3125	}
M_souta	22:7d93f79a3686	3126	return l;
M_souta	22:7d93f79a3686	3127	}
kishibekairohan	16:3f2c2d89372b	3128
t_yamamoto	0:669ef71cba68	3129	#pragma endregion

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	02 Oct 2019
Imports:	1
Forks:	0
Commits:	28
Dependents:	0
Dependencies:	5
Followers:	1

System/Process/Process.cpp@26:4c0ce2f05688, 2019-09-28 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning