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@30:76b140d176f0, 2020-03-27 (annotated)

Committer:: Ryosei
Date:: Fri Mar 27 14:03:12 2020 +0000
Revision:: 30:76b140d176f0
Parent:: 29:5365ee0521f6

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

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning