Main_2019_B_Auto_TAMAYA - The last version programs

Users » Ryosei » Code » Main_2019_B_Auto_TAMAYA

Miya Miyagawa / Mbed 2 deprecated Main_2019_B_Auto_TAMAYA

The last version programs

Dependencies: mbed TrapezoidControl Pulse QEI

System/Process/Process.cpp@28:479631c2de29, 2019-10-03 (annotated)

Committer:: Ryosei
Date:: Thu Oct 03 05:33:48 2019 +0000
Revision:: 28:479631c2de29
Parent:: 27:dd9f27fce7d1
Child:: 29:5365ee0521f6

The Last version

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"
Ryosei	27:dd9f27fce7d1	3	#include "Pulse.h"
Ryosei	27:dd9f27fce7d1	4	#include <stdlib.h>
7ka884	4:ba9df71868df	5	#include "../../CommonLibraries/PID/PID.h"
t_yamamoto	0:669ef71cba68	6	#include "../../Communication/RS485/ActuatorHub/ActuatorHub.h"
M_souta	21:e3b58d675c1c	7	#include "../../Communication/RS485/LineHub/LineHub.h"
t_yamamoto	0:669ef71cba68	8	#include "../../Communication/Controller/Controller.h"
t_yamamoto	0:669ef71cba68	9	#include "../../Input/ExternalInt/ExternalInt.h"
t_yamamoto	0:669ef71cba68	10	#include "../../Input/Switch/Switch.h"
kishibekairohan	16:3f2c2d89372b	11	#include "../../Input/Encoder/Encoder.h"
Ryosei	27:dd9f27fce7d1	12
t_yamamoto	0:669ef71cba68	13	#include "../../LED/LED.h"
t_yamamoto	0:669ef71cba68	14	#include "../../Safty/Safty.h"
t_yamamoto	0:669ef71cba68	15	#include "../Using.h"
t_yamamoto	0:669ef71cba68	16
t_yamamoto	0:669ef71cba68	17	using namespace SWITCH;
7ka884	4:ba9df71868df	18	using namespace PID_SPACE;
kishibekairohan	16:3f2c2d89372b	19	using namespace ENCODER;
Ryosei	27:dd9f27fce7d1	20
M_souta	21:e3b58d675c1c	21	using namespace LINEHUB;
t_yamamoto	0:669ef71cba68	22
M_souta	22:7d93f79a3686	23
t_yamamoto	0:669ef71cba68	24	static CONTROLLER::ControllerData *controller;
t_yamamoto	0:669ef71cba68	25	ACTUATORHUB::MOTOR::MotorStatus motor[MOUNTING_MOTOR_NUM];
t_yamamoto	0:669ef71cba68	26	ACTUATORHUB::SOLENOID::SolenoidStatus solenoid;
t_yamamoto	0:669ef71cba68	27
t_yamamoto	0:669ef71cba68	28	static bool lock;
t_yamamoto	0:669ef71cba68	29	static bool processChangeComp;
t_yamamoto	0:669ef71cba68	30	static int current;
t_yamamoto	0:669ef71cba68	31
t_yamamoto	0:669ef71cba68	32	static void AllActuatorReset();
t_yamamoto	0:669ef71cba68	33
t_yamamoto	0:669ef71cba68	34	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	35	static void (*Process[USE_PROCESS_NUM])(void);
t_yamamoto	0:669ef71cba68	36	#endif
t_yamamoto	0:669ef71cba68	37
t_yamamoto	0:669ef71cba68	38	#pragma region USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto	0:669ef71cba68	39
t_yamamoto	0:669ef71cba68	40	/Replace here with the definition code of your variables./
t_yamamoto	0:669ef71cba68	41
kishibekairohan	2:c015739085d3	42
M_souta	22:7d93f79a3686	43	Serial pc(USBTX, USBRX);
kishibekairohan	7:e88c5d47a3be	44
kishibekairohan	16:3f2c2d89372b	45	//************Buzzer**************
kishibekairohan	11:028a150943b5	46	//DigitalOut buzzer(BUZZER_PIN);
kishibekairohan	9:f93fc79a49ea	47	void BuzzerTimer_func();
kishibekairohan	9:f93fc79a49ea	48	Ticker BuzzerTimer;
kishibekairohan	16:3f2c2d89372b	49	bool EMGflag = false;
kishibekairohan	16:3f2c2d89372b	50	PwmOut buzzer(BUZZER_PIN);
kishibekairohan	16:3f2c2d89372b	51	//************Buzzer**************
kishibekairohan	11:028a150943b5	52
kishibekairohan	11:028a150943b5	53	//**********TapeLed***************
kishibekairohan	11:028a150943b5	54	void TapeLedEms_func();
kishibekairohan	11:028a150943b5	55	TapeLedData tapeLED;
kishibekairohan	11:028a150943b5	56	TapeLedData sendLedData;
kishibekairohan	11:028a150943b5	57	TapeLED_Mode ledMode = Normal;
kishibekairohan	11:028a150943b5	58	Ticker tapeLedTimer;
kishibekairohan	11:028a150943b5	59	//**********TapaLed***************
kishibekairohan	16:3f2c2d89372b	60
M_souta	22:7d93f79a3686	61	//************* lift *************
M_souta	22:7d93f79a3686	62	#define LOWER 1
M_souta	22:7d93f79a3686	63	#define MIDDLRE 2
M_souta	22:7d93f79a3686	64	#define UPPER 3
M_souta	22:7d93f79a3686	65	uint8_t liftState = LOWER;
M_souta	22:7d93f79a3686	66	bool moving = false;
M_souta	22:7d93f79a3686	67	bool switchFlag_LB = false;
M_souta	22:7d93f79a3686	68	bool switchFlag_RB = false;
M_souta	22:7d93f79a3686	69
M_souta	22:7d93f79a3686	70	//************* lift *************
M_souta	22:7d93f79a3686	71
Ryosei	27:dd9f27fce7d1	72	//***************Air******************
Ryosei	27:dd9f27fce7d1	73	DigitalOut air[]= {
Ryosei	27:dd9f27fce7d1	74	DigitalOut(ECD_A_0),
Ryosei	27:dd9f27fce7d1	75	DigitalOut(ECD_B_0),
Ryosei	27:dd9f27fce7d1	76	DigitalOut(ECD_A_1),
Ryosei	27:dd9f27fce7d1	77	DigitalOut(ECD_B_1),
Ryosei	27:dd9f27fce7d1	78	};
Ryosei	27:dd9f27fce7d1	79
Ryosei	27:dd9f27fce7d1	80	bool Air[4];
Ryosei	27:dd9f27fce7d1	81	void AirUpdate()
Ryosei	27:dd9f27fce7d1	82	{
Ryosei	27:dd9f27fce7d1	83	for(int i=0; i<=3; i++) {
Ryosei	27:dd9f27fce7d1	84	air[i]=Air[i];
Ryosei	27:dd9f27fce7d1	85	}
Ryosei	27:dd9f27fce7d1	86	}
Ryosei	27:dd9f27fce7d1	87	//***************Air******************
Ryosei	27:dd9f27fce7d1	88
M_souta	22:7d93f79a3686	89	//***********tire***********
M_souta	22:7d93f79a3686	90	PID rotaconPID[] = {
Ryosei	27:dd9f27fce7d1	91	PID(0.0001,-1,1,0.05,0,0), //LF
Ryosei	27:dd9f27fce7d1	92	PID(0.0001,-1,1,0.05,0,0), //LB
Ryosei	27:dd9f27fce7d1	93	PID(0.0001,-1,1,0.05,0,0), //RB
Ryosei	27:dd9f27fce7d1	94	PID(0.0001,-1,1,0.05,0,0), //RF
M_souta	22:7d93f79a3686	95	};
M_souta	22:7d93f79a3686	96
M_souta	22:7d93f79a3686	97	#define FL 0
M_souta	22:7d93f79a3686	98	#define BL 1
M_souta	22:7d93f79a3686	99	#define BR 2
M_souta	22:7d93f79a3686	100	#define FR 3
M_souta	22:7d93f79a3686	101
M_souta	22:7d93f79a3686	102	#define PI 3.141592
M_souta	22:7d93f79a3686	103
M_souta	22:7d93f79a3686	104	const float tireR = 101.6; //タイヤの半径
M_souta	22:7d93f79a3686	105	const float ucR = 420.0; //中心からのタイヤの距離
M_souta	22:7d93f79a3686	106
M_souta	22:7d93f79a3686	107	typedef struct {
Ryosei	27:dd9f27fce7d1	108	float Vx; //X方向の速度
Ryosei	27:dd9f27fce7d1	109	float Vy; //Y方向の速度
Ryosei	27:dd9f27fce7d1	110	float Va; //角速度
M_souta	22:7d93f79a3686	111	} Vvector;
M_souta	22:7d93f79a3686	112
M_souta	22:7d93f79a3686	113	Vvector move; //進む速度
M_souta	22:7d93f79a3686	114	Vvector correction_LT; //ライントレースの補正速度
M_souta	22:7d93f79a3686	115	Vvector synthetic; //合成速度
M_souta	22:7d93f79a3686	116
M_souta	22:7d93f79a3686	117	float sita = 0;
M_souta	22:7d93f79a3686	118
M_souta	22:7d93f79a3686	119	bool PIDflag = false;
M_souta	22:7d93f79a3686	120
M_souta	22:7d93f79a3686	121	int linePara[8];
M_souta	22:7d93f79a3686	122	int linePara_U;
M_souta	22:7d93f79a3686	123	int linePara_B;
M_souta	22:7d93f79a3686	124	int linePara_L;
M_souta	22:7d93f79a3686	125	int linePara_R;
M_souta	22:7d93f79a3686	126
M_souta	22:7d93f79a3686	127	#define FL 0
M_souta	22:7d93f79a3686	128	#define BL 1
M_souta	22:7d93f79a3686	129	#define BR 2
M_souta	22:7d93f79a3686	130	#define FR 3
M_souta	22:7d93f79a3686	131
M_souta	22:7d93f79a3686	132	float tireProcessRPM[4];
M_souta	22:7d93f79a3686	133	float tireTargetMaxRPM[4];
M_souta	22:7d93f79a3686	134	float tireTargetRPM[4];
M_souta	22:7d93f79a3686	135
M_souta	22:7d93f79a3686	136	float tirePWM[4];
M_souta	22:7d93f79a3686	137
M_souta	22:7d93f79a3686	138	float timePV[4];
M_souta	22:7d93f79a3686	139	float timeCV[4];
M_souta	22:7d93f79a3686	140	float pulsePV[4];
M_souta	22:7d93f79a3686	141	float pulseCV[4];
M_souta	22:7d93f79a3686	142
M_souta	22:7d93f79a3686	143	void tirePID();
M_souta	22:7d93f79a3686	144	int lineCast(char k);
M_souta	22:7d93f79a3686	145
M_souta	22:7d93f79a3686	146	Timer rotaconSampling;
M_souta	22:7d93f79a3686	147	Ticker rotaconPIDtimer;
M_souta	22:7d93f79a3686	148
M_souta	22:7d93f79a3686	149	bool countFlag;
M_souta	22:7d93f79a3686	150	//***********tire************//
M_souta	22:7d93f79a3686	151
Ryosei	27:dd9f27fce7d1	152	// *********** Ultra ************ //
Ryosei	27:dd9f27fce7d1	153	//double temp=( Temp.read()* 3.3 - 0.6) / 0.01;
Ryosei	27:dd9f27fce7d1	154	PulseInOut Echo0(ECHO_0);
Ryosei	27:dd9f27fce7d1	155	PulseInOut Echo1(ECHO_1);
Ryosei	27:dd9f27fce7d1	156	PulseInOut Trig0(TRIG_0);
Ryosei	27:dd9f27fce7d1	157	PulseInOut Trig1(TRIG_1);
Ryosei	27:dd9f27fce7d1	158	double UltraRead(int num)
Ryosei	27:dd9f27fce7d1	159	{
Ryosei	27:dd9f27fce7d1	160	double Distance=0;
Ryosei	27:dd9f27fce7d1	161	double Duration=0;
Ryosei	27:dd9f27fce7d1	162	double temp=28;////////////////////////////////////////////////温度
Ryosei	27:dd9f27fce7d1	163	if(num==0) {
Ryosei	27:dd9f27fce7d1	164	Trig0.write_us(1,10);
Ryosei	27:dd9f27fce7d1	165	Duration=Echo0.read_high_us(5000);
Ryosei	27:dd9f27fce7d1	166	} else if(num==1) {
Ryosei	27:dd9f27fce7d1	167	Trig1.write_us(1,10);
Ryosei	27:dd9f27fce7d1	168	Duration=Echo1.read_high_us(5000);
Ryosei	27:dd9f27fce7d1	169	}
Ryosei	27:dd9f27fce7d1	170	if(Duration>0) {
Ryosei	27:dd9f27fce7d1	171	Duration=Duration/2;
Ryosei	27:dd9f27fce7d1	172	double sspead=331.5+0.6*temp;
Ryosei	27:dd9f27fce7d1	173	Distance=Durationsspead100/1000000;
Ryosei	27:dd9f27fce7d1	174	} else {
Ryosei	27:dd9f27fce7d1	175	return 0;
Ryosei	27:dd9f27fce7d1	176	}
Ryosei	27:dd9f27fce7d1	177	return Distance;
Ryosei	27:dd9f27fce7d1	178	}
Ryosei	27:dd9f27fce7d1	179	//*******Ultra*************
Ryosei	27:dd9f27fce7d1	180
Ryosei	27:dd9f27fce7d1	181	//*******Tape LED************************
Ryosei	27:dd9f27fce7d1	182	DigitalOut select1(SELECT_1);
Ryosei	27:dd9f27fce7d1	183	DigitalOut select2(SELECT_2);
Ryosei	27:dd9f27fce7d1	184	DigitalOut select3(SELECT_3);
Ryosei	27:dd9f27fce7d1	185	void LedOut(int num)
Ryosei	27:dd9f27fce7d1	186	{
Ryosei	27:dd9f27fce7d1	187	int selectnum[8][3]= {
Ryosei	27:dd9f27fce7d1	188	{0,0,0},
Ryosei	27:dd9f27fce7d1	189	{0,0,1},
Ryosei	27:dd9f27fce7d1	190	{0,1,0},
Ryosei	27:dd9f27fce7d1	191	{0,1,1},
Ryosei	27:dd9f27fce7d1	192	{1,0,0},
Ryosei	27:dd9f27fce7d1	193	{1,0,1},
Ryosei	27:dd9f27fce7d1	194	{1,1,0},
Ryosei	27:dd9f27fce7d1	195	{1,1,1}
Ryosei	27:dd9f27fce7d1	196	};
Ryosei	27:dd9f27fce7d1	197	select1=selectnum[num][0];
Ryosei	27:dd9f27fce7d1	198	select2=selectnum[num][1];
Ryosei	27:dd9f27fce7d1	199	select3=selectnum[num][2];
Ryosei	27:dd9f27fce7d1	200	}
Ryosei	27:dd9f27fce7d1	201	//*******Tape LED************************
Ryosei	27:dd9f27fce7d1	202
M_souta	22:7d93f79a3686	203	// *********** Line ************ //
M_souta	22:7d93f79a3686	204
M_souta	26:4c0ce2f05688	205	Timer tow_stop;
M_souta	26:4c0ce2f05688	206
M_souta	22:7d93f79a3686	207	float pw = 0;
M_souta	22:7d93f79a3686	208	int lineFase = 0;
M_souta	22:7d93f79a3686	209	bool lineCheck = false;
M_souta	25:b3a9f34b201d	210	bool lineFlag = false;
M_souta	25:b3a9f34b201d	211	bool adjAnable = false;
M_souta	25:b3a9f34b201d	212	int adj = 0;
M_souta	25:b3a9f34b201d	213
M_souta	26:4c0ce2f05688	214	int countW = 0;
M_souta	25:b3a9f34b201d	215	int lineCount = 0;
M_souta	25:b3a9f34b201d	216	int targetCount = 0;
M_souta	25:b3a9f34b201d	217
M_souta	25:b3a9f34b201d	218	bool startFlag = true;
M_souta	25:b3a9f34b201d	219
M_souta	22:7d93f79a3686	220	int linePWM;
M_souta	22:7d93f79a3686	221	int adj_F;
M_souta	22:7d93f79a3686	222	int adj_B;
M_souta	22:7d93f79a3686	223
M_souta	22:7d93f79a3686	224	int mode = 0;
M_souta	22:7d93f79a3686	225
M_souta	22:7d93f79a3686	226	// *********** Line ************ //
M_souta	22:7d93f79a3686	227
Ryosei	27:dd9f27fce7d1	228	const int omni[15][15] = {
Ryosei	27:dd9f27fce7d1	229	{ 0, 5, 21, 47, 83, 130, 187, 255, 255, 255, 255, 255, 255, 255, 255 },
Ryosei	27:dd9f27fce7d1	230	{ -5, 0, 5, 21, 47, 83, 130, 187, 193, 208, 234, 255, 255, 255, 255 },
Ryosei	27:dd9f27fce7d1	231	{ -21, -5, 0, 5, 21, 47, 83, 130, 135, 151, 177, 213, 255, 255, 255 },
Ryosei	27:dd9f27fce7d1	232	{ -47, -21, 5, 0, 5, 21, 47, 83, 88, 104, 130, 167, 213, 255, 255 },
Ryosei	27:dd9f27fce7d1	233	{ -83, -47, -21, 5, 0, 5, 21, 47, 52, 68, 94, 130, 177, 234, 255 },
Ryosei	27:dd9f27fce7d1	234	{ -130, -83, -47, -21, 5, 0, 5, 21, 26, 42, 68, 104, 151, 208, 255 },
Ryosei	27:dd9f27fce7d1	235	{ -187, -130, -83, -47, -21, -5, 0, 5, 10, 26, 52, 88, 135, 193, 255 },
Ryosei	27:dd9f27fce7d1	236	{ -255, -187, -130, -83, -47, -21, -5, 0, 5, 21, 47, 83, 130, 187, 255 },
Ryosei	27:dd9f27fce7d1	237	{ -255, -193, -135, -88, -52, -26, -10, -5, 0, 5, 21, 47, 83, 130, 187 },
Ryosei	27:dd9f27fce7d1	238	{ -255, -208, -151, -104, -68, -42, -26, -21, -5, 0, 5, 21, 47, 83, 130 },
Ryosei	27:dd9f27fce7d1	239	{ -255, -234, -177, -130, -94, -68, -52, -47, -21, -7, 0, 7, 21, 47, 83 },
Ryosei	27:dd9f27fce7d1	240	{ -255, -255, -213, -167, -130, -104, -88, -83, -47, -21, -5, 0, 5, 21, 47 },
Ryosei	27:dd9f27fce7d1	241	{ -255, -255, -255, -213, -177, -151, -135, -130, -83, -47, -21, -5, 0, 5, 21 },
Ryosei	27:dd9f27fce7d1	242	{ -255, -255, -255, -255, -234, -208, -193, -187, -130, -83, -47, -21, -5, 0, 5 },
Ryosei	27:dd9f27fce7d1	243	{ -255, -255, -255, -255, -255, -255, -255, -255, -187, -130, -83, -47, -21, -5, 0 }
M_souta	21:e3b58d675c1c	244	};
M_souta	21:e3b58d675c1c	245
M_souta	21:e3b58d675c1c	246	const int curve[15] = { -204, -150, -104, -66, -38, -17, -4, 0, 4, 17, 38, 66, 104, 150, 204 };
M_souta	21:e3b58d675c1c	247
M_souta	21:e3b58d675c1c	248	uint8_t SetStatus(int);
Ryosei	27:dd9f27fce7d1	249	uint8_t SetStatus(int pwmVal)
Ryosei	27:dd9f27fce7d1	250	{
Ryosei	27:dd9f27fce7d1	251	if (pwmVal < 0) return BACK;
Ryosei	27:dd9f27fce7d1	252	else if (pwmVal > 0) return FOR;
Ryosei	27:dd9f27fce7d1	253	else if (pwmVal == 0) return BRAKE;
Ryosei	27:dd9f27fce7d1	254	else return BRAKE;
M_souta	20:eae8c84f318c	255	}
M_souta	21:e3b58d675c1c	256	uint8_t SetPWM(int);
Ryosei	27:dd9f27fce7d1	257	uint8_t SetPWM(int pwmVal)
Ryosei	27:dd9f27fce7d1	258	{
Ryosei	27:dd9f27fce7d1	259	if (pwmVal == 0 \|\| pwmVal > 255 \|\| pwmVal < -255) return 255;
Ryosei	27:dd9f27fce7d1	260	else return abs(pwmVal);
M_souta	21:e3b58d675c1c	261	}
M_souta	18:c694bae76e51	262
t_yamamoto	0:669ef71cba68	263	#pragma endregion USER-DEFINED_VARIABLES_AND_PROTOTYPE
t_yamamoto	0:669ef71cba68	264
t_yamamoto	0:669ef71cba68	265	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	266	#if USE_PROCESS_NUM>0
t_yamamoto	0:669ef71cba68	267	static void Process0(void);
t_yamamoto	0:669ef71cba68	268	#endif
t_yamamoto	0:669ef71cba68	269	#if USE_PROCESS_NUM>1
t_yamamoto	0:669ef71cba68	270	static void Process1(void);
t_yamamoto	0:669ef71cba68	271	#endif
t_yamamoto	0:669ef71cba68	272	#if USE_PROCESS_NUM>2
t_yamamoto	0:669ef71cba68	273	static void Process2(void);
t_yamamoto	0:669ef71cba68	274	#endif
t_yamamoto	0:669ef71cba68	275	#if USE_PROCESS_NUM>3
t_yamamoto	0:669ef71cba68	276	static void Process3(void);
t_yamamoto	0:669ef71cba68	277	#endif
t_yamamoto	0:669ef71cba68	278	#if USE_PROCESS_NUM>4
t_yamamoto	0:669ef71cba68	279	static void Process4(void);
t_yamamoto	0:669ef71cba68	280	#endif
t_yamamoto	0:669ef71cba68	281	#if USE_PROCESS_NUM>5
t_yamamoto	0:669ef71cba68	282	static void Process5(void);
t_yamamoto	0:669ef71cba68	283	#endif
t_yamamoto	0:669ef71cba68	284	#if USE_PROCESS_NUM>6
t_yamamoto	0:669ef71cba68	285	static void Process6(void);
t_yamamoto	0:669ef71cba68	286	#endif
t_yamamoto	0:669ef71cba68	287	#if USE_PROCESS_NUM>7
t_yamamoto	0:669ef71cba68	288	static void Process7(void);
t_yamamoto	0:669ef71cba68	289	#endif
t_yamamoto	0:669ef71cba68	290	#if USE_PROCESS_NUM>8
t_yamamoto	0:669ef71cba68	291	static void Process8(void);
t_yamamoto	0:669ef71cba68	292	#endif
t_yamamoto	0:669ef71cba68	293	#if USE_PROCESS_NUM>9
t_yamamoto	0:669ef71cba68	294	static void Process9(void);
t_yamamoto	0:669ef71cba68	295	#endif
t_yamamoto	0:669ef71cba68	296	#endif
t_yamamoto	0:669ef71cba68	297
t_yamamoto	0:669ef71cba68	298	void SystemProcessInitialize()
t_yamamoto	0:669ef71cba68	299	{
Ryosei	27:dd9f27fce7d1	300	#pragma region USER-DEFINED_VARIABLE_INIT
Ryosei	27:dd9f27fce7d1	301	/Replace here with the initialization code of your variables./
Ryosei	27:dd9f27fce7d1	302	//rotaconPIDtimer.attach(tirePID,0.1);
t_yamamoto	0:669ef71cba68	303
Ryosei	27:dd9f27fce7d1	304	//DigitalOut Air_16(LS_16);
Ryosei	27:dd9f27fce7d1	305	//DigitalOut Air_17(LS_17);
Ryosei	27:dd9f27fce7d1	306	//DigitalOut Air_18(LS_18);
Ryosei	27:dd9f27fce7d1	307	//DigitalOut Air_19(LS_19);
Ryosei	27:dd9f27fce7d1	308
Ryosei	27:dd9f27fce7d1	309	#pragma endregion USER-DEFINED_VARIABLE_INIT
Ryosei	27:dd9f27fce7d1	310
Ryosei	27:dd9f27fce7d1	311	lock = true;
Ryosei	27:dd9f27fce7d1	312	processChangeComp = true;
Ryosei	27:dd9f27fce7d1	313	current = DEFAULT_PROCESS;
t_yamamoto	0:669ef71cba68	314
Ryosei	27:dd9f27fce7d1	315	#ifdef USE_SUBPROCESS
Ryosei	27:dd9f27fce7d1	316	#if USE_PROCESS_NUM>0
Ryosei	27:dd9f27fce7d1	317	Process[0] = Process0;
Ryosei	27:dd9f27fce7d1	318	#endif
Ryosei	27:dd9f27fce7d1	319	#if USE_PROCESS_NUM>1
Ryosei	27:dd9f27fce7d1	320	Process[1] = Process1;
Ryosei	27:dd9f27fce7d1	321	#endif
Ryosei	27:dd9f27fce7d1	322	#if USE_PROCESS_NUM>2
Ryosei	27:dd9f27fce7d1	323	Process[2] = Process2;
Ryosei	27:dd9f27fce7d1	324	#endif
Ryosei	27:dd9f27fce7d1	325	#if USE_PROCESS_NUM>3
Ryosei	27:dd9f27fce7d1	326	Process[3] = Process3;
Ryosei	27:dd9f27fce7d1	327	#endif
Ryosei	27:dd9f27fce7d1	328	#if USE_PROCESS_NUM>4
Ryosei	27:dd9f27fce7d1	329	Process[4] = Process4;
Ryosei	27:dd9f27fce7d1	330	#endif
Ryosei	27:dd9f27fce7d1	331	#if USE_PROCESS_NUM>5
Ryosei	27:dd9f27fce7d1	332	Process[5] = Process5;
Ryosei	27:dd9f27fce7d1	333	#endif
Ryosei	27:dd9f27fce7d1	334	#if USE_PROCESS_NUM>6
Ryosei	27:dd9f27fce7d1	335	Process[6] = Process6;
Ryosei	27:dd9f27fce7d1	336	#endif
Ryosei	27:dd9f27fce7d1	337	#if USE_PROCESS_NUM>7
Ryosei	27:dd9f27fce7d1	338	Process[7] = Process7;
Ryosei	27:dd9f27fce7d1	339	#endif
Ryosei	27:dd9f27fce7d1	340	#if USE_PROCESS_NUM>8
Ryosei	27:dd9f27fce7d1	341	Process[8] = Process8;
Ryosei	27:dd9f27fce7d1	342	#endif
Ryosei	27:dd9f27fce7d1	343	#if USE_PROCESS_NUM>9
Ryosei	27:dd9f27fce7d1	344	Process[9] = Process9;
Ryosei	27:dd9f27fce7d1	345	#endif
Ryosei	27:dd9f27fce7d1	346	#endif
t_yamamoto	0:669ef71cba68	347	}
t_yamamoto	0:669ef71cba68	348
t_yamamoto	0:669ef71cba68	349	static void SystemProcessUpdate()
t_yamamoto	0:669ef71cba68	350	{
Ryosei	27:dd9f27fce7d1	351	#ifdef USE_SUBPROCESS
Ryosei	27:dd9f27fce7d1	352	if(controller->Button.HOME) lock = false;
Ryosei	27:dd9f27fce7d1	353
Ryosei	27:dd9f27fce7d1	354	if(controller->Button.START && processChangeComp) {
Ryosei	27:dd9f27fce7d1	355	current++;
Ryosei	27:dd9f27fce7d1	356	if (USE_PROCESS_NUM < current) current = USE_PROCESS_NUM;
Ryosei	27:dd9f27fce7d1	357	processChangeComp = false;
Ryosei	27:dd9f27fce7d1	358	} else if(controller->Button.SELECT && processChangeComp) {
Ryosei	27:dd9f27fce7d1	359	current--;
Ryosei	27:dd9f27fce7d1	360	if (current < 0) current = 0;
Ryosei	27:dd9f27fce7d1	361	processChangeComp = false;
Ryosei	27:dd9f27fce7d1	362	} else if(!controller->Button.SELECT && !controller->Button.START) processChangeComp = true;
Ryosei	27:dd9f27fce7d1	363	#endif
Ryosei	27:dd9f27fce7d1	364
Ryosei	27:dd9f27fce7d1	365	#ifdef USE_MOTOR
Ryosei	27:dd9f27fce7d1	366	ACTUATORHUB::MOTOR::Motor::Update(motor);
Ryosei	27:dd9f27fce7d1	367	#endif
Ryosei	27:dd9f27fce7d1	368
Ryosei	27:dd9f27fce7d1	369	#ifdef USE_SOLENOID
Ryosei	27:dd9f27fce7d1	370	ACTUATORHUB::SOLENOID::Solenoid::Update(solenoid);
Ryosei	27:dd9f27fce7d1	371	#endif
Ryosei	27:dd9f27fce7d1	372
Ryosei	27:dd9f27fce7d1	373	#ifdef USE_RS485
Ryosei	27:dd9f27fce7d1	374	ACTUATORHUB::ActuatorHub::Update();
Ryosei	27:dd9f27fce7d1	375	//LINEHUB::LineHub::Update();
Ryosei	27:dd9f27fce7d1	376	#endif
Ryosei	27:dd9f27fce7d1	377
Ryosei	27:dd9f27fce7d1	378	}
Ryosei	27:dd9f27fce7d1	379
Ryosei	27:dd9f27fce7d1	380
Ryosei	27:dd9f27fce7d1	381	double Ult_left=UltraRead(0);///////////////////////////////////////////left sensor
Ryosei	27:dd9f27fce7d1	382	double Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
Ryosei	27:dd9f27fce7d1	383	static int Limitphase=0;
Ryosei	27:dd9f27fce7d1	384	void SystemProcess()
Ryosei	27:dd9f27fce7d1	385	{
Ryosei	27:dd9f27fce7d1	386	SystemProcessInitialize();
Ryosei	27:dd9f27fce7d1	387	while(1) {
Ryosei	27:dd9f27fce7d1	388	AirUpdate();
Ryosei	27:dd9f27fce7d1	389	int g[8];
Ryosei	27:dd9f27fce7d1	390	for(int i = 0; i < 8; i++) {
Ryosei	27:dd9f27fce7d1	391	g[i] = lineCast(LineHub::GetPara(i));
Ryosei	27:dd9f27fce7d1	392	}
Ryosei	27:dd9f27fce7d1	393	Ult_left=UltraRead(0);//////////////////////////////////////////left sensor
Ryosei	27:dd9f27fce7d1	394	Ult_right=UltraRead(1);//////////////////////////////////////////right sensor
Ryosei	27:dd9f27fce7d1	395	pc.printf("%lf,%lf",UltraRead(0),UltraRead(1));
Ryosei	27:dd9f27fce7d1	396	// 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]);
t_yamamoto	0:669ef71cba68	397
Ryosei	27:dd9f27fce7d1	398	pc.printf("%d\n\r",current);
Ryosei	27:dd9f27fce7d1	399	/*上
Ryosei	27:dd9f27fce7d1	400	motor[LIFT_LB].dir = FOR;
Ryosei	27:dd9f27fce7d1	401	motor[LIFT_LB].pwm = 180;
Ryosei	27:dd9f27fce7d1	402	motor[LIFT_RB].dir = BACK;
Ryosei	27:dd9f27fce7d1	403	motor[LIFT_RB].pwm = 200;
Ryosei	27:dd9f27fce7d1	404	*/
Ryosei	27:dd9f27fce7d1	405	if(!(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW))&&startFlag==true) {
Ryosei	27:dd9f27fce7d1	406	startFlag=false;
Ryosei	27:dd9f27fce7d1	407	lock=false;
Ryosei	27:dd9f27fce7d1	408	current=7;
Ryosei	27:dd9f27fce7d1	409	} else {
Ryosei	27:dd9f27fce7d1	410	motor[LIFT_LB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	411	motor[LIFT_LB].pwm = 180;
Ryosei	27:dd9f27fce7d1	412	motor[LIFT_RB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	413	motor[LIFT_RB].pwm = 200;
Ryosei	27:dd9f27fce7d1	414	}
Ryosei	27:dd9f27fce7d1	415	if(LimitSw::IsPressed(START_SW) && startFlag == true) {
Ryosei	27:dd9f27fce7d1	416	LedOut(6);
Ryosei	27:dd9f27fce7d1	417	startFlag = false;
Ryosei	27:dd9f27fce7d1	418	lock = false;
Ryosei	27:dd9f27fce7d1	419	lineFase = 0;
Ryosei	27:dd9f27fce7d1	420	lineCount = 0;
Ryosei	27:dd9f27fce7d1	421	lineCheck = false;
Ryosei	27:dd9f27fce7d1	422	countW = 0;
Ryosei	27:dd9f27fce7d1	423	if(LimitSw::IsPressed(REDBLUE_SW)) {
Ryosei	27:dd9f27fce7d1	424	current = 4;
Ryosei	27:dd9f27fce7d1	425	} else {
Ryosei	27:dd9f27fce7d1	426	current = 5;
Ryosei	27:dd9f27fce7d1	427	}
Ryosei	27:dd9f27fce7d1	428	}
Ryosei	27:dd9f27fce7d1	429	buzzer.period(1.0/800);
Ryosei	27:dd9f27fce7d1	430
Ryosei	27:dd9f27fce7d1	431	#ifdef USE_MU
Ryosei	27:dd9f27fce7d1	432	controller = CONTROLLER::Controller::GetData();
Ryosei	27:dd9f27fce7d1	433	#endif
Ryosei	27:dd9f27fce7d1	434
Ryosei	27:dd9f27fce7d1	435	#ifdef USE_ERRORCHECK
Ryosei	27:dd9f27fce7d1	436	if(SAFTY::ErrorCheck::Check() & SAFTY::Error::ControllerLost & startFlag) {
Ryosei	27:dd9f27fce7d1	437	CONTROLLER::Controller::DataReset();
Ryosei	27:dd9f27fce7d1	438	AllActuatorReset();
Ryosei	27:dd9f27fce7d1	439	lock = true;
Ryosei	27:dd9f27fce7d1	440	} else
Ryosei	27:dd9f27fce7d1	441	#endif
Ryosei	27:dd9f27fce7d1	442	{
Ryosei	27:dd9f27fce7d1	443
Ryosei	27:dd9f27fce7d1	444	#ifdef USE_SUBPROCESS
Ryosei	27:dd9f27fce7d1	445	if(!lock) {
Ryosei	27:dd9f27fce7d1	446	Process[current]();
Ryosei	27:dd9f27fce7d1	447	} else
Ryosei	27:dd9f27fce7d1	448	#endif
Ryosei	27:dd9f27fce7d1	449	{
Ryosei	27:dd9f27fce7d1	450	//ロック時の処理
Ryosei	27:dd9f27fce7d1	451	}
Ryosei	27:dd9f27fce7d1	452	}
Ryosei	27:dd9f27fce7d1	453
Ryosei	27:dd9f27fce7d1	454
Ryosei	27:dd9f27fce7d1	455	//Emergency!
Ryosei	27:dd9f27fce7d1	456	/*
Ryosei	27:dd9f27fce7d1	457	if(!EMG_0 && !EMG_1 && !EMGflag){
Ryosei	27:dd9f27fce7d1	458	buzzer = 0;
Ryosei	27:dd9f27fce7d1	459	BuzzerTimer.attach(BuzzerTimer_func, 1);
Ryosei	27:dd9f27fce7d1	460	EMGflag = true;
Ryosei	27:dd9f27fce7d1	461	LED_DEBUG0 = 1;
Ryosei	27:dd9f27fce7d1	462	}
Ryosei	27:dd9f27fce7d1	463	if(EMG_0 && EMG_1 && EMGflag){
Ryosei	27:dd9f27fce7d1	464	buzzer = 1;
Ryosei	27:dd9f27fce7d1	465	BuzzerTimer.detach();
Ryosei	27:dd9f27fce7d1	466	EMGflag = false;
Ryosei	27:dd9f27fce7d1	467	}
Ryosei	27:dd9f27fce7d1	468	*/
Ryosei	27:dd9f27fce7d1	469	SystemProcessUpdate();
Ryosei	27:dd9f27fce7d1	470	}
t_yamamoto	0:669ef71cba68	471	}
t_yamamoto	0:669ef71cba68	472
kishibekairohan	2:c015739085d3	473
kishibekairohan	2:c015739085d3	474
kishibekairohan	2:c015739085d3	475
t_yamamoto	0:669ef71cba68	476	#pragma region PROCESS
t_yamamoto	0:669ef71cba68	477	#ifdef USE_SUBPROCESS
t_yamamoto	0:669ef71cba68	478	#if USE_PROCESS_NUM>0
Ryosei	27:dd9f27fce7d1	479	static void Process0()
Ryosei	27:dd9f27fce7d1	480	{
Ryosei	27:dd9f27fce7d1	481	AllActuatorReset();
t_yamamoto	0:669ef71cba68	482	}
t_yamamoto	0:669ef71cba68	483	#endif
t_yamamoto	0:669ef71cba68	484
t_yamamoto	0:669ef71cba68	485	#if USE_PROCESS_NUM>1
kishibekairohan	16:3f2c2d89372b	486	static void Process1()
t_yamamoto	0:669ef71cba68	487	{
Ryosei	27:dd9f27fce7d1	488
M_souta	22:7d93f79a3686	489	PIDflag = false;
Ryosei	27:dd9f27fce7d1	490	LedOut(0);
M_souta	21:e3b58d675c1c	491	if(controller->Button.UP) {
Ryosei	27:dd9f27fce7d1	492	motor[LIFT_LB].dir = FOR;
Ryosei	27:dd9f27fce7d1	493	motor[LIFT_LB].pwm = 180;
Ryosei	27:dd9f27fce7d1	494	motor[LIFT_RB].dir = BACK;
Ryosei	27:dd9f27fce7d1	495	motor[LIFT_RB].pwm = 200;
M_souta	21:e3b58d675c1c	496	} else if(controller->Button.DOWN) {
Ryosei	27:dd9f27fce7d1	497	motor[LIFT_LB].dir = BACK;
Ryosei	27:dd9f27fce7d1	498	motor[LIFT_LB].pwm = 180;
Ryosei	27:dd9f27fce7d1	499	motor[LIFT_RB].dir = FOR;
Ryosei	27:dd9f27fce7d1	500	motor[LIFT_RB].pwm = 200;
M_souta	23:c853372cf626	501	} else if(controller->Button.LEFT) {
Ryosei	27:dd9f27fce7d1	502	motor[LIFT_LB].dir = FOR;
Ryosei	27:dd9f27fce7d1	503	motor[LIFT_LB].pwm = 180;
M_souta	23:c853372cf626	504	} else if(controller->Button.RIGHT) {
Ryosei	27:dd9f27fce7d1	505	motor[LIFT_RB].dir = BACK;
Ryosei	27:dd9f27fce7d1	506	motor[LIFT_RB].pwm = 180;
M_souta	21:e3b58d675c1c	507	} else {
Ryosei	27:dd9f27fce7d1	508	motor[LIFT_LB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	509	motor[LIFT_LB].pwm = 255;
Ryosei	27:dd9f27fce7d1	510	motor[LIFT_RB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	511	motor[LIFT_RB].pwm = 255;
M_souta	21:e3b58d675c1c	512	}
M_souta	21:e3b58d675c1c	513
Ryosei	27:dd9f27fce7d1	514
Ryosei	27:dd9f27fce7d1	515	if(controller->Button.X) {
Ryosei	27:dd9f27fce7d1	516	motor[LIFT_U].dir = FOR;
Ryosei	27:dd9f27fce7d1	517	motor[LIFT_U].pwm = 180;
Ryosei	27:dd9f27fce7d1	518	} else if(controller->Button.Y) {
Ryosei	27:dd9f27fce7d1	519	motor[LIFT_U].dir = BACK;
Ryosei	27:dd9f27fce7d1	520	motor[LIFT_U].pwm = 230;
Ryosei	27:dd9f27fce7d1	521	} else {
Ryosei	27:dd9f27fce7d1	522	motor[LIFT_U].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	523	motor[LIFT_U].pwm = 255;
Ryosei	27:dd9f27fce7d1	524	}
Ryosei	27:dd9f27fce7d1	525
Ryosei	27:dd9f27fce7d1	526	if(!(controller->AnalogL.Y == 7) \|\| !(controller->AnalogL.X == 7)) {
Ryosei	27:dd9f27fce7d1	527	motor[TIRE_FR].dir = SetStatus(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] );
Ryosei	27:dd9f27fce7d1	528	motor[TIRE_FL].dir = SetStatus(omni[controller->AnalogL.Y][controller->AnalogL.X] );
Ryosei	27:dd9f27fce7d1	529	motor[TIRE_BR].dir = SetStatus(-omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] );
Ryosei	27:dd9f27fce7d1	530	motor[TIRE_BL].dir = SetStatus(omni[controller->AnalogL.X][14-controller->AnalogL.Y] );
Ryosei	27:dd9f27fce7d1	531
Ryosei	27:dd9f27fce7d1	532	motor[TIRE_FR].pwm = SetPWM(-omni[controller->AnalogL.Y][14-controller->AnalogL.X] * 0.2) ;
Ryosei	27:dd9f27fce7d1	533	motor[TIRE_FL].pwm = SetPWM(omni[controller->AnalogL.Y][controller->AnalogL.X] * 0.2) ;
Ryosei	27:dd9f27fce7d1	534	motor[TIRE_BR].pwm = SetPWM(omni[14-controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
Ryosei	27:dd9f27fce7d1	535	motor[TIRE_BL].pwm = SetPWM(-omni[controller->AnalogL.X][14-controller->AnalogL.Y] * 0.2) ;
Ryosei	27:dd9f27fce7d1	536	} else {
Ryosei	27:dd9f27fce7d1	537	motor[TIRE_FR].dir = SetStatus(curve[controller->AnalogR.X]);
Ryosei	27:dd9f27fce7d1	538	motor[TIRE_FL].dir = SetStatus(curve[controller->AnalogR.X]);
Ryosei	27:dd9f27fce7d1	539	motor[TIRE_BR].dir = SetStatus(curve[controller->AnalogR.X]);
Ryosei	27:dd9f27fce7d1	540	motor[TIRE_BL].dir = SetStatus(curve[controller->AnalogR.X]);
Ryosei	27:dd9f27fce7d1	541
Ryosei	27:dd9f27fce7d1	542	motor[TIRE_FR].pwm = SetPWM(curve[controller->AnalogR.X]);
Ryosei	27:dd9f27fce7d1	543	motor[TIRE_FL].pwm = SetPWM(curve[controller->AnalogR.X]);
Ryosei	27:dd9f27fce7d1	544	motor[TIRE_BR].pwm = SetPWM(curve[controller->AnalogR.X]);
Ryosei	27:dd9f27fce7d1	545	motor[TIRE_BL].pwm = SetPWM(curve[controller->AnalogR.X]);
Ryosei	27:dd9f27fce7d1	546	}
Ryosei	27:dd9f27fce7d1	547
Ryosei	27:dd9f27fce7d1	548	if(controller->Button.ZR) {
Ryosei	27:dd9f27fce7d1	549	Air[CLOTHESPIN] = SOLENOID_ON;
Ryosei	27:dd9f27fce7d1	550	}
Ryosei	27:dd9f27fce7d1	551	if(controller->Button.ZL) {
Ryosei	27:dd9f27fce7d1	552	Air[CLOTHESPIN] = SOLENOID_OFF;
Ryosei	27:dd9f27fce7d1	553	}
t_yamamoto	0:669ef71cba68	554	}
t_yamamoto	0:669ef71cba68	555	#endif
t_yamamoto	0:669ef71cba68	556
t_yamamoto	0:669ef71cba68	557	#if USE_PROCESS_NUM>2
kishibekairohan	16:3f2c2d89372b	558	static void Process2()
Ryosei	27:dd9f27fce7d1	559	{
M_souta	26:4c0ce2f05688	560
kishibekairohan	13:b6e02d6261d7	561	}
kishibekairohan	13:b6e02d6261d7	562	#endif
kishibekairohan	13:b6e02d6261d7	563
kishibekairohan	13:b6e02d6261d7	564	#if USE_PROCESS_NUM>3
Ryosei	27:dd9f27fce7d1	565	static void Process3()
kishibekairohan	13:b6e02d6261d7	566	{
Ryosei	27:dd9f27fce7d1	567	startFlag = true;
Ryosei	27:dd9f27fce7d1	568	AllActuatorReset();
Ryosei	27:dd9f27fce7d1	569	lineFase = 0;
Ryosei	27:dd9f27fce7d1	570	lineCheck = false;
Ryosei	27:dd9f27fce7d1	571	lineCount = 0;
Ryosei	27:dd9f27fce7d1	572	countW = 0;
kishibekairohan	13:b6e02d6261d7	573	}
kishibekairohan	13:b6e02d6261d7	574	#endif
kishibekairohan	13:b6e02d6261d7	575
kishibekairohan	13:b6e02d6261d7	576	#if USE_PROCESS_NUM>4
Ryosei	27:dd9f27fce7d1	577	static void Process4()
M_souta	22:7d93f79a3686	578	{
Ryosei	27:dd9f27fce7d1	579	LED_DEBUG0 = LED_ON;
Ryosei	27:dd9f27fce7d1	580
Ryosei	27:dd9f27fce7d1	581	/* ************************************** //
Ryosei	27:dd9f27fce7d1	582
Ryosei	27:dd9f27fce7d1	583	赤ゾーン赤ゾーン赤ゾーン
Ryosei	27:dd9f27fce7d1	584
Ryosei	27:dd9f27fce7d1	585	// ************************************** */
Ryosei	27:dd9f27fce7d1	586
Ryosei	27:dd9f27fce7d1	587
Ryosei	27:dd9f27fce7d1	588	for(int i = 0; i < 8; i++) {
Ryosei	27:dd9f27fce7d1	589	linePara[i] = lineCast(LineHub::GetPara(i));
Ryosei	27:dd9f27fce7d1	590	}
Ryosei	27:dd9f27fce7d1	591
Ryosei	27:dd9f27fce7d1	592	if(lineFase == 0) {
Ryosei	27:dd9f27fce7d1	593	LedOut(2);
Ryosei	27:dd9f27fce7d1	594	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	595	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	596	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	597	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	598	if(linePara[0] != 'N' && linePara[0] != 'A') {
Ryosei	27:dd9f27fce7d1	599	lineFase = 1;
Ryosei	27:dd9f27fce7d1	600	}
Ryosei	27:dd9f27fce7d1	601	motor[TIRE_FL].pwm = 30;
Ryosei	27:dd9f27fce7d1	602	motor[TIRE_BL].pwm = 30;
Ryosei	27:dd9f27fce7d1	603	motor[TIRE_BR].pwm = 30;
Ryosei	27:dd9f27fce7d1	604	motor[TIRE_FR].pwm = 30;
Ryosei	27:dd9f27fce7d1	605
Ryosei	27:dd9f27fce7d1	606	} else if(lineFase == 1) { // 前ライントレース
Ryosei	27:dd9f27fce7d1	607	switch(linePara[0]) {
Ryosei	27:dd9f27fce7d1	608	case -2:
Ryosei	27:dd9f27fce7d1	609	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	610	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	611	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	612	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	613	adjAnable = true;
Ryosei	27:dd9f27fce7d1	614	break;
Ryosei	27:dd9f27fce7d1	615	case -3:
Ryosei	27:dd9f27fce7d1	616	tirePWM[TIRE_FL] = 10;
Ryosei	27:dd9f27fce7d1	617	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	618	tirePWM[TIRE_BR] = -10;
Ryosei	27:dd9f27fce7d1	619	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	620	adjAnable = true;
Ryosei	27:dd9f27fce7d1	621	break;
Ryosei	27:dd9f27fce7d1	622	case -1:
Ryosei	27:dd9f27fce7d1	623	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	624	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	625	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	626	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	627	adjAnable = true;
Ryosei	27:dd9f27fce7d1	628	break;
Ryosei	27:dd9f27fce7d1	629	case 0:
Ryosei	27:dd9f27fce7d1	630	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	631	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	632	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	633	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	634	adjAnable = true;
Ryosei	27:dd9f27fce7d1	635	break;
Ryosei	27:dd9f27fce7d1	636	case 1:
Ryosei	27:dd9f27fce7d1	637	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	638	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	639	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	640	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	641	adjAnable = true;
Ryosei	27:dd9f27fce7d1	642	break;
Ryosei	27:dd9f27fce7d1	643	case 3:
Ryosei	27:dd9f27fce7d1	644	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	645	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	646	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	647	tirePWM[TIRE_FR] = -10;
Ryosei	27:dd9f27fce7d1	648	adjAnable = true;
Ryosei	27:dd9f27fce7d1	649	break;
Ryosei	27:dd9f27fce7d1	650	case 2:
Ryosei	27:dd9f27fce7d1	651	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	652	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	653	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	654	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	655	adjAnable = true;
Ryosei	27:dd9f27fce7d1	656	break;
Ryosei	27:dd9f27fce7d1	657	case 'A':
Ryosei	27:dd9f27fce7d1	658	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	659	lineCheck = true;
Ryosei	27:dd9f27fce7d1	660	lineCount = 0;
Ryosei	27:dd9f27fce7d1	661	countW++;
Ryosei	27:dd9f27fce7d1	662	}
Ryosei	27:dd9f27fce7d1	663	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	664	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	665	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	666	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	667	adjAnable = true;
Ryosei	27:dd9f27fce7d1	668	break;
Ryosei	27:dd9f27fce7d1	669	case 'N':
Ryosei	27:dd9f27fce7d1	670	tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei	27:dd9f27fce7d1	671	tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei	27:dd9f27fce7d1	672	tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei	27:dd9f27fce7d1	673	tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei	27:dd9f27fce7d1	674	adjAnable = false;
Ryosei	27:dd9f27fce7d1	675	break;
Ryosei	27:dd9f27fce7d1	676	default:
Ryosei	27:dd9f27fce7d1	677	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	678	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	679	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	680	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	681	adjAnable = false;
Ryosei	27:dd9f27fce7d1	682	}
Ryosei	27:dd9f27fce7d1	683
Ryosei	27:dd9f27fce7d1	684	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	685	if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
Ryosei	27:dd9f27fce7d1	686	} else {
Ryosei	27:dd9f27fce7d1	687	adj = 0;
Ryosei	27:dd9f27fce7d1	688	}
Ryosei	27:dd9f27fce7d1	689
Ryosei	27:dd9f27fce7d1	690	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	691	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	692	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	693	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	694	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	695	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	696	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	697	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	698
Ryosei	27:dd9f27fce7d1	699	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	700	lineCount++;
Ryosei	27:dd9f27fce7d1	701	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	702	}
Ryosei	27:dd9f27fce7d1	703	if(countW == 3) {
Ryosei	27:dd9f27fce7d1	704	countW = 0;
Ryosei	27:dd9f27fce7d1	705	lineFase = 2;
Ryosei	27:dd9f27fce7d1	706	lineCount = 0;
Ryosei	27:dd9f27fce7d1	707	lineCheck = false;
Ryosei	27:dd9f27fce7d1	708	}
Ryosei	27:dd9f27fce7d1	709	} else if(lineFase == 2) { // 前低速
Ryosei	27:dd9f27fce7d1	710	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	711	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	712	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	713	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	714	if(linePara[4] >= -1 && linePara[4] <= 1) {
Ryosei	27:dd9f27fce7d1	715	lineFase = 3;
Ryosei	27:dd9f27fce7d1	716	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	717	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	718	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	719	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	720	}
Ryosei	27:dd9f27fce7d1	721	motor[TIRE_FL].pwm = 20;
Ryosei	27:dd9f27fce7d1	722	motor[TIRE_BL].pwm = 20;
Ryosei	27:dd9f27fce7d1	723	motor[TIRE_BR].pwm = 20;
Ryosei	27:dd9f27fce7d1	724	motor[TIRE_FR].pwm = 20;
Ryosei	27:dd9f27fce7d1	725	} else if(lineFase == 3) { // 右ライントレース
Ryosei	27:dd9f27fce7d1	726	switch(linePara[4]) {
Ryosei	27:dd9f27fce7d1	727	case -2:
Ryosei	27:dd9f27fce7d1	728	LedOut(2);
Ryosei	27:dd9f27fce7d1	729	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	730	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	731	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	732	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	733	adjAnable = true;
Ryosei	27:dd9f27fce7d1	734	break;
Ryosei	27:dd9f27fce7d1	735	case -3:
Ryosei	27:dd9f27fce7d1	736	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	737	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	738	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	739	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	740	adjAnable = true;
Ryosei	27:dd9f27fce7d1	741	break;
Ryosei	27:dd9f27fce7d1	742	case -1:
Ryosei	27:dd9f27fce7d1	743	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	744	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	745	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	746	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	747	adjAnable = true;
Ryosei	27:dd9f27fce7d1	748	break;
Ryosei	27:dd9f27fce7d1	749	case 0:
Ryosei	27:dd9f27fce7d1	750	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	751	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	752	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	753	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	754	adjAnable = true;
Ryosei	27:dd9f27fce7d1	755	break;
Ryosei	27:dd9f27fce7d1	756	case 1:
Ryosei	27:dd9f27fce7d1	757	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	758	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	759	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	760	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	761	adjAnable = true;
Ryosei	27:dd9f27fce7d1	762	break;
Ryosei	27:dd9f27fce7d1	763	case 3:
Ryosei	27:dd9f27fce7d1	764	tirePWM[TIRE_FL] = 10;
Ryosei	27:dd9f27fce7d1	765	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	766	tirePWM[TIRE_BR] = -10;
Ryosei	27:dd9f27fce7d1	767	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	768	adjAnable = true;
Ryosei	27:dd9f27fce7d1	769	break;
Ryosei	27:dd9f27fce7d1	770	case 2:
Ryosei	27:dd9f27fce7d1	771	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	772	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	773	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	774	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	775	adjAnable = true;
Ryosei	27:dd9f27fce7d1	776	break;
Ryosei	27:dd9f27fce7d1	777	case 'A':
Ryosei	27:dd9f27fce7d1	778	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	779	lineCheck = true;
Ryosei	27:dd9f27fce7d1	780	lineCount = 0;
Ryosei	27:dd9f27fce7d1	781	countW++;
Ryosei	27:dd9f27fce7d1	782	}
Ryosei	27:dd9f27fce7d1	783	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	784	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	785	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	786	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	787	adjAnable = true;
Ryosei	27:dd9f27fce7d1	788	break;
Ryosei	27:dd9f27fce7d1	789	case 'N':
Ryosei	27:dd9f27fce7d1	790	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	791	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	792	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	793	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	794	adjAnable = false;
Ryosei	27:dd9f27fce7d1	795	break;
Ryosei	27:dd9f27fce7d1	796	default:
Ryosei	27:dd9f27fce7d1	797	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	798	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	799	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	800	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	801	adjAnable = false;
Ryosei	27:dd9f27fce7d1	802	}
Ryosei	27:dd9f27fce7d1	803
Ryosei	27:dd9f27fce7d1	804	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	805	if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
Ryosei	27:dd9f27fce7d1	806	} else {
Ryosei	27:dd9f27fce7d1	807	adj = 0;
Ryosei	27:dd9f27fce7d1	808	}
Ryosei	27:dd9f27fce7d1	809
Ryosei	27:dd9f27fce7d1	810	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei	27:dd9f27fce7d1	811	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	812	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	813	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	814	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei	27:dd9f27fce7d1	815	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	816	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	817	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	818
Ryosei	27:dd9f27fce7d1	819	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	820	lineCount++;
Ryosei	27:dd9f27fce7d1	821	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	822	}
Ryosei	27:dd9f27fce7d1	823
Ryosei	27:dd9f27fce7d1	824	targetCount = 1;
Ryosei	27:dd9f27fce7d1	825
Ryosei	27:dd9f27fce7d1	826	if(countW == targetCount) {
Ryosei	27:dd9f27fce7d1	827	countW = 0;
Ryosei	27:dd9f27fce7d1	828	lineFase = 4;
Ryosei	27:dd9f27fce7d1	829	lineCount = 0;
Ryosei	27:dd9f27fce7d1	830	lineCheck = false;
Ryosei	27:dd9f27fce7d1	831	}
Ryosei	27:dd9f27fce7d1	832	} else if(lineFase == 4) { // 右低速
Ryosei	27:dd9f27fce7d1	833	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	834	motor[TIRE_BL].dir = BACK;
Ryosei	27:dd9f27fce7d1	835	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	836	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	837	if (linePara[LINE_TOW_2] == 0) { // １と２逆になります。
Ryosei	27:dd9f27fce7d1	838	lineFase = 6;
Ryosei	27:dd9f27fce7d1	839	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	840	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	841	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	842	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	843	}
Ryosei	27:dd9f27fce7d1	844	if(linePara[LINE_TOW_1] == 0) {
Ryosei	27:dd9f27fce7d1	845	if(!LimitSw::IsPressed(SHEETS_SW)) {
Ryosei	27:dd9f27fce7d1	846	lineFase=100;
Ryosei	27:dd9f27fce7d1	847	} else {
Ryosei	27:dd9f27fce7d1	848	lineFase = 6;
Ryosei	27:dd9f27fce7d1	849	}
Ryosei	27:dd9f27fce7d1	850	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	851	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	852	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	853	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	854	}
Ryosei	27:dd9f27fce7d1	855	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	856	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	857	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	858	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	859	} else if (lineFase == 5) {
Ryosei	27:dd9f27fce7d1	860	lineFase = 6;
Ryosei	27:dd9f27fce7d1	861	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	862	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	863	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	864	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	865	} else if(lineFase == 6) { // タオル1 検知
Ryosei	27:dd9f27fce7d1	866	LedOut(4);
Ryosei	27:dd9f27fce7d1	867	if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
Ryosei	27:dd9f27fce7d1	868	lineFase = 7;
Ryosei	27:dd9f27fce7d1	869	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	870	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	871	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	872	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	873	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	874	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	875	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	876	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	877	} else if(LimitSw::IsPressed(TOW_1L)) {
Ryosei	27:dd9f27fce7d1	878	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	879	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	880	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	881	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	882	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	883	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	884	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	885	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	886	} else if(LimitSw::IsPressed(TOW_1R)) {
Ryosei	27:dd9f27fce7d1	887	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	888	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	889	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	890	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	891	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	892	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	893	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	894	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	895	} else {
Ryosei	27:dd9f27fce7d1	896	if(LimitSw::IsPressed(QF_SW)) {
Ryosei	27:dd9f27fce7d1	897	switch(linePara[LINE_TOW_2]) {
Ryosei	27:dd9f27fce7d1	898	case -2:
Ryosei	27:dd9f27fce7d1	899	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	900	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	901	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	902	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	903	adjAnable = true;
Ryosei	27:dd9f27fce7d1	904	break;
Ryosei	27:dd9f27fce7d1	905	case -3:
Ryosei	27:dd9f27fce7d1	906	tirePWM[TIRE_FL] = -14;
Ryosei	27:dd9f27fce7d1	907	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	908	tirePWM[TIRE_BR] = 14;
Ryosei	27:dd9f27fce7d1	909	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	910	adjAnable = true;
Ryosei	27:dd9f27fce7d1	911	break;
Ryosei	27:dd9f27fce7d1	912	case -1:
Ryosei	27:dd9f27fce7d1	913	tirePWM[TIRE_FL] = -17;
Ryosei	27:dd9f27fce7d1	914	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	915	tirePWM[TIRE_BR] = 17;
Ryosei	27:dd9f27fce7d1	916	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	917	adjAnable = true;
Ryosei	27:dd9f27fce7d1	918	break;
Ryosei	27:dd9f27fce7d1	919	case 0:
Ryosei	27:dd9f27fce7d1	920	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	921	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	922	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	923	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	924	adjAnable = true;
Ryosei	27:dd9f27fce7d1	925	break;
Ryosei	27:dd9f27fce7d1	926	case 1:
Ryosei	27:dd9f27fce7d1	927	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	928	tirePWM[TIRE_BL] = -17;
Ryosei	27:dd9f27fce7d1	929	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	930	tirePWM[TIRE_FR] = 17;
Ryosei	27:dd9f27fce7d1	931	adjAnable = true;
Ryosei	27:dd9f27fce7d1	932	break;
Ryosei	27:dd9f27fce7d1	933	case 3:
Ryosei	27:dd9f27fce7d1	934	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	935	tirePWM[TIRE_BL] = -14;
Ryosei	27:dd9f27fce7d1	936	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	937	tirePWM[TIRE_FR] = 14;
Ryosei	27:dd9f27fce7d1	938	adjAnable = true;
Ryosei	27:dd9f27fce7d1	939	break;
Ryosei	27:dd9f27fce7d1	940	case 2:
Ryosei	27:dd9f27fce7d1	941	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	942	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	943	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	944	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	945	adjAnable = true;
Ryosei	27:dd9f27fce7d1	946	break;
Ryosei	27:dd9f27fce7d1	947	case 'A':
Ryosei	27:dd9f27fce7d1	948	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	949	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	950	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	951	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	952	adjAnable = true;
Ryosei	27:dd9f27fce7d1	953	break;
Ryosei	27:dd9f27fce7d1	954	case 'N':
Ryosei	27:dd9f27fce7d1	955	tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei	27:dd9f27fce7d1	956	tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei	27:dd9f27fce7d1	957	tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei	27:dd9f27fce7d1	958	tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei	27:dd9f27fce7d1	959	adjAnable = false;
Ryosei	27:dd9f27fce7d1	960	break;
Ryosei	27:dd9f27fce7d1	961	default:
Ryosei	27:dd9f27fce7d1	962	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	963	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	964	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	965	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	966	adjAnable = false;
Ryosei	27:dd9f27fce7d1	967	}
Ryosei	27:dd9f27fce7d1	968	} else {
Ryosei	27:dd9f27fce7d1	969	switch(linePara[LINE_TOW_2]) {
Ryosei	27:dd9f27fce7d1	970	case 2:
Ryosei	27:dd9f27fce7d1	971	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	972	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	973	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	974	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	975	adjAnable = true;
Ryosei	27:dd9f27fce7d1	976	break;
Ryosei	27:dd9f27fce7d1	977	case 3:
Ryosei	27:dd9f27fce7d1	978	tirePWM[TIRE_FL] = -14;
Ryosei	27:dd9f27fce7d1	979	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	980	tirePWM[TIRE_BR] = 14;
Ryosei	27:dd9f27fce7d1	981	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	982	adjAnable = true;
Ryosei	27:dd9f27fce7d1	983	break;
Ryosei	27:dd9f27fce7d1	984	case 1:
Ryosei	27:dd9f27fce7d1	985	tirePWM[TIRE_FL] = -17;
Ryosei	27:dd9f27fce7d1	986	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	987	tirePWM[TIRE_BR] = 17;
Ryosei	27:dd9f27fce7d1	988	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	989	adjAnable = true;
Ryosei	27:dd9f27fce7d1	990	break;
Ryosei	27:dd9f27fce7d1	991	case 0:
Ryosei	27:dd9f27fce7d1	992	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	993	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	994	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	995	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	996	adjAnable = true;
Ryosei	27:dd9f27fce7d1	997	break;
Ryosei	27:dd9f27fce7d1	998	case -1:
Ryosei	27:dd9f27fce7d1	999	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1000	tirePWM[TIRE_BL] = -17;
Ryosei	27:dd9f27fce7d1	1001	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1002	tirePWM[TIRE_FR] = 17;
Ryosei	27:dd9f27fce7d1	1003	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1004	break;
Ryosei	27:dd9f27fce7d1	1005	case -3:
Ryosei	27:dd9f27fce7d1	1006	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1007	tirePWM[TIRE_BL] = -14;
Ryosei	27:dd9f27fce7d1	1008	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1009	tirePWM[TIRE_FR] = 14;
Ryosei	27:dd9f27fce7d1	1010	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1011	break;
Ryosei	27:dd9f27fce7d1	1012	case -2:
Ryosei	27:dd9f27fce7d1	1013	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1014	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	1015	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1016	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	1017	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1018	break;
Ryosei	27:dd9f27fce7d1	1019	case 'A':
Ryosei	27:dd9f27fce7d1	1020	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1021	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1022	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1023	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1024	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1025	break;
Ryosei	27:dd9f27fce7d1	1026	case 'N':
Ryosei	27:dd9f27fce7d1	1027	tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei	27:dd9f27fce7d1	1028	tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei	27:dd9f27fce7d1	1029	tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei	27:dd9f27fce7d1	1030	tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei	27:dd9f27fce7d1	1031	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1032	break;
Ryosei	27:dd9f27fce7d1	1033	default:
Ryosei	27:dd9f27fce7d1	1034	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1035	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1036	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1037	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1038	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1039	}
Ryosei	27:dd9f27fce7d1	1040	}
Ryosei	27:dd9f27fce7d1	1041
Ryosei	27:dd9f27fce7d1	1042	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	1043	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	1044	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	1045	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	1046	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	1047	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	1048	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	1049	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1050	}
Ryosei	27:dd9f27fce7d1	1051	} else if(lineFase == 7) { // ライン修正
Ryosei	27:dd9f27fce7d1	1052	if(linePara[LINE_TOW_2] == 'A' \|\| linePara[LINE_TOW_2] == 'N') {
Ryosei	27:dd9f27fce7d1	1053	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1054	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	1055	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1056	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	1057	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1058	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	1059	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1060	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	1061	} else if(linePara[LINE_TOW_2] > 0) {
Ryosei	27:dd9f27fce7d1	1062	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	1063	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	1064	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	1065	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	1066	motor[TIRE_BR].dir = FOR;
Ryosei	27:dd9f27fce7d1	1067	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	1068	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	1069	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	1070	} else if(linePara[LINE_TOW_2] < 0) {
Ryosei	27:dd9f27fce7d1	1071	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	1072	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	1073	motor[TIRE_BL].dir = BACK;
Ryosei	27:dd9f27fce7d1	1074	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	1075	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	1076	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	1077	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	1078	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	1079	} else if(linePara[LINE_TOW_2] == 0) {
Ryosei	27:dd9f27fce7d1	1080	lineFase = 8;
Ryosei	27:dd9f27fce7d1	1081	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1082	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	1083	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1084	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	1085	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1086	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	1087	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1088	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	1089	} else {
Ryosei	27:dd9f27fce7d1	1090	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1091	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	1092	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1093	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	1094	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1095	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	1096	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1097	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1098	}
Ryosei	27:dd9f27fce7d1	1099	} else if(lineFase == 8) { // タオル1 解放
Ryosei	27:dd9f27fce7d1	1100	Air[0] = SOLENOID_ON;
Ryosei	27:dd9f27fce7d1	1101	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1102	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1103	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1104	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1105	lineFase = 9;
Ryosei	27:dd9f27fce7d1	1106	} else if(lineFase == 9) { // 前ライントレース
Ryosei	27:dd9f27fce7d1	1107	LedOut(2);
Ryosei	27:dd9f27fce7d1	1108	if(LimitSw::IsPressed(QF_SW)) {
Ryosei	27:dd9f27fce7d1	1109	switch(linePara[LINE_TOW_2]) {
Ryosei	27:dd9f27fce7d1	1110	case -2:
Ryosei	27:dd9f27fce7d1	1111	tirePWM[TIRE_FL] = 10;
Ryosei	27:dd9f27fce7d1	1112	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1113	tirePWM[TIRE_BR] = -10;
Ryosei	27:dd9f27fce7d1	1114	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1115	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1116	break;
Ryosei	27:dd9f27fce7d1	1117	case -3:
Ryosei	27:dd9f27fce7d1	1118	tirePWM[TIRE_FL] = 14;
Ryosei	27:dd9f27fce7d1	1119	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1120	tirePWM[TIRE_BR] = -14;
Ryosei	27:dd9f27fce7d1	1121	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1122	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1123	break;
Ryosei	27:dd9f27fce7d1	1124	case -1:
Ryosei	27:dd9f27fce7d1	1125	tirePWM[TIRE_FL] = 17;
Ryosei	27:dd9f27fce7d1	1126	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1127	tirePWM[TIRE_BR] = -17;
Ryosei	27:dd9f27fce7d1	1128	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1129	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1130	break;
Ryosei	27:dd9f27fce7d1	1131	case 0:
Ryosei	27:dd9f27fce7d1	1132	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1133	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1134	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1135	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1136	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1137	break;
Ryosei	27:dd9f27fce7d1	1138	case 1:
Ryosei	27:dd9f27fce7d1	1139	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1140	tirePWM[TIRE_BL] = 17;
Ryosei	27:dd9f27fce7d1	1141	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1142	tirePWM[TIRE_FR] = -17;
Ryosei	27:dd9f27fce7d1	1143	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1144	break;
Ryosei	27:dd9f27fce7d1	1145	case 3:
Ryosei	27:dd9f27fce7d1	1146	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1147	tirePWM[TIRE_BL] = 14;
Ryosei	27:dd9f27fce7d1	1148	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1149	tirePWM[TIRE_FR] = -14;
Ryosei	27:dd9f27fce7d1	1150	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1151	break;
Ryosei	27:dd9f27fce7d1	1152	case 2:
Ryosei	27:dd9f27fce7d1	1153	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1154	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	1155	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1156	tirePWM[TIRE_FR] = -10;
Ryosei	27:dd9f27fce7d1	1157	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1158	break;
Ryosei	27:dd9f27fce7d1	1159	case 'A':
Ryosei	27:dd9f27fce7d1	1160	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	1161	lineCheck = true;
Ryosei	27:dd9f27fce7d1	1162	lineCount = 0;
Ryosei	27:dd9f27fce7d1	1163	countW++;
Ryosei	27:dd9f27fce7d1	1164	}
Ryosei	27:dd9f27fce7d1	1165	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1166	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1167	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1168	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1169	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1170	break;
Ryosei	27:dd9f27fce7d1	1171	case 'N':
Ryosei	27:dd9f27fce7d1	1172	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1173	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1174	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1175	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1176	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1177	break;
Ryosei	27:dd9f27fce7d1	1178	default:
Ryosei	27:dd9f27fce7d1	1179	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1180	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1181	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1182	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1183	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1184	}
M_souta	25:b3a9f34b201d	1185	} else {
Ryosei	27:dd9f27fce7d1	1186	switch(linePara[LINE_TOW_2]) {
Ryosei	27:dd9f27fce7d1	1187	case 2:
Ryosei	27:dd9f27fce7d1	1188	tirePWM[TIRE_FL] = 10;
Ryosei	27:dd9f27fce7d1	1189	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1190	tirePWM[TIRE_BR] = -10;
Ryosei	27:dd9f27fce7d1	1191	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1192	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1193	break;
Ryosei	27:dd9f27fce7d1	1194	case 3:
Ryosei	27:dd9f27fce7d1	1195	tirePWM[TIRE_FL] = 14;
Ryosei	27:dd9f27fce7d1	1196	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1197	tirePWM[TIRE_BR] = -14;
Ryosei	27:dd9f27fce7d1	1198	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1199	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1200	break;
Ryosei	27:dd9f27fce7d1	1201	case 1:
Ryosei	27:dd9f27fce7d1	1202	tirePWM[TIRE_FL] = 17;
Ryosei	27:dd9f27fce7d1	1203	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1204	tirePWM[TIRE_BR] = -17;
Ryosei	27:dd9f27fce7d1	1205	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1206	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1207	break;
Ryosei	27:dd9f27fce7d1	1208	case 0:
Ryosei	27:dd9f27fce7d1	1209	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1210	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1211	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1212	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1213	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1214	break;
Ryosei	27:dd9f27fce7d1	1215	case -1:
Ryosei	27:dd9f27fce7d1	1216	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1217	tirePWM[TIRE_BL] = 17;
Ryosei	27:dd9f27fce7d1	1218	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1219	tirePWM[TIRE_FR] = -17;
Ryosei	27:dd9f27fce7d1	1220	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1221	break;
Ryosei	27:dd9f27fce7d1	1222	case -3:
Ryosei	27:dd9f27fce7d1	1223	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1224	tirePWM[TIRE_BL] = 14;
Ryosei	27:dd9f27fce7d1	1225	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1226	tirePWM[TIRE_FR] = -14;
Ryosei	27:dd9f27fce7d1	1227	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1228	break;
Ryosei	27:dd9f27fce7d1	1229	case -2:
Ryosei	27:dd9f27fce7d1	1230	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1231	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	1232	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1233	tirePWM[TIRE_FR] = -10;
Ryosei	27:dd9f27fce7d1	1234	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1235	break;
Ryosei	27:dd9f27fce7d1	1236	case 'A':
Ryosei	27:dd9f27fce7d1	1237	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	1238	lineCheck = true;
Ryosei	27:dd9f27fce7d1	1239	lineCount = 0;
Ryosei	27:dd9f27fce7d1	1240	countW++;
Ryosei	27:dd9f27fce7d1	1241	}
Ryosei	27:dd9f27fce7d1	1242	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1243	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1244	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1245	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1246	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1247	break;
Ryosei	27:dd9f27fce7d1	1248	case 'N':
Ryosei	27:dd9f27fce7d1	1249	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1250	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1251	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1252	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1253	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1254	break;
Ryosei	27:dd9f27fce7d1	1255	default:
Ryosei	27:dd9f27fce7d1	1256	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1257	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1258	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1259	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1260	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1261	}
M_souta	25:b3a9f34b201d	1262	}
M_souta	25:b3a9f34b201d	1263
M_souta	25:b3a9f34b201d	1264	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	1265	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	1266	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1267	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1268	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	1269	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	1270	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1271	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1272
M_souta	25:b3a9f34b201d	1273	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1274	lineCount++;
M_souta	25:b3a9f34b201d	1275	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1276	}
Ryosei	27:dd9f27fce7d1	1277	if(countW == 1) {
M_souta	26:4c0ce2f05688	1278	countW = 0;
Ryosei	27:dd9f27fce7d1	1279	lineFase = 10;
M_souta	25:b3a9f34b201d	1280	lineCount = 0;
M_souta	25:b3a9f34b201d	1281	lineCheck = false;
M_souta	25:b3a9f34b201d	1282	}
Ryosei	27:dd9f27fce7d1	1283	} else if(lineFase == 10) { // 前低速
M_souta	25:b3a9f34b201d	1284	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1285	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1286	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1287	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1288	if(linePara[4] == 0) {
Ryosei	27:dd9f27fce7d1	1289	lineFase = 11;
M_souta	25:b3a9f34b201d	1290	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1291	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1292	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1293	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1294	}
M_souta	25:b3a9f34b201d	1295	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1296	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1297	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	1298	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	1299	} else if(lineFase == 11) {
Ryosei	27:dd9f27fce7d1	1300	switch(linePara[4]) { // 右ライントレース
M_souta	25:b3a9f34b201d	1301	case -2:
M_souta	25:b3a9f34b201d	1302	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1303	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1304	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1305	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1306	adjAnable = true;
M_souta	22:7d93f79a3686	1307	break;
M_souta	22:7d93f79a3686	1308	case -3:
M_souta	25:b3a9f34b201d	1309	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1310	tirePWM[TIRE_BL] = -10;
M_souta	25:b3a9f34b201d	1311	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1312	tirePWM[TIRE_FR] = 10;
M_souta	25:b3a9f34b201d	1313	adjAnable = true;
M_souta	22:7d93f79a3686	1314	break;
M_souta	22:7d93f79a3686	1315	case -1:
M_souta	25:b3a9f34b201d	1316	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1317	tirePWM[TIRE_BL] = -20;
M_souta	25:b3a9f34b201d	1318	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1319	tirePWM[TIRE_FR] = 20;
M_souta	25:b3a9f34b201d	1320	adjAnable = true;
M_souta	22:7d93f79a3686	1321	break;
M_souta	22:7d93f79a3686	1322	case 0:
M_souta	25:b3a9f34b201d	1323	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1324	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1325	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1326	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1327	adjAnable = true;
M_souta	22:7d93f79a3686	1328	break;
M_souta	22:7d93f79a3686	1329	case 1:
M_souta	25:b3a9f34b201d	1330	tirePWM[TIRE_FL] = 20;
M_souta	25:b3a9f34b201d	1331	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1332	tirePWM[TIRE_BR] = -20;
M_souta	25:b3a9f34b201d	1333	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1334	adjAnable = true;
M_souta	22:7d93f79a3686	1335	break;
M_souta	22:7d93f79a3686	1336	case 3:
M_souta	25:b3a9f34b201d	1337	tirePWM[TIRE_FL] = 10;
M_souta	25:b3a9f34b201d	1338	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1339	tirePWM[TIRE_BR] = -10;
M_souta	25:b3a9f34b201d	1340	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1341	adjAnable = true;
M_souta	22:7d93f79a3686	1342	break;
M_souta	22:7d93f79a3686	1343	case 2:
M_souta	25:b3a9f34b201d	1344	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1345	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1346	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1347	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1348	adjAnable = true;
M_souta	22:7d93f79a3686	1349	break;
M_souta	22:7d93f79a3686	1350	case 'A':
M_souta	25:b3a9f34b201d	1351	if(lineCheck == false) {
M_souta	25:b3a9f34b201d	1352	lineCheck = true;
M_souta	25:b3a9f34b201d	1353	lineCount = 0;
M_souta	26:4c0ce2f05688	1354	countW++;
M_souta	25:b3a9f34b201d	1355	}
M_souta	25:b3a9f34b201d	1356	tirePWM[TIRE_FL] = 30;
M_souta	25:b3a9f34b201d	1357	tirePWM[TIRE_BL] = -30;
M_souta	25:b3a9f34b201d	1358	tirePWM[TIRE_BR] = -30;
M_souta	25:b3a9f34b201d	1359	tirePWM[TIRE_FR] = 30;
M_souta	25:b3a9f34b201d	1360	adjAnable = true;
M_souta	25:b3a9f34b201d	1361	break;
M_souta	25:b3a9f34b201d	1362	case 'N':
M_souta	25:b3a9f34b201d	1363	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1364	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1365	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1366	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1367	adjAnable = false;
M_souta	25:b3a9f34b201d	1368	break;
M_souta	25:b3a9f34b201d	1369	default:
M_souta	25:b3a9f34b201d	1370	tirePWM[TIRE_FL] = 0;
M_souta	25:b3a9f34b201d	1371	tirePWM[TIRE_BL] = 0;
M_souta	25:b3a9f34b201d	1372	tirePWM[TIRE_BR] = 0;
M_souta	25:b3a9f34b201d	1373	tirePWM[TIRE_FR] = 0;
M_souta	25:b3a9f34b201d	1374	adjAnable = false;
M_souta	25:b3a9f34b201d	1375	}
M_souta	25:b3a9f34b201d	1376
M_souta	25:b3a9f34b201d	1377	if(adjAnable){
M_souta	25:b3a9f34b201d	1378	if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
M_souta	25:b3a9f34b201d	1379	} else {
M_souta	25:b3a9f34b201d	1380	adj = 0;
M_souta	25:b3a9f34b201d	1381	}
M_souta	25:b3a9f34b201d	1382
M_souta	25:b3a9f34b201d	1383	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	1384	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1385	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1386	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1387	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
M_souta	25:b3a9f34b201d	1388	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
M_souta	25:b3a9f34b201d	1389	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
M_souta	25:b3a9f34b201d	1390	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1391
M_souta	25:b3a9f34b201d	1392	if(lineCheck == true) {
M_souta	25:b3a9f34b201d	1393	lineCount++;
M_souta	25:b3a9f34b201d	1394	if(lineCount > 20) lineCheck = false;
M_souta	25:b3a9f34b201d	1395	}
Ryosei	27:dd9f27fce7d1	1396	if(countW == 2) {
M_souta	26:4c0ce2f05688	1397	countW = 0;
Ryosei	27:dd9f27fce7d1	1398	lineFase = 12;
M_souta	25:b3a9f34b201d	1399	lineCount = 0;
M_souta	25:b3a9f34b201d	1400	lineCheck = false;
M_souta	25:b3a9f34b201d	1401	}
Ryosei	27:dd9f27fce7d1	1402	} else if(lineFase == 12) { // 右低速
M_souta	25:b3a9f34b201d	1403	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1404	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1405	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1406	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	1407	if(linePara[LINE_TOW_1] == 0) {
Ryosei	27:dd9f27fce7d1	1408	lineFase = 13;
M_souta	25:b3a9f34b201d	1409	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1410	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1411	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1412	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1413	}
Ryosei	27:dd9f27fce7d1	1414	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	1415	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	1416	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	1417	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	1418	} else if (lineFase == 13) {
Ryosei	27:dd9f27fce7d1	1419	lineFase = 14;
M_souta	25:b3a9f34b201d	1420	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1421	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1422	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1423	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	1424	} else if(lineFase == 14) { // タオル2 竿検知
Ryosei	27:dd9f27fce7d1	1425	LedOut(4);
Ryosei	27:dd9f27fce7d1	1426	if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
Ryosei	27:dd9f27fce7d1	1427	lineFase = 15;
M_souta	25:b3a9f34b201d	1428	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1429	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1430	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1431	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1432	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1433	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1434	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1435	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	1436	} else if(LimitSw::IsPressed(TOW_2L)) {
Ryosei	27:dd9f27fce7d1	1437	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	1438	motor[TIRE_FL].pwm = 20;
Ryosei	27:dd9f27fce7d1	1439	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	1440	motor[TIRE_BL].pwm = 20;
Ryosei	27:dd9f27fce7d1	1441	motor[TIRE_BR].dir = FOR;
Ryosei	27:dd9f27fce7d1	1442	motor[TIRE_BR].pwm = 20;
M_souta	25:b3a9f34b201d	1443	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	1444	motor[TIRE_FR].pwm = 20;
Ryosei	27:dd9f27fce7d1	1445	} else if(LimitSw::IsPressed(TOW_2R)) {
M_souta	25:b3a9f34b201d	1446	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	1447	motor[TIRE_FL].pwm = 20;
Ryosei	27:dd9f27fce7d1	1448	motor[TIRE_BL].dir = BACK;
Ryosei	27:dd9f27fce7d1	1449	motor[TIRE_BL].pwm = 20;
Ryosei	27:dd9f27fce7d1	1450	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	1451	motor[TIRE_BR].pwm = 20;
M_souta	25:b3a9f34b201d	1452	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	1453	motor[TIRE_FR].pwm = 20;
M_souta	25:b3a9f34b201d	1454	} else {
Ryosei	27:dd9f27fce7d1	1455	if(LimitSw::IsPressed(QF_SW)) {
Ryosei	27:dd9f27fce7d1	1456	switch(linePara[LINE_TOW_1]) {
Ryosei	27:dd9f27fce7d1	1457	case -2:
Ryosei	27:dd9f27fce7d1	1458	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	1459	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1460	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	1461	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1462	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1463	break;
Ryosei	27:dd9f27fce7d1	1464	case -3:
Ryosei	27:dd9f27fce7d1	1465	tirePWM[TIRE_FL] = -14;
Ryosei	27:dd9f27fce7d1	1466	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1467	tirePWM[TIRE_BR] = 14;
Ryosei	27:dd9f27fce7d1	1468	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1469	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1470	break;
Ryosei	27:dd9f27fce7d1	1471	case -1:
Ryosei	27:dd9f27fce7d1	1472	tirePWM[TIRE_FL] = -17;
Ryosei	27:dd9f27fce7d1	1473	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1474	tirePWM[TIRE_BR] = 17;
Ryosei	27:dd9f27fce7d1	1475	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1476	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1477	break;
Ryosei	27:dd9f27fce7d1	1478	case 0:
Ryosei	27:dd9f27fce7d1	1479	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1480	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1481	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1482	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1483	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1484	break;
Ryosei	27:dd9f27fce7d1	1485	case 1:
Ryosei	27:dd9f27fce7d1	1486	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1487	tirePWM[TIRE_BL] = -17;
Ryosei	27:dd9f27fce7d1	1488	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1489	tirePWM[TIRE_FR] = 17;
Ryosei	27:dd9f27fce7d1	1490	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1491	break;
Ryosei	27:dd9f27fce7d1	1492	case 3:
Ryosei	27:dd9f27fce7d1	1493	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1494	tirePWM[TIRE_BL] = -14;
Ryosei	27:dd9f27fce7d1	1495	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1496	tirePWM[TIRE_FR] = 14;
Ryosei	27:dd9f27fce7d1	1497	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1498	break;
Ryosei	27:dd9f27fce7d1	1499	case 2:
Ryosei	27:dd9f27fce7d1	1500	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1501	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	1502	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1503	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	1504	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1505	break;
Ryosei	27:dd9f27fce7d1	1506	case 'A':
Ryosei	27:dd9f27fce7d1	1507	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1508	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1509	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1510	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1511	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1512	break;
Ryosei	27:dd9f27fce7d1	1513	case 'N':
Ryosei	27:dd9f27fce7d1	1514	tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei	27:dd9f27fce7d1	1515	tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei	27:dd9f27fce7d1	1516	tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei	27:dd9f27fce7d1	1517	tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei	27:dd9f27fce7d1	1518	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1519	break;
Ryosei	27:dd9f27fce7d1	1520	default:
Ryosei	27:dd9f27fce7d1	1521	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1522	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1523	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1524	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1525	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1526	}
Ryosei	27:dd9f27fce7d1	1527	} else {
Ryosei	27:dd9f27fce7d1	1528	switch(linePara[LINE_TOW_1]) {
Ryosei	27:dd9f27fce7d1	1529	case 2:
Ryosei	27:dd9f27fce7d1	1530	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	1531	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1532	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	1533	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1534	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1535	break;
Ryosei	27:dd9f27fce7d1	1536	case 3:
Ryosei	27:dd9f27fce7d1	1537	tirePWM[TIRE_FL] = -14;
Ryosei	27:dd9f27fce7d1	1538	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1539	tirePWM[TIRE_BR] = 14;
Ryosei	27:dd9f27fce7d1	1540	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1541	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1542	break;
Ryosei	27:dd9f27fce7d1	1543	case 1:
Ryosei	27:dd9f27fce7d1	1544	tirePWM[TIRE_FL] = -17;
Ryosei	27:dd9f27fce7d1	1545	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1546	tirePWM[TIRE_BR] = 17;
Ryosei	27:dd9f27fce7d1	1547	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1548	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1549	break;
Ryosei	27:dd9f27fce7d1	1550	case 0:
Ryosei	27:dd9f27fce7d1	1551	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1552	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1553	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1554	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1555	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1556	break;
Ryosei	27:dd9f27fce7d1	1557	case -1:
Ryosei	27:dd9f27fce7d1	1558	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1559	tirePWM[TIRE_BL] = -17;
Ryosei	27:dd9f27fce7d1	1560	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1561	tirePWM[TIRE_FR] = 17;
Ryosei	27:dd9f27fce7d1	1562	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1563	break;
Ryosei	27:dd9f27fce7d1	1564	case -3:
Ryosei	27:dd9f27fce7d1	1565	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1566	tirePWM[TIRE_BL] = -14;
Ryosei	27:dd9f27fce7d1	1567	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1568	tirePWM[TIRE_FR] = 14;
Ryosei	27:dd9f27fce7d1	1569	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1570	break;
Ryosei	27:dd9f27fce7d1	1571	case -2:
Ryosei	27:dd9f27fce7d1	1572	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1573	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	1574	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1575	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	1576	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1577	break;
Ryosei	27:dd9f27fce7d1	1578	case 'A':
Ryosei	27:dd9f27fce7d1	1579	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	1580	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	1581	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	1582	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	1583	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1584	break;
Ryosei	27:dd9f27fce7d1	1585	case 'N':
Ryosei	27:dd9f27fce7d1	1586	tirePWM[TIRE_FL] = tirePWM[TIRE_FL];
Ryosei	27:dd9f27fce7d1	1587	tirePWM[TIRE_BL] = tirePWM[TIRE_BL];
Ryosei	27:dd9f27fce7d1	1588	tirePWM[TIRE_BR] = tirePWM[TIRE_BR];
Ryosei	27:dd9f27fce7d1	1589	tirePWM[TIRE_FR] = tirePWM[TIRE_FR];
Ryosei	27:dd9f27fce7d1	1590	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1591	break;
Ryosei	27:dd9f27fce7d1	1592	default:
Ryosei	27:dd9f27fce7d1	1593	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1594	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1595	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1596	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1597	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1598	}
Ryosei	27:dd9f27fce7d1	1599	}
Ryosei	27:dd9f27fce7d1	1600
Ryosei	27:dd9f27fce7d1	1601	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	1602	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	1603	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	1604	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	1605	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	1606	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	1607	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	1608	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
M_souta	25:b3a9f34b201d	1609	}
Ryosei	27:dd9f27fce7d1	1610	} else if(lineFase == 15 ){ // ライン修正
M_souta	25:b3a9f34b201d	1611	if(linePara[LINE_TOW_1] == 'A' \|\| linePara[LINE_TOW_1] == 'N') {
M_souta	25:b3a9f34b201d	1612	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1613	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1614	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1615	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1616	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1617	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1618	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1619	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1620	} else if(linePara[LINE_TOW_1] > 0) {
M_souta	25:b3a9f34b201d	1621	motor[TIRE_FL].dir = BACK;
M_souta	25:b3a9f34b201d	1622	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1623	motor[TIRE_BL].dir = FOR;
M_souta	25:b3a9f34b201d	1624	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1625	motor[TIRE_BR].dir = FOR;
M_souta	25:b3a9f34b201d	1626	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1627	motor[TIRE_FR].dir = BACK;
M_souta	25:b3a9f34b201d	1628	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1629	} else if(linePara[LINE_TOW_1] < 0) {
M_souta	25:b3a9f34b201d	1630	motor[TIRE_FL].dir = FOR;
M_souta	25:b3a9f34b201d	1631	motor[TIRE_FL].pwm = 16;
M_souta	25:b3a9f34b201d	1632	motor[TIRE_BL].dir = BACK;
M_souta	25:b3a9f34b201d	1633	motor[TIRE_BL].pwm = 16;
M_souta	25:b3a9f34b201d	1634	motor[TIRE_BR].dir = BACK;
M_souta	25:b3a9f34b201d	1635	motor[TIRE_BR].pwm = 16;
M_souta	25:b3a9f34b201d	1636	motor[TIRE_FR].dir = FOR;
M_souta	25:b3a9f34b201d	1637	motor[TIRE_FR].pwm = 16;
M_souta	25:b3a9f34b201d	1638	} else if(linePara[LINE_TOW_1] == 0) {
M_souta	25:b3a9f34b201d	1639	lineFase = 16;
M_souta	25:b3a9f34b201d	1640	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1641	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1642	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1643	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1644	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1645	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1646	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1647	motor[TIRE_FR].pwm = 50;
M_souta	25:b3a9f34b201d	1648	} else {
M_souta	25:b3a9f34b201d	1649	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1650	motor[TIRE_FL].pwm = 50;
M_souta	25:b3a9f34b201d	1651	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1652	motor[TIRE_BL].pwm = 50;
M_souta	25:b3a9f34b201d	1653	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1654	motor[TIRE_BR].pwm = 50;
M_souta	25:b3a9f34b201d	1655	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1656	motor[TIRE_FR].pwm = 50;
M_souta	22:7d93f79a3686	1657	}
M_souta	25:b3a9f34b201d	1658	} else if(lineFase == 16) { // タオル2 解放
Ryosei	27:dd9f27fce7d1	1659	Air[1] = SOLENOID_ON;
M_souta	25:b3a9f34b201d	1660	motor[TIRE_FL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1661	motor[TIRE_BL].dir = BRAKE;
M_souta	25:b3a9f34b201d	1662	motor[TIRE_BR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1663	motor[TIRE_FR].dir = BRAKE;
M_souta	25:b3a9f34b201d	1664	lineFase = 17;
M_souta	25:b3a9f34b201d	1665	} else if(lineFase == 17) { // 前
Ryosei	27:dd9f27fce7d1	1666	if(LimitSw::IsPressed(LSW_UU)) {
Ryosei	27:dd9f27fce7d1	1667	motor[LIFT_U].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1668	motor[LIFT_U].pwm=100;
Ryosei	27:dd9f27fce7d1	1669	} else {
Ryosei	27:dd9f27fce7d1	1670	motor[LIFT_U].dir=BACK;
Ryosei	27:dd9f27fce7d1	1671	motor[LIFT_U].pwm=180;
Ryosei	27:dd9f27fce7d1	1672	}
Ryosei	27:dd9f27fce7d1	1673	switch(linePara[LINE_TOW_2]) {
Ryosei	27:dd9f27fce7d1	1674	case 2:
Ryosei	27:dd9f27fce7d1	1675	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1676	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	1677	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1678	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	1679	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1680	break;
Ryosei	27:dd9f27fce7d1	1681	case 3:
Ryosei	27:dd9f27fce7d1	1682	tirePWM[TIRE_FL] = 10;
Ryosei	27:dd9f27fce7d1	1683	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	1684	tirePWM[TIRE_BR] = -10;
Ryosei	27:dd9f27fce7d1	1685	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	1686	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1687	break;
Ryosei	27:dd9f27fce7d1	1688	case 1:
Ryosei	27:dd9f27fce7d1	1689	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	1690	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	1691	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	1692	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	1693	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1694	break;
Ryosei	27:dd9f27fce7d1	1695	case 0:
Ryosei	27:dd9f27fce7d1	1696	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	1697	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	1698	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	1699	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	1700	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1701	break;
Ryosei	27:dd9f27fce7d1	1702	case -1:
Ryosei	27:dd9f27fce7d1	1703	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	1704	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	1705	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	1706	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	1707	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1708	break;
Ryosei	27:dd9f27fce7d1	1709	case -3:
Ryosei	27:dd9f27fce7d1	1710	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	1711	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	1712	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	1713	tirePWM[TIRE_FR] = -10;
Ryosei	27:dd9f27fce7d1	1714	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1715	break;
Ryosei	27:dd9f27fce7d1	1716	case -2:
Ryosei	27:dd9f27fce7d1	1717	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	1718	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1719	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	1720	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1721	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1722	break;
Ryosei	27:dd9f27fce7d1	1723	case 'A':
Ryosei	27:dd9f27fce7d1	1724	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	1725	lineCheck = true;
Ryosei	27:dd9f27fce7d1	1726	lineCount = 0;
Ryosei	27:dd9f27fce7d1	1727	countW++;
Ryosei	27:dd9f27fce7d1	1728	}
Ryosei	27:dd9f27fce7d1	1729	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	1730	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	1731	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	1732	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	1733	adjAnable = true;
Ryosei	27:dd9f27fce7d1	1734	break;
Ryosei	27:dd9f27fce7d1	1735	case 'N':
Ryosei	27:dd9f27fce7d1	1736	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1737	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1738	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1739	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1740	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1741	break;
Ryosei	27:dd9f27fce7d1	1742	default:
Ryosei	27:dd9f27fce7d1	1743	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	1744	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	1745	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	1746	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	1747	adjAnable = false;
Ryosei	27:dd9f27fce7d1	1748	}
Ryosei	27:dd9f27fce7d1	1749
Ryosei	27:dd9f27fce7d1	1750	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	1751	adj = 0;
Ryosei	27:dd9f27fce7d1	1752	} else {
Ryosei	27:dd9f27fce7d1	1753	adj = 0;
Ryosei	27:dd9f27fce7d1	1754	}
Ryosei	27:dd9f27fce7d1	1755
Ryosei	27:dd9f27fce7d1	1756	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	1757	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	1758	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	1759	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	1760	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	1761	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	1762	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	1763	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	1764
Ryosei	27:dd9f27fce7d1	1765	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	1766	lineCount++;
Ryosei	27:dd9f27fce7d1	1767	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	1768	}
Ryosei	27:dd9f27fce7d1	1769	if(countW == 1) {
Ryosei	27:dd9f27fce7d1	1770	countW = 0;
Ryosei	27:dd9f27fce7d1	1771	lineFase = 100;
Ryosei	27:dd9f27fce7d1	1772	lineCount = 0;
Ryosei	27:dd9f27fce7d1	1773	lineCheck = false;
Ryosei	27:dd9f27fce7d1	1774	}
Ryosei	27:dd9f27fce7d1	1775	} else if(lineFase==100) {
Ryosei	27:dd9f27fce7d1	1776	LedOut(6);
Ryosei	27:dd9f27fce7d1	1777	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	1778	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	1779	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	1780	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	1781	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	1782	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	1783	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	1784	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	1785	if(LimitSw::IsPressed(LSW_UU)) {
Ryosei	27:dd9f27fce7d1	1786	motor[LIFT_U].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1787	motor[LIFT_U].pwm=100;
Ryosei	27:dd9f27fce7d1	1788	} else {
Ryosei	27:dd9f27fce7d1	1789	motor[LIFT_U].dir=BACK;
Ryosei	27:dd9f27fce7d1	1790	motor[LIFT_U].pwm=150;
Ryosei	27:dd9f27fce7d1	1791	}
Ryosei	27:dd9f27fce7d1	1792	if((Ult_left>0)&&(Ult_right>0)&&(Ult_left<30)&&(Ult_right<30)) {
Ryosei	27:dd9f27fce7d1	1793	lineFase=101;
Ryosei	27:dd9f27fce7d1	1794	}
Ryosei	27:dd9f27fce7d1	1795	} else if(lineFase==101) {
Ryosei	27:dd9f27fce7d1	1796	//位置調整
Ryosei	27:dd9f27fce7d1	1797	//(P制御)
Ryosei	27:dd9f27fce7d1	1798	if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei	27:dd9f27fce7d1	1799	if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei	27:dd9f27fce7d1	1800	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1801	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1802	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1803	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1804	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	1805	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	1806	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	1807	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	1808	lineFase=102;//system lineFase increasing
Ryosei	27:dd9f27fce7d1	1809	} else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei	27:dd9f27fce7d1	1810	if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei	27:dd9f27fce7d1	1811	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1812	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1813	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1814	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1815	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	1816	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	1817	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	1818	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	1819	} else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei	27:dd9f27fce7d1	1820	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1821	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1822	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1823	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1824	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	1825	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	1826	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	1827	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	1828	}
Ryosei	27:dd9f27fce7d1	1829	} else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei	27:dd9f27fce7d1	1830	if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei	27:dd9f27fce7d1	1831	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1832	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1833	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1834	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1835	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	1836	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	1837	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	1838	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	1839	} else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei	27:dd9f27fce7d1	1840	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1841	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1842	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1843	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1844	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	1845	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	1846	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	1847	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	1848	}
Ryosei	27:dd9f27fce7d1	1849	} else { //どっちもあってない場合
Ryosei	27:dd9f27fce7d1	1850	if( (Ult_left+Ult_right)<=25\|\|(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei	27:dd9f27fce7d1	1851	if((Ult_left-Ult_right)>10\|\|((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき
Ryosei	27:dd9f27fce7d1	1852	if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	1853	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1854	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1855	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1856	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1857	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	1858	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	1859	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	1860	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	1861	} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	1862	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1863	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1864	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1865	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1866	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	1867	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	1868	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	1869	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	1870	}
Ryosei	27:dd9f27fce7d1	1871	} else { //傾きが大きくなくて離れているとき
Ryosei	27:dd9f27fce7d1	1872	if((Ult_right+Ult_left)<=25) { //近すぎるとき
Ryosei	27:dd9f27fce7d1	1873	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1874	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1875	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1876	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1877	motor[TIRE_FL].pwm=20;
Ryosei	27:dd9f27fce7d1	1878	motor[TIRE_FR].pwm=20;
Ryosei	27:dd9f27fce7d1	1879	motor[TIRE_BL].pwm=20;
Ryosei	27:dd9f27fce7d1	1880	motor[TIRE_BR].pwm=20;
Ryosei	27:dd9f27fce7d1	1881	} else if((Ult_right+Ult_left)>=35) { //離れているとき
Ryosei	27:dd9f27fce7d1	1882	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1883	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1884	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1885	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1886	motor[TIRE_FL].pwm=20;
Ryosei	27:dd9f27fce7d1	1887	motor[TIRE_FR].pwm=20;
Ryosei	27:dd9f27fce7d1	1888	motor[TIRE_BL].pwm=20;
Ryosei	27:dd9f27fce7d1	1889	motor[TIRE_BR].pwm=20;
Ryosei	27:dd9f27fce7d1	1890	}
Ryosei	27:dd9f27fce7d1	1891	}
Ryosei	27:dd9f27fce7d1	1892
Ryosei	27:dd9f27fce7d1	1893	} else { //さほど離れてはいないが傾きが大きいとき
Ryosei	27:dd9f27fce7d1	1894	if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	1895	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1896	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1897	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1898	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1899	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	1900	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	1901	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	1902	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	1903	} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	1904	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1905	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1906	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1907	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1908	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	1909	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	1910	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	1911	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	1912	}
Ryosei	27:dd9f27fce7d1	1913	}
Ryosei	27:dd9f27fce7d1	1914	}
Ryosei	27:dd9f27fce7d1	1915
Ryosei	27:dd9f27fce7d1	1916	} else {//データを受け取ってないとき
Ryosei	27:dd9f27fce7d1	1917	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1918	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1919	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1920	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	1921	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	1922	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	1923	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	1924	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	1925	}
Ryosei	27:dd9f27fce7d1	1926
Ryosei	27:dd9f27fce7d1	1927	} else if(lineFase==102) {
Ryosei	27:dd9f27fce7d1	1928	Air[CLOTHESPIN]=SOLENOID_OFF;
Ryosei	27:dd9f27fce7d1	1929	LedOut(1);
Ryosei	27:dd9f27fce7d1	1930	//リミットスイッチに当てる
Ryosei	27:dd9f27fce7d1	1931	static int count2=0;
Ryosei	27:dd9f27fce7d1	1932	if(count2==0) {
Ryosei	27:dd9f27fce7d1	1933	if(LimitSw::IsPressed(RIGHTlim)) {
Ryosei	27:dd9f27fce7d1	1934	count2=1;
Ryosei	27:dd9f27fce7d1	1935	} else if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei	27:dd9f27fce7d1	1936	if( ((Ult_left+Ult_right)<=25)\|\|((Ult_left+Ult_right)>=35) ) {
Ryosei	27:dd9f27fce7d1	1937	if(((Ult_left-Ult_right)>=5)\|\|((Ult_left-Ult_right)<=-5)) {
Ryosei	27:dd9f27fce7d1	1938	if(Ult_left-Ult_right<=0) {
Ryosei	27:dd9f27fce7d1	1939	//右
Ryosei	27:dd9f27fce7d1	1940	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1941	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1942	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1943	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1944
Ryosei	27:dd9f27fce7d1	1945	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	1946	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	1947
Ryosei	27:dd9f27fce7d1	1948	motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	1949	motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	1950	} else if(Ult_left-Ult_right>0) {
Ryosei	27:dd9f27fce7d1	1951	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1952	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1953	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1954	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1955
Ryosei	27:dd9f27fce7d1	1956	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	1957	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	1958	motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	1959	motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	1960	}
Ryosei	27:dd9f27fce7d1	1961	} else if((Ult_left+Ult_right)<=25) {
Ryosei	27:dd9f27fce7d1	1962	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1963	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1964	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1965	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1966
Ryosei	27:dd9f27fce7d1	1967	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	1968	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	1969	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	1970	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	1971	} else if((Ult_left+Ult_right)>=35) {
Ryosei	27:dd9f27fce7d1	1972	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1973	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1974	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1975	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1976
Ryosei	27:dd9f27fce7d1	1977	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	1978	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	1979	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	1980	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	1981	}
Ryosei	27:dd9f27fce7d1	1982	} else {
Ryosei	27:dd9f27fce7d1	1983	if(Ult_left-Ult_right<=0) {
Ryosei	27:dd9f27fce7d1	1984	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1985	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1986	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1987	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1988
Ryosei	27:dd9f27fce7d1	1989	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	1990	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	1991
Ryosei	27:dd9f27fce7d1	1992	motor[TIRE_BL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	1993	motor[TIRE_BR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	1994	} else if(Ult_left-Ult_right>0) {
Ryosei	27:dd9f27fce7d1	1995	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	1996	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	1997	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	1998	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	1999
Ryosei	27:dd9f27fce7d1	2000	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	2001	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	2002	motor[TIRE_FL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	2003	motor[TIRE_FR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	2004	}
Ryosei	27:dd9f27fce7d1	2005	}
Ryosei	27:dd9f27fce7d1	2006
Ryosei	27:dd9f27fce7d1	2007	} else {
Ryosei	27:dd9f27fce7d1	2008	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2009	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2010	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2011	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2012	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	2013	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	2014	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	2015	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	2016
Ryosei	27:dd9f27fce7d1	2017	}
Ryosei	27:dd9f27fce7d1	2018	} else if(count2==1) {
Ryosei	27:dd9f27fce7d1	2019	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2020	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2021	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2022	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2023	motor[TIRE_FL].pwm=255;
Ryosei	27:dd9f27fce7d1	2024	motor[TIRE_FR].pwm=255;
Ryosei	27:dd9f27fce7d1	2025	motor[TIRE_BL].pwm=255;
Ryosei	27:dd9f27fce7d1	2026	motor[TIRE_BR].pwm=255;
Ryosei	27:dd9f27fce7d1	2027	lineFase=103;
Ryosei	27:dd9f27fce7d1	2028	}
Ryosei	27:dd9f27fce7d1	2029	} else if(lineFase==103) {
Ryosei	27:dd9f27fce7d1	2030	if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei	27:dd9f27fce7d1	2031	if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei	27:dd9f27fce7d1	2032	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2033	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2034	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2035	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2036	motor[TIRE_FL].pwm=255;
Ryosei	27:dd9f27fce7d1	2037	motor[TIRE_FR].pwm=255;
Ryosei	27:dd9f27fce7d1	2038	motor[TIRE_BL].pwm=255;
Ryosei	27:dd9f27fce7d1	2039	motor[TIRE_BR].pwm=255;
Ryosei	27:dd9f27fce7d1	2040	lineFase=104;//system lineFase increasing
Ryosei	27:dd9f27fce7d1	2041	} else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei	27:dd9f27fce7d1	2042	if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei	27:dd9f27fce7d1	2043	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2044	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2045	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2046	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2047	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	2048	motor[TIRE_FR].pwm=13;
Ryosei	27:dd9f27fce7d1	2049	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	2050	motor[TIRE_BR].pwm=13;
Ryosei	27:dd9f27fce7d1	2051	} else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei	27:dd9f27fce7d1	2052	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2053	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2054	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2055	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2056	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	2057	motor[TIRE_FR].pwm=13;
Ryosei	27:dd9f27fce7d1	2058	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	2059	motor[TIRE_BR].pwm=13;
Ryosei	27:dd9f27fce7d1	2060	}
Ryosei	27:dd9f27fce7d1	2061	} else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei	27:dd9f27fce7d1	2062	if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei	27:dd9f27fce7d1	2063	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2064	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2065	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2066	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2067	motor[TIRE_FL].pwm=13;
Ryosei	27:dd9f27fce7d1	2068	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	2069	motor[TIRE_BL].pwm=13;
Ryosei	27:dd9f27fce7d1	2070	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	2071	} else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei	27:dd9f27fce7d1	2072	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2073	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2074	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2075	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2076	motor[TIRE_FL].pwm=13;
Ryosei	27:dd9f27fce7d1	2077	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	2078	motor[TIRE_BL].pwm=13;
Ryosei	27:dd9f27fce7d1	2079	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	2080	}
Ryosei	27:dd9f27fce7d1	2081	} else { //どっちもあってない場合
Ryosei	27:dd9f27fce7d1	2082	if( (Ult_left+Ult_right)<=25\|\|(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei	27:dd9f27fce7d1	2083	if((Ult_left-Ult_right)>0\|\|((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
Ryosei	27:dd9f27fce7d1	2084	if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	2085	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2086	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2087	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2088	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2089	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	2090	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	2091	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	2092	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	2093	} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	2094	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2095	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2096	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2097	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2098	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	2099	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	2100	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	2101	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	2102	}
Ryosei	27:dd9f27fce7d1	2103	}
Ryosei	27:dd9f27fce7d1	2104	} else { //さほど離れてはいないが傾きが大きいとき
Ryosei	27:dd9f27fce7d1	2105	if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	2106	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2107	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2108	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2109	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2110	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	2111	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	2112	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	2113	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	2114	} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	2115	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2116	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2117	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2118	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2119	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	2120	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	2121	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	2122	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	2123	}
Ryosei	27:dd9f27fce7d1	2124	}
Ryosei	27:dd9f27fce7d1	2125	}
Ryosei	27:dd9f27fce7d1	2126
Ryosei	27:dd9f27fce7d1	2127	} else {//データを受け取ってないとき
Ryosei	27:dd9f27fce7d1	2128	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2129	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2130	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2131	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2132	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	2133	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	2134	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	2135	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	2136	}
Ryosei	27:dd9f27fce7d1	2137	} else if(lineFase==104) {
Ryosei	27:dd9f27fce7d1	2138	static int count3=0;
Ryosei	27:dd9f27fce7d1	2139	static int loop=0;
Ryosei	27:dd9f27fce7d1	2140	pc.printf("%d\r\n",loop);
Ryosei	27:dd9f27fce7d1	2141	Air[CLOTHESPIN]=1;
Ryosei	27:dd9f27fce7d1	2142	if(count3==0) {
Ryosei	27:dd9f27fce7d1	2143	loop++;
Ryosei	27:dd9f27fce7d1	2144	if(loop==60) {
Ryosei	27:dd9f27fce7d1	2145	count3=1;
Ryosei	27:dd9f27fce7d1	2146	}
Ryosei	27:dd9f27fce7d1	2147	} else if(count3==1) {
Ryosei	27:dd9f27fce7d1	2148	Air[CLOTHESPIN]=0;
Ryosei	27:dd9f27fce7d1	2149	lineFase=105;
Ryosei	27:dd9f27fce7d1	2150
Ryosei	27:dd9f27fce7d1	2151	}
Ryosei	27:dd9f27fce7d1	2152	} else if(lineFase==105) {
Ryosei	27:dd9f27fce7d1	2153	LedOut(6);
Ryosei	27:dd9f27fce7d1	2154	Air[CLOTHESPIN]=0;
Ryosei	27:dd9f27fce7d1	2155	if(!(LimitSw::IsPressed(LEFTlim))) {
Ryosei	27:dd9f27fce7d1	2156
Ryosei	27:dd9f27fce7d1	2157	//超音波
Ryosei	27:dd9f27fce7d1	2158	if(Ult_right>0) {
Ryosei	27:dd9f27fce7d1	2159	if(Ult_right<15) {
Ryosei	27:dd9f27fce7d1	2160	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2161	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2162	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2163	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2164	motor[TIRE_FL].pwm=25+(15-Ult_left);
Ryosei	27:dd9f27fce7d1	2165	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	2166	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	2167	motor[TIRE_BR].pwm=25+(15-Ult_left);
Ryosei	27:dd9f27fce7d1	2168	} else if(Ult_right>=15) {
Ryosei	27:dd9f27fce7d1	2169	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2170	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2171	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2172	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2173	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	2174	motor[TIRE_FR].pwm=25+(15-Ult_left);
Ryosei	27:dd9f27fce7d1	2175	motor[TIRE_BL].pwm=25+(15-Ult_left);
Ryosei	27:dd9f27fce7d1	2176	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	2177	}
Ryosei	27:dd9f27fce7d1	2178	} else {
Ryosei	27:dd9f27fce7d1	2179	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2180	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	2181	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	2182	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2183	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	2184	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	2185	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	2186	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	2187	}
Ryosei	27:dd9f27fce7d1	2188
Ryosei	27:dd9f27fce7d1	2189
Ryosei	27:dd9f27fce7d1	2190	} else {
Ryosei	27:dd9f27fce7d1	2191	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2192	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2193	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2194	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	2195	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	2196	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	2197	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	2198	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	2199	lineFase=106;
Ryosei	27:dd9f27fce7d1	2200	}
Ryosei	27:dd9f27fce7d1	2201
Ryosei	27:dd9f27fce7d1	2202	} else if(lineFase==106) {
Ryosei	27:dd9f27fce7d1	2203	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2204	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2205	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	2206	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	2207	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	2208	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	2209	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	2210	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	2211	if(linePara[4]!='N') {
Ryosei	27:dd9f27fce7d1	2212	lineFase=19;
Ryosei	27:dd9f27fce7d1	2213	}
Ryosei	27:dd9f27fce7d1	2214	} else if(lineFase == 18) { // 前低速
Ryosei	27:dd9f27fce7d1	2215	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2216	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2217	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2218	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2219	if(linePara[4] != 'N') {
Ryosei	27:dd9f27fce7d1	2220	lineFase = 19;
Ryosei	27:dd9f27fce7d1	2221	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2222	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2223	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2224	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2225	}
Ryosei	27:dd9f27fce7d1	2226	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2227	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2228	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2229	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2230	} else if(lineFase == 19) { // 左
Ryosei	27:dd9f27fce7d1	2231	if(!(LimitSw::IsPressed(LSW_UB))) {
Ryosei	27:dd9f27fce7d1	2232	motor[LIFT_U].dir=FOR;
Ryosei	27:dd9f27fce7d1	2233	motor[LIFT_U].pwm=150;
Ryosei	27:dd9f27fce7d1	2234	} else {
Ryosei	27:dd9f27fce7d1	2235	motor[LIFT_U].dir=FOR;
Ryosei	27:dd9f27fce7d1	2236	motor[LIFT_U].pwm=150;
Ryosei	27:dd9f27fce7d1	2237	}
Ryosei	27:dd9f27fce7d1	2238	switch(linePara[3]) {
Ryosei	27:dd9f27fce7d1	2239	case -2:
Ryosei	27:dd9f27fce7d1	2240	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2241	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2242	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2243	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2244	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2245	break;
Ryosei	27:dd9f27fce7d1	2246	case -3:
Ryosei	27:dd9f27fce7d1	2247	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2248	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	2249	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2250	tirePWM[TIRE_FR] = -10;
Ryosei	27:dd9f27fce7d1	2251	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2252	break;
Ryosei	27:dd9f27fce7d1	2253	case -1:
Ryosei	27:dd9f27fce7d1	2254	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2255	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	2256	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2257	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	2258	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2259	break;
Ryosei	27:dd9f27fce7d1	2260	case 0:
Ryosei	27:dd9f27fce7d1	2261	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2262	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2263	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2264	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2265	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2266	break;
Ryosei	27:dd9f27fce7d1	2267	case 1:
Ryosei	27:dd9f27fce7d1	2268	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	2269	tirePWM[TIRE_BL] = 40;
Ryosei	27:dd9f27fce7d1	2270	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	2271	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2272	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2273	break;
Ryosei	27:dd9f27fce7d1	2274	case 3:
Ryosei	27:dd9f27fce7d1	2275	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	2276	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2277	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	2278	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2279	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2280	break;
Ryosei	27:dd9f27fce7d1	2281	case 2:
Ryosei	27:dd9f27fce7d1	2282	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2283	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2284	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2285	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2286	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2287	break;
Ryosei	27:dd9f27fce7d1	2288	case 'A':
Ryosei	27:dd9f27fce7d1	2289	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	2290	lineCheck = true;
Ryosei	27:dd9f27fce7d1	2291	lineCount = 0;
Ryosei	27:dd9f27fce7d1	2292	countW++;
Ryosei	27:dd9f27fce7d1	2293	}
Ryosei	27:dd9f27fce7d1	2294	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2295	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2296	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2297	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2298	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2299	break;
Ryosei	27:dd9f27fce7d1	2300	case 'N':
Ryosei	27:dd9f27fce7d1	2301	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2302	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2303	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2304	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2305	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2306	break;
Ryosei	27:dd9f27fce7d1	2307	default:
Ryosei	27:dd9f27fce7d1	2308	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2309	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2310	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2311	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2312	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2313	}
Ryosei	27:dd9f27fce7d1	2314
Ryosei	27:dd9f27fce7d1	2315	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	2316	if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
Ryosei	27:dd9f27fce7d1	2317	} else {
Ryosei	27:dd9f27fce7d1	2318	adj = 0;
Ryosei	27:dd9f27fce7d1	2319	}
Ryosei	27:dd9f27fce7d1	2320
Ryosei	27:dd9f27fce7d1	2321	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	2322	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	2323	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	2324	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	2325	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	2326	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	2327	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	2328	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	2329
Ryosei	27:dd9f27fce7d1	2330	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	2331	lineCount++;
Ryosei	27:dd9f27fce7d1	2332	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	2333	}
Ryosei	27:dd9f27fce7d1	2334
Ryosei	27:dd9f27fce7d1	2335	/*
Ryosei	27:dd9f27fce7d1	2336	if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
Ryosei	27:dd9f27fce7d1	2337	targetCount = 3;
Ryosei	27:dd9f27fce7d1	2338	} else if(LimitSw::IsPressed(TOWEL1_SW)) {
Ryosei	27:dd9f27fce7d1	2339	targetCount = 3;
Ryosei	27:dd9f27fce7d1	2340	} else {
Ryosei	27:dd9f27fce7d1	2341	targetCount = 2;
Ryosei	27:dd9f27fce7d1	2342	}
Ryosei	27:dd9f27fce7d1	2343	*/
Ryosei	27:dd9f27fce7d1	2344
Ryosei	27:dd9f27fce7d1	2345	targetCount = 3;
Ryosei	27:dd9f27fce7d1	2346
Ryosei	27:dd9f27fce7d1	2347	if(countW == targetCount) {
Ryosei	27:dd9f27fce7d1	2348	countW = 0;
Ryosei	27:dd9f27fce7d1	2349	lineFase = 20;
Ryosei	27:dd9f27fce7d1	2350	lineCount = 0;
Ryosei	27:dd9f27fce7d1	2351	lineCheck = false;
Ryosei	27:dd9f27fce7d1	2352	}
Ryosei	27:dd9f27fce7d1	2353	} else if(lineFase == 20) { // 左低速
Ryosei	27:dd9f27fce7d1	2354	if(!(LimitSw::IsPressed(LSW_UB))) {
Ryosei	27:dd9f27fce7d1	2355	motor[LIFT_U].dir=FOR;
Ryosei	27:dd9f27fce7d1	2356	motor[LIFT_U].pwm=150;
Ryosei	27:dd9f27fce7d1	2357	} else {
Ryosei	27:dd9f27fce7d1	2358	motor[LIFT_U].dir=FOR;
Ryosei	27:dd9f27fce7d1	2359	motor[LIFT_U].pwm=150;
Ryosei	27:dd9f27fce7d1	2360	}
Ryosei	27:dd9f27fce7d1	2361	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	2362	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2363	motor[TIRE_BR].dir = FOR;
Ryosei	27:dd9f27fce7d1	2364	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2365	if (linePara[2] == 'N') {
Ryosei	27:dd9f27fce7d1	2366	//(!!LimitSw::Ispressed(SHEETS_SW)) {
Ryosei	27:dd9f27fce7d1	2367	//ineFase = 20;
Ryosei	27:dd9f27fce7d1	2368	//else if(LimitSw::IsPressed(TOWEL2_SW) {
Ryosei	27:dd9f27fce7d1	2369	//ineFase = 14;
Ryosei	27:dd9f27fce7d1	2370	//else {
Ryosei	27:dd9f27fce7d1	2371	lineFase = 21;
Ryosei	27:dd9f27fce7d1	2372	//
Ryosei	27:dd9f27fce7d1	2373	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2374	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2375	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2376	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2377	}
Ryosei	27:dd9f27fce7d1	2378	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2379	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2380	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2381	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2382	} else if(lineFase == 21) {
Ryosei	27:dd9f27fce7d1	2383	if(!(LimitSw::IsPressed(LSW_UB))) {
Ryosei	27:dd9f27fce7d1	2384	motor[LIFT_U].dir=FOR;
Ryosei	27:dd9f27fce7d1	2385	motor[LIFT_U].pwm=150;
Ryosei	27:dd9f27fce7d1	2386	} else {
Ryosei	27:dd9f27fce7d1	2387	motor[LIFT_U].dir=FOR;
Ryosei	27:dd9f27fce7d1	2388	motor[LIFT_U].pwm=150;
Ryosei	27:dd9f27fce7d1	2389	}
Ryosei	27:dd9f27fce7d1	2390	switch(linePara[2]) {
Ryosei	27:dd9f27fce7d1	2391	case -2:
Ryosei	27:dd9f27fce7d1	2392	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2393	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	2394	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2395	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	2396	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2397	break;
Ryosei	27:dd9f27fce7d1	2398	case -3:
Ryosei	27:dd9f27fce7d1	2399	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	2400	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	2401	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	2402	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	2403	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2404	break;
Ryosei	27:dd9f27fce7d1	2405	case -1:
Ryosei	27:dd9f27fce7d1	2406	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	2407	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	2408	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	2409	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	2410	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2411	break;
Ryosei	27:dd9f27fce7d1	2412	case 0:
Ryosei	27:dd9f27fce7d1	2413	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2414	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	2415	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2416	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	2417	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2418	break;
Ryosei	27:dd9f27fce7d1	2419	case 1:
Ryosei	27:dd9f27fce7d1	2420	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2421	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	2422	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2423	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	2424	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2425	break;
Ryosei	27:dd9f27fce7d1	2426	case 3:
Ryosei	27:dd9f27fce7d1	2427	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2428	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	2429	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2430	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	2431	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2432	break;
Ryosei	27:dd9f27fce7d1	2433	case 2:
Ryosei	27:dd9f27fce7d1	2434	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2435	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2436	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2437	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2438	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2439	break;
Ryosei	27:dd9f27fce7d1	2440	case 'A':
Ryosei	27:dd9f27fce7d1	2441	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	2442	lineCheck = true;
Ryosei	27:dd9f27fce7d1	2443	lineCount = 0;
Ryosei	27:dd9f27fce7d1	2444	countW++;
Ryosei	27:dd9f27fce7d1	2445	}
Ryosei	27:dd9f27fce7d1	2446	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2447	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	2448	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2449	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	2450	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2451	break;
Ryosei	27:dd9f27fce7d1	2452	case 'N':
Ryosei	27:dd9f27fce7d1	2453	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2454	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2455	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2456	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2457	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2458	break;
Ryosei	27:dd9f27fce7d1	2459	default:
Ryosei	27:dd9f27fce7d1	2460	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2461	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2462	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2463	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2464	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2465	}
Ryosei	27:dd9f27fce7d1	2466
Ryosei	27:dd9f27fce7d1	2467	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	2468	if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
Ryosei	27:dd9f27fce7d1	2469	} else {
Ryosei	27:dd9f27fce7d1	2470	adj = 0;
Ryosei	27:dd9f27fce7d1	2471	}
Ryosei	27:dd9f27fce7d1	2472
Ryosei	27:dd9f27fce7d1	2473	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei	27:dd9f27fce7d1	2474	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	2475	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	2476	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	2477	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei	27:dd9f27fce7d1	2478	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	2479	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	2480	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	2481
Ryosei	27:dd9f27fce7d1	2482	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	2483	lineCount++;
Ryosei	27:dd9f27fce7d1	2484	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	2485	}
Ryosei	27:dd9f27fce7d1	2486	if(countW == 3) {
Ryosei	27:dd9f27fce7d1	2487	countW = 0;
Ryosei	27:dd9f27fce7d1	2488	lineFase = 22;
Ryosei	27:dd9f27fce7d1	2489	lineCount = 0;
Ryosei	27:dd9f27fce7d1	2490	lineCheck = false;
Ryosei	27:dd9f27fce7d1	2491	}
Ryosei	27:dd9f27fce7d1	2492	} else if(lineFase == 22) {
Ryosei	27:dd9f27fce7d1	2493	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	2494	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2495	motor[TIRE_BR].dir = FOR;
Ryosei	27:dd9f27fce7d1	2496	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2497	if (linePara[2] == 'N') {
Ryosei	27:dd9f27fce7d1	2498	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2499	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2500	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2501	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2502	}
Ryosei	27:dd9f27fce7d1	2503	motor[TIRE_FL].pwm = 30;
Ryosei	27:dd9f27fce7d1	2504	motor[TIRE_BL].pwm = 30;
Ryosei	27:dd9f27fce7d1	2505	motor[TIRE_BR].pwm = 30;
Ryosei	27:dd9f27fce7d1	2506	motor[TIRE_FR].pwm = 30;
Ryosei	27:dd9f27fce7d1	2507	} else {
Ryosei	27:dd9f27fce7d1	2508	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2509	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2510	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2511	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2512	}
Ryosei	27:dd9f27fce7d1	2513
kishibekairohan	13:b6e02d6261d7	2514	}
kishibekairohan	13:b6e02d6261d7	2515	#endif
kishibekairohan	13:b6e02d6261d7	2516
kishibekairohan	13:b6e02d6261d7	2517	#if USE_PROCESS_NUM>5
Ryosei	27:dd9f27fce7d1	2518	static void Process5()
Ryosei	27:dd9f27fce7d1	2519	{
Ryosei	27:dd9f27fce7d1	2520
Ryosei	27:dd9f27fce7d1	2521	/* ************************************** //
Ryosei	27:dd9f27fce7d1	2522
Ryosei	27:dd9f27fce7d1	2523	青ゾーン青ゾーン青ゾーン
Ryosei	27:dd9f27fce7d1	2524
Ryosei	27:dd9f27fce7d1	2525	// ************************************** */
Ryosei	27:dd9f27fce7d1	2526
Ryosei	27:dd9f27fce7d1	2527	for(int i = 0; i < 8; i++) {
Ryosei	27:dd9f27fce7d1	2528	linePara[i] = lineCast(LineHub::GetPara(i));
Ryosei	27:dd9f27fce7d1	2529	}
Ryosei	27:dd9f27fce7d1	2530
Ryosei	27:dd9f27fce7d1	2531	if(lineFase == 0) {
Ryosei	28:479631c2de29	2532	LedOut(1);
Ryosei	27:dd9f27fce7d1	2533	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2534	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2535	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2536	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2537	if(linePara[0] != 'N' && linePara[0] != 'A') {
Ryosei	27:dd9f27fce7d1	2538	lineFase = 1;
Ryosei	27:dd9f27fce7d1	2539	}
Ryosei	27:dd9f27fce7d1	2540	motor[TIRE_FL].pwm = 30;
Ryosei	27:dd9f27fce7d1	2541	motor[TIRE_BL].pwm = 30;
Ryosei	27:dd9f27fce7d1	2542	motor[TIRE_BR].pwm = 30;
Ryosei	27:dd9f27fce7d1	2543	motor[TIRE_FR].pwm = 30;
Ryosei	27:dd9f27fce7d1	2544	} else if(lineFase == 1) { // 前ライントレース
Ryosei	27:dd9f27fce7d1	2545	switch(linePara[0]) {
Ryosei	27:dd9f27fce7d1	2546	case -2:
Ryosei	27:dd9f27fce7d1	2547	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2548	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2549	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2550	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2551	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2552	break;
Ryosei	27:dd9f27fce7d1	2553	case -3:
Ryosei	27:dd9f27fce7d1	2554	tirePWM[TIRE_FL] = 10;
Ryosei	27:dd9f27fce7d1	2555	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2556	tirePWM[TIRE_BR] = -10;
Ryosei	27:dd9f27fce7d1	2557	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2558	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2559	break;
Ryosei	27:dd9f27fce7d1	2560	case -1:
Ryosei	27:dd9f27fce7d1	2561	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	2562	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2563	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	2564	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2565	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2566	break;
Ryosei	27:dd9f27fce7d1	2567	case 0:
Ryosei	27:dd9f27fce7d1	2568	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	2569	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2570	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	2571	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2572	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2573	break;
Ryosei	27:dd9f27fce7d1	2574	case 1:
Ryosei	27:dd9f27fce7d1	2575	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	2576	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	2577	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	2578	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	2579	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2580	break;
Ryosei	27:dd9f27fce7d1	2581	case 3:
Ryosei	27:dd9f27fce7d1	2582	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	2583	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	2584	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	2585	tirePWM[TIRE_FR] = -10;
Ryosei	27:dd9f27fce7d1	2586	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2587	break;
Ryosei	27:dd9f27fce7d1	2588	case 2:
Ryosei	27:dd9f27fce7d1	2589	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	2590	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2591	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	2592	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2593	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2594	break;
Ryosei	27:dd9f27fce7d1	2595	case 'A':
Ryosei	27:dd9f27fce7d1	2596	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	2597	lineCheck = true;
Ryosei	27:dd9f27fce7d1	2598	lineCount = 0;
Ryosei	27:dd9f27fce7d1	2599	countW++;
Ryosei	27:dd9f27fce7d1	2600	}
Ryosei	27:dd9f27fce7d1	2601	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	2602	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2603	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	2604	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2605	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2606	break;
Ryosei	27:dd9f27fce7d1	2607	case 'N':
Ryosei	27:dd9f27fce7d1	2608	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2609	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2610	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2611	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2612	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2613	break;
Ryosei	27:dd9f27fce7d1	2614	default:
Ryosei	27:dd9f27fce7d1	2615	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2616	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2617	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2618	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2619	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2620	}
Ryosei	27:dd9f27fce7d1	2621
Ryosei	27:dd9f27fce7d1	2622	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	2623	if(linePara[2] != 'A' && linePara[2] != 'N') adj = linePara[2];
Ryosei	27:dd9f27fce7d1	2624	} else {
Ryosei	27:dd9f27fce7d1	2625	adj = 0;
Ryosei	27:dd9f27fce7d1	2626	}
Ryosei	27:dd9f27fce7d1	2627
Ryosei	27:dd9f27fce7d1	2628	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	2629	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	2630	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	2631	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	2632	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	2633	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	2634	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	2635	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	2636
Ryosei	27:dd9f27fce7d1	2637	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	2638	lineCount++;
Ryosei	27:dd9f27fce7d1	2639	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	2640	}
Ryosei	27:dd9f27fce7d1	2641	if(countW == 3) {
Ryosei	27:dd9f27fce7d1	2642	countW = 0;
Ryosei	27:dd9f27fce7d1	2643	lineFase = 2;
Ryosei	27:dd9f27fce7d1	2644	lineCount = 0;
Ryosei	27:dd9f27fce7d1	2645	lineCheck = false;
Ryosei	27:dd9f27fce7d1	2646	}
Ryosei	27:dd9f27fce7d1	2647	} else if(lineFase == 2) { // 前低速
Ryosei	27:dd9f27fce7d1	2648	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2649	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2650	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2651	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2652	if(linePara[3] == 0) {
Ryosei	27:dd9f27fce7d1	2653	lineFase = 3;
Ryosei	27:dd9f27fce7d1	2654	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2655	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2656	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2657	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2658	}
Ryosei	27:dd9f27fce7d1	2659	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2660	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2661	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2662	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2663	} else if(lineFase == 3) { // 左ライントレース
Ryosei	27:dd9f27fce7d1	2664	switch(linePara[3]) {
Ryosei	27:dd9f27fce7d1	2665	case -2:
Ryosei	27:dd9f27fce7d1	2666	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2667	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2668	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2669	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2670	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2671	break;
Ryosei	27:dd9f27fce7d1	2672	case -3:
Ryosei	27:dd9f27fce7d1	2673	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2674	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	2675	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2676	tirePWM[TIRE_FR] = -10;
Ryosei	27:dd9f27fce7d1	2677	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2678	break;
Ryosei	27:dd9f27fce7d1	2679	case -1:
Ryosei	27:dd9f27fce7d1	2680	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2681	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	2682	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2683	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	2684	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2685	break;
Ryosei	27:dd9f27fce7d1	2686	case 0:
Ryosei	27:dd9f27fce7d1	2687	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2688	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2689	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2690	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2691	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2692	break;
Ryosei	27:dd9f27fce7d1	2693	case 1:
Ryosei	27:dd9f27fce7d1	2694	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	2695	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2696	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	2697	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2698	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2699	break;
Ryosei	27:dd9f27fce7d1	2700	case 3:
Ryosei	27:dd9f27fce7d1	2701	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	2702	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2703	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	2704	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2705	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2706	break;
Ryosei	27:dd9f27fce7d1	2707	case 2:
Ryosei	27:dd9f27fce7d1	2708	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2709	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2710	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2711	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2712	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2713	break;
Ryosei	27:dd9f27fce7d1	2714	case 'A':
Ryosei	27:dd9f27fce7d1	2715	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	2716	lineCheck = true;
Ryosei	27:dd9f27fce7d1	2717	lineCount = 0;
Ryosei	27:dd9f27fce7d1	2718	countW++;
Ryosei	27:dd9f27fce7d1	2719	}
Ryosei	27:dd9f27fce7d1	2720	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	2721	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	2722	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	2723	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	2724	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2725	break;
Ryosei	27:dd9f27fce7d1	2726	case 'N':
Ryosei	27:dd9f27fce7d1	2727	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2728	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2729	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2730	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2731	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2732	break;
Ryosei	27:dd9f27fce7d1	2733	default:
Ryosei	27:dd9f27fce7d1	2734	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2735	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2736	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2737	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2738	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2739	}
Ryosei	27:dd9f27fce7d1	2740
Ryosei	27:dd9f27fce7d1	2741	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	2742	if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
Ryosei	27:dd9f27fce7d1	2743	} else {
Ryosei	27:dd9f27fce7d1	2744	adj = 0;
Ryosei	27:dd9f27fce7d1	2745	}
Ryosei	27:dd9f27fce7d1	2746
Ryosei	27:dd9f27fce7d1	2747	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	2748	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	2749	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	2750	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	2751	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	2752	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	2753	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	2754	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	2755
Ryosei	27:dd9f27fce7d1	2756	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	2757	lineCount++;
Ryosei	27:dd9f27fce7d1	2758	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	2759	}
Ryosei	27:dd9f27fce7d1	2760	if(countW == 1) {
Ryosei	27:dd9f27fce7d1	2761	countW = 0;
Ryosei	27:dd9f27fce7d1	2762	lineFase = 4;
Ryosei	27:dd9f27fce7d1	2763	lineCount = 0;
Ryosei	27:dd9f27fce7d1	2764	lineCheck = false;
Ryosei	27:dd9f27fce7d1	2765	}
Ryosei	27:dd9f27fce7d1	2766	} else if(lineFase == 4) { // 右低速
Ryosei	27:dd9f27fce7d1	2767	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	2768	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2769	motor[TIRE_BR].dir = FOR;
Ryosei	27:dd9f27fce7d1	2770	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2771	if(linePara[LINE_TOW_1] == 0) {
Ryosei	27:dd9f27fce7d1	2772	if(!LimitSw::IsPressed(SHEETS_SW)) {
Ryosei	27:dd9f27fce7d1	2773	lineFase=100;
Ryosei	27:dd9f27fce7d1	2774	} else {
Ryosei	27:dd9f27fce7d1	2775	lineFase = 6;
Ryosei	27:dd9f27fce7d1	2776	}
Ryosei	27:dd9f27fce7d1	2777	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2778	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2779	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2780	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2781	}
Ryosei	27:dd9f27fce7d1	2782	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2783	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2784	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2785	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2786	} else if (lineFase == 5) {
Ryosei	27:dd9f27fce7d1	2787	lineFase = 6;
Ryosei	27:dd9f27fce7d1	2788	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2789	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2790	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2791	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2792	} else if(lineFase == 6) { // タオル1 竿検知
Ryosei	28:479631c2de29	2793	LedOut(4);
Ryosei	27:dd9f27fce7d1	2794	if(LimitSw::IsPressed(TOW_1L) && LimitSw::IsPressed(TOW_1R)) {
Ryosei	27:dd9f27fce7d1	2795	lineFase = 7;
Ryosei	27:dd9f27fce7d1	2796	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2797	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2798	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2799	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2800	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2801	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2802	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2803	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2804	} else if(LimitSw::IsPressed(TOW_1L)) {
Ryosei	27:dd9f27fce7d1	2805	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2806	motor[TIRE_FL].pwm = 20;
Ryosei	27:dd9f27fce7d1	2807	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2808	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2809	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2810	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2811	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	2812	motor[TIRE_FR].pwm = 20;
Ryosei	27:dd9f27fce7d1	2813	} else if(LimitSw::IsPressed(TOW_1R)) {
Ryosei	27:dd9f27fce7d1	2814	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	2815	motor[TIRE_FL].pwm = 20;
Ryosei	27:dd9f27fce7d1	2816	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2817	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2818	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2819	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2820	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2821	motor[TIRE_FR].pwm = 20;
Ryosei	27:dd9f27fce7d1	2822	} else {
Ryosei	27:dd9f27fce7d1	2823	switch(linePara[LINE_TOW_1]) {
Ryosei	27:dd9f27fce7d1	2824	case -2:
Ryosei	27:dd9f27fce7d1	2825	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	2826	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	2827	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	2828	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	2829	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2830	break;
Ryosei	27:dd9f27fce7d1	2831	case -3:
Ryosei	27:dd9f27fce7d1	2832	tirePWM[TIRE_FL] = -14;
Ryosei	27:dd9f27fce7d1	2833	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	2834	tirePWM[TIRE_BR] = 14;
Ryosei	27:dd9f27fce7d1	2835	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	2836	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2837	break;
Ryosei	27:dd9f27fce7d1	2838	case -1:
Ryosei	27:dd9f27fce7d1	2839	tirePWM[TIRE_FL] = -17;
Ryosei	27:dd9f27fce7d1	2840	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	2841	tirePWM[TIRE_BR] = 17;
Ryosei	27:dd9f27fce7d1	2842	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	2843	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2844	break;
Ryosei	27:dd9f27fce7d1	2845	case 0:
Ryosei	27:dd9f27fce7d1	2846	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	2847	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	2848	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	2849	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	2850	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2851	break;
Ryosei	27:dd9f27fce7d1	2852	case 1:
Ryosei	27:dd9f27fce7d1	2853	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	2854	tirePWM[TIRE_BL] = -17;
Ryosei	27:dd9f27fce7d1	2855	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	2856	tirePWM[TIRE_FR] = 17;
Ryosei	27:dd9f27fce7d1	2857	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2858	break;
Ryosei	27:dd9f27fce7d1	2859	case 3:
Ryosei	27:dd9f27fce7d1	2860	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	2861	tirePWM[TIRE_BL] = -14;
Ryosei	27:dd9f27fce7d1	2862	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	2863	tirePWM[TIRE_FR] = 14;
Ryosei	27:dd9f27fce7d1	2864	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2865	break;
Ryosei	27:dd9f27fce7d1	2866	case 2:
Ryosei	27:dd9f27fce7d1	2867	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	2868	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	2869	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	2870	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	2871	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2872	break;
Ryosei	27:dd9f27fce7d1	2873	case 'A':
Ryosei	27:dd9f27fce7d1	2874	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	2875	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	2876	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	2877	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	2878	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2879	break;
Ryosei	27:dd9f27fce7d1	2880	case 'N':
Ryosei	27:dd9f27fce7d1	2881	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2882	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2883	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2884	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2885	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2886	break;
Ryosei	27:dd9f27fce7d1	2887	default:
Ryosei	27:dd9f27fce7d1	2888	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	2889	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	2890	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	2891	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	2892	adjAnable = false;
Ryosei	27:dd9f27fce7d1	2893	}
Ryosei	27:dd9f27fce7d1	2894
Ryosei	27:dd9f27fce7d1	2895	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	2896	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	2897	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	2898	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	2899	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	2900	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	2901	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	2902	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	2903	}
Ryosei	27:dd9f27fce7d1	2904	} else if(lineFase == 7) { // ライン修正
Ryosei	27:dd9f27fce7d1	2905	if(linePara[LINE_TOW_1] == 'A' \|\| linePara[LINE_TOW_1] == 'N') {
Ryosei	27:dd9f27fce7d1	2906	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2907	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2908	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2909	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2910	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2911	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2912	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2913	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2914	} else if(linePara[LINE_TOW_1] > 0) {
Ryosei	27:dd9f27fce7d1	2915	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	2916	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2917	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2918	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2919	motor[TIRE_BR].dir = FOR;
Ryosei	27:dd9f27fce7d1	2920	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2921	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2922	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2923	} else if(linePara[LINE_TOW_1] < 0) {
Ryosei	27:dd9f27fce7d1	2924	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	2925	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2926	motor[TIRE_BL].dir = BACK;
Ryosei	27:dd9f27fce7d1	2927	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	2928	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	2929	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2930	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	2931	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	2932	} else if(linePara[LINE_TOW_1] == 0) {
Ryosei	27:dd9f27fce7d1	2933	//tow_stop.reset();
Ryosei	27:dd9f27fce7d1	2934	//tow_stop.start();
Ryosei	27:dd9f27fce7d1	2935	lineFase = 8;
Ryosei	27:dd9f27fce7d1	2936	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2937	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2938	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2939	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2940	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2941	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2942	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2943	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2944	} else {
Ryosei	27:dd9f27fce7d1	2945	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2946	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2947	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2948	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	2949	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2950	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2951	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2952	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	2953	}
Ryosei	27:dd9f27fce7d1	2954	} else if(lineFase == 8) { // タオル1 解放
Ryosei	27:dd9f27fce7d1	2955	Air[TOWEL1] = SOLENOID_ON;
Ryosei	27:dd9f27fce7d1	2956	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2957	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2958	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2959	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	2960	lineFase = 9;
Ryosei	27:dd9f27fce7d1	2961	} else if(lineFase == 9) { // 前
Ryosei	28:479631c2de29	2962	LedOut(2);
Ryosei	27:dd9f27fce7d1	2963	switch(linePara[LINE_TOW_1]) {
Ryosei	27:dd9f27fce7d1	2964	case 2:
Ryosei	27:dd9f27fce7d1	2965	tirePWM[TIRE_FL] = 10;
Ryosei	27:dd9f27fce7d1	2966	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	2967	tirePWM[TIRE_BR] = -10;
Ryosei	27:dd9f27fce7d1	2968	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	2969	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2970	break;
Ryosei	27:dd9f27fce7d1	2971	case 3:
Ryosei	27:dd9f27fce7d1	2972	tirePWM[TIRE_FL] = 14;
Ryosei	27:dd9f27fce7d1	2973	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	2974	tirePWM[TIRE_BR] = -14;
Ryosei	27:dd9f27fce7d1	2975	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	2976	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2977	break;
Ryosei	27:dd9f27fce7d1	2978	case 1:
Ryosei	27:dd9f27fce7d1	2979	tirePWM[TIRE_FL] = 17;
Ryosei	27:dd9f27fce7d1	2980	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	2981	tirePWM[TIRE_BR] = -17;
Ryosei	27:dd9f27fce7d1	2982	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	2983	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2984	break;
Ryosei	27:dd9f27fce7d1	2985	case 0:
Ryosei	27:dd9f27fce7d1	2986	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	2987	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	2988	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	2989	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	2990	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2991	break;
Ryosei	27:dd9f27fce7d1	2992	case -1:
Ryosei	27:dd9f27fce7d1	2993	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	2994	tirePWM[TIRE_BL] = 17;
Ryosei	27:dd9f27fce7d1	2995	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	2996	tirePWM[TIRE_FR] = -17;
Ryosei	27:dd9f27fce7d1	2997	adjAnable = true;
Ryosei	27:dd9f27fce7d1	2998	break;
Ryosei	27:dd9f27fce7d1	2999	case -3:
Ryosei	27:dd9f27fce7d1	3000	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	3001	tirePWM[TIRE_BL] = 14;
Ryosei	27:dd9f27fce7d1	3002	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	3003	tirePWM[TIRE_FR] = -14;
Ryosei	27:dd9f27fce7d1	3004	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3005	break;
Ryosei	27:dd9f27fce7d1	3006	case -2:
Ryosei	27:dd9f27fce7d1	3007	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	3008	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	3009	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	3010	tirePWM[TIRE_FR] = -10;
Ryosei	27:dd9f27fce7d1	3011	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3012	break;
Ryosei	27:dd9f27fce7d1	3013	case 'A':
Ryosei	27:dd9f27fce7d1	3014	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	3015	lineCheck = true;
Ryosei	27:dd9f27fce7d1	3016	lineCount = 0;
Ryosei	27:dd9f27fce7d1	3017	countW++;
Ryosei	27:dd9f27fce7d1	3018	}
Ryosei	27:dd9f27fce7d1	3019	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	3020	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	3021	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	3022	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	3023	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3024	break;
Ryosei	27:dd9f27fce7d1	3025	case 'N':
Ryosei	27:dd9f27fce7d1	3026	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3027	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3028	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3029	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3030	adjAnable = false;
Ryosei	27:dd9f27fce7d1	3031	break;
Ryosei	27:dd9f27fce7d1	3032	default:
Ryosei	27:dd9f27fce7d1	3033	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3034	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3035	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3036	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3037	adjAnable = false;
Ryosei	27:dd9f27fce7d1	3038	}
Ryosei	27:dd9f27fce7d1	3039
Ryosei	27:dd9f27fce7d1	3040	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	3041	adj = 0;
Ryosei	27:dd9f27fce7d1	3042	} else {
Ryosei	27:dd9f27fce7d1	3043	adj = 0;
Ryosei	27:dd9f27fce7d1	3044	}
Ryosei	27:dd9f27fce7d1	3045
Ryosei	27:dd9f27fce7d1	3046	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	3047	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	3048	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	3049	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	3050	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	3051	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	3052	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	3053	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	3054
Ryosei	27:dd9f27fce7d1	3055	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	3056	lineCount++;
Ryosei	27:dd9f27fce7d1	3057	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	3058	}
Ryosei	27:dd9f27fce7d1	3059	if(countW == 1) {
Ryosei	27:dd9f27fce7d1	3060	countW = 0;
Ryosei	27:dd9f27fce7d1	3061	lineFase = 10;
Ryosei	27:dd9f27fce7d1	3062	lineCount = 0;
Ryosei	27:dd9f27fce7d1	3063	lineCheck = false;
Ryosei	27:dd9f27fce7d1	3064	}
Ryosei	27:dd9f27fce7d1	3065	} else if(lineFase == 10) { // 前低速
Ryosei	27:dd9f27fce7d1	3066	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3067	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3068	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3069	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3070	if(linePara[3] == 0) {
Ryosei	27:dd9f27fce7d1	3071	lineFase = 11;
Ryosei	27:dd9f27fce7d1	3072	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3073	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3074	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3075	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3076	}
Ryosei	27:dd9f27fce7d1	3077	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3078	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3079	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3080	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3081	} else if(lineFase == 11) { // 左
Ryosei	27:dd9f27fce7d1	3082	switch(linePara[3]) {
Ryosei	27:dd9f27fce7d1	3083	case -2:
Ryosei	27:dd9f27fce7d1	3084	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	3085	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3086	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	3087	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3088	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3089	break;
Ryosei	27:dd9f27fce7d1	3090	case -3:
Ryosei	27:dd9f27fce7d1	3091	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	3092	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	3093	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	3094	tirePWM[TIRE_FR] = -10;
Ryosei	27:dd9f27fce7d1	3095	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3096	break;
Ryosei	27:dd9f27fce7d1	3097	case -1:
Ryosei	27:dd9f27fce7d1	3098	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	3099	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	3100	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	3101	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	3102	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3103	break;
Ryosei	27:dd9f27fce7d1	3104	case 0:
Ryosei	27:dd9f27fce7d1	3105	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	3106	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	3107	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	3108	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	3109	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3110	break;
Ryosei	27:dd9f27fce7d1	3111	case 1:
Ryosei	27:dd9f27fce7d1	3112	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	3113	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	3114	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	3115	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	3116	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3117	break;
Ryosei	27:dd9f27fce7d1	3118	case 3:
Ryosei	27:dd9f27fce7d1	3119	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	3120	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	3121	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	3122	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	3123	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3124	break;
Ryosei	27:dd9f27fce7d1	3125	case 2:
Ryosei	27:dd9f27fce7d1	3126	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3127	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	3128	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3129	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	3130	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3131	break;
Ryosei	27:dd9f27fce7d1	3132	case 'A':
Ryosei	27:dd9f27fce7d1	3133	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	3134	lineCheck = true;
Ryosei	27:dd9f27fce7d1	3135	lineCount = 0;
Ryosei	27:dd9f27fce7d1	3136	countW++;
Ryosei	27:dd9f27fce7d1	3137	}
Ryosei	27:dd9f27fce7d1	3138	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	3139	tirePWM[TIRE_BL] = 30;
Ryosei	27:dd9f27fce7d1	3140	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	3141	tirePWM[TIRE_FR] = -30;
Ryosei	27:dd9f27fce7d1	3142	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3143	break;
Ryosei	27:dd9f27fce7d1	3144	case 'N':
Ryosei	27:dd9f27fce7d1	3145	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3146	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3147	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3148	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3149	adjAnable = false;
Ryosei	27:dd9f27fce7d1	3150	break;
Ryosei	27:dd9f27fce7d1	3151	default:
Ryosei	27:dd9f27fce7d1	3152	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3153	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3154	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3155	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3156	adjAnable = false;
Ryosei	27:dd9f27fce7d1	3157	}
Ryosei	27:dd9f27fce7d1	3158
Ryosei	27:dd9f27fce7d1	3159	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	3160	if(linePara[4] != 'A' && linePara[4] != 'N') adj = linePara[4];
Ryosei	27:dd9f27fce7d1	3161	} else {
Ryosei	27:dd9f27fce7d1	3162	adj = 0;
Ryosei	27:dd9f27fce7d1	3163	}
Ryosei	27:dd9f27fce7d1	3164
Ryosei	27:dd9f27fce7d1	3165	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	3166	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	3167	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	3168	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	3169	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	3170	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	3171	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	3172	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	3173	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	3174	lineCount++;
Ryosei	27:dd9f27fce7d1	3175	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	3176	}
Ryosei	27:dd9f27fce7d1	3177	if(countW == 2) {
Ryosei	27:dd9f27fce7d1	3178	countW = 0;
Ryosei	27:dd9f27fce7d1	3179	lineFase = 12;
Ryosei	27:dd9f27fce7d1	3180	lineCount = 0;
Ryosei	27:dd9f27fce7d1	3181	lineCheck = false;
Ryosei	27:dd9f27fce7d1	3182	}
Ryosei	27:dd9f27fce7d1	3183	} else if(lineFase == 12) { // 左低速
Ryosei	27:dd9f27fce7d1	3184	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	3185	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3186	motor[TIRE_BR].dir = FOR;
Ryosei	27:dd9f27fce7d1	3187	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3188	if(linePara[LINE_TOW_2] == 0) {
Ryosei	27:dd9f27fce7d1	3189	lineFase = 13;
Ryosei	27:dd9f27fce7d1	3190	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3191	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3192	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3193	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3194	}
Ryosei	27:dd9f27fce7d1	3195	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3196	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3197	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3198	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3199	} else if (lineFase == 13) {
Ryosei	27:dd9f27fce7d1	3200	lineFase = 14;
Ryosei	27:dd9f27fce7d1	3201	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3202	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3203	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3204	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3205	} else if(lineFase == 14) { // タオル2 竿検知
Ryosei	28:479631c2de29	3206	LedOut(4);
Ryosei	27:dd9f27fce7d1	3207	if(LimitSw::IsPressed(TOW_2L) && LimitSw::IsPressed(TOW_2R)) {
Ryosei	27:dd9f27fce7d1	3208	lineFase = 15;
Ryosei	27:dd9f27fce7d1	3209	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3210	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3211	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3212	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3213	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3214	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3215	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3216	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3217	} else if(LimitSw::IsPressed(TOW_2L)) {
Ryosei	27:dd9f27fce7d1	3218	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3219	motor[TIRE_FL].pwm = 20;
Ryosei	27:dd9f27fce7d1	3220	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3221	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3222	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3223	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3224	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	3225	motor[TIRE_FR].pwm = 20;
Ryosei	27:dd9f27fce7d1	3226	} else if(LimitSw::IsPressed(TOW_2R)) {
Ryosei	27:dd9f27fce7d1	3227	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	3228	motor[TIRE_FL].pwm = 20;
Ryosei	27:dd9f27fce7d1	3229	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3230	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3231	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3232	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3233	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3234	motor[TIRE_FR].pwm = 20;
Ryosei	27:dd9f27fce7d1	3235	} else {
Ryosei	27:dd9f27fce7d1	3236	switch(linePara[LINE_TOW_2]) {
Ryosei	27:dd9f27fce7d1	3237	case -2:
Ryosei	27:dd9f27fce7d1	3238	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	3239	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	3240	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	3241	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	3242	break;
Ryosei	27:dd9f27fce7d1	3243	case -3:
Ryosei	27:dd9f27fce7d1	3244	tirePWM[TIRE_FL] = -14;
Ryosei	27:dd9f27fce7d1	3245	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	3246	tirePWM[TIRE_BR] = 14;
Ryosei	27:dd9f27fce7d1	3247	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	3248	break;
Ryosei	27:dd9f27fce7d1	3249	case -1:
Ryosei	27:dd9f27fce7d1	3250	tirePWM[TIRE_FL] = -17;
Ryosei	27:dd9f27fce7d1	3251	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	3252	tirePWM[TIRE_BR] = 17;
Ryosei	27:dd9f27fce7d1	3253	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	3254	break;
Ryosei	27:dd9f27fce7d1	3255	case 0:
Ryosei	27:dd9f27fce7d1	3256	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	3257	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	3258	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	3259	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	3260	break;
Ryosei	27:dd9f27fce7d1	3261	case 1:
Ryosei	27:dd9f27fce7d1	3262	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	3263	tirePWM[TIRE_BL] = -17;
Ryosei	27:dd9f27fce7d1	3264	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	3265	tirePWM[TIRE_FR] = 17;
Ryosei	27:dd9f27fce7d1	3266	break;
Ryosei	27:dd9f27fce7d1	3267	case 3:
Ryosei	27:dd9f27fce7d1	3268	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	3269	tirePWM[TIRE_BL] = -14;
Ryosei	27:dd9f27fce7d1	3270	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	3271	tirePWM[TIRE_FR] = 14;
Ryosei	27:dd9f27fce7d1	3272	break;
Ryosei	27:dd9f27fce7d1	3273	case 2:
Ryosei	27:dd9f27fce7d1	3274	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	3275	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	3276	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	3277	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	3278	break;
Ryosei	27:dd9f27fce7d1	3279	case 'A':
Ryosei	27:dd9f27fce7d1	3280
Ryosei	27:dd9f27fce7d1	3281	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	3282	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	3283	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	3284	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	3285	break;
Ryosei	27:dd9f27fce7d1	3286	case 'N':
Ryosei	27:dd9f27fce7d1	3287	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3288	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3289	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3290	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3291	break;
Ryosei	27:dd9f27fce7d1	3292	default:
Ryosei	27:dd9f27fce7d1	3293	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3294	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3295	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3296	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3297	}
Ryosei	27:dd9f27fce7d1	3298
Ryosei	27:dd9f27fce7d1	3299	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	3300	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	3301	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	3302	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	3303	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	3304	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	3305	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	3306	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	3307	}
Ryosei	27:dd9f27fce7d1	3308	} else if(lineFase == 15 ) { // ライン修正
Ryosei	27:dd9f27fce7d1	3309	if(linePara[LINE_TOW_2] == 'A' \|\| linePara[LINE_TOW_2] == 'N') {
Ryosei	27:dd9f27fce7d1	3310	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3311	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3312	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3313	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3314	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3315	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3316	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3317	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3318	} else if(linePara[LINE_TOW_2] > 0) {
Ryosei	27:dd9f27fce7d1	3319	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	3320	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3321	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3322	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3323	motor[TIRE_BR].dir = FOR;
Ryosei	27:dd9f27fce7d1	3324	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3325	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3326	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3327	} else if(linePara[LINE_TOW_2] < 0) {
Ryosei	27:dd9f27fce7d1	3328	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3329	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3330	motor[TIRE_BL].dir = BACK;
Ryosei	27:dd9f27fce7d1	3331	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3332	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3333	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3334	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	3335	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3336	} else if(linePara[LINE_TOW_2] == 0) {
Ryosei	27:dd9f27fce7d1	3337	lineFase = 16;
Ryosei	27:dd9f27fce7d1	3338	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3339	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3340	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3341	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3342	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3343	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3344	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3345	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3346	} else {
Ryosei	27:dd9f27fce7d1	3347	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3348	motor[TIRE_FL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3349	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3350	motor[TIRE_BL].pwm = 50;
Ryosei	27:dd9f27fce7d1	3351	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3352	motor[TIRE_BR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3353	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3354	motor[TIRE_FR].pwm = 50;
Ryosei	27:dd9f27fce7d1	3355	}
Ryosei	27:dd9f27fce7d1	3356	} else if(lineFase == 16) { // タオル2 解放
Ryosei	27:dd9f27fce7d1	3357	Air[TOWEL2] = SOLENOID_ON;
Ryosei	27:dd9f27fce7d1	3358	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3359	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3360	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3361	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3362	lineFase = 17;
Ryosei	27:dd9f27fce7d1	3363	} else if(lineFase == 17) { // 前
Ryosei	27:dd9f27fce7d1	3364	switch(linePara[LINE_TOW_2]) {
Ryosei	27:dd9f27fce7d1	3365	case 2:
Ryosei	27:dd9f27fce7d1	3366	tirePWM[TIRE_FL] = 10;
Ryosei	27:dd9f27fce7d1	3367	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	3368	tirePWM[TIRE_BR] = -10;
Ryosei	27:dd9f27fce7d1	3369	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	3370	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3371	break;
Ryosei	27:dd9f27fce7d1	3372	case 3:
Ryosei	27:dd9f27fce7d1	3373	tirePWM[TIRE_FL] = 14;
Ryosei	27:dd9f27fce7d1	3374	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	3375	tirePWM[TIRE_BR] = -14;
Ryosei	27:dd9f27fce7d1	3376	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	3377	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3378	break;
Ryosei	27:dd9f27fce7d1	3379	case 1:
Ryosei	27:dd9f27fce7d1	3380	tirePWM[TIRE_FL] = 17;
Ryosei	27:dd9f27fce7d1	3381	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	3382	tirePWM[TIRE_BR] = -17;
Ryosei	27:dd9f27fce7d1	3383	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	3384	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3385	break;
Ryosei	27:dd9f27fce7d1	3386	case 0:
Ryosei	27:dd9f27fce7d1	3387	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	3388	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	3389	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	3390	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	3391	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3392	break;
Ryosei	27:dd9f27fce7d1	3393	case -1:
Ryosei	27:dd9f27fce7d1	3394	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	3395	tirePWM[TIRE_BL] = 17;
Ryosei	27:dd9f27fce7d1	3396	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	3397	tirePWM[TIRE_FR] = -17;
Ryosei	27:dd9f27fce7d1	3398	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3399	break;
Ryosei	27:dd9f27fce7d1	3400	case -3:
Ryosei	27:dd9f27fce7d1	3401	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	3402	tirePWM[TIRE_BL] = 14;
Ryosei	27:dd9f27fce7d1	3403	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	3404	tirePWM[TIRE_FR] = -14;
Ryosei	27:dd9f27fce7d1	3405	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3406	break;
Ryosei	27:dd9f27fce7d1	3407	case -2:
Ryosei	27:dd9f27fce7d1	3408	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	3409	tirePWM[TIRE_BL] = 10;
Ryosei	27:dd9f27fce7d1	3410	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	3411	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	3412	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3413	break;
Ryosei	27:dd9f27fce7d1	3414	case 'A':
Ryosei	27:dd9f27fce7d1	3415	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	3416	lineCheck = true;
Ryosei	27:dd9f27fce7d1	3417	lineCount = 0;
Ryosei	27:dd9f27fce7d1	3418	countW++;
Ryosei	27:dd9f27fce7d1	3419	}
Ryosei	27:dd9f27fce7d1	3420	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	3421	tirePWM[TIRE_BL] = 20;
Ryosei	27:dd9f27fce7d1	3422	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	3423	tirePWM[TIRE_FR] = -20;
Ryosei	27:dd9f27fce7d1	3424	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3425	break;
Ryosei	27:dd9f27fce7d1	3426	case 'N':
Ryosei	27:dd9f27fce7d1	3427	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3428	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3429	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3430	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3431	adjAnable = false;
Ryosei	27:dd9f27fce7d1	3432	break;
Ryosei	27:dd9f27fce7d1	3433	default:
Ryosei	27:dd9f27fce7d1	3434	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3435	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3436	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3437	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3438	adjAnable = false;
Ryosei	27:dd9f27fce7d1	3439	}
Ryosei	27:dd9f27fce7d1	3440
Ryosei	27:dd9f27fce7d1	3441	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	3442	adj = 0;
Ryosei	27:dd9f27fce7d1	3443	} else {
Ryosei	27:dd9f27fce7d1	3444	adj = 0;
Ryosei	27:dd9f27fce7d1	3445	}
Ryosei	27:dd9f27fce7d1	3446
Ryosei	27:dd9f27fce7d1	3447	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	3448	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	3449	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	3450	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	3451	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL]);
Ryosei	27:dd9f27fce7d1	3452	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	3453	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR] + adj);
Ryosei	27:dd9f27fce7d1	3454	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	3455
Ryosei	27:dd9f27fce7d1	3456	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	3457	lineCount++;
Ryosei	27:dd9f27fce7d1	3458	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	3459	}
Ryosei	27:dd9f27fce7d1	3460	if(countW == 1) {
Ryosei	27:dd9f27fce7d1	3461	countW = 0;
Ryosei	27:dd9f27fce7d1	3462	lineFase = 100;
Ryosei	27:dd9f27fce7d1	3463	lineCount = 0;
Ryosei	27:dd9f27fce7d1	3464	lineCheck = false;
Ryosei	27:dd9f27fce7d1	3465	}
Ryosei	27:dd9f27fce7d1	3466	} else if(lineFase==100) {
Ryosei	28:479631c2de29	3467	LedOut(6);
Ryosei	27:dd9f27fce7d1	3468	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3469	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3470	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3471	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3472	if(LimitSw::IsPressed(LSW_UU)) {
Ryosei	27:dd9f27fce7d1	3473	motor[LIFT_U].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3474	motor[LIFT_U].pwm=100;
Ryosei	27:dd9f27fce7d1	3475	} else {
Ryosei	27:dd9f27fce7d1	3476	motor[LIFT_U].dir=BACK;
Ryosei	27:dd9f27fce7d1	3477	motor[LIFT_U].pwm=150;
Ryosei	27:dd9f27fce7d1	3478	}
Ryosei	27:dd9f27fce7d1	3479	if((Ult_left>0)&&(Ult_right>0)&&(Ult_left<30)&&(Ult_right<30)) {
Ryosei	27:dd9f27fce7d1	3480	lineFase=101;
Ryosei	27:dd9f27fce7d1	3481	}
Ryosei	27:dd9f27fce7d1	3482	} else if(lineFase==101) {
Ryosei	27:dd9f27fce7d1	3483	//位置調整
Ryosei	27:dd9f27fce7d1	3484	//(P制御)
Ryosei	27:dd9f27fce7d1	3485	if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei	27:dd9f27fce7d1	3486	if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei	27:dd9f27fce7d1	3487	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3488	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3489	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3490	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3491	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	3492	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	3493	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	3494	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	3495	lineFase=102;//system lineFase increasing
Ryosei	27:dd9f27fce7d1	3496	} else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei	27:dd9f27fce7d1	3497	if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei	27:dd9f27fce7d1	3498	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3499	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3500	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3501	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3502	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	3503	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	3504	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	3505	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	3506	} else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei	27:dd9f27fce7d1	3507	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3508	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3509	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3510	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3511	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	3512	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	3513	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	3514	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	3515	}
Ryosei	27:dd9f27fce7d1	3516	} else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei	27:dd9f27fce7d1	3517	if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei	27:dd9f27fce7d1	3518	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3519	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3520	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3521	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3522	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	3523	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	3524	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	3525	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	3526	} else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei	27:dd9f27fce7d1	3527	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3528	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3529	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3530	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3531	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	3532	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	3533	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	3534	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	3535	}
Ryosei	27:dd9f27fce7d1	3536	} else { //どっちもあってない場合
Ryosei	27:dd9f27fce7d1	3537	if( (Ult_left+Ult_right)<=25\|\|(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei	27:dd9f27fce7d1	3538	if((Ult_left-Ult_right)>10\|\|((Ult_left-Ult_right)<-10) ) { //傾きが大きいとき
Ryosei	27:dd9f27fce7d1	3539	if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	3540	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3541	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3542	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3543	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3544	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	3545	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	3546	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	3547	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	3548	} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	3549	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3550	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3551	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3552	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3553	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	3554	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	3555	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	3556	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	3557	}
Ryosei	27:dd9f27fce7d1	3558	} else { //傾きが大きくなくて離れているとき
Ryosei	27:dd9f27fce7d1	3559	if((Ult_right+Ult_left)<=25) { //近すぎるとき
Ryosei	27:dd9f27fce7d1	3560	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3561	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3562	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3563	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3564	motor[TIRE_FL].pwm=20;
Ryosei	27:dd9f27fce7d1	3565	motor[TIRE_FR].pwm=20;
Ryosei	27:dd9f27fce7d1	3566	motor[TIRE_BL].pwm=20;
Ryosei	27:dd9f27fce7d1	3567	motor[TIRE_BR].pwm=20;
Ryosei	27:dd9f27fce7d1	3568	} else if((Ult_right+Ult_left)>=35) { //離れているとき
Ryosei	27:dd9f27fce7d1	3569	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3570	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3571	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3572	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3573	motor[TIRE_FL].pwm=20;
Ryosei	27:dd9f27fce7d1	3574	motor[TIRE_FR].pwm=20;
Ryosei	27:dd9f27fce7d1	3575	motor[TIRE_BL].pwm=20;
Ryosei	27:dd9f27fce7d1	3576	motor[TIRE_BR].pwm=20;
Ryosei	27:dd9f27fce7d1	3577	}
Ryosei	27:dd9f27fce7d1	3578	}
Ryosei	27:dd9f27fce7d1	3579
Ryosei	27:dd9f27fce7d1	3580	} else { //さほど離れてはいないが傾きが大きいとき
Ryosei	27:dd9f27fce7d1	3581	if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	3582	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3583	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3584	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3585	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3586	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	3587	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	3588	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	3589	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	3590	} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	3591	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3592	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3593	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3594	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3595	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	3596	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	3597	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	3598	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	3599	}
Ryosei	27:dd9f27fce7d1	3600	}
Ryosei	27:dd9f27fce7d1	3601	}
Ryosei	27:dd9f27fce7d1	3602
Ryosei	27:dd9f27fce7d1	3603	} else {//データを受け取ってないとき
Ryosei	27:dd9f27fce7d1	3604	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3605	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3606	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3607	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3608	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	3609	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	3610	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	3611	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	3612	}
Ryosei	27:dd9f27fce7d1	3613
Ryosei	27:dd9f27fce7d1	3614	} else if(lineFase==102) {
Ryosei	27:dd9f27fce7d1	3615	Air[CLOTHESPIN]=SOLENOID_OFF;
Ryosei	27:dd9f27fce7d1	3616	LedOut(1);
Ryosei	27:dd9f27fce7d1	3617	//リミットスイッチに当てる
Ryosei	27:dd9f27fce7d1	3618	static int count2=0;
Ryosei	27:dd9f27fce7d1	3619	if(count2==0) {
Ryosei	27:dd9f27fce7d1	3620	if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei	27:dd9f27fce7d1	3621	if( (Ult_left+Ult_right)<=25\|\|(Ult_left+Ult_right)>=35) {
Ryosei	27:dd9f27fce7d1	3622	if(((Ult_left-Ult_right)>=5)\|\|((Ult_left-Ult_right)<=-5)) {
Ryosei	27:dd9f27fce7d1	3623	if(Ult_left-Ult_right<=0) {
Ryosei	27:dd9f27fce7d1	3624	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3625	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3626	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3627	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3628
Ryosei	27:dd9f27fce7d1	3629	motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3630	motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3631
Ryosei	27:dd9f27fce7d1	3632	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	3633	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	3634	} else if(Ult_left-Ult_right>0) {
Ryosei	27:dd9f27fce7d1	3635	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3636	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3637	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3638	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3639	motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3640	motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3641	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	3642	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	3643	}
Ryosei	27:dd9f27fce7d1	3644	} else if((Ult_left+Ult_right)<=25) {
Ryosei	27:dd9f27fce7d1	3645	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3646	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3647	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3648	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3649	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	3650	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	3651	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	3652	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	3653	} else if((Ult_left+Ult_right)>=35) {
Ryosei	27:dd9f27fce7d1	3654	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3655	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3656	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3657	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3658	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	3659	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	3660	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	3661	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	3662	}
Ryosei	27:dd9f27fce7d1	3663	} else {
Ryosei	27:dd9f27fce7d1	3664	if(Ult_left-Ult_right<=0) {
Ryosei	27:dd9f27fce7d1	3665	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3666	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3667	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3668	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3669
Ryosei	27:dd9f27fce7d1	3670	motor[TIRE_FL].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3671	motor[TIRE_FR].pwm=25+(-1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3672
Ryosei	27:dd9f27fce7d1	3673	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	3674	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	3675	} else if(Ult_left-Ult_right>0) {
Ryosei	27:dd9f27fce7d1	3676	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3677	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3678	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3679	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3680	motor[TIRE_BL].pwm=25+(1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3681	motor[TIRE_BR].pwm=25+(1*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3682	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	3683	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	3684	}
Ryosei	27:dd9f27fce7d1	3685	}
Ryosei	27:dd9f27fce7d1	3686
Ryosei	27:dd9f27fce7d1	3687	} else {
Ryosei	27:dd9f27fce7d1	3688	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3689	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3690	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3691	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3692	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	3693	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	3694	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	3695	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	3696
Ryosei	27:dd9f27fce7d1	3697	}
Ryosei	27:dd9f27fce7d1	3698	if(LimitSw::IsPressed(LEFTlim)) {
Ryosei	27:dd9f27fce7d1	3699	count2=1;
Ryosei	27:dd9f27fce7d1	3700	}
Ryosei	27:dd9f27fce7d1	3701
Ryosei	27:dd9f27fce7d1	3702	} else if(count2==1) {
Ryosei	27:dd9f27fce7d1	3703	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3704	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3705	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3706	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3707	motor[TIRE_FL].pwm=255;
Ryosei	27:dd9f27fce7d1	3708	motor[TIRE_FR].pwm=255;
Ryosei	27:dd9f27fce7d1	3709	motor[TIRE_BL].pwm=255;
Ryosei	27:dd9f27fce7d1	3710	motor[TIRE_BR].pwm=255;
Ryosei	27:dd9f27fce7d1	3711	lineFase=103;
Ryosei	27:dd9f27fce7d1	3712	}
Ryosei	27:dd9f27fce7d1	3713	} else if(lineFase==103) {
Ryosei	27:dd9f27fce7d1	3714	if((Ult_left>0)&&(Ult_right>0)) {//データを受け取っているとき
Ryosei	27:dd9f27fce7d1	3715	if((Ult_left<16)&&(Ult_left>14)&&(Ult_right<16)&&(Ult_right>14)) { //合った場合
Ryosei	27:dd9f27fce7d1	3716	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3717	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3718	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3719	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3720	motor[TIRE_FL].pwm=255;
Ryosei	27:dd9f27fce7d1	3721	motor[TIRE_FR].pwm=255;
Ryosei	27:dd9f27fce7d1	3722	motor[TIRE_BL].pwm=255;
Ryosei	27:dd9f27fce7d1	3723	motor[TIRE_BR].pwm=255;
Ryosei	27:dd9f27fce7d1	3724	lineFase=104;//system lineFase increasing
Ryosei	27:dd9f27fce7d1	3725	} else if((Ult_left<16)&&(Ult_left>14)) { //Ult_leftのみあった場合
Ryosei	27:dd9f27fce7d1	3726	if(Ult_right>16) { //Ult_rightが遠い場合
Ryosei	27:dd9f27fce7d1	3727	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3728	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3729	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3730	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3731	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	3732	motor[TIRE_FR].pwm=13;
Ryosei	27:dd9f27fce7d1	3733	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	3734	motor[TIRE_BR].pwm=13;
Ryosei	27:dd9f27fce7d1	3735	} else if(Ult_right<14) { //Ult_rightが近い場合
Ryosei	27:dd9f27fce7d1	3736	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3737	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3738	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3739	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3740	motor[TIRE_FL].pwm=0;
Ryosei	27:dd9f27fce7d1	3741	motor[TIRE_FR].pwm=13;
Ryosei	27:dd9f27fce7d1	3742	motor[TIRE_BL].pwm=0;
Ryosei	27:dd9f27fce7d1	3743	motor[TIRE_BR].pwm=13;
Ryosei	27:dd9f27fce7d1	3744	}
Ryosei	27:dd9f27fce7d1	3745	} else if((Ult_right<16)&&(Ult_right>14)) { //Ult_rightのみあった場合
Ryosei	27:dd9f27fce7d1	3746	if(Ult_left>16) { //Ult_leftが遠い場合
Ryosei	27:dd9f27fce7d1	3747	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3748	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3749	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3750	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3751	motor[TIRE_FL].pwm=13;
Ryosei	27:dd9f27fce7d1	3752	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	3753	motor[TIRE_BL].pwm=13;
Ryosei	27:dd9f27fce7d1	3754	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	3755	} else if(Ult_left<14) { //Ult_leftが近い場合
Ryosei	27:dd9f27fce7d1	3756	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3757	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3758	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3759	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3760	motor[TIRE_FL].pwm=13;
Ryosei	27:dd9f27fce7d1	3761	motor[TIRE_FR].pwm=0;
Ryosei	27:dd9f27fce7d1	3762	motor[TIRE_BL].pwm=13;
Ryosei	27:dd9f27fce7d1	3763	motor[TIRE_BR].pwm=0;
Ryosei	27:dd9f27fce7d1	3764	}
Ryosei	27:dd9f27fce7d1	3765	} else { //どっちもあってない場合
Ryosei	27:dd9f27fce7d1	3766	if( (Ult_left+Ult_right)<=25\|\|(Ult_left+Ult_right)>=35) {//離れすぎているor近すぎるとき
Ryosei	27:dd9f27fce7d1	3767	if((Ult_left-Ult_right)>0\|\|((Ult_left-Ult_right)<0) ) { //傾きが大きいとき
Ryosei	27:dd9f27fce7d1	3768	if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	3769	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3770	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3771	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3772	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3773	motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3774	motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3775	motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3776	motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3777	} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	3778	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3779	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3780	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3781	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3782	motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3783	motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3784	motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3785	motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3786	}
Ryosei	27:dd9f27fce7d1	3787	}
Ryosei	27:dd9f27fce7d1	3788	} else { //さほど離れてはいないが傾きが大きいとき
Ryosei	27:dd9f27fce7d1	3789	if((Ult_left-Ult_right)>0) { //Ult_leftの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	3790	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3791	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3792	motor[TIRE_BL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3793	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3794	motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3795	motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3796	motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3797	motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3798	} else if((Ult_left-Ult_right)<=0) { //Ult_rightの方が後ろに来ているとき
Ryosei	27:dd9f27fce7d1	3799	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3800	motor[TIRE_FR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3801	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3802	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3803	motor[TIRE_FL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3804	motor[TIRE_FR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3805	motor[TIRE_BL].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3806	motor[TIRE_BR].pwm=abs(10*(Ult_left-Ult_right));
Ryosei	27:dd9f27fce7d1	3807	}
Ryosei	27:dd9f27fce7d1	3808	}
Ryosei	27:dd9f27fce7d1	3809	}
Ryosei	27:dd9f27fce7d1	3810
Ryosei	27:dd9f27fce7d1	3811	} else {//データを受け取ってないとき
Ryosei	27:dd9f27fce7d1	3812	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3813	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3814	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3815	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3816	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	3817	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	3818	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	3819	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	3820	}
Ryosei	27:dd9f27fce7d1	3821	} else if(lineFase==104) {
Ryosei	27:dd9f27fce7d1	3822	static int count3=0;
Ryosei	27:dd9f27fce7d1	3823	static int loop=0;
Ryosei	27:dd9f27fce7d1	3824	pc.printf("%d\r\n",loop);
Ryosei	27:dd9f27fce7d1	3825	Air[CLOTHESPIN]=1;
Ryosei	27:dd9f27fce7d1	3826	if(count3==0) {
Ryosei	27:dd9f27fce7d1	3827	loop++;
Ryosei	27:dd9f27fce7d1	3828	if(loop==60) {
Ryosei	27:dd9f27fce7d1	3829	count3=1;
Ryosei	27:dd9f27fce7d1	3830	}
Ryosei	27:dd9f27fce7d1	3831	} else if(count3==1) {
Ryosei	27:dd9f27fce7d1	3832	Air[CLOTHESPIN]=0;
Ryosei	27:dd9f27fce7d1	3833	lineFase=105;
Ryosei	27:dd9f27fce7d1	3834
Ryosei	27:dd9f27fce7d1	3835	}
Ryosei	27:dd9f27fce7d1	3836	} else if(lineFase==105) {
Ryosei	27:dd9f27fce7d1	3837	LedOut(6);
Ryosei	27:dd9f27fce7d1	3838	Air[CLOTHESPIN]=0;
Ryosei	27:dd9f27fce7d1	3839	if(!(LimitSw::IsPressed(RIGHTlim))) {
Ryosei	27:dd9f27fce7d1	3840
Ryosei	27:dd9f27fce7d1	3841	//超音波
Ryosei	27:dd9f27fce7d1	3842	if(Ult_right>0) {
Ryosei	27:dd9f27fce7d1	3843	if(Ult_right<15) {
Ryosei	27:dd9f27fce7d1	3844	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3845	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3846	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3847	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3848	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	3849	motor[TIRE_FR].pwm=25+(15-Ult_right);
Ryosei	27:dd9f27fce7d1	3850	motor[TIRE_BL].pwm=25+(15-Ult_right);
Ryosei	27:dd9f27fce7d1	3851	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	3852	} else if(Ult_right>=15) {
Ryosei	27:dd9f27fce7d1	3853	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3854	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3855	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3856	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3857	motor[TIRE_FL].pwm=25+(Ult_right-15);
Ryosei	27:dd9f27fce7d1	3858	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	3859	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	3860	motor[TIRE_BR].pwm=25+(Ult_right-15);
Ryosei	27:dd9f27fce7d1	3861	}
Ryosei	27:dd9f27fce7d1	3862	} else {
Ryosei	27:dd9f27fce7d1	3863	motor[TIRE_FL].dir=FOR;
Ryosei	27:dd9f27fce7d1	3864	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3865	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3866	motor[TIRE_BR].dir=BACK;
Ryosei	27:dd9f27fce7d1	3867	motor[TIRE_FL].pwm=25;
Ryosei	27:dd9f27fce7d1	3868	motor[TIRE_FR].pwm=25;
Ryosei	27:dd9f27fce7d1	3869	motor[TIRE_BL].pwm=25;
Ryosei	27:dd9f27fce7d1	3870	motor[TIRE_BR].pwm=25;
Ryosei	27:dd9f27fce7d1	3871	}
Ryosei	27:dd9f27fce7d1	3872
Ryosei	27:dd9f27fce7d1	3873
Ryosei	27:dd9f27fce7d1	3874	} else {
Ryosei	27:dd9f27fce7d1	3875	motor[TIRE_FL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3876	motor[TIRE_FR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3877	motor[TIRE_BL].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3878	motor[TIRE_BR].dir=BRAKE;
Ryosei	27:dd9f27fce7d1	3879	motor[TIRE_FL].pwm=100;
Ryosei	27:dd9f27fce7d1	3880	motor[TIRE_FR].pwm=100;
Ryosei	27:dd9f27fce7d1	3881	motor[TIRE_BL].pwm=100;
Ryosei	27:dd9f27fce7d1	3882	motor[TIRE_BR].pwm=100;
Ryosei	27:dd9f27fce7d1	3883	lineFase=106;
Ryosei	27:dd9f27fce7d1	3884	}
Ryosei	27:dd9f27fce7d1	3885
Ryosei	27:dd9f27fce7d1	3886	} else if(lineFase==106) {
Ryosei	27:dd9f27fce7d1	3887	motor[TIRE_FL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3888	motor[TIRE_FR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3889	motor[TIRE_BL].dir=BACK;
Ryosei	27:dd9f27fce7d1	3890	motor[TIRE_BR].dir=FOR;
Ryosei	27:dd9f27fce7d1	3891	motor[TIRE_FL].pwm=15;
Ryosei	27:dd9f27fce7d1	3892	motor[TIRE_FR].pwm=15;
Ryosei	27:dd9f27fce7d1	3893	motor[TIRE_BL].pwm=15;
Ryosei	27:dd9f27fce7d1	3894	motor[TIRE_BR].pwm=15;
Ryosei	27:dd9f27fce7d1	3895	if(linePara[4]!='N') {
Ryosei	27:dd9f27fce7d1	3896	lineFase=19;
Ryosei	27:dd9f27fce7d1	3897	}
Ryosei	27:dd9f27fce7d1	3898	} else if(lineFase == 18) { // 前低速
Ryosei	27:dd9f27fce7d1	3899	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3900	motor[TIRE_BL].dir = FOR;
Ryosei	27:dd9f27fce7d1	3901	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3902	motor[TIRE_FR].dir = BACK;
Ryosei	27:dd9f27fce7d1	3903	if(linePara[4] == 0) {
Ryosei	27:dd9f27fce7d1	3904	lineFase = 19;
Ryosei	27:dd9f27fce7d1	3905	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3906	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3907	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3908	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	3909	}
Ryosei	27:dd9f27fce7d1	3910	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3911	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	3912	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3913	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	3914	} else if(lineFase == 19) { // 右
Ryosei	27:dd9f27fce7d1	3915	switch(linePara[4]) {
Ryosei	27:dd9f27fce7d1	3916	case -2:
Ryosei	27:dd9f27fce7d1	3917	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	3918	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3919	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	3920	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3921	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3922	break;
Ryosei	27:dd9f27fce7d1	3923	case -3:
Ryosei	27:dd9f27fce7d1	3924	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	3925	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	3926	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	3927	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	3928	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3929	break;
Ryosei	27:dd9f27fce7d1	3930	case -1:
Ryosei	27:dd9f27fce7d1	3931	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	3932	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	3933	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	3934	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	3935	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3936	break;
Ryosei	27:dd9f27fce7d1	3937	case 0:
Ryosei	27:dd9f27fce7d1	3938	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	3939	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	3940	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	3941	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	3942	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3943	break;
Ryosei	27:dd9f27fce7d1	3944	case 1:
Ryosei	27:dd9f27fce7d1	3945	tirePWM[TIRE_FL] = 20;
Ryosei	27:dd9f27fce7d1	3946	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	3947	tirePWM[TIRE_BR] = -20;
Ryosei	27:dd9f27fce7d1	3948	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	3949	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3950	break;
Ryosei	27:dd9f27fce7d1	3951	case 3:
Ryosei	27:dd9f27fce7d1	3952	tirePWM[TIRE_FL] = 10;
Ryosei	27:dd9f27fce7d1	3953	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	3954	tirePWM[TIRE_BR] = -10;
Ryosei	27:dd9f27fce7d1	3955	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	3956	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3957	break;
Ryosei	27:dd9f27fce7d1	3958	case 2:
Ryosei	27:dd9f27fce7d1	3959	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3960	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	3961	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3962	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	3963	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3964	break;
Ryosei	27:dd9f27fce7d1	3965	case 'A':
Ryosei	27:dd9f27fce7d1	3966	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	3967	lineCheck = true;
Ryosei	27:dd9f27fce7d1	3968	lineCount = 0;
Ryosei	27:dd9f27fce7d1	3969	countW++;
Ryosei	27:dd9f27fce7d1	3970	}
Ryosei	27:dd9f27fce7d1	3971	tirePWM[TIRE_FL] = 30;
Ryosei	27:dd9f27fce7d1	3972	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	3973	tirePWM[TIRE_BR] = -30;
Ryosei	27:dd9f27fce7d1	3974	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	3975	adjAnable = true;
Ryosei	27:dd9f27fce7d1	3976	break;
Ryosei	27:dd9f27fce7d1	3977	case 'N':
Ryosei	27:dd9f27fce7d1	3978	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3979	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3980	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3981	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3982	adjAnable = false;
Ryosei	27:dd9f27fce7d1	3983	break;
Ryosei	27:dd9f27fce7d1	3984	default:
Ryosei	27:dd9f27fce7d1	3985	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	3986	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	3987	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	3988	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	3989	adjAnable = false;
Ryosei	27:dd9f27fce7d1	3990	}
Ryosei	27:dd9f27fce7d1	3991
Ryosei	27:dd9f27fce7d1	3992	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	3993	if(linePara[3] != 'A' && linePara[3] != 'N') adj = linePara[3];
Ryosei	27:dd9f27fce7d1	3994	} else {
Ryosei	27:dd9f27fce7d1	3995	adj = 0;
Ryosei	27:dd9f27fce7d1	3996	}
Ryosei	27:dd9f27fce7d1	3997
Ryosei	27:dd9f27fce7d1	3998	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei	27:dd9f27fce7d1	3999	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	4000	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	4001	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	4002	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei	27:dd9f27fce7d1	4003	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL] + adj);
Ryosei	27:dd9f27fce7d1	4004	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	4005	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR]);
Ryosei	27:dd9f27fce7d1	4006
Ryosei	27:dd9f27fce7d1	4007	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	4008	lineCount++;
Ryosei	27:dd9f27fce7d1	4009	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	4010	}
Ryosei	27:dd9f27fce7d1	4011
Ryosei	27:dd9f27fce7d1	4012	/*
Ryosei	27:dd9f27fce7d1	4013	if(LimitSw::IsPressed(TOWEL1_SW) && LimitSw::IsPressed(TOWEL2_SW)) {
Ryosei	27:dd9f27fce7d1	4014	targetCount = 3;
Ryosei	27:dd9f27fce7d1	4015	} else if(LimitSw::IsPressed(TOWEL1_SW)) {
Ryosei	27:dd9f27fce7d1	4016	targetCount = 3;
Ryosei	27:dd9f27fce7d1	4017	} else {
Ryosei	27:dd9f27fce7d1	4018	targetCount = 2;
Ryosei	27:dd9f27fce7d1	4019	}
Ryosei	27:dd9f27fce7d1	4020	*/
Ryosei	27:dd9f27fce7d1	4021
Ryosei	27:dd9f27fce7d1	4022	targetCount = 3;
Ryosei	27:dd9f27fce7d1	4023
Ryosei	27:dd9f27fce7d1	4024	if(countW == targetCount) {
Ryosei	27:dd9f27fce7d1	4025	countW = 0;
Ryosei	27:dd9f27fce7d1	4026	lineFase = 20;
Ryosei	27:dd9f27fce7d1	4027	lineCount = 0;
Ryosei	27:dd9f27fce7d1	4028	lineCheck = false;
Ryosei	27:dd9f27fce7d1	4029	}
Ryosei	27:dd9f27fce7d1	4030	} else if(lineFase == 20) { // 右低速
Ryosei	27:dd9f27fce7d1	4031	motor[TIRE_FL].dir = FOR;
Ryosei	27:dd9f27fce7d1	4032	motor[TIRE_BL].dir = BACK;
Ryosei	27:dd9f27fce7d1	4033	motor[TIRE_BR].dir = BACK;
Ryosei	27:dd9f27fce7d1	4034	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	4035	if (linePara[2] == 0) {
Ryosei	27:dd9f27fce7d1	4036	//(!!LimitSw::Ispressed(SHEETS_SW)) {
Ryosei	27:dd9f27fce7d1	4037	//ineFase = 20;
Ryosei	27:dd9f27fce7d1	4038	//else if(LimitSw::IsPressed(TOWEL2_SW) {
Ryosei	27:dd9f27fce7d1	4039	//ineFase = 14;
Ryosei	27:dd9f27fce7d1	4040	//else {
Ryosei	27:dd9f27fce7d1	4041	lineFase = 21;
Ryosei	27:dd9f27fce7d1	4042	//
Ryosei	27:dd9f27fce7d1	4043	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4044	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4045	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4046	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4047	}
Ryosei	27:dd9f27fce7d1	4048	motor[TIRE_FL].pwm = 16;
Ryosei	27:dd9f27fce7d1	4049	motor[TIRE_BL].pwm = 16;
Ryosei	27:dd9f27fce7d1	4050	motor[TIRE_BR].pwm = 16;
Ryosei	27:dd9f27fce7d1	4051	motor[TIRE_FR].pwm = 16;
Ryosei	27:dd9f27fce7d1	4052	} else if(lineFase == 21) {
Ryosei	27:dd9f27fce7d1	4053	switch(linePara[2]) {
Ryosei	27:dd9f27fce7d1	4054	case -2:
Ryosei	27:dd9f27fce7d1	4055	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	4056	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	4057	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	4058	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	4059	adjAnable = true;
Ryosei	27:dd9f27fce7d1	4060	break;
Ryosei	27:dd9f27fce7d1	4061	case -3:
Ryosei	27:dd9f27fce7d1	4062	tirePWM[TIRE_FL] = -10;
Ryosei	27:dd9f27fce7d1	4063	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	4064	tirePWM[TIRE_BR] = 10;
Ryosei	27:dd9f27fce7d1	4065	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	4066	adjAnable = true;
Ryosei	27:dd9f27fce7d1	4067	break;
Ryosei	27:dd9f27fce7d1	4068	case -1:
Ryosei	27:dd9f27fce7d1	4069	tirePWM[TIRE_FL] = -20;
Ryosei	27:dd9f27fce7d1	4070	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	4071	tirePWM[TIRE_BR] = 20;
Ryosei	27:dd9f27fce7d1	4072	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	4073	adjAnable = true;
Ryosei	27:dd9f27fce7d1	4074	break;
Ryosei	27:dd9f27fce7d1	4075	case 0:
Ryosei	27:dd9f27fce7d1	4076	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	4077	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	4078	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	4079	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	4080	adjAnable = true;
Ryosei	27:dd9f27fce7d1	4081	break;
Ryosei	27:dd9f27fce7d1	4082	case 1:
Ryosei	27:dd9f27fce7d1	4083	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	4084	tirePWM[TIRE_BL] = -20;
Ryosei	27:dd9f27fce7d1	4085	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	4086	tirePWM[TIRE_FR] = 20;
Ryosei	27:dd9f27fce7d1	4087	adjAnable = true;
Ryosei	27:dd9f27fce7d1	4088	break;
Ryosei	27:dd9f27fce7d1	4089	case 3:
Ryosei	27:dd9f27fce7d1	4090	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	4091	tirePWM[TIRE_BL] = -10;
Ryosei	27:dd9f27fce7d1	4092	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	4093	tirePWM[TIRE_FR] = 10;
Ryosei	27:dd9f27fce7d1	4094	adjAnable = true;
Ryosei	27:dd9f27fce7d1	4095	break;
Ryosei	27:dd9f27fce7d1	4096	case 2:
Ryosei	27:dd9f27fce7d1	4097	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	4098	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	4099	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	4100	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	4101	adjAnable = true;
Ryosei	27:dd9f27fce7d1	4102	break;
Ryosei	27:dd9f27fce7d1	4103	case 'A':
Ryosei	27:dd9f27fce7d1	4104	if(lineCheck == false) {
Ryosei	27:dd9f27fce7d1	4105	lineCheck = true;
Ryosei	27:dd9f27fce7d1	4106	lineCount = 0;
Ryosei	27:dd9f27fce7d1	4107	countW++;
Ryosei	27:dd9f27fce7d1	4108	}
Ryosei	27:dd9f27fce7d1	4109	tirePWM[TIRE_FL] = -30;
Ryosei	27:dd9f27fce7d1	4110	tirePWM[TIRE_BL] = -30;
Ryosei	27:dd9f27fce7d1	4111	tirePWM[TIRE_BR] = 30;
Ryosei	27:dd9f27fce7d1	4112	tirePWM[TIRE_FR] = 30;
Ryosei	27:dd9f27fce7d1	4113	adjAnable = true;
Ryosei	27:dd9f27fce7d1	4114	break;
Ryosei	27:dd9f27fce7d1	4115	case 'N':
Ryosei	27:dd9f27fce7d1	4116	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	4117	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	4118	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	4119	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	4120	adjAnable = false;
Ryosei	27:dd9f27fce7d1	4121	break;
Ryosei	27:dd9f27fce7d1	4122	default:
Ryosei	27:dd9f27fce7d1	4123	tirePWM[TIRE_FL] = 0;
Ryosei	27:dd9f27fce7d1	4124	tirePWM[TIRE_BL] = 0;
Ryosei	27:dd9f27fce7d1	4125	tirePWM[TIRE_BR] = 0;
Ryosei	27:dd9f27fce7d1	4126	tirePWM[TIRE_FR] = 0;
Ryosei	27:dd9f27fce7d1	4127	adjAnable = false;
Ryosei	27:dd9f27fce7d1	4128	}
Ryosei	27:dd9f27fce7d1	4129
Ryosei	27:dd9f27fce7d1	4130	if(adjAnable) {
Ryosei	27:dd9f27fce7d1	4131	if(linePara[0] != 'A' && linePara[0] != 'N') adj = linePara[0];
Ryosei	27:dd9f27fce7d1	4132	} else {
Ryosei	27:dd9f27fce7d1	4133	adj = 0;
Ryosei	27:dd9f27fce7d1	4134	}
Ryosei	27:dd9f27fce7d1	4135
Ryosei	27:dd9f27fce7d1	4136	motor[TIRE_FL].dir = SetStatus(tirePWM[TIRE_FL] + adj);
Ryosei	27:dd9f27fce7d1	4137	motor[TIRE_BL].dir = SetStatus(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	4138	motor[TIRE_BR].dir = SetStatus(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	4139	motor[TIRE_FR].dir = SetStatus(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	4140	motor[TIRE_FL].pwm = SetPWM(tirePWM[TIRE_FL] + adj);
Ryosei	27:dd9f27fce7d1	4141	motor[TIRE_BL].pwm = SetPWM(tirePWM[TIRE_BL]);
Ryosei	27:dd9f27fce7d1	4142	motor[TIRE_BR].pwm = SetPWM(tirePWM[TIRE_BR]);
Ryosei	27:dd9f27fce7d1	4143	motor[TIRE_FR].pwm = SetPWM(tirePWM[TIRE_FR] + adj);
Ryosei	27:dd9f27fce7d1	4144
Ryosei	27:dd9f27fce7d1	4145	if(lineCheck == true) {
Ryosei	27:dd9f27fce7d1	4146	lineCount++;
Ryosei	27:dd9f27fce7d1	4147	if(lineCount > 20) lineCheck = false;
Ryosei	27:dd9f27fce7d1	4148	}
Ryosei	27:dd9f27fce7d1	4149	if(countW == 2) {
Ryosei	27:dd9f27fce7d1	4150	countW = 0;
Ryosei	27:dd9f27fce7d1	4151	lineFase = 22;
Ryosei	27:dd9f27fce7d1	4152	lineCount = 0;
Ryosei	27:dd9f27fce7d1	4153	lineCheck = false;
Ryosei	27:dd9f27fce7d1	4154	}
Ryosei	27:dd9f27fce7d1	4155	} else if(lineFase == 22) {
Ryosei	27:dd9f27fce7d1	4156	motor[TIRE_FL].dir = BACK;
Ryosei	27:dd9f27fce7d1	4157	motor[TIRE_BL].dir = BACK;
Ryosei	27:dd9f27fce7d1	4158	motor[TIRE_BR].dir = FOR;
Ryosei	27:dd9f27fce7d1	4159	motor[TIRE_FR].dir = FOR;
Ryosei	27:dd9f27fce7d1	4160	if (linePara[0] == 'N') {
Ryosei	27:dd9f27fce7d1	4161	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4162	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4163	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4164	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4165	}
Ryosei	27:dd9f27fce7d1	4166	motor[TIRE_FL].pwm = 30;
Ryosei	27:dd9f27fce7d1	4167	motor[TIRE_BL].pwm = 30;
Ryosei	27:dd9f27fce7d1	4168	motor[TIRE_BR].pwm = 30;
Ryosei	27:dd9f27fce7d1	4169	motor[TIRE_FR].pwm = 30;
Ryosei	27:dd9f27fce7d1	4170	} else {
Ryosei	27:dd9f27fce7d1	4171	motor[TIRE_FL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4172	motor[TIRE_BL].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4173	motor[TIRE_BR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4174	motor[TIRE_FR].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4175	}
Ryosei	27:dd9f27fce7d1	4176
kishibekairohan	13:b6e02d6261d7	4177	}
kishibekairohan	13:b6e02d6261d7	4178	#endif
kishibekairohan	13:b6e02d6261d7	4179
kishibekairohan	13:b6e02d6261d7	4180	#if USE_PROCESS_NUM>6
Ryosei	27:dd9f27fce7d1	4181	static void Process6()
kishibekairohan	13:b6e02d6261d7	4182	{
Ryosei	27:dd9f27fce7d1	4183
kishibekairohan	13:b6e02d6261d7	4184	}
kishibekairohan	13:b6e02d6261d7	4185	#endif
kishibekairohan	13:b6e02d6261d7	4186
kishibekairohan	13:b6e02d6261d7	4187	#if USE_PROCESS_NUM>7
kishibekairohan	13:b6e02d6261d7	4188	static void Process7()
kishibekairohan	13:b6e02d6261d7	4189	{
Ryosei	27:dd9f27fce7d1	4190	static int SW_flag=0;
Ryosei	27:dd9f27fce7d1	4191	static int Processflag=0;
Ryosei	27:dd9f27fce7d1	4192	if(Processflag==0) {
Ryosei	27:dd9f27fce7d1	4193	if(Limitphase==0) {
Ryosei	27:dd9f27fce7d1	4194	//下待機
Ryosei	27:dd9f27fce7d1	4195	Limitphase=2;
Ryosei	27:dd9f27fce7d1	4196	} else if(Limitphase==1) {
Ryosei	27:dd9f27fce7d1	4197	} else if(Limitphase==2) {
Ryosei	27:dd9f27fce7d1	4198	//下→上とタオル展開
Ryosei	27:dd9f27fce7d1	4199	pc.printf("%d\r\n",SW_flag);
Ryosei	27:dd9f27fce7d1	4200
Ryosei	27:dd9f27fce7d1	4201	motor[LIFT_LB].dir = FOR;
Ryosei	27:dd9f27fce7d1	4202	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4203	motor[LIFT_RB].dir = BACK;
Ryosei	27:dd9f27fce7d1	4204	motor[LIFT_RB].pwm = 180;
Ryosei	27:dd9f27fce7d1	4205
Ryosei	27:dd9f27fce7d1	4206	if(SW_flag==0) {
Ryosei	27:dd9f27fce7d1	4207	motor[LIFT_LB].dir = FOR;
Ryosei	27:dd9f27fce7d1	4208	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4209	motor[LIFT_RB].dir = BACK;
Ryosei	27:dd9f27fce7d1	4210	motor[LIFT_RB].pwm = 180;
Ryosei	27:dd9f27fce7d1	4211	if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
Ryosei	27:dd9f27fce7d1	4212	SW_flag=1;
Ryosei	27:dd9f27fce7d1	4213	}
Ryosei	27:dd9f27fce7d1	4214	} else if(SW_flag==1) {
Ryosei	27:dd9f27fce7d1	4215	motor[LIFT_LB].dir = FOR;
Ryosei	27:dd9f27fce7d1	4216	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4217	motor[LIFT_RB].dir = BACK;
Ryosei	27:dd9f27fce7d1	4218	motor[LIFT_RB].pwm = 180;
Ryosei	27:dd9f27fce7d1	4219	if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Ryosei	27:dd9f27fce7d1	4220	SW_flag=2;
Ryosei	27:dd9f27fce7d1	4221	}
Ryosei	27:dd9f27fce7d1	4222	} else if(SW_flag==2) {
Ryosei	27:dd9f27fce7d1	4223	motor[LIFT_LB].dir = FOR;
Ryosei	27:dd9f27fce7d1	4224	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4225	motor[LIFT_RB].dir = BACK;
Ryosei	27:dd9f27fce7d1	4226	motor[LIFT_RB].pwm = 180;
Ryosei	27:dd9f27fce7d1	4227	if(!LimitSw::IsPressed(LSW_LB)&&!LimitSw::IsPressed(LSW_RB)) {
Ryosei	27:dd9f27fce7d1	4228	SW_flag=3;
Ryosei	27:dd9f27fce7d1	4229	}
Ryosei	27:dd9f27fce7d1	4230	} else if(SW_flag==3) {
Ryosei	27:dd9f27fce7d1	4231	motor[LIFT_LB].dir = FOR;
Ryosei	27:dd9f27fce7d1	4232	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4233	motor[LIFT_RB].dir = BACK;
Ryosei	27:dd9f27fce7d1	4234	motor[LIFT_RB].pwm = 180;
Ryosei	27:dd9f27fce7d1	4235	if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Ryosei	27:dd9f27fce7d1	4236	motor[LIFT_LB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4237	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4238	motor[LIFT_RB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4239	motor[LIFT_RB].pwm = 180;
Ryosei	27:dd9f27fce7d1	4240	Limitphase=4;
Ryosei	27:dd9f27fce7d1	4241	}
Ryosei	27:dd9f27fce7d1	4242	}
Ryosei	27:dd9f27fce7d1	4243	} else if(Limitphase==3) {
Ryosei	27:dd9f27fce7d1	4244	} else if(Limitphase==4) {
Ryosei	27:dd9f27fce7d1	4245	//上段待機
Ryosei	27:dd9f27fce7d1	4246	motor[LIFT_LB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4247	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4248	motor[LIFT_RB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4249	motor[LIFT_RB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4250	current=0;
Ryosei	27:dd9f27fce7d1	4251	startFlag=true;
Ryosei	27:dd9f27fce7d1	4252	} else {
Ryosei	27:dd9f27fce7d1	4253	motor[LIFT_LB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4254	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4255	motor[LIFT_RB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4256	motor[LIFT_RB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4257	startFlag=true;
Ryosei	27:dd9f27fce7d1	4258	current=0;
Ryosei	27:dd9f27fce7d1	4259	}
Ryosei	27:dd9f27fce7d1	4260	if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)) {
Ryosei	27:dd9f27fce7d1	4261	motor[LIFT_LB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4262	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4263	motor[LIFT_RB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4264	motor[LIFT_RB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4265	Air[TOWEL0]=1;
Ryosei	27:dd9f27fce7d1	4266	SW_flag=0;
Ryosei	27:dd9f27fce7d1	4267	Limitphase=0;
Ryosei	27:dd9f27fce7d1	4268	current=0;
Ryosei	27:dd9f27fce7d1	4269	startFlag=true;
Ryosei	27:dd9f27fce7d1	4270	Processflag=1;
Ryosei	27:dd9f27fce7d1	4271	}
Ryosei	27:dd9f27fce7d1	4272	} else if(Processflag==1) {
Ryosei	27:dd9f27fce7d1	4273	motor[LIFT_LB].dir = FOR;
Ryosei	27:dd9f27fce7d1	4274	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4275	motor[LIFT_RB].dir = BACK;
Ryosei	27:dd9f27fce7d1	4276	motor[LIFT_RB].pwm = 180;
Ryosei	27:dd9f27fce7d1	4277	if(LimitSw::IsPressed(LSW_LB)&&LimitSw::IsPressed(LSW_RB)) {
Ryosei	27:dd9f27fce7d1	4278	motor[LIFT_LB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4279	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4280	motor[LIFT_RB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4281	motor[LIFT_RB].pwm = 180;
Ryosei	27:dd9f27fce7d1	4282	current=0;
Ryosei	27:dd9f27fce7d1	4283	startFlag=true;
Ryosei	27:dd9f27fce7d1	4284	Processflag=1;
Ryosei	27:dd9f27fce7d1	4285	}
Ryosei	27:dd9f27fce7d1	4286	if(LimitSw::IsPressed(UNFOLD_ZYOUGE_SW)){
Ryosei	27:dd9f27fce7d1	4287	motor[LIFT_LB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4288	motor[LIFT_LB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4289	motor[LIFT_RB].dir = BRAKE;
Ryosei	27:dd9f27fce7d1	4290	motor[LIFT_RB].pwm = 200;
Ryosei	27:dd9f27fce7d1	4291	Air[TOWEL0]=1;
Ryosei	27:dd9f27fce7d1	4292	SW_flag=0;
Ryosei	27:dd9f27fce7d1	4293	Limitphase=0;
Ryosei	27:dd9f27fce7d1	4294	current=0;
Ryosei	27:dd9f27fce7d1	4295	startFlag=true;
Ryosei	27:dd9f27fce7d1	4296	Processflag=1;
Ryosei	27:dd9f27fce7d1	4297	}
Ryosei	27:dd9f27fce7d1	4298	}
kishibekairohan	12:c09b3e08a316	4299	}
Ryosei	27:dd9f27fce7d1	4300
Ryosei	27:dd9f27fce7d1	4301
Ryosei	27:dd9f27fce7d1	4302
kishibekairohan	12:c09b3e08a316	4303	#endif
kishibekairohan	12:c09b3e08a316	4304
Ryosei	27:dd9f27fce7d1	4305	#if USE_PROCESS_NUM>8
kishibekairohan	13:b6e02d6261d7	4306	static void Process8()
kishibekairohan	12:c09b3e08a316	4307	{
Ryosei	27:dd9f27fce7d1	4308	if(controller->Button.A) {
Ryosei	27:dd9f27fce7d1	4309	rotaconSampling.start();
Ryosei	27:dd9f27fce7d1	4310	PIDflag = true;
Ryosei	27:dd9f27fce7d1	4311
Ryosei	27:dd9f27fce7d1	4312	//linePara_U = LineHub::GetPara(0);
Ryosei	27:dd9f27fce7d1	4313	//linePara_B = LineHub::GetPara(3);
Ryosei	27:dd9f27fce7d1	4314
Ryosei	27:dd9f27fce7d1	4315
Ryosei	27:dd9f27fce7d1	4316	pulsePV[FL] = encoder[FL].getPulses();
Ryosei	27:dd9f27fce7d1	4317	pulsePV[BL] = encoder[BL].getPulses();
Ryosei	27:dd9f27fce7d1	4318	pulsePV[BR] = encoder[BR].getPulses();
Ryosei	27:dd9f27fce7d1	4319	pulsePV[FR] = encoder[FR].getPulses();
Ryosei	27:dd9f27fce7d1	4320
Ryosei	27:dd9f27fce7d1	4321
Ryosei	27:dd9f27fce7d1	4322	for (int i = 0; i < 4; i++) {
Ryosei	27:dd9f27fce7d1	4323	timeCV[i] = timePV[i];
Ryosei	27:dd9f27fce7d1	4324	timePV[i] = rotaconSampling.read();
Ryosei	27:dd9f27fce7d1	4325	tireProcessRPM[i] = (pulsePV[i] - pulseCV[i])/ (float)(256 * 2) / (timePV[i] - timeCV[i]) * 60;
Ryosei	27:dd9f27fce7d1	4326	pulseCV[i] = pulsePV[i];
Ryosei	27:dd9f27fce7d1	4327	}
Ryosei	27:dd9f27fce7d1	4328
Ryosei	27:dd9f27fce7d1	4329	move.Vx = 0.5;
Ryosei	27:dd9f27fce7d1	4330	move.Vy = 0.5;
Ryosei	27:dd9f27fce7d1	4331	move.Va = 0;
Ryosei	27:dd9f27fce7d1	4332
Ryosei	27:dd9f27fce7d1	4333	correction_LT.Vx = 0; //0.1 * linePara_U;
Ryosei	27:dd9f27fce7d1	4334	correction_LT.Vy = 0;
Ryosei	27:dd9f27fce7d1	4335	correction_LT.Va = 0;
Ryosei	27:dd9f27fce7d1	4336
Ryosei	27:dd9f27fce7d1	4337	synthetic.Vx = move.Vx + correction_LT.Vx;
Ryosei	27:dd9f27fce7d1	4338	synthetic.Vy = move.Vy + correction_LT.Vy;
Ryosei	27:dd9f27fce7d1	4339	synthetic.Va = move.Va + correction_LT.Va;
Ryosei	27:dd9f27fce7d1	4340
Ryosei	27:dd9f27fce7d1	4341	sita = 0;
Ryosei	27:dd9f27fce7d1	4342
Ryosei	27:dd9f27fce7d1	4343	//タイヤの目標速度算出
Ryosei	27:dd9f27fce7d1	4344	float sinR = 0.7071 * (float)sin(sita);
Ryosei	27:dd9f27fce7d1	4345	float cosR = 0.7071 * (float)cos(sita);
Ryosei	27:dd9f27fce7d1	4346	float nv = (60 * 1000) / ( 2.00 * PI * tireR);
Ryosei	27:dd9f27fce7d1	4347	tireTargetRPM[FL] = ((+ synthetic.Vx * (sinR - cosR)) - (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei	27:dd9f27fce7d1	4348	tireTargetRPM[BL] = ((+ synthetic.Vx * (sinR + cosR)) + (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei	27:dd9f27fce7d1	4349	tireTargetRPM[BR] = ((- synthetic.Vx * (sinR - cosR)) + (synthetic.Vy * (sinR + cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei	27:dd9f27fce7d1	4350	tireTargetRPM[FR] = ((- synthetic.Vx * (sinR + cosR)) - (synthetic.Vy * (sinR - cosR)) + (ucR * synthetic.Va)) * nv;
Ryosei	27:dd9f27fce7d1	4351
Ryosei	27:dd9f27fce7d1	4352	//pc.printf("process : %f target : %f\n\r",tireProcessRPM[0],tireTargetRPM[0]);
Ryosei	27:dd9f27fce7d1	4353
Ryosei	27:dd9f27fce7d1	4354	//PIDによるPWM算出
Ryosei	27:dd9f27fce7d1	4355
Ryosei	27:dd9f27fce7d1	4356	//モータの駆動
Ryosei	27:dd9f27fce7d1	4357	for (int i = 0; i < 4; i++) {
Ryosei	27:dd9f27fce7d1	4358	if (tirePWM[i] > 255) {
Ryosei	27:dd9f27fce7d1	4359	tirePWM[i] = 255;
Ryosei	27:dd9f27fce7d1	4360	} else if (tirePWM[i] < -255) {
Ryosei	27:dd9f27fce7d1	4361	tirePWM[i] = -255;
Ryosei	27:dd9f27fce7d1	4362	}
Ryosei	27:dd9f27fce7d1	4363	}
Ryosei	27:dd9f27fce7d1	4364
Ryosei	27:dd9f27fce7d1	4365	for(int i = 0; i < 4; i++) {
Ryosei	27:dd9f27fce7d1	4366	motor[i].dir = SetStatus(tirePWM[i]);
Ryosei	27:dd9f27fce7d1	4367	motor[i].pwm = SetPWM(tirePWM[i]);
Ryosei	27:dd9f27fce7d1	4368	}
Ryosei	27:dd9f27fce7d1	4369	} else {
Ryosei	27:dd9f27fce7d1	4370	PIDflag = false;
Ryosei	27:dd9f27fce7d1	4371	rotaconSampling.stop();
Ryosei	27:dd9f27fce7d1	4372	rotaconSampling.reset();
Ryosei	27:dd9f27fce7d1	4373	for(int i = 0; i < 4; i++) {
Ryosei	27:dd9f27fce7d1	4374	encoder[i].reset();
Ryosei	27:dd9f27fce7d1	4375	pulsePV[i] = 0;
Ryosei	27:dd9f27fce7d1	4376	pulseCV[i] = 0;
Ryosei	27:dd9f27fce7d1	4377	timePV[i] = 0;
Ryosei	27:dd9f27fce7d1	4378	timeCV[i] = 0;
Ryosei	27:dd9f27fce7d1	4379	tirePWM[i] = 0;
Ryosei	27:dd9f27fce7d1	4380	motor[i].dir = SetStatus(tirePWM[i]);
Ryosei	27:dd9f27fce7d1	4381	motor[i].pwm = SetPWM(tirePWM[i]);
Ryosei	27:dd9f27fce7d1	4382	}
Ryosei	27:dd9f27fce7d1	4383	}
kishibekairohan	12:c09b3e08a316	4384	}
kishibekairohan	12:c09b3e08a316	4385	#endif
kishibekairohan	12:c09b3e08a316	4386
kishibekairohan	13:b6e02d6261d7	4387	#if USE_PROCESS_NUM>9
kishibekairohan	13:b6e02d6261d7	4388	static void Process9()
kishibekairohan	12:c09b3e08a316	4389	{
Ryosei	27:dd9f27fce7d1	4390
t_yamamoto	0:669ef71cba68	4391	}
t_yamamoto	0:669ef71cba68	4392	#endif
t_yamamoto	0:669ef71cba68	4393	#endif
t_yamamoto	0:669ef71cba68	4394	#pragma endregion PROCESS
t_yamamoto	0:669ef71cba68	4395
t_yamamoto	0:669ef71cba68	4396	static void AllActuatorReset()
t_yamamoto	0:669ef71cba68	4397	{
t_yamamoto	0:669ef71cba68	4398
Ryosei	27:dd9f27fce7d1	4399	#ifdef USE_SOLENOID
Ryosei	27:dd9f27fce7d1	4400	solenoid.all = ALL_SOLENOID_OFF;
Ryosei	27:dd9f27fce7d1	4401	#endif
t_yamamoto	0:669ef71cba68	4402
Ryosei	27:dd9f27fce7d1	4403	#ifdef USE_MOTOR
Ryosei	27:dd9f27fce7d1	4404	for (uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
Ryosei	27:dd9f27fce7d1	4405	motor[i].dir = FREE;
Ryosei	27:dd9f27fce7d1	4406	motor[i].pwm = 0;
Ryosei	27:dd9f27fce7d1	4407	}
Ryosei	27:dd9f27fce7d1	4408	#endif
t_yamamoto	0:669ef71cba68	4409	}
t_yamamoto	0:669ef71cba68	4410
Ryosei	27:dd9f27fce7d1	4411	void BuzzerTimer_func()
Ryosei	27:dd9f27fce7d1	4412	{
kishibekairohan	16:3f2c2d89372b	4413	buzzer = !buzzer;
kishibekairohan	17:50dc4b449e69	4414	//LED_DEBUG0 = !LED_DEBUG0;
kishibekairohan	11:028a150943b5	4415	}
kishibekairohan	11:028a150943b5	4416
Ryosei	27:dd9f27fce7d1	4417	void TapeLedEms_func()
Ryosei	27:dd9f27fce7d1	4418	{
kishibekairohan	16:3f2c2d89372b	4419	sendLedData.code = sendLedData.code == (uint32_t)Red ? (uint32_t)Black : (uint32_t)Red;
kishibekairohan	9:f93fc79a49ea	4420	}
kishibekairohan	16:3f2c2d89372b	4421
kishibekairohan	16:3f2c2d89372b	4422	#pragma region USER-DEFINED-FUNCTIONS
Ryosei	27:dd9f27fce7d1	4423	void tirePID()
Ryosei	27:dd9f27fce7d1	4424	{
Ryosei	27:dd9f27fce7d1	4425	if(PIDflag == true) {
Ryosei	27:dd9f27fce7d1	4426	//加算するPID値の算出
Ryosei	27:dd9f27fce7d1	4427	rotaconPID[0].SetPV(tireProcessRPM[FL],tireTargetRPM[FL]);
Ryosei	27:dd9f27fce7d1	4428	rotaconPID[1].SetPV(tireProcessRPM[BL],tireTargetRPM[BL]);
Ryosei	27:dd9f27fce7d1	4429	rotaconPID[2].SetPV(tireProcessRPM[FR],tireTargetRPM[FR]);
Ryosei	27:dd9f27fce7d1	4430	rotaconPID[3].SetPV(tireProcessRPM[BR],tireTargetRPM[BR]);
Ryosei	27:dd9f27fce7d1	4431	//PID値の加算
Ryosei	27:dd9f27fce7d1	4432	tirePWM[FL] += rotaconPID[0].GetMV();
Ryosei	27:dd9f27fce7d1	4433	tirePWM[BL] += rotaconPID[1].GetMV();
Ryosei	27:dd9f27fce7d1	4434	tirePWM[FR] += rotaconPID[2].GetMV();
Ryosei	27:dd9f27fce7d1	4435	tirePWM[BR] += rotaconPID[3].GetMV();
Ryosei	27:dd9f27fce7d1	4436	}
M_souta	22:7d93f79a3686	4437	}
kishibekairohan	16:3f2c2d89372b	4438
Ryosei	27:dd9f27fce7d1	4439	int lineCast(char k)
Ryosei	27:dd9f27fce7d1	4440	{
Ryosei	27:dd9f27fce7d1	4441	int l;
Ryosei	27:dd9f27fce7d1	4442	switch(k) {
Ryosei	27:dd9f27fce7d1	4443	case 255:
Ryosei	27:dd9f27fce7d1	4444	l = -1;
Ryosei	27:dd9f27fce7d1	4445	break;
Ryosei	27:dd9f27fce7d1	4446	case 254:
Ryosei	27:dd9f27fce7d1	4447	l = -2;
Ryosei	27:dd9f27fce7d1	4448	break;
Ryosei	27:dd9f27fce7d1	4449	case 253:
Ryosei	27:dd9f27fce7d1	4450	l = -3;
Ryosei	27:dd9f27fce7d1	4451	break;
Ryosei	27:dd9f27fce7d1	4452	default:
Ryosei	27:dd9f27fce7d1	4453	l = k;
Ryosei	27:dd9f27fce7d1	4454	}
Ryosei	27:dd9f27fce7d1	4455	return l;
M_souta	22:7d93f79a3686	4456	}
kishibekairohan	16:3f2c2d89372b	4457
t_yamamoto	0:669ef71cba68	4458	#pragma endregion

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	03 Oct 2019
Imports:	4
Forks:	0
Commits:	31
Dependents:	0
Dependencies:	4
Followers:	1

System/Process/Process.cpp@28:479631c2de29, 2019-10-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning