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@29:5365ee0521f6, 2019-10-06 (annotated)

Committer:: Ryosei
Date:: Sun Oct 06 03:53:06 2019 +0000
Revision:: 29:5365ee0521f6
Parent:: 28:479631c2de29
Child:: 30:76b140d176f0

jh

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

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning