MB2019_main_alltimes_old - aa

Users » yabahiro » Code » MB2019_main_alltimes_old

大季矢花 / Mbed 2 deprecated MB2019_main_alltimes_old

System/Process/Process.cpp@34:2fce61e3ebc4, 2019-10-29 (annotated)

Committer:: kishibekairohan
Date:: Tue Oct 29 08:35:05 2019 +0000
Revision:: 34:2fce61e3ebc4
Parent:: 33:dd598bb23ad1
Child:: 35:33c6cdba5630

kakakakkaka

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

Repository toolbox

Export to desktop IDE

Build repository

Repository details

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

System/Process/Process.cpp@34:2fce61e3ebc4, 2019-10-29 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning