MB2019_main_alltimes_1123 - aa

Users » yabahiro » Code » MB2019_main_alltimes_1123

大季矢花 / Mbed 2 deprecated MB2019_main_alltimes_1123

System/Process/Process.cpp@33:dd598bb23ad1, 2019-10-28 (annotated)

Committer:: yabahiro
Date:: Mon Oct 28 21:11:17 2019 +0000
Revision:: 33:dd598bb23ad1
Parent:: 32:985ec7913f2a
Child:: 34:2fce61e3ebc4

aa;

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

System/Process/Process.cpp@33:dd598bb23ad1, 2019-10-28 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning