UNKO_FINAL_PlanA - Bteam tactics plan A _ move table ver

NITICRobotClub 2018Team-B » Code » UNKO_FINAL_PlanA

NITICRobotClub 2018Team-B / Mbed 2 deprecated UNKO_FINAL_PlanA

Bteam tactics plan A _ move table ver

Fork of UNKO_FINAL by NITICRobotClub 2018Team-B

main.cpp@11:5a0d6f69e751, 2018-10-10 (annotated)

Committer:: yuron
Date:: Wed Oct 10 11:27:09 2018 +0000
Revision:: 11:5a0d6f69e751
Parent:: 10:b672aa81b226
Child:: 12:1a22b9797004

??????2

Who changed what in which revision?

User	Revision	Line number	New contents of line
yuron	4:df334779a69e	1	/* ------------------------------------------------------------------- */
yuron	4:df334779a69e	2	/* NHKロボコン2018-Bチーム自動ロボット(設計者: 4S 関) */
yuron	4:df334779a69e	3	/* ------------------------------------------------------------------- */
yuron	4:df334779a69e	4	/* このプログラムは上記のロボット専用の制御プログラムである。 */
yuron	4:df334779a69e	5	/* ------------------------------------------------------------------- */
yuron	4:df334779a69e	6	/* 対応機種: NUCLEO-F446RE */
yuron	4:df334779a69e	7	/* ------------------------------------------------------------------- */
yuron	4:df334779a69e	8	/* 製作者: 4D 高久雄飛, mail: rab1sy23@gmail.com */
yuron	4:df334779a69e	9	/* ------------------------------------------------------------------- */
yuron	4:df334779a69e	10	/* 使用センサ: リミットスイッチ1個, ロータリーエンコーダ: 7個, 超音波センサ: 1個, フォトインタラプタ: 1個 */
yuron	4:df334779a69e	11	/* 他にラインセンサ基板のPIC(16F1938)と通信をしてライントレースをさせている */
yuron	4:df334779a69e	12	/* ------------------------------------------------------------------- */
yuron	5:167327a82430	13
yuron	0:f73c1b076ae4	14	#include "mbed.h"
yuron	0:f73c1b076ae4	15	#include "math.h"
yuron	0:f73c1b076ae4	16	#include "QEI.h"
yuron	0:f73c1b076ae4	17	#include "PID.h"
yuron	4:df334779a69e	18	#include "hcsr04.h"
yuron	5:167327a82430	19
yuron	5:167327a82430	20	//Pi
yuron	0:f73c1b076ae4	21	#define PI 3.14159265359
yuron	5:167327a82430	22	//PIDGain of wheels(fast_mode)
yuron	4:df334779a69e	23	#define Kp 20.0
yuron	4:df334779a69e	24	#define Ki 0.02
yuron	4:df334779a69e	25	#define Kd 0.0
yuron	5:167327a82430	26	//PIDGain of rollers
yuron	6:5a051a378e42	27	#define front_roller_Kp 1.3
yuron	6:5a051a378e42	28	#define front_roller_Ki 0.19
yuron	6:5a051a378e42	29	#define front_roller_Kd 0.0
yuron	6:5a051a378e42	30	#define back_roller_Kp 1.3
yuron	6:5a051a378e42	31	#define back_roller_Ki 0.1
yuron	6:5a051a378e42	32	#define back_roller_Kd 0.0
yuron	5:167327a82430	33	//PIDGain of wheels(slow_mode)
yuron	5:167327a82430	34	#define Kp_slow 0.6
yuron	5:167327a82430	35	#define Ki_slow 0.03
yuron	5:167327a82430	36	#define Kd_slow 0.0
yuron	5:167327a82430	37
yuron	7:7f16fb8b0192	38	//搭載ボトル数
yuron	8:3df97287c825	39	//1弾倉当たり9発、残り3発になったら弾倉切り替えしよう
yuron	8:3df97287c825	40	#define bottles 14
yuron	7:7f16fb8b0192	41
yuron	5:167327a82430	42	//停止
yuron	5:167327a82430	43	#define stop 0
yuron	5:167327a82430	44	//低速右移動
yuron	5:167327a82430	45	#define right_slow 1
yuron	5:167327a82430	46	//低速左移動
yuron	5:167327a82430	47	#define left_slow 2
yuron	5:167327a82430	48	//超低速右移動
yuron	5:167327a82430	49	#define right_super_slow 3
yuron	5:167327a82430	50	//超低速左移動
yuron	5:167327a82430	51	#define left_super_slow 4
yuron	5:167327a82430	52	//右旋回
yuron	5:167327a82430	53	#define turn_right 5
yuron	5:167327a82430	54	//左旋回
yuron	5:167327a82430	55	#define turn_left 6
yuron	5:167327a82430	56	//前進
yuron	5:167327a82430	57	#define front_trace 7
yuron	5:167327a82430	58	//後進
yuron	5:167327a82430	59	#define back_trace 8
yuron	5:167327a82430	60	//前前進後ろ右旋回
yuron	5:167327a82430	61	#define front_front_back_right 9
yuron	5:167327a82430	62	//前前進後ろ左旋回
yuron	5:167327a82430	63	#define front_front_back_left 10
yuron	5:167327a82430	64	//前右旋回後ろ前進
yuron	5:167327a82430	65	#define front_right_back_front 11
yuron	5:167327a82430	66	//前左旋回後ろ前進
yuron	5:167327a82430	67	#define front_left_back_front 12
yuron	5:167327a82430	68
yuron	8:3df97287c825	69	#define right_fast 13
yuron	8:3df97287c825	70	#define left_fast 14
yuron	8:3df97287c825	71
yuron	9:1359f0c813b1	72	//2段低
yuron	9:1359f0c813b1	73	#define low_grade 1
yuron	9:1359f0c813b1	74	//2段高
yuron	9:1359f0c813b1	75	#define high_grade 2
yuron	9:1359f0c813b1	76	//移動低
yuron	9:1359f0c813b1	77	#define low_table 3
yuron	9:1359f0c813b1	78	//移動中
yuron	9:1359f0c813b1	79	#define medium_table 4
yuron	9:1359f0c813b1	80	//移動高
yuron	9:1359f0c813b1	81	#define high_table 5
yuron	9:1359f0c813b1	82
yuron	10:b672aa81b226	83	//2段低最小距離
yuron	11:5a0d6f69e751	84	#define low_grade_minimum_distance 7
yuron	10:b672aa81b226	85	//2段低最大距離
yuron	11:5a0d6f69e751	86	#define low_grade_maximum_distance 16
yuron	10:b672aa81b226	87
yuron	10:b672aa81b226	88	//2段高最小距離
yuron	10:b672aa81b226	89	//#define high_grade_minimum_distance 1
yuron	10:b672aa81b226	90	//2段高最大距離
yuron	10:b672aa81b226	91	//#define high_grade_maximum_distance 1
yuron	10:b672aa81b226	92
yuron	10:b672aa81b226	93	//移動低最小距離
yuron	10:b672aa81b226	94	#define low_table_minimum_distance 10
yuron	10:b672aa81b226	95	//移動低最大距離
yuron	10:b672aa81b226	96	#define low_table_maximum_distance 15
yuron	10:b672aa81b226	97
yuron	10:b672aa81b226	98	//移動中最小距離
yuron	10:b672aa81b226	99	#define medium_table_minimum_distance 20
yuron	10:b672aa81b226	100	//移動中最大距離
yuron	10:b672aa81b226	101	#define medium_table_maximum_distance 30
yuron	10:b672aa81b226	102
yuron	10:b672aa81b226	103	//移動高最小距離
yuron	10:b672aa81b226	104	#define high_table_minimum_distance 30
yuron	10:b672aa81b226	105	//移動高最大距離
yuron	10:b672aa81b226	106	#define high_table_maximum_distance 40
yuron	10:b672aa81b226	107
yuron	10:b672aa81b226	108
yuron	5:167327a82430	109	//赤ゾーン
yuron	5:167327a82430	110	#define red 1
yuron	5:167327a82430	111	//青ゾーン
yuron	5:167327a82430	112	#define blue 0
yuron	4:df334779a69e	113
yuron	4:df334779a69e	114	//右前オムニ
yuron	4:df334779a69e	115	PID migimae(Kp, Ki, Kd, 0.001);
yuron	4:df334779a69e	116	//右後オムニ
yuron	4:df334779a69e	117	PID migiusiro(Kp, Ki, Kd, 0.001);
yuron	4:df334779a69e	118	//左前オムニ
yuron	4:df334779a69e	119	PID hidarimae(Kp, Ki, Kd, 0.001);
yuron	4:df334779a69e	120	//左後オムニ
yuron	4:df334779a69e	121	PID hidariusiro(Kp, Ki, Kd, 0.001);
yuron	5:167327a82430	122
yuron	5:167327a82430	123	//右前オムニ(スローモード)
yuron	5:167327a82430	124	PID migimae_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
yuron	5:167327a82430	125	//右後オムニ(スローモード)
yuron	5:167327a82430	126	PID migiusiro_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
yuron	5:167327a82430	127	//左前オムニ(スローモード)
yuron	5:167327a82430	128	PID hidarimae_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
yuron	5:167327a82430	129	//左後オムニ(スローモード)
yuron	5:167327a82430	130	PID hidariusiro_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
yuron	5:167327a82430	131
yuron	4:df334779a69e	132	//前ローラー
yuron	6:5a051a378e42	133	PID front_roller(front_roller_Kp, front_roller_Ki, front_roller_Kd, 0.001);
yuron	4:df334779a69e	134	//後ローラー
yuron	6:5a051a378e42	135	PID back_roller (back_roller_Kp, back_roller_Ki, back_roller_Kd, 0.001);
yuron	0:f73c1b076ae4	136
yuron	4:df334779a69e	137	//MDとの通信ポート
yuron	4:df334779a69e	138	I2C i2c(PB_9, PB_8); //SDA, SCL
yuron	4:df334779a69e	139	//PCとの通信ポート
yuron	4:df334779a69e	140	Serial pc(USBTX, USBRX); //TX, RX
yuron	4:df334779a69e	141	//フォトインタラプタ制御基板からの受信ポート
yuron	4:df334779a69e	142	Serial photo(NC, PC_11); //TX, RX
yuron	4:df334779a69e	143	//TWE-Liteからの受信ポート
yuron	4:df334779a69e	144	Serial twe(PC_12, PD_2); //TX, RX
yuron	4:df334779a69e	145
yuron	4:df334779a69e	146	//超音波センサ1
yuron	4:df334779a69e	147	HCSR04 sonic(PB_3, PB_10); //Trig, Echo
yuron	4:df334779a69e	148	//超音波センサ2
yuron	4:df334779a69e	149	//HCSR04 sonic2(PC_13, PA_15); //Trig, Echo
yuron	4:df334779a69e	150	//超音波センサ3
yuron	4:df334779a69e	151	//HCSR04 sonic3(PC_15, PC_14); //Trig, Echo
yuron	4:df334779a69e	152	//超音波センサ4
yuron	4:df334779a69e	153	//HCSR04 sonic4(PH_1 , PH_0 ); //Trig, Echo
yuron	4:df334779a69e	154
yuron	4:df334779a69e	155	//赤、青ゾーン切り替え用スイッチ
yuron	4:df334779a69e	156	DigitalIn side(PC_1);
yuron	4:df334779a69e	157	//スタートボタン
yuron	4:df334779a69e	158	DigitalIn start_button(PC_4);
yuron	5:167327a82430	159	//フェンス検知用リミットスイッチ
yuron	5:167327a82430	160	DigitalIn limit(PH_1);
yuron	4:df334779a69e	161	//スイッチ1
yuron	4:df334779a69e	162	DigitalIn user_switch1(PB_0);
yuron	4:df334779a69e	163	//スイッチ2
yuron	4:df334779a69e	164	DigitalIn user_switch2(PA_4);
yuron	4:df334779a69e	165	//スイッチ3
yuron	4:df334779a69e	166	DigitalIn user_switch3(PA_3);
yuron	4:df334779a69e	167	//スイッチ4
yuron	4:df334779a69e	168	//以下の文を入れるとロリコンが読めなくなる
yuron	4:df334779a69e	169	//DigitalIn user_switch4(PA_2);
yuron	4:df334779a69e	170	//スイッチ5
yuron	4:df334779a69e	171	DigitalIn user_switch5(PA_10);
yuron	4:df334779a69e	172
yuron	4:df334779a69e	173	//フォトインタラプタ
yuron	4:df334779a69e	174	DigitalIn photo_interrupter(PA_15);
yuron	4:df334779a69e	175
yuron	4:df334779a69e	176	//12V停止信号ピン
yuron	0:f73c1b076ae4	177	DigitalOut emergency(PA_13);
yuron	4:df334779a69e	178
yuron	4:df334779a69e	179	//赤ゾーンLED
yuron	4:df334779a69e	180	DigitalOut blue_side(PC_0);
yuron	4:df334779a69e	181	//青ゾーンLED
yuron	4:df334779a69e	182	DigitalOut red_side(PC_3);
yuron	4:df334779a69e	183	//スタートLED
yuron	6:5a051a378e42	184	DigitalOut start_LED(PC_13);
yuron	6:5a051a378e42	185	//DigitalOut start_LED(PA_8);
yuron	8:3df97287c825	186
yuron	4:df334779a69e	187	//LED1
yuron	4:df334779a69e	188	DigitalOut myled1(PC_6);
yuron	4:df334779a69e	189	//LED2
yuron	4:df334779a69e	190	DigitalOut myled2(PC_5);
yuron	4:df334779a69e	191	//LED3
yuron	4:df334779a69e	192	DigitalOut myled3(PA_12);
yuron	4:df334779a69e	193	//LED4
yuron	4:df334779a69e	194	DigitalOut myled4(PB_1);
yuron	4:df334779a69e	195	//LED5
yuron	4:df334779a69e	196	DigitalOut myled5(PA_5);
yuron	0:f73c1b076ae4	197
yuron	4:df334779a69e	198	//前ローラー
yuron	4:df334779a69e	199	QEI front_roller_wheel(PA_0, PA_1, NC, 624);
yuron	4:df334779a69e	200	//後ローラー
yuron	4:df334779a69e	201	QEI back_roller_wheel(PB_5, PB_4, NC, 624);
yuron	4:df334779a69e	202	//計測オムニ1
yuron	5:167327a82430	203	QEI measure_wheel(PC_9, PC_8, NC, 624);
yuron	4:df334779a69e	204	//計測オムニ2(使用不可)
yuron	4:df334779a69e	205	//QEI measure2_wheel(PB_3, PB_10, NC, 624);
yuron	4:df334779a69e	206	//右前オムニ
yuron	4:df334779a69e	207	QEI migimae_wheel(PB_6 , PA_7 , NC, 624);
yuron	4:df334779a69e	208	//右後オムニ
yuron	4:df334779a69e	209	QEI migiusiro_wheel(PA_11, PB_12, NC, 624);
yuron	4:df334779a69e	210	//左前オムニ
yuron	4:df334779a69e	211	QEI hidarimae_wheel(PB_2, PB_15, NC, 624);
yuron	4:df334779a69e	212	//左後オムニ
yuron	4:df334779a69e	213	QEI hidariusiro_wheel(PB_14, PB_13, NC, 624);
yuron	0:f73c1b076ae4	214
yuron	6:5a051a378e42	215	Ticker get_rpm;
yuron	1:62b321f6c9c3	216	Timer timer;
yuron	5:167327a82430	217	Timer back_timer1;
yuron	5:167327a82430	218	Timer back_timer2;
yuron	5:167327a82430	219	Timer back_timer3;
yuron	8:3df97287c825	220	Timer start_timer;
yuron	0:f73c1b076ae4	221
yuron	7:7f16fb8b0192	222	bool roller_flag = 0;
yuron	7:7f16fb8b0192	223	bool start_flag = 0;
yuron	11:5a0d6f69e751	224	bool trace_flag = 1;
yuron	8:3df97287c825	225
yuron	8:3df97287c825	226	//2段下段乗ったかなフラグ
yuron	8:3df97287c825	227	bool low_grade_flag = 0;
yuron	8:3df97287c825	228	//2段上段乗ったかなフラグ
yuron	8:3df97287c825	229	bool high_grade_flag = 0;
yuron	8:3df97287c825	230	//移動(低)乗ったかなフラグ
yuron	8:3df97287c825	231	bool low_table_flag = 0;
yuron	8:3df97287c825	232	//移動(中)乗ったかなフラグ
yuron	8:3df97287c825	233	bool medium_table_flag = 0;
yuron	8:3df97287c825	234	//移動(高)乗ったかなフラグ
yuron	8:3df97287c825	235	bool high_table_flag = 0;
yuron	8:3df97287c825	236
yuron	9:1359f0c813b1	237	int table_fire = 0;
yuron	9:1359f0c813b1	238	/*
yuron	9:1359f0c813b1	239	//2段下段撃ったかなフラグ
yuron	9:1359f0c813b1	240	bool low_grade_fire_flag = 0;
yuron	9:1359f0c813b1	241	//2段上段撃ったかなフラグ
yuron	9:1359f0c813b1	242	bool high_grade_fire_flag = 0;
yuron	9:1359f0c813b1	243	//移動(低)撃ったかなフラグ
yuron	9:1359f0c813b1	244	bool low_table_fire_flag = 0;
yuron	9:1359f0c813b1	245	//移動(中)撃ったかなフラグ
yuron	9:1359f0c813b1	246	bool medium_table_fire_flag = 0;
yuron	9:1359f0c813b1	247	//移動(高)撃ったかなフラグ
yuron	9:1359f0c813b1	248	bool high_table_fire_flag = 0;
yuron	9:1359f0c813b1	249	*/
yuron	8:3df97287c825	250
yuron	8:3df97287c825	251	static int mouted_bottles = 0;
yuron	7:7f16fb8b0192	252	int loading_state = 0;
yuron	7:7f16fb8b0192	253	int line_state = 0;
yuron	5:167327a82430	254	int front_line_state = 0;
yuron	5:167327a82430	255	int back_line_state = 0;
yuron	7:7f16fb8b0192	256	int line_state_pettern = 0;
yuron	5:167327a82430	257	unsigned int distance;
yuron	5:167327a82430	258	int trace_direction = 0;
yuron	5:167327a82430	259	static int i = 0;
yuron	6:5a051a378e42	260	int lateral_frag = 0;
yuron	0:f73c1b076ae4	261
yuron	9:1359f0c813b1	262	/*
yuron	9:1359f0c813b1	263	int bottles_flag[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
yuron	9:1359f0c813b1	264	0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
yuron	9:1359f0c813b1	265	*/
yuron	9:1359f0c813b1	266
yuron	10:b672aa81b226	267	int firing_minimum_distamce = 0;
yuron	10:b672aa81b226	268	int firing_maximum_distance = 0;
yuron	10:b672aa81b226	269
yuron	4:df334779a69e	270	int migimae_rpm;
yuron	4:df334779a69e	271	int migiusiro_rpm;
yuron	4:df334779a69e	272	int hidarimae_rpm;
yuron	4:df334779a69e	273	int hidariusiro_rpm;
yuron	5:167327a82430	274	//int measure_rpm;
yuron	4:df334779a69e	275	int front_roller_rpm;
yuron	4:df334779a69e	276	int back_roller_rpm;
yuron	0:f73c1b076ae4	277
yuron	5:167327a82430	278	//int migimae_pulse;
yuron	5:167327a82430	279	//int migiusiro_pulse;
yuron	5:167327a82430	280	//int hidarimae_pulse;
yuron	5:167327a82430	281	//int hidariusiro_pulse;
yuron	5:167327a82430	282	int measure_pulse;
yuron	5:167327a82430	283	//int front_roller_pulse;
yuron	5:167327a82430	284	//int back_roller_pulse;
yuron	5:167327a82430	285
yuron	5:167327a82430	286	int migimae_pulse[10];
yuron	5:167327a82430	287	int migiusiro_pulse[10];
yuron	5:167327a82430	288	int hidarimae_pulse[10];
yuron	5:167327a82430	289	int hidariusiro_pulse[10];
yuron	5:167327a82430	290	//int measure_pulse[10];
yuron	5:167327a82430	291	int front_roller_pulse[10];
yuron	5:167327a82430	292	int back_roller_pulse[10];
yuron	0:f73c1b076ae4	293
yuron	5:167327a82430	294	int sum_migimae_pulse;
yuron	5:167327a82430	295	int sum_migiusiro_pulse;
yuron	5:167327a82430	296	int sum_hidarimae_pulse;
yuron	5:167327a82430	297	int sum_hidariusiro_pulse;
yuron	5:167327a82430	298	//int sum_measure_pulse;
yuron	5:167327a82430	299	int sum_front_roller_pulse;
yuron	5:167327a82430	300	int sum_back_roller_pulse;
yuron	0:f73c1b076ae4	301
yuron	5:167327a82430	302	int ave_migimae_pulse;
yuron	5:167327a82430	303	int ave_migiusiro_pulse;
yuron	5:167327a82430	304	int ave_hidarimae_pulse;
yuron	5:167327a82430	305	int ave_hidariusiro_pulse;
yuron	5:167327a82430	306	//int ave_measure_pulse;
yuron	5:167327a82430	307	int ave_front_roller_pulse;
yuron	5:167327a82430	308	int ave_back_roller_pulse;
yuron	0:f73c1b076ae4	309
yuron	0:f73c1b076ae4	310	char send_data[1];
yuron	0:f73c1b076ae4	311	char true_send_data[1];
yuron	0:f73c1b076ae4	312
yuron	4:df334779a69e	313	char migimae_data[1];
yuron	4:df334779a69e	314	char migiusiro_data[1];
yuron	4:df334779a69e	315	char hidarimae_data[1];
yuron	4:df334779a69e	316	char hidariusiro_data[1];
yuron	4:df334779a69e	317	char front_roller_data[1];
yuron	4:df334779a69e	318	char back_roller_data[1];
yuron	4:df334779a69e	319	char loading_data[1];
yuron	10:b672aa81b226	320	char cylinder_data[1] = {0x80};
yuron	4:df334779a69e	321
yuron	4:df334779a69e	322	char true_migimae_data[1];
yuron	4:df334779a69e	323	char true_migiusiro_data[1];
yuron	4:df334779a69e	324	char true_hidarimae_data[1];
yuron	4:df334779a69e	325	char true_hidariusiro_data[1];
yuron	4:df334779a69e	326
yuron	5:167327a82430	327	//各関数のプロトタイプ宣言
yuron	5:167327a82430	328	//回転数取得関数
yuron	0:f73c1b076ae4	329	void flip();
yuron	5:167327a82430	330	//前進PID(fast_mode)
yuron	4:df334779a69e	331	int front_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	332	//前進PID(slow_mode)
yuron	5:167327a82430	333	int front_PID_slow(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	334	//前進PID(fast_mode)
yuron	4:df334779a69e	335	int back_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	336	//後進PID(slow_mode)
yuron	5:167327a82430	337	int back_PID_slow(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	338	//右進PID(fast_mode)
yuron	4:df334779a69e	339	int rihgt_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	340	//左進PID(fast_mode)
yuron	4:df334779a69e	341	int left_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	342	//右前PID(slow_mode)
yuron	4:df334779a69e	343	int right_front_PID(int RB, int LF);
yuron	5:167327a82430	344	//右後PID(slow_mode)
yuron	4:df334779a69e	345	int right_back_PID(int RF, int LB);
yuron	5:167327a82430	346	//左前PID(slow_mode)
yuron	4:df334779a69e	347	int left_front_PID(int RF, int LB);
yuron	5:167327a82430	348	//左後PID(slow_mode)
yuron	4:df334779a69e	349	int left_back_PID(int RB, int RF);
yuron	5:167327a82430	350	//右旋回PID(fast mode)
yuron	4:df334779a69e	351	int turn_right_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	352	//右旋回PID(slow mode)
yuron	5:167327a82430	353	int turn_right_PID_slow(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	354	//左旋回PID(fast mode)
yuron	4:df334779a69e	355	int turn_left_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	356	//左旋回PID(sow mode)
yuron	5:167327a82430	357	int turn_left_PID_slow(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	358	//前前進後右旋回(slow_mode)
yuron	5:167327a82430	359	int front_front_back_right_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	360	//前前進後左旋回(slow_mode)
yuron	5:167327a82430	361	int front_front_back_left_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	362	//前右旋回後前進(slow_mode)
yuron	5:167327a82430	363	int front_right_back_front_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	364	//前左旋回後前進(slow_mode)
yuron	5:167327a82430	365	int front_left_back_front_PID(int RF, int RB, int LF, int LB);
yuron	5:167327a82430	366	//ローラーPID
yuron	4:df334779a69e	367	int roller_PID(int front, int back);
yuron	5:167327a82430	368
yuron	5:167327a82430	369	//ラインセンサ制御基板との通信用関数
yuron	4:df334779a69e	370	void linetrace();
yuron	5:167327a82430	371	//超音波センサ用関数
yuron	4:df334779a69e	372	void ultrasonic();
yuron	0:f73c1b076ae4	373
yuron	5:167327a82430	374	//移動平均をPID
yuron	4:df334779a69e	375	void flip() {
yuron	5:167327a82430	376
yuron	5:167327a82430	377	//前回のi番目のデータを消して新たにそこにデータを書き込む(キューのような考え?)
yuron	5:167327a82430	378	sum_migimae_pulse -= migimae_pulse[i];
yuron	5:167327a82430	379	sum_migiusiro_pulse -= migiusiro_pulse[i];
yuron	5:167327a82430	380	sum_hidarimae_pulse -= hidarimae_pulse[i];
yuron	5:167327a82430	381	sum_hidariusiro_pulse -= hidariusiro_pulse[i];
yuron	5:167327a82430	382	sum_front_roller_pulse -= front_roller_pulse[i];
yuron	5:167327a82430	383	sum_back_roller_pulse -= back_roller_pulse[i];
yuron	4:df334779a69e	384
yuron	5:167327a82430	385	//配列のi番目の箱に取得パルスを代入
yuron	5:167327a82430	386	migimae_pulse[i] = migimae_wheel.getPulses();
yuron	5:167327a82430	387	migiusiro_pulse[i] = migiusiro_wheel.getPulses();
yuron	5:167327a82430	388	hidarimae_pulse[i] = hidarimae_wheel.getPulses();
yuron	5:167327a82430	389	hidariusiro_pulse[i] = hidariusiro_wheel.getPulses();
yuron	5:167327a82430	390	measure_pulse = measure_wheel.getPulses();
yuron	5:167327a82430	391	front_roller_pulse[i] = front_roller_wheel.getPulses();
yuron	5:167327a82430	392	back_roller_pulse[i] = back_roller_wheel.getPulses();
yuron	5:167327a82430	393	//migimae_pulse = migimae_wheel.getPulses();
yuron	5:167327a82430	394	//migiusiro_pulse = migiusiro_wheel.getPulses();
yuron	5:167327a82430	395	//hidarimae_pulse = hidarimae_wheel.getPulses();
yuron	5:167327a82430	396	//hidariusiro_pulse = hidariusiro_wheel.getPulses();
yuron	4:df334779a69e	397
yuron	4:df334779a69e	398	migimae_wheel.reset();
yuron	4:df334779a69e	399	migiusiro_wheel.reset();
yuron	4:df334779a69e	400	hidarimae_wheel.reset();
yuron	4:df334779a69e	401	hidariusiro_wheel.reset();
yuron	4:df334779a69e	402	front_roller_wheel.reset();
yuron	4:df334779a69e	403	back_roller_wheel.reset();
yuron	5:167327a82430	404
yuron	5:167327a82430	405	//i[0]~i[9]までの合計値を代入
yuron	5:167327a82430	406	sum_migimae_pulse += migimae_pulse[i];
yuron	5:167327a82430	407	sum_migiusiro_pulse += migiusiro_pulse[i];
yuron	5:167327a82430	408	sum_hidarimae_pulse += hidarimae_pulse[i];
yuron	5:167327a82430	409	sum_hidariusiro_pulse += hidariusiro_pulse[i];
yuron	5:167327a82430	410	sum_front_roller_pulse += front_roller_pulse[i];
yuron	5:167327a82430	411	sum_back_roller_pulse += back_roller_pulse[i];
yuron	5:167327a82430	412
yuron	5:167327a82430	413	//インクリメント
yuron	5:167327a82430	414	i++;
yuron	5:167327a82430	415
yuron	5:167327a82430	416	//iが最大値(9)になったらリセット
yuron	5:167327a82430	417	if(i > 9) {
yuron	5:167327a82430	418	i = 0;
yuron	5:167327a82430	419	}
yuron	5:167327a82430	420
yuron	5:167327a82430	421	//10回分の合計値を10で割り、取得パルスの平均を出す
yuron	5:167327a82430	422	ave_migimae_pulse = sum_migimae_pulse / 10;
yuron	5:167327a82430	423	ave_migiusiro_pulse = sum_migiusiro_pulse / 10;
yuron	5:167327a82430	424	ave_hidarimae_pulse = sum_hidarimae_pulse / 10;
yuron	5:167327a82430	425	ave_hidariusiro_pulse = sum_hidariusiro_pulse / 10;
yuron	5:167327a82430	426	ave_front_roller_pulse = sum_front_roller_pulse / 10;
yuron	5:167327a82430	427	ave_back_roller_pulse = sum_back_roller_pulse / 10;
yuron	5:167327a82430	428
yuron	5:167327a82430	429	//平均値をRPMへ変換
yuron	5:167327a82430	430	migimae_rpm = ave_migimae_pulse * 25 * 60 / 1200;
yuron	5:167327a82430	431	migiusiro_rpm = ave_migiusiro_pulse * 25 * 60 / 1200;
yuron	5:167327a82430	432	hidarimae_rpm = ave_hidarimae_pulse * 25 * 60 / 1200;
yuron	5:167327a82430	433	hidariusiro_rpm = ave_hidariusiro_pulse * 25 * 60 / 1200;
yuron	5:167327a82430	434	front_roller_rpm = ave_front_roller_pulse * 25 * 60 / 1200;
yuron	5:167327a82430	435	back_roller_rpm = ave_back_roller_pulse * 25 * 60 / 1200;
yuron	5:167327a82430	436
yuron	5:167327a82430	437	//pc.printf("左後 %d\n", abs(hidariusiro_rpm));
yuron	5:167327a82430	438	//pc.printf("%d\n\r", measure_pulse);
yuron	5:167327a82430	439	//pc.printf("%d %d\n", abs(front_roller_rpm), abs(back_roller_rpm));
yuron	5:167327a82430	440	//pc.printf("前 %d 後 %d %d %d\n", abs(front_roller_rpm), abs(back_roller_rpm), distance, line_state);
yuron	4:df334779a69e	441	}
yuron	5:167327a82430	442	//前進(fast mode)
yuron	4:df334779a69e	443	int front_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	444	// センサ出力値の最小、最大
yuron	4:df334779a69e	445	migimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	446	migiusiro.setInputLimits(-400, 400);
yuron	4:df334779a69e	447	hidarimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	448	hidariusiro.setInputLimits(-400, 400);
yuron	5:167327a82430	449
yuron	5:167327a82430	450	// 制御量の最小、最大
yuron	4:df334779a69e	451	migimae.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	452	migiusiro.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	453	hidarimae.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	454	hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	455
yuron	5:167327a82430	456	// よくわからんやつ
yuron	4:df334779a69e	457	migimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	458	migiusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	459	hidarimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	460	hidariusiro.setMode(AUTO_MODE);
yuron	0:f73c1b076ae4	461
yuron	5:167327a82430	462	// 目標値
yuron	4:df334779a69e	463	migimae.setSetPoint(RF);
yuron	4:df334779a69e	464	migiusiro.setSetPoint(RB);
yuron	4:df334779a69e	465	hidarimae.setSetPoint(LF);
yuron	4:df334779a69e	466	hidariusiro.setSetPoint(LB);
yuron	5:167327a82430	467
yuron	5:167327a82430	468	// センサ出力
yuron	4:df334779a69e	469	migimae.setProcessValue(abs(migimae_rpm));
yuron	4:df334779a69e	470	migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron	4:df334779a69e	471	hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron	4:df334779a69e	472	hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	473
yuron	5:167327a82430	474	// 制御量(計算結果)
yuron	4:df334779a69e	475	migimae_data[0] = migimae.compute();
yuron	4:df334779a69e	476	migiusiro_data[0] = migiusiro.compute();
yuron	4:df334779a69e	477	hidarimae_data[0] = hidarimae.compute();
yuron	4:df334779a69e	478	hidariusiro_data[0] = hidariusiro.compute();
yuron	0:f73c1b076ae4	479
yuron	5:167327a82430	480	// 制御量をPWM値に変換
yuron	4:df334779a69e	481	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	4:df334779a69e	482	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	4:df334779a69e	483	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	4:df334779a69e	484	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	485
yuron	4:df334779a69e	486	return 0;
yuron	0:f73c1b076ae4	487	}
yuron	5:167327a82430	488	//前進(slow mode)
yuron	5:167327a82430	489	int front_PID_slow(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	490	// センサ出力値の最小、最大
yuron	5:167327a82430	491	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	492	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	493	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	494	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	495
yuron	5:167327a82430	496	// 制御量の最小、最大
yuron	5:167327a82430	497	migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	498	migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	499	hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	500	hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	501
yuron	5:167327a82430	502	// よくわからんやつ
yuron	5:167327a82430	503	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	504	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	505	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	506	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	507
yuron	5:167327a82430	508	// 目標値
yuron	5:167327a82430	509	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	510	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	511	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	512	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	513
yuron	5:167327a82430	514	// センサ出力
yuron	5:167327a82430	515	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	516	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	517	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	518	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	519
yuron	5:167327a82430	520	// 制御量(計算結果)
yuron	5:167327a82430	521	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	522	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	523	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	524	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	525
yuron	5:167327a82430	526	// 制御量をPWM値に変換
yuron	5:167327a82430	527	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	5:167327a82430	528	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	5:167327a82430	529	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	5:167327a82430	530	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	531
yuron	5:167327a82430	532	return 0;
yuron	5:167327a82430	533	}
yuron	5:167327a82430	534	//後進(fast mode)
yuron	4:df334779a69e	535	int back_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	536	// センサ出力値の最小、最大
yuron	4:df334779a69e	537	migimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	538	migiusiro.setInputLimits(-400, 400);
yuron	4:df334779a69e	539	hidarimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	540	hidariusiro.setInputLimits(-400, 400);
yuron	5:167327a82430	541
yuron	5:167327a82430	542	// 制御量の最小、最大
yuron	4:df334779a69e	543	migimae.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	544	migiusiro.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	545	hidarimae.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	546	hidariusiro.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	547
yuron	5:167327a82430	548	// よくわからんやつ
yuron	4:df334779a69e	549	migimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	550	migiusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	551	hidarimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	552	hidariusiro.setMode(AUTO_MODE);
yuron	5:167327a82430	553
yuron	5:167327a82430	554	// 目標値
yuron	4:df334779a69e	555	migimae.setSetPoint(RF);
yuron	4:df334779a69e	556	migiusiro.setSetPoint(RB);
yuron	4:df334779a69e	557	hidarimae.setSetPoint(LF);
yuron	4:df334779a69e	558	hidariusiro.setSetPoint(LB);
yuron	5:167327a82430	559
yuron	5:167327a82430	560	// センサ出力
yuron	4:df334779a69e	561	//目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron	4:df334779a69e	562	migimae.setProcessValue(abs(migimae_rpm));
yuron	4:df334779a69e	563	migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron	4:df334779a69e	564	hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron	4:df334779a69e	565	hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	566
yuron	5:167327a82430	567	// 制御量(計算結果)
yuron	4:df334779a69e	568	migimae_data[0] = migimae.compute();
yuron	4:df334779a69e	569	migiusiro_data[0] = migiusiro.compute();
yuron	4:df334779a69e	570	hidarimae_data[0] = hidarimae.compute();
yuron	4:df334779a69e	571	hidariusiro_data[0] = hidariusiro.compute();
yuron	5:167327a82430	572
yuron	4:df334779a69e	573	true_migimae_data[0] = migimae_data[0];
yuron	4:df334779a69e	574	true_migiusiro_data[0] = migiusiro_data[0];
yuron	4:df334779a69e	575	true_hidarimae_data[0] = hidarimae_data[0];
yuron	4:df334779a69e	576	true_hidariusiro_data[0] = hidariusiro_data[0];
yuron	5:167327a82430	577
yuron	4:df334779a69e	578	return 0;
yuron	4:df334779a69e	579	}
yuron	5:167327a82430	580	//後進(slow mode)
yuron	5:167327a82430	581	int back_PID_slow(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	582	// センサ出力値の最小、最大
yuron	5:167327a82430	583	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	584	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	585	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	586	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	587
yuron	5:167327a82430	588	// 制御量の最小、最大
yuron	5:167327a82430	589	migimae_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	590	migiusiro_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	591	hidarimae_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	592	hidariusiro_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	593
yuron	5:167327a82430	594	// よくわからんやつ
yuron	5:167327a82430	595	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	596	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	597	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	598	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	599
yuron	5:167327a82430	600	// 目標値
yuron	5:167327a82430	601	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	602	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	603	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	604	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	605
yuron	5:167327a82430	606	// センサ出力
yuron	5:167327a82430	607	//目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron	5:167327a82430	608	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	609	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	610	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	611	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	612
yuron	5:167327a82430	613	// 制御量(計算結果)
yuron	5:167327a82430	614	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	615	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	616	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	617	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	618
yuron	5:167327a82430	619	true_migimae_data[0] = migimae_data[0];
yuron	5:167327a82430	620	true_migiusiro_data[0] = migiusiro_data[0];
yuron	5:167327a82430	621	true_hidarimae_data[0] = hidarimae_data[0];
yuron	5:167327a82430	622	true_hidariusiro_data[0] = hidariusiro_data[0];
yuron	5:167327a82430	623
yuron	5:167327a82430	624	return 0;
yuron	5:167327a82430	625	}
yuron	5:167327a82430	626	//右進(fast mode)
yuron	4:df334779a69e	627	int right_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	628	// センサ出力値の最小、最大
yuron	4:df334779a69e	629	migimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	630	migiusiro.setInputLimits(-400, 400);
yuron	4:df334779a69e	631	hidarimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	632	hidariusiro.setInputLimits(-400, 400);
yuron	5:167327a82430	633
yuron	5:167327a82430	634	// 制御量の最小、最大
yuron	4:df334779a69e	635	migimae.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	636	migiusiro.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	637	hidarimae.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	638	hidariusiro.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	639
yuron	5:167327a82430	640	// よくわからんやつ
yuron	4:df334779a69e	641	migimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	642	migiusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	643	hidarimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	644	hidariusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	645
yuron	5:167327a82430	646	// 目標値
yuron	4:df334779a69e	647	migimae.setSetPoint(RF);
yuron	4:df334779a69e	648	migiusiro.setSetPoint(RB);
yuron	4:df334779a69e	649	hidarimae.setSetPoint(LF);
yuron	4:df334779a69e	650	hidariusiro.setSetPoint(LB);
yuron	5:167327a82430	651
yuron	5:167327a82430	652	// センサ出力
yuron	4:df334779a69e	653	migimae.setProcessValue(abs(migimae_rpm));
yuron	4:df334779a69e	654	migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron	4:df334779a69e	655	hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron	4:df334779a69e	656	hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	657
yuron	5:167327a82430	658	// 制御量(計算結果)
yuron	4:df334779a69e	659	migimae_data[0] = migimae.compute();
yuron	4:df334779a69e	660	migiusiro_data[0] = migiusiro.compute();
yuron	4:df334779a69e	661	hidarimae_data[0] = hidarimae.compute();
yuron	4:df334779a69e	662	hidariusiro_data[0] = hidariusiro.compute();
yuron	4:df334779a69e	663
yuron	5:167327a82430	664	// 制御量をPWM値に変換
yuron	4:df334779a69e	665	true_migimae_data[0] = migimae_data[0];
yuron	4:df334779a69e	666	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	4:df334779a69e	667	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	4:df334779a69e	668	true_hidariusiro_data[0] = hidariusiro_data[0];
yuron	5:167327a82430	669
yuron	4:df334779a69e	670	return 0;
yuron	4:df334779a69e	671	}
yuron	5:167327a82430	672	//右進(slow mode)
yuron	5:167327a82430	673	int right_PID_slow(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	674	// センサ出力値の最小、最大
yuron	5:167327a82430	675	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	676	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	677	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	678	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	679
yuron	5:167327a82430	680	// 制御量の最小、最大
yuron	5:167327a82430	681	migimae_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	682	migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	683	hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	684	hidariusiro_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	685
yuron	5:167327a82430	686	// よくわからんやつ
yuron	5:167327a82430	687	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	688	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	689	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	690	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	691
yuron	5:167327a82430	692	// 目標値
yuron	5:167327a82430	693	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	694	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	695	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	696	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	697
yuron	5:167327a82430	698	// センサ出力
yuron	5:167327a82430	699	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	700	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	701	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	702	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	703
yuron	5:167327a82430	704	// 制御量(計算結果)
yuron	5:167327a82430	705	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	706	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	707	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	708	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	709
yuron	5:167327a82430	710	// 制御量をPWM値に変換
yuron	5:167327a82430	711	true_migimae_data[0] = migimae_data[0];
yuron	5:167327a82430	712	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	5:167327a82430	713	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	5:167327a82430	714	true_hidariusiro_data[0] = hidariusiro_data[0];
yuron	5:167327a82430	715
yuron	5:167327a82430	716	return 0;
yuron	5:167327a82430	717	}
yuron	5:167327a82430	718	//左進(fast mode)
yuron	4:df334779a69e	719	int left_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	720	// センサ出力値の最小、最大
yuron	4:df334779a69e	721	migimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	722	migiusiro.setInputLimits(-400, 400);
yuron	4:df334779a69e	723	hidarimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	724	hidariusiro.setInputLimits(-400, 400);
yuron	5:167327a82430	725
yuron	5:167327a82430	726	// 制御量の最小、最大
yuron	4:df334779a69e	727	migimae.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	728	migiusiro.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	729	hidarimae.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	730	hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron	0:f73c1b076ae4	731
yuron	5:167327a82430	732	// よくわからんやつ
yuron	4:df334779a69e	733	migimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	734	migiusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	735	hidarimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	736	hidariusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	737
yuron	5:167327a82430	738	// 目標値
yuron	4:df334779a69e	739	migimae.setSetPoint(RF);
yuron	4:df334779a69e	740	migiusiro.setSetPoint(RB);
yuron	4:df334779a69e	741	hidarimae.setSetPoint(LF);
yuron	4:df334779a69e	742	hidariusiro.setSetPoint(LB);
yuron	5:167327a82430	743
yuron	5:167327a82430	744	// センサ出力
yuron	4:df334779a69e	745	migimae.setProcessValue(abs(migimae_rpm));
yuron	4:df334779a69e	746	migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron	4:df334779a69e	747	hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron	4:df334779a69e	748	hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	749
yuron	5:167327a82430	750	// 制御量(計算結果)
yuron	4:df334779a69e	751	migimae_data[0] = migimae.compute();
yuron	4:df334779a69e	752	migiusiro_data[0] = migiusiro.compute();
yuron	4:df334779a69e	753	hidarimae_data[0] = hidarimae.compute();
yuron	4:df334779a69e	754	hidariusiro_data[0] = hidariusiro.compute();
yuron	4:df334779a69e	755
yuron	5:167327a82430	756	// 制御量をPWM値に変換
yuron	5:167327a82430	757	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	5:167327a82430	758	true_migiusiro_data[0] = migiusiro_data[0];
yuron	5:167327a82430	759	true_hidarimae_data[0] = hidarimae_data[0];
yuron	5:167327a82430	760	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	761
yuron	5:167327a82430	762	return 0;
yuron	5:167327a82430	763	}
yuron	5:167327a82430	764	//左進(slow mode)
yuron	5:167327a82430	765	int left_PID_slow(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	766	// センサ出力値の最小、最大
yuron	5:167327a82430	767	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	768	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	769	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	770	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	771
yuron	5:167327a82430	772	// 制御量の最小、最大
yuron	5:167327a82430	773	migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	774	migiusiro_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	775	hidarimae_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	776	hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	777
yuron	5:167327a82430	778	// よくわからんやつ
yuron	5:167327a82430	779	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	780	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	781	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	782	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	783
yuron	5:167327a82430	784	// 目標値
yuron	5:167327a82430	785	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	786	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	787	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	788	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	789
yuron	5:167327a82430	790	// センサ出力
yuron	5:167327a82430	791	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	792	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	793	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	794	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	795
yuron	5:167327a82430	796	// 制御量(計算結果)
yuron	5:167327a82430	797	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	798	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	799	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	800	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	801
yuron	5:167327a82430	802	// 制御量をPWM値に変換
yuron	4:df334779a69e	803	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	4:df334779a69e	804	true_migiusiro_data[0] = migiusiro_data[0];
yuron	4:df334779a69e	805	true_hidarimae_data[0] = hidarimae_data[0];
yuron	4:df334779a69e	806	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	807
yuron	4:df334779a69e	808	return 0;
yuron	4:df334779a69e	809	}
yuron	5:167327a82430	810	//斜め右前(fast mode)
yuron	4:df334779a69e	811	int right_front_PID(int RB, int LF) {
yuron	5:167327a82430	812	// センサ出力値の最小、最大
yuron	5:167327a82430	813	migiusiro.setInputLimits(0, 400);
yuron	5:167327a82430	814	hidarimae.setInputLimits(0, 400);
yuron	5:167327a82430	815
yuron	5:167327a82430	816	// 制御量の最小、最大
yuron	4:df334779a69e	817	migiusiro.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	818	hidarimae.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	819
yuron	5:167327a82430	820	// よくわからんやつ
yuron	4:df334779a69e	821	migiusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	822	hidarimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	823
yuron	5:167327a82430	824	// 目標値
yuron	4:df334779a69e	825	migiusiro.setSetPoint(RB);
yuron	4:df334779a69e	826	hidarimae.setSetPoint(LF);
yuron	5:167327a82430	827
yuron	5:167327a82430	828	// センサ出力
yuron	4:df334779a69e	829	migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron	4:df334779a69e	830	hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	831
yuron	5:167327a82430	832	// 制御量(計算結果)
yuron	4:df334779a69e	833	migiusiro_data[0] = migiusiro.compute();
yuron	4:df334779a69e	834	hidarimae_data[0] = hidarimae.compute();
yuron	4:df334779a69e	835
yuron	5:167327a82430	836	// 制御量をPWM値に変換
yuron	5:167327a82430	837	true_migimae_data[0] = 0x80;
yuron	4:df334779a69e	838	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	4:df334779a69e	839	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	5:167327a82430	840	true_hidariusiro_data[0] = 0x80;
yuron	5:167327a82430	841
yuron	4:df334779a69e	842	return 0;
yuron	4:df334779a69e	843	}
yuron	5:167327a82430	844	//斜め右後(fast mode)
yuron	4:df334779a69e	845	int right_back_PID(int RF, int LB) {
yuron	5:167327a82430	846	// センサ出力値の最小、最大
yuron	4:df334779a69e	847	migimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	848	hidariusiro.setInputLimits(-400, 400);
yuron	5:167327a82430	849
yuron	5:167327a82430	850	// 制御量の最小、最大
yuron	4:df334779a69e	851	migimae.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	852	hidariusiro.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	853
yuron	5:167327a82430	854	// よくわからんやつ
yuron	4:df334779a69e	855	migimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	856	hidariusiro.setMode(AUTO_MODE);
yuron	5:167327a82430	857
yuron	5:167327a82430	858	// 目標値
yuron	4:df334779a69e	859	migimae.setSetPoint(RF);
yuron	4:df334779a69e	860	hidariusiro.setSetPoint(LB);
yuron	5:167327a82430	861
yuron	5:167327a82430	862	// センサ出力
yuron	4:df334779a69e	863	//目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron	4:df334779a69e	864	migimae.setProcessValue(abs(migimae_rpm));
yuron	4:df334779a69e	865	hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	866
yuron	5:167327a82430	867	// 制御量(計算結果)
yuron	4:df334779a69e	868	migimae_data[0] = migimae.compute();
yuron	4:df334779a69e	869	hidariusiro_data[0] = hidariusiro.compute();
yuron	5:167327a82430	870
yuron	4:df334779a69e	871	true_migimae_data[0] = migimae_data[0];
yuron	5:167327a82430	872	true_migiusiro_data[0] = 0x80;
yuron	5:167327a82430	873	true_hidarimae_data[0] = 0x80;
yuron	4:df334779a69e	874	true_hidariusiro_data[0] = hidariusiro_data[0];
yuron	5:167327a82430	875
yuron	4:df334779a69e	876	return 0;
yuron	4:df334779a69e	877	}
yuron	5:167327a82430	878	//斜め左前(fast mode)
yuron	4:df334779a69e	879	int left_front_PID(int RF, int LB) {
yuron	5:167327a82430	880	// センサ出力値の最小、最大
yuron	4:df334779a69e	881	migimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	882	hidariusiro.setInputLimits(-400, 400);
yuron	5:167327a82430	883
yuron	5:167327a82430	884	// 制御量の最小、最大
yuron	4:df334779a69e	885	migimae.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	886	hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	887
yuron	5:167327a82430	888	// よくわからんやつ
yuron	4:df334779a69e	889	migimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	890	hidariusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	891
yuron	5:167327a82430	892	// 目標値
yuron	4:df334779a69e	893	migimae.setSetPoint(RF);
yuron	4:df334779a69e	894	hidariusiro.setSetPoint(LB);
yuron	5:167327a82430	895
yuron	5:167327a82430	896	// センサ出力
yuron	4:df334779a69e	897	migimae.setProcessValue(abs(migimae_rpm));
yuron	4:df334779a69e	898	hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	899
yuron	5:167327a82430	900	// 制御量(計算結果)
yuron	4:df334779a69e	901	migimae_data[0] = migimae.compute();
yuron	4:df334779a69e	902	hidariusiro_data[0] = hidariusiro.compute();
yuron	4:df334779a69e	903
yuron	5:167327a82430	904	// 制御量をPWM値に変換
yuron	4:df334779a69e	905	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	5:167327a82430	906	true_migiusiro_data[0] = 0x80;
yuron	5:167327a82430	907	true_hidarimae_data[0] = 0x80;
yuron	4:df334779a69e	908	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	909
yuron	2:c32991ba628f	910	return 0;
yuron	0:f73c1b076ae4	911	}
yuron	5:167327a82430	912	//斜め左後(fast mode)
yuron	4:df334779a69e	913	int left_back_PID(int RB, int LF) {
yuron	5:167327a82430	914	// センサ出力値の最小、最大
yuron	4:df334779a69e	915	migiusiro.setInputLimits(-400, 400);
yuron	4:df334779a69e	916	hidarimae.setInputLimits(-400, 400);
yuron	5:167327a82430	917
yuron	5:167327a82430	918	// 制御量の最小、最大
yuron	4:df334779a69e	919	migiusiro.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	920	hidarimae.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	921
yuron	5:167327a82430	922	// よくわからんやつ
yuron	4:df334779a69e	923	migiusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	924	hidarimae.setMode(AUTO_MODE);
yuron	5:167327a82430	925
yuron	5:167327a82430	926	// 目標値
yuron	4:df334779a69e	927	migiusiro.setSetPoint(RB);
yuron	4:df334779a69e	928	hidarimae.setSetPoint(LF);
yuron	5:167327a82430	929
yuron	5:167327a82430	930	// センサ出力
yuron	2:c32991ba628f	931	//目標値が負の数になると出力がバグるため目標値は絶対値で指定して出力は逆回転になるようにしてます。
yuron	4:df334779a69e	932	migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron	4:df334779a69e	933	hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	934
yuron	5:167327a82430	935	// 制御量(計算結果)
yuron	4:df334779a69e	936	migiusiro_data[0] = migiusiro.compute();
yuron	4:df334779a69e	937	hidarimae_data[0] = hidarimae.compute();
yuron	5:167327a82430	938
yuron	5:167327a82430	939	true_migimae_data[0] = 0x80;
yuron	4:df334779a69e	940	true_migiusiro_data[0] = migiusiro_data[0];
yuron	4:df334779a69e	941	true_hidarimae_data[0] = hidarimae_data[0];
yuron	5:167327a82430	942	true_hidariusiro_data[0] = 0x80;
yuron	5:167327a82430	943
yuron	2:c32991ba628f	944	return 0;
yuron	1:62b321f6c9c3	945	}
yuron	5:167327a82430	946	//右旋回(fast mode)
yuron	4:df334779a69e	947	int turn_right_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	948	// センサ出力値の最小、最大
yuron	4:df334779a69e	949	migimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	950	migiusiro.setInputLimits(-400, 400);
yuron	4:df334779a69e	951	hidarimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	952	hidariusiro.setInputLimits(-400, 400);
yuron	5:167327a82430	953
yuron	5:167327a82430	954	// 制御量の最小、最大
yuron	4:df334779a69e	955	migimae.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	956	migiusiro.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	957	hidarimae.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	958	hidariusiro.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	959
yuron	5:167327a82430	960	// よくわからんやつ
yuron	4:df334779a69e	961	migimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	962	migiusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	963	hidarimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	964	hidariusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	965
yuron	5:167327a82430	966	// 目標値
yuron	4:df334779a69e	967	migimae.setSetPoint(RF);
yuron	4:df334779a69e	968	migiusiro.setSetPoint(RB);
yuron	4:df334779a69e	969	hidarimae.setSetPoint(LF);
yuron	4:df334779a69e	970	hidariusiro.setSetPoint(LB);
yuron	5:167327a82430	971
yuron	5:167327a82430	972	// センサ出力
yuron	4:df334779a69e	973	migimae.setProcessValue(abs(migimae_rpm));
yuron	4:df334779a69e	974	migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron	4:df334779a69e	975	hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron	4:df334779a69e	976	hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	977
yuron	5:167327a82430	978	// 制御量(計算結果)
yuron	4:df334779a69e	979	migimae_data[0] = migimae.compute();
yuron	4:df334779a69e	980	migiusiro_data[0] = migiusiro.compute();
yuron	4:df334779a69e	981	hidarimae_data[0] = hidarimae.compute();
yuron	4:df334779a69e	982	hidariusiro_data[0] = hidariusiro.compute();
yuron	4:df334779a69e	983
yuron	5:167327a82430	984	// 制御量をPWM値に変換
yuron	4:df334779a69e	985	true_migimae_data[0] = migimae_data[0];
yuron	4:df334779a69e	986	true_migiusiro_data[0] = migiusiro_data[0];
yuron	4:df334779a69e	987	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	4:df334779a69e	988	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	989
yuron	4:df334779a69e	990	return 0;
yuron	4:df334779a69e	991	}
yuron	5:167327a82430	992	//右旋回(slow mode)
yuron	5:167327a82430	993	int turn_right_PID_slow(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	994	// センサ出力値の最小、最大
yuron	5:167327a82430	995	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	996	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	997	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	998	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	999
yuron	5:167327a82430	1000	// 制御量の最小、最大
yuron	5:167327a82430	1001	migimae_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	1002	migiusiro_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	1003	hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1004	hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1005
yuron	5:167327a82430	1006	// よくわからんやつ
yuron	5:167327a82430	1007	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1008	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1009	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1010	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1011
yuron	5:167327a82430	1012	// 目標値
yuron	5:167327a82430	1013	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	1014	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	1015	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	1016	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	1017
yuron	5:167327a82430	1018	// センサ出力
yuron	5:167327a82430	1019	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	1020	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	1021	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	1022	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	1023
yuron	5:167327a82430	1024	// 制御量(計算結果)
yuron	5:167327a82430	1025	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	1026	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	1027	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	1028	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	1029
yuron	5:167327a82430	1030	// 制御量をPWM値に変換
yuron	5:167327a82430	1031	true_migimae_data[0] = migimae_data[0];
yuron	5:167327a82430	1032	true_migiusiro_data[0] = migiusiro_data[0];
yuron	5:167327a82430	1033	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	5:167327a82430	1034	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	1035
yuron	5:167327a82430	1036	return 0;
yuron	5:167327a82430	1037	}
yuron	5:167327a82430	1038	//左旋回(fast mode)
yuron	4:df334779a69e	1039	int turn_left_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	1040	// センサ出力値の最小、最大
yuron	4:df334779a69e	1041	migimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	1042	migiusiro.setInputLimits(-400, 400);
yuron	4:df334779a69e	1043	hidarimae.setInputLimits(-400, 400);
yuron	4:df334779a69e	1044	hidariusiro.setInputLimits(-400, 400);
yuron	5:167327a82430	1045
yuron	5:167327a82430	1046	// 制御量の最小、最大
yuron	4:df334779a69e	1047	migimae.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	1048	migiusiro.setOutputLimits(0x0C, 0x7C);
yuron	4:df334779a69e	1049	hidarimae.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	1050	hidariusiro.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	1051
yuron	5:167327a82430	1052	// よくわからんやつ
yuron	4:df334779a69e	1053	migimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	1054	migiusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	1055	hidarimae.setMode(AUTO_MODE);
yuron	4:df334779a69e	1056	hidariusiro.setMode(AUTO_MODE);
yuron	4:df334779a69e	1057
yuron	5:167327a82430	1058	// 目標値
yuron	4:df334779a69e	1059	migimae.setSetPoint(RF);
yuron	4:df334779a69e	1060	migiusiro.setSetPoint(RB);
yuron	4:df334779a69e	1061	hidarimae.setSetPoint(LF);
yuron	4:df334779a69e	1062	hidariusiro.setSetPoint(LB);
yuron	5:167327a82430	1063
yuron	5:167327a82430	1064	// センサ出力
yuron	4:df334779a69e	1065	migimae.setProcessValue(abs(migimae_rpm));
yuron	4:df334779a69e	1066	migiusiro.setProcessValue(abs(migiusiro_rpm));
yuron	4:df334779a69e	1067	hidarimae.setProcessValue(abs(hidarimae_rpm));
yuron	4:df334779a69e	1068	hidariusiro.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	1069
yuron	5:167327a82430	1070	// 制御量(計算結果)
yuron	4:df334779a69e	1071	migimae_data[0] = migimae.compute();
yuron	4:df334779a69e	1072	migiusiro_data[0] = migiusiro.compute();
yuron	4:df334779a69e	1073	hidarimae_data[0] = hidarimae.compute();
yuron	4:df334779a69e	1074	hidariusiro_data[0] = hidariusiro.compute();
yuron	4:df334779a69e	1075
yuron	5:167327a82430	1076	// 制御量をPWM値に変換
yuron	5:167327a82430	1077	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	5:167327a82430	1078	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	5:167327a82430	1079	true_hidarimae_data[0] = hidarimae_data[0];
yuron	5:167327a82430	1080	true_hidariusiro_data[0] = hidariusiro_data[0];
yuron	5:167327a82430	1081
yuron	5:167327a82430	1082	return 0;
yuron	5:167327a82430	1083	}
yuron	5:167327a82430	1084	//左旋回(slow mode)
yuron	5:167327a82430	1085	int turn_left_PID_slow(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	1086	// センサ出力値の最小、最大
yuron	5:167327a82430	1087	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1088	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1089	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1090	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1091
yuron	5:167327a82430	1092	// 制御量の最小、最大
yuron	5:167327a82430	1093	migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1094	migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1095	hidarimae_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	1096	hidariusiro_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	1097
yuron	5:167327a82430	1098	// よくわからんやつ
yuron	5:167327a82430	1099	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1100	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1101	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1102	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1103
yuron	5:167327a82430	1104	// 目標値
yuron	5:167327a82430	1105	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	1106	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	1107	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	1108	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	1109
yuron	5:167327a82430	1110	// センサ出力
yuron	5:167327a82430	1111	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	1112	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	1113	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	1114	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	1115
yuron	5:167327a82430	1116	// 制御量(計算結果)
yuron	5:167327a82430	1117	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	1118	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	1119	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	1120	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	1121
yuron	5:167327a82430	1122	// 制御量をPWM値に変換
yuron	4:df334779a69e	1123	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	4:df334779a69e	1124	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	4:df334779a69e	1125	true_hidarimae_data[0] = hidarimae_data[0];
yuron	4:df334779a69e	1126	true_hidariusiro_data[0] = hidariusiro_data[0];
yuron	5:167327a82430	1127
yuron	5:167327a82430	1128	return 0;
yuron	5:167327a82430	1129	}
yuron	5:167327a82430	1130	//前前進後右旋回(slow_mode)
yuron	5:167327a82430	1131	int front_front_back_right_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	1132	// センサ出力値の最小、最大
yuron	5:167327a82430	1133	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1134	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1135	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1136	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1137
yuron	5:167327a82430	1138	// 制御量の最小、最大
yuron	5:167327a82430	1139	migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1140	migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1141	hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1142	hidariusiro_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	1143
yuron	5:167327a82430	1144	// よくわからんやつ
yuron	5:167327a82430	1145	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1146	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1147	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1148	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1149
yuron	5:167327a82430	1150	// 目標値
yuron	5:167327a82430	1151	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	1152	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	1153	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	1154	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	1155
yuron	5:167327a82430	1156	// センサ出力
yuron	5:167327a82430	1157	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	1158	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	1159	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	1160	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	1161
yuron	5:167327a82430	1162	// 制御量(計算結果)
yuron	5:167327a82430	1163	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	1164	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	1165	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	1166	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	1167
yuron	5:167327a82430	1168	// 制御量をPWM値に変換
yuron	5:167327a82430	1169	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	5:167327a82430	1170	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	5:167327a82430	1171	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	5:167327a82430	1172	true_hidariusiro_data[0] = hidariusiro_data[0];
yuron	5:167327a82430	1173
yuron	4:df334779a69e	1174	return 0;
yuron	4:df334779a69e	1175	}
yuron	5:167327a82430	1176	//前前進後左旋回(slow_mode)
yuron	5:167327a82430	1177	int front_front_back_left_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	1178	// センサ出力値の最小、最大
yuron	5:167327a82430	1179	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1180	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1181	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1182	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1183
yuron	5:167327a82430	1184	// 制御量の最小、最大
yuron	5:167327a82430	1185	migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1186	migiusiro_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	1187	hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1188	hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1189
yuron	5:167327a82430	1190	// よくわからんやつ
yuron	5:167327a82430	1191	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1192	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1193	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1194	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1195
yuron	5:167327a82430	1196	// 目標値
yuron	5:167327a82430	1197	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	1198	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	1199	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	1200	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	1201
yuron	5:167327a82430	1202	// センサ出力
yuron	5:167327a82430	1203	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	1204	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	1205	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	1206	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	1207
yuron	5:167327a82430	1208	// 制御量(計算結果)
yuron	5:167327a82430	1209	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	1210	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	1211	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	1212	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	1213
yuron	5:167327a82430	1214	// 制御量をPWM値に変換
yuron	5:167327a82430	1215	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	5:167327a82430	1216	true_migiusiro_data[0] = migiusiro_data[0];
yuron	5:167327a82430	1217	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	5:167327a82430	1218	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	1219
yuron	5:167327a82430	1220	return 0;
yuron	5:167327a82430	1221	}
yuron	5:167327a82430	1222	//前右旋回後前進(slow_mode)
yuron	5:167327a82430	1223	int front_right_back_front_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	1224	// センサ出力値の最小、最大
yuron	5:167327a82430	1225	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1226	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1227	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1228	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1229
yuron	5:167327a82430	1230	// 制御量の最小、最大
yuron	5:167327a82430	1231	migimae_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	1232	migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1233	hidarimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1234	hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1235
yuron	5:167327a82430	1236	// よくわからんやつ
yuron	5:167327a82430	1237	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1238	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1239	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1240	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1241
yuron	5:167327a82430	1242	// 目標値
yuron	5:167327a82430	1243	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	1244	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	1245	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	1246	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	1247
yuron	5:167327a82430	1248	// センサ出力
yuron	5:167327a82430	1249	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	1250	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	1251	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	1252	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	1253
yuron	5:167327a82430	1254	// 制御量(計算結果)
yuron	5:167327a82430	1255	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	1256	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	1257	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	1258	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	1259
yuron	5:167327a82430	1260	// 制御量をPWM値に変換
yuron	5:167327a82430	1261	true_migimae_data[0] = migimae_data[0];
yuron	5:167327a82430	1262	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	5:167327a82430	1263	true_hidarimae_data[0] = 0x7D - hidarimae_data[0];
yuron	5:167327a82430	1264	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	1265
yuron	5:167327a82430	1266	return 0;
yuron	5:167327a82430	1267	}
yuron	5:167327a82430	1268	//前左旋回後前進(slow_mode)
yuron	5:167327a82430	1269	int front_left_back_front_PID(int RF, int RB, int LF, int LB) {
yuron	5:167327a82430	1270	// センサ出力値の最小、最大
yuron	5:167327a82430	1271	migimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1272	migiusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1273	hidarimae_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1274	hidariusiro_slow.setInputLimits(-400, 400);
yuron	5:167327a82430	1275
yuron	5:167327a82430	1276	// 制御量の最小、最大
yuron	5:167327a82430	1277	migimae_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1278	migiusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1279	hidarimae_slow.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	1280	hidariusiro_slow.setOutputLimits(0x0C, 0x7C);
yuron	5:167327a82430	1281
yuron	5:167327a82430	1282	// よくわからんやつ
yuron	5:167327a82430	1283	migimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1284	migiusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1285	hidarimae_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1286	hidariusiro_slow.setMode(AUTO_MODE);
yuron	5:167327a82430	1287
yuron	5:167327a82430	1288	// 目標値
yuron	5:167327a82430	1289	migimae_slow.setSetPoint(RF);
yuron	5:167327a82430	1290	migiusiro_slow.setSetPoint(RB);
yuron	5:167327a82430	1291	hidarimae_slow.setSetPoint(LF);
yuron	5:167327a82430	1292	hidariusiro_slow.setSetPoint(LB);
yuron	5:167327a82430	1293
yuron	5:167327a82430	1294	// センサ出力
yuron	5:167327a82430	1295	migimae_slow.setProcessValue(abs(migimae_rpm));
yuron	5:167327a82430	1296	migiusiro_slow.setProcessValue(abs(migiusiro_rpm));
yuron	5:167327a82430	1297	hidarimae_slow.setProcessValue(abs(hidarimae_rpm));
yuron	5:167327a82430	1298	hidariusiro_slow.setProcessValue(abs(hidariusiro_rpm));
yuron	5:167327a82430	1299
yuron	5:167327a82430	1300	// 制御量(計算結果)
yuron	5:167327a82430	1301	migimae_data[0] = migimae_slow.compute();
yuron	5:167327a82430	1302	migiusiro_data[0] = migiusiro_slow.compute();
yuron	5:167327a82430	1303	hidarimae_data[0] = hidarimae_slow.compute();
yuron	5:167327a82430	1304	hidariusiro_data[0] = hidariusiro_slow.compute();
yuron	5:167327a82430	1305
yuron	5:167327a82430	1306	// 制御量をPWM値に変換
yuron	5:167327a82430	1307	true_migimae_data[0] = 0x7D - migimae_data[0];
yuron	5:167327a82430	1308	true_migiusiro_data[0] = 0x7D - migiusiro_data[0];
yuron	5:167327a82430	1309	true_hidarimae_data[0] = hidarimae_data[0];
yuron	5:167327a82430	1310	true_hidariusiro_data[0] = 0x7D - hidariusiro_data[0];
yuron	5:167327a82430	1311
yuron	5:167327a82430	1312	return 0;
yuron	5:167327a82430	1313	}
yuron	5:167327a82430	1314	//ローラー
yuron	4:df334779a69e	1315	int roller_PID(int front, int back) {
yuron	4:df334779a69e	1316	front_roller.setInputLimits(-2000, 2000);
yuron	4:df334779a69e	1317	back_roller.setInputLimits(-2000, 2000);
yuron	5:167327a82430	1318
yuron	4:df334779a69e	1319	front_roller.setOutputLimits(0x84, 0xFF);
yuron	4:df334779a69e	1320	back_roller.setOutputLimits(0x84, 0xFF);
yuron	5:167327a82430	1321
yuron	4:df334779a69e	1322	front_roller.setMode(AUTO_MODE);
yuron	4:df334779a69e	1323	back_roller.setMode(AUTO_MODE);
yuron	5:167327a82430	1324
yuron	4:df334779a69e	1325	front_roller.setSetPoint(front);
yuron	4:df334779a69e	1326	back_roller.setSetPoint(back);
yuron	5:167327a82430	1327
yuron	4:df334779a69e	1328	front_roller.setProcessValue(abs(front_roller_rpm));
yuron	4:df334779a69e	1329	back_roller.setProcessValue(abs(back_roller_rpm));
yuron	5:167327a82430	1330
yuron	4:df334779a69e	1331	front_roller_data[0] = front_roller.compute();
yuron	4:df334779a69e	1332	back_roller_data[0] = back_roller.compute();
yuron	5:167327a82430	1333
yuron	4:df334779a69e	1334	return 0;
yuron	4:df334779a69e	1335	}
yuron	5:167327a82430	1336	//ラインセンサ基板との通信用関数
yuron	4:df334779a69e	1337	void linetrace() {
yuron	4:df334779a69e	1338	line_state = photo.getc();
yuron	5:167327a82430	1339	//7bit目が1だったら7bit目を0に戻す
yuron	5:167327a82430	1340	if((0b10000000 & line_state) == 0b10000000) {
yuron	5:167327a82430	1341	back_line_state = 0b01111111 & line_state;
yuron	8:3df97287c825	1342
yuron	6:5a051a378e42	1343	} else {
yuron	5:167327a82430	1344	front_line_state = line_state;
yuron	5:167327a82430	1345	}
yuron	5:167327a82430	1346	//4byte以上は出力できないよ
yuron	5:167327a82430	1347	//pc.printf("%d\n\r", line_state);
yuron	5:167327a82430	1348	//pc.printf("NAMA %d, front: %d, back: %d\n\r", line_state, front_line_state, back_line_state);
yuron	4:df334779a69e	1349	}
yuron	5:167327a82430	1350	//超音波センサ用関数
yuron	4:df334779a69e	1351	void ultrasonic() {
yuron	5:167327a82430	1352	//超音波発射
yuron	4:df334779a69e	1353	sonic.start();
yuron	5:167327a82430	1354	//10ms待機
yuron	4:df334779a69e	1355	wait(0.01);
yuron	4:df334779a69e	1356	//get_dist_cm関数は、計測から得られた距離(cm)を返します。
yuron	4:df334779a69e	1357	distance = sonic.get_dist_cm();
yuron	5:167327a82430	1358	//pc.printf("%d[cm]\r\n", distance);
yuron	4:df334779a69e	1359	}
yuron	4:df334779a69e	1360	int main(void) {
yuron	5:167327a82430	1361	//こんにちは世界
yuron	4:df334779a69e	1362	pc.printf("HelloWorld\n");
yuron	5:167327a82430	1363	//緊急停止用信号ピンをLow
yuron	0:f73c1b076ae4	1364	emergency = 0;
yuron	10:b672aa81b226	1365	//スタートボタンのLEDをHigh
yuron	10:b672aa81b226	1366	start_LED = 1;
yuron	5:167327a82430	1367	//回転数取得関数のタイマ割り込み(40ms)
yuron	6:5a051a378e42	1368	get_rpm.attach_us(&flip, 40000);
yuron	5:167327a82430	1369
yuron	5:167327a82430	1370	//フォトセンサ制御基板との通信ボーレートの設定(115200)
yuron	4:df334779a69e	1371	photo.baud(115200);
yuron	5:167327a82430	1372	pc.baud(460800);
yuron	5:167327a82430	1373	//フォトセンサ制御基板との通信関数の受信割り込み
yuron	4:df334779a69e	1374	photo.attach(linetrace, Serial::RxIrq);
yuron	5:167327a82430	1375	//サイドチェンジボタンをPullDownに設定
yuron	4:df334779a69e	1376	side.mode(PullDown);
yuron	5:167327a82430	1377	limit.mode(PullDown);
yuron	10:b672aa81b226	1378
yuron	5:167327a82430	1379	//超音波センサの温度設定
yuron	4:df334779a69e	1380	// setTemp関数は、HCSR04のメンバ変数temperature(気温を保持する変数)を引数として受け取った値に変更します。
yuron	4:df334779a69e	1381	sonic.setTemp(25);
yuron	5:167327a82430	1382
yuron	5:167327a82430	1383	//各MD, エアシリ制御基板へ起動後0.1秒間0x80を送りつける(通信切断防止用)
yuron	2:c32991ba628f	1384	send_data[0] = 0x80;
yuron	4:df334779a69e	1385	i2c.write(0x10, send_data, 1);
yuron	4:df334779a69e	1386	i2c.write(0x12, send_data, 1);
yuron	4:df334779a69e	1387	i2c.write(0x14, send_data, 1);
yuron	4:df334779a69e	1388	i2c.write(0x16, send_data, 1);
yuron	3:1223cab367d9	1389	i2c.write(0x20, send_data, 1);
yuron	3:1223cab367d9	1390	i2c.write(0x22, send_data, 1);
yuron	4:df334779a69e	1391	i2c.write(0x30, send_data, 1);
yuron	4:df334779a69e	1392	i2c.write(0x40, send_data, 1);
yuron	0:f73c1b076ae4	1393	wait(0.1);
yuron	5:167327a82430	1394
yuron	8:3df97287c825	1395	myled1 = 0;
yuron	8:3df97287c825	1396	myled2 = 0;
yuron	8:3df97287c825	1397	myled3 = 0;
yuron	8:3df97287c825	1398	myled4 = 0;
yuron	8:3df97287c825	1399	myled5 = 0;
yuron	8:3df97287c825	1400
yuron	4:df334779a69e	1401	while(1) {
yuron	5:167327a82430	1402
yuron	5:167327a82430	1403	//超音波取得関数の呼び出し
yuron	4:df334779a69e	1404	ultrasonic();
yuron	8:3df97287c825	1405
yuron	5:167327a82430	1406	//赤ゾーン選択
yuron	5:167327a82430	1407	if(side == red){
yuron	4:df334779a69e	1408	red_side = 1;
yuron	4:df334779a69e	1409	blue_side = 0;
yuron	5:167327a82430	1410	//青ゾーン選択
yuron	4:df334779a69e	1411	} else {
yuron	4:df334779a69e	1412	red_side = 0;
yuron	4:df334779a69e	1413	blue_side = 1;
yuron	4:df334779a69e	1414	}
yuron	5:167327a82430	1415
yuron	6:5a051a378e42	1416	//スタートボタン押したらエアシリ伸びる
yuron	5:167327a82430	1417	if(start_button == 0){
yuron	8:3df97287c825	1418	start_flag = 1;
yuron	8:3df97287c825	1419	}
yuron	8:3df97287c825	1420	//2段下段スイッチでフラグ立てる
yuron	8:3df97287c825	1421	if(user_switch1 == 0) {
yuron	8:3df97287c825	1422	low_grade_flag = 1;
yuron	4:df334779a69e	1423	myled1 = 1;
yuron	5:167327a82430	1424	}
yuron	8:3df97287c825	1425	//2段上段スイッチでフラグ立てる
yuron	8:3df97287c825	1426	if(user_switch2 == 0) {
yuron	8:3df97287c825	1427	high_grade_flag = 1;
yuron	5:167327a82430	1428	myled2 = 1;
yuron	5:167327a82430	1429	}
yuron	8:3df97287c825	1430	//移動(低)スイッチでフラグ立てる
yuron	8:3df97287c825	1431	if(user_switch3 == 0) {
yuron	8:3df97287c825	1432	low_table_flag = 1;
yuron	5:167327a82430	1433	myled3 = 1;
yuron	5:167327a82430	1434	}
yuron	5:167327a82430	1435
yuron	8:3df97287c825	1436	//移動(中)スイッチでフラグ立てる
yuron	8:3df97287c825	1437	/*
yuron	8:3df97287c825	1438	if(user_switch4 == 0) {
yuron	8:3df97287c825	1439	medium_table_flag = 1;
yuron	8:3df97287c825	1440	myled4 = 1;
yuron	8:3df97287c825	1441	}
yuron	8:3df97287c825	1442	*/
yuron	8:3df97287c825	1443
yuron	8:3df97287c825	1444	//移動(高)スイッチでフラグ立てる
yuron	8:3df97287c825	1445	if(user_switch5 == 0) {
yuron	8:3df97287c825	1446	high_table_flag = 1;
yuron	8:3df97287c825	1447	myled5 = 1;
yuron	8:3df97287c825	1448	}
yuron	8:3df97287c825	1449
yuron	9:1359f0c813b1	1450	/*
yuron	9:1359f0c813b1	1451	//2段高ローラー回転
yuron	9:1359f0c813b1	1452	if(high_grade_fire_flag == 0) {
yuron	9:1359f0c813b1	1453	//ローラー回転開始
yuron	9:1359f0c813b1	1454	roller_PID(1006, 928);
yuron	9:1359f0c813b1	1455	i2c.write(0x20, front_roller_data, 1, false);
yuron	9:1359f0c813b1	1456	i2c.write(0x22, back_roller_data, 1, false);
yuron	9:1359f0c813b1	1457	wait_us(20);
yuron	9:1359f0c813b1	1458	}
yuron	9:1359f0c813b1	1459	*/
yuron	9:1359f0c813b1	1460
yuron	8:3df97287c825	1461	//フォトインタラプタ検知でLチカ
yuron	8:3df97287c825	1462	if(photo_interrupter == 1) {
yuron	8:3df97287c825	1463	myled4 = 1;
yuron	8:3df97287c825	1464	} else {
yuron	8:3df97287c825	1465	myled4 = 0;
yuron	8:3df97287c825	1466	}
yuron	9:1359f0c813b1	1467
yuron	10:b672aa81b226	1468	/*
yuron	8:3df97287c825	1469	//6発発射したら弾倉切り替え
yuron	8:3df97287c825	1470	if(mouted_bottles == 6) {
yuron	8:3df97287c825	1471	if(photo_interrupter == 0) {
yuron	8:3df97287c825	1472	loading_data[0] = 0x0C;
yuron	9:1359f0c813b1	1473	i2c.write(0x30, loading_data, 1, false);
yuron	8:3df97287c825	1474	} else {
yuron	8:3df97287c825	1475	loading_data[0] = 0x80;
yuron	9:1359f0c813b1	1476	i2c.write(0x30, loading_data, 1, false);
yuron	8:3df97287c825	1477	}
yuron	8:3df97287c825	1478	}
yuron	10:b672aa81b226	1479	*/
yuron	8:3df97287c825	1480
yuron	11:5a0d6f69e751	1481	//pc.printf("%3d %3d %3d %3d %3d %3d\n\r", front_line_state, back_line_state, line_state_pettern, abs(measure_pulse), distance, mouted_bottles);
yuron	9:1359f0c813b1	1482	//pc.printf("%d %3lf\n\r", mouted_bottles, timer.read());
yuron	11:5a0d6f69e751	1483	pc.printf("%3d %3d %3d %3lf\n\r", trace_flag, line_state_pettern, distance, timer.read());
yuron	5:167327a82430	1484
yuron	7:7f16fb8b0192	1485	//スタートボタンで動作開始
yuron	7:7f16fb8b0192	1486	if(start_flag == 1) {
yuron	8:3df97287c825	1487	//2段下段
yuron	8:3df97287c825	1488	if(low_grade_flag == 0) {
yuron	8:3df97287c825	1489	//パルスがnを満たすまで無条件で横移動
yuron	8:3df97287c825	1490	//if(abs(measure_pulse) < 100) {
yuron	8:3df97287c825	1491	/*
yuron	8:3df97287c825	1492	start_timer.start();
yuron	8:3df97287c825	1493	if(start_timer.read() <= 1.0f) {
yuron	8:3df97287c825	1494	//青ゾーンの時指定のパルス値まで無条件高速右移動
yuron	8:3df97287c825	1495	if(side == blue) {
yuron	8:3df97287c825	1496	line_state_pettern = right_fast;
yuron	8:3df97287c825	1497	//赤ゾーンの時指定のパルス値まで無条件高速左移動
yuron	8:3df97287c825	1498	}
yuron	8:3df97287c825	1499	else if(side == red) {
yuron	8:3df97287c825	1500	line_state_pettern = left_fast;
yuron	8:3df97287c825	1501	}
yuron	8:3df97287c825	1502	} else {
yuron	8:3df97287c825	1503	//else if(start_timer.read() > 2.0f) {
yuron	10:b672aa81b226	1504	}
yuron	8:3df97287c825	1505	*/
yuron	8:3df97287c825	1506	//一定の距離を超えたらゆっくり移動してライン見つけたら(ry
yuron	9:1359f0c813b1	1507	//} else {
yuron	9:1359f0c813b1	1508
yuron	8:3df97287c825	1509	//距離が11cm~29cmだったらトレースせずに停止
yuron	11:5a0d6f69e751	1510	if(distance >= firing_minimum_distamce && distance <= firing_maximum_distance && distance != 0) {
yuron	11:5a0d6f69e751	1511	//ライントレースの停止
yuron	11:5a0d6f69e751	1512	trace_flag = 0;
yuron	11:5a0d6f69e751	1513
yuron	8:3df97287c825	1514	//タイマスタート
yuron	8:3df97287c825	1515	timer.start();
yuron	10:b672aa81b226	1516
yuron	9:1359f0c813b1	1517	/*
yuron	9:1359f0c813b1	1518	if(timer.read() < 1.0f) {
yuron	9:1359f0c813b1	1519	cylinder_data[0] = 0x80;
yuron	9:1359f0c813b1	1520	i2c.write(0x40, cylinder_data, 1);
yuron	9:1359f0c813b1	1521	} else {
yuron	9:1359f0c813b1	1522	line_state_pettern = stop;
yuron	8:3df97287c825	1523	cylinder_data[0] = 0x0F;
yuron	8:3df97287c825	1524	i2c.write(0x40, cylinder_data, 1);
yuron	9:1359f0c813b1	1525	mouted_bottles++;
yuron	9:1359f0c813b1	1526	}
yuron	9:1359f0c813b1	1527
yuron	9:1359f0c813b1	1528	*/
yuron	9:1359f0c813b1	1529	//発射距離に到達して1秒待って発射
yuron	11:5a0d6f69e751	1530	if(timer.read() > 1.0f && timer.read() <= 2.5f) {
yuron	11:5a0d6f69e751	1531	cylinder_data[0] = 5;
yuron	10:b672aa81b226	1532	i2c.write(0x40, cylinder_data, 1);
yuron	10:b672aa81b226	1533	mouted_bottles++;
yuron	10:b672aa81b226	1534	}
yuron	10:b672aa81b226	1535	/*
yuron	10:b672aa81b226	1536	if(timer.read() > 1.0f && timer.read() <= 9.0f) {
yuron	10:b672aa81b226	1537	cylinder_data[0] = 4;
yuron	9:1359f0c813b1	1538	i2c.write(0x40, cylinder_data, 1);
yuron	9:1359f0c813b1	1539	//2段下段撃ったよ-
yuron	9:1359f0c813b1	1540	//high_grade_fire_flag = 1;
yuron	9:1359f0c813b1	1541	mouted_bottles++;
yuron	8:3df97287c825	1542	}
yuron	10:b672aa81b226	1543	*/
yuron	11:5a0d6f69e751	1544	else if(timer.read() > 12.5f && timer.read() <= 15.0f) {
yuron	11:5a0d6f69e751	1545	cylinder_data[0] = 0x80;
yuron	11:5a0d6f69e751	1546	i2c.write(0x40, cylinder_data, 1);
yuron	11:5a0d6f69e751	1547	//line_state_pettern = right_super_slow;
yuron	11:5a0d6f69e751	1548	}
yuron	11:5a0d6f69e751	1549	else if(timer.read() > 15.0f && timer.read() <= 15.5f) {
yuron	11:5a0d6f69e751	1550	line_state_pettern = right_slow;
yuron	11:5a0d6f69e751	1551	}
yuron	11:5a0d6f69e751	1552	else if(timer.read() > 15.5f && timer.read() <= 18.0f) {
yuron	11:5a0d6f69e751	1553	line_state_pettern = stop;
yuron	11:5a0d6f69e751	1554	cylinder_data[0] = 5;
yuron	11:5a0d6f69e751	1555	i2c.write(0x40, cylinder_data, 1);
yuron	11:5a0d6f69e751	1556	mouted_bottles++;
yuron	11:5a0d6f69e751	1557	}
yuron	11:5a0d6f69e751	1558	else if(timer.read() > 28.0f && timer.read() <= 30.0f) {
yuron	11:5a0d6f69e751	1559	line_state_pettern = stop;
yuron	11:5a0d6f69e751	1560	cylinder_data[0] = 0x80;
yuron	11:5a0d6f69e751	1561	i2c.write(0x40, cylinder_data, 1);
yuron	11:5a0d6f69e751	1562	}
yuron	11:5a0d6f69e751	1563	else if(timer.read() > 30.0f && timer.read() <= 31.0f) {
yuron	11:5a0d6f69e751	1564	line_state_pettern = left_slow;
yuron	11:5a0d6f69e751	1565	}
yuron	11:5a0d6f69e751	1566	else if(timer.read() > 31.0f && timer.read() <= 33.0f) {
yuron	11:5a0d6f69e751	1567	line_state_pettern = stop;
yuron	11:5a0d6f69e751	1568	cylinder_data[0] = 5;
yuron	11:5a0d6f69e751	1569	i2c.write(0x40, cylinder_data, 1);
yuron	11:5a0d6f69e751	1570	mouted_bottles++;
yuron	11:5a0d6f69e751	1571	}
yuron	11:5a0d6f69e751	1572	else if(timer.read() > 33.0f && timer.read() <= 43.0f) {
yuron	11:5a0d6f69e751	1573	line_state_pettern = stop;
yuron	11:5a0d6f69e751	1574	}
yuron	11:5a0d6f69e751	1575	else if(timer.read() > 43.0f && timer.read() <= 43.5f) {
yuron	11:5a0d6f69e751	1576	line_state_pettern = back_trace;
yuron	11:5a0d6f69e751	1577	//ライントレースの復帰
yuron	11:5a0d6f69e751	1578	trace_flag = 1;
yuron	11:5a0d6f69e751	1579	//トレース方向の反転
yuron	11:5a0d6f69e751	1580	trace_direction = 1;
yuron	11:5a0d6f69e751	1581	cylinder_data[0] = 0x80;
yuron	11:5a0d6f69e751	1582	i2c.write(0x40, cylinder_data, 1);
yuron	11:5a0d6f69e751	1583	}
yuron	11:5a0d6f69e751	1584	else if(timer.read() > 45.0f) {
yuron	11:5a0d6f69e751	1585	line_state_pettern = right_slow;
yuron	11:5a0d6f69e751	1586	//ライントレースの復帰
yuron	11:5a0d6f69e751	1587	trace_flag = 1;
yuron	11:5a0d6f69e751	1588	//トレース方向の反転
yuron	11:5a0d6f69e751	1589	trace_direction = 1;
yuron	11:5a0d6f69e751	1590	cylinder_data[0] = 0x80;
yuron	11:5a0d6f69e751	1591	i2c.write(0x40, cylinder_data, 1);
yuron	11:5a0d6f69e751	1592
yuron	11:5a0d6f69e751	1593
yuron	11:5a0d6f69e751	1594	/*
yuron	10:b672aa81b226	1595	//発射して0.5秒たったら後進開始
yuron	11:5a0d6f69e751	1596	else if(timer.read() > 15.0f) {
yuron	10:b672aa81b226	1597	//トレース方向の反転
yuron	10:b672aa81b226	1598	trace_direction = 1;
yuron	10:b672aa81b226	1599
yuron	8:3df97287c825	1600	line_state_pettern = back_trace;
yuron	8:3df97287c825	1601	cylinder_data[0] = 0x80;
yuron	8:3df97287c825	1602	i2c.write(0x40, cylinder_data, 1);
yuron	11:5a0d6f69e751	1603	*/
yuron	8:3df97287c825	1604	} else {
yuron	8:3df97287c825	1605	line_state_pettern = stop;
yuron	8:3df97287c825	1606	cylinder_data[0] = 0x80;
yuron	8:3df97287c825	1607	i2c.write(0x40, cylinder_data, 1);
yuron	8:3df97287c825	1608	}
yuron	9:1359f0c813b1	1609
yuron	8:3df97287c825	1610	//上記以外の距離でライントレースするよん
yuron	7:7f16fb8b0192	1611	} else {
yuron	11:5a0d6f69e751	1612	if(trace_flag == 1) {
yuron	11:5a0d6f69e751	1613
yuron	8:3df97287c825	1614	//タイマリセット
yuron	8:3df97287c825	1615	timer.reset();
yuron	8:3df97287c825	1616
yuron	8:3df97287c825	1617	//ライン検知するまで右移動
yuron	8:3df97287c825	1618	if(back_line_state == 0) {
yuron	8:3df97287c825	1619	//青ゾーンの時ライン検知するまで右移動
yuron	8:3df97287c825	1620	if(side == blue) {
yuron	8:3df97287c825	1621	line_state_pettern = right_slow;
yuron	8:3df97287c825	1622	//赤ゾーンの時ライン検知するまで左移動
yuron	8:3df97287c825	1623	} else {
yuron	8:3df97287c825	1624	line_state_pettern = left_slow;
yuron	8:3df97287c825	1625	}
yuron	8:3df97287c825	1626	}
yuron	8:3df97287c825	1627
yuron	8:3df97287c825	1628	//ライン検知するまで右移動
yuron	8:3df97287c825	1629	if((front_line_state == 0) && (back_line_state == 0)) {
yuron	8:3df97287c825	1630	//青ゾーンの時ライン検知するまで右移動
yuron	8:3df97287c825	1631	if(side == blue) {
yuron	8:3df97287c825	1632	line_state_pettern = right_slow;
yuron	8:3df97287c825	1633	//赤ゾーンの時ライン検知するまで左移動
yuron	8:3df97287c825	1634	} else {
yuron	8:3df97287c825	1635	line_state_pettern = left_slow;
yuron	8:3df97287c825	1636	}
yuron	8:3df97287c825	1637	}
yuron	8:3df97287c825	1638
yuron	8:3df97287c825	1639	//前はライン検知してるけど後ろは検知できない時左移動
yuron	8:3df97287c825	1640	else if((front_line_state >= 1) && (back_line_state == 0)) {
yuron	8:3df97287c825	1641	if(side == red) {
yuron	8:3df97287c825	1642	line_state_pettern = left_super_slow;
yuron	8:3df97287c825	1643	}
yuron	8:3df97287c825	1644	else if(side == blue) {
yuron	8:3df97287c825	1645	line_state_pettern = right_super_slow;
yuron	8:3df97287c825	1646	}
yuron	8:3df97287c825	1647	}
yuron	8:3df97287c825	1648	//前はライン検知できないけど後ろは検知してる時左移動
yuron	8:3df97287c825	1649	else if((front_line_state == 0) && (back_line_state >= 1)) {
yuron	8:3df97287c825	1650	if(side == red) {
yuron	8:3df97287c825	1651	line_state_pettern = left_super_slow;
yuron	8:3df97287c825	1652	}
yuron	8:3df97287c825	1653	else if(side == blue) {
yuron	8:3df97287c825	1654	line_state_pettern = right_super_slow;
yuron	8:3df97287c825	1655	}
yuron	8:3df97287c825	1656	}
yuron	8:3df97287c825	1657
yuron	8:3df97287c825	1658	//前が右寄りの時
yuron	8:3df97287c825	1659	else if((front_line_state <= 5) && (front_line_state != 0)) {
yuron	6:5a051a378e42	1660
yuron	8:3df97287c825	1661	//前も後ろも右寄りの時右移動
yuron	8:3df97287c825	1662	if((back_line_state >= 13) && (back_line_state <= 17)) {
yuron	8:3df97287c825	1663	line_state_pettern = right_super_slow;
yuron	8:3df97287c825	1664	}
yuron	8:3df97287c825	1665	//前が右寄りで後ろが真ん中の時前右旋回後ろ前進
yuron	8:3df97287c825	1666	else if((back_line_state >= 6) && (back_line_state <= 12)) {
yuron	8:3df97287c825	1667	line_state_pettern = front_right_back_front;
yuron	8:3df97287c825	1668	}
yuron	8:3df97287c825	1669	//前が右寄りで後ろが左寄りの時左旋回
yuron	8:3df97287c825	1670	else if((back_line_state <= 5) && (back_line_state != 0)) {
yuron	8:3df97287c825	1671	line_state_pettern = turn_left;
yuron	8:3df97287c825	1672	}
yuron	8:3df97287c825	1673
yuron	8:3df97287c825	1674	}
yuron	8:3df97287c825	1675
yuron	8:3df97287c825	1676	//前が真ん中寄りの時
yuron	8:3df97287c825	1677	else if((front_line_state >= 6) && (front_line_state <= 12)) {
yuron	8:3df97287c825	1678
yuron	8:3df97287c825	1679	//前が真ん中で後ろが右寄りの時前前進後ろ右旋回
yuron	8:3df97287c825	1680	if((back_line_state >= 13) && (back_line_state <= 17)) {
yuron	8:3df97287c825	1681	line_state_pettern = front_front_back_right;
yuron	8:3df97287c825	1682	}
yuron	8:3df97287c825	1683	//前も後ろも真ん中の時前進
yuron	8:3df97287c825	1684	else if((back_line_state >= 6) && (back_line_state <= 12)) {
yuron	8:3df97287c825	1685	if(trace_direction == 0) {
yuron	8:3df97287c825	1686	//20cm未満で後進
yuron	10:b672aa81b226	1687	if(distance < firing_minimum_distamce && distance != 0) {
yuron	8:3df97287c825	1688	line_state_pettern = back_trace;
yuron	8:3df97287c825	1689	}
yuron	8:3df97287c825	1690	//25cmより遠くて前進
yuron	10:b672aa81b226	1691	else if(distance > firing_maximum_distance) {
yuron	8:3df97287c825	1692	line_state_pettern = front_trace;
yuron	8:3df97287c825	1693	} else {
yuron	8:3df97287c825	1694	line_state_pettern = front_trace;
yuron	8:3df97287c825	1695	}
yuron	8:3df97287c825	1696	}
yuron	8:3df97287c825	1697	else if(trace_direction == 1) {
yuron	8:3df97287c825	1698	line_state_pettern = back_trace;
yuron	8:3df97287c825	1699	}
yuron	8:3df97287c825	1700	}
yuron	8:3df97287c825	1701	//前が真ん中で後ろが左寄りの時前前進後ろ左旋回
yuron	8:3df97287c825	1702	else if((back_line_state <= 5) && (back_line_state != 0)) {
yuron	8:3df97287c825	1703	line_state_pettern = front_front_back_left;
yuron	8:3df97287c825	1704	}
yuron	8:3df97287c825	1705	}
yuron	8:3df97287c825	1706	//前が左寄りの時
yuron	8:3df97287c825	1707	else if((front_line_state >= 13) && (front_line_state <= 17)) {
yuron	8:3df97287c825	1708
yuron	8:3df97287c825	1709	//前が左寄りで後ろが右寄りの時右旋回
yuron	8:3df97287c825	1710	if((back_line_state >= 13) && (back_line_state <= 17)) {
yuron	8:3df97287c825	1711	line_state_pettern = turn_right;
yuron	8:3df97287c825	1712	}
yuron	8:3df97287c825	1713	//前が左寄りで後ろが真ん中の時前左旋回後ろ前進
yuron	8:3df97287c825	1714	else if((back_line_state >= 6) && (back_line_state <= 12)) {
yuron	8:3df97287c825	1715	line_state_pettern = front_left_back_front;
yuron	8:3df97287c825	1716	}
yuron	8:3df97287c825	1717	//前が左よりで後ろも左寄りの時左移動
yuron	8:3df97287c825	1718	else if((back_line_state <= 5) && (back_line_state != 0)) {
yuron	8:3df97287c825	1719	line_state_pettern = left_super_slow;
yuron	10:b672aa81b226	1720	}
yuron	10:b672aa81b226	1721	//前で白線の長いの検知したら無視するよ～ん
yuron	11:5a0d6f69e751	1722	else if((front_line_state == 40) \|\| (front_line_state == 80)) {
yuron	11:5a0d6f69e751	1723	if(trace_direction == 0) {
yuron	11:5a0d6f69e751	1724	line_state_pettern = front_trace;
yuron	11:5a0d6f69e751	1725	}
yuron	11:5a0d6f69e751	1726	else if(trace_direction == 1) {
yuron	11:5a0d6f69e751	1727	line_state_pettern = back_trace;
yuron	11:5a0d6f69e751	1728	}
yuron	10:b672aa81b226	1729	}
yuron	11:5a0d6f69e751	1730	//後で白線の長いの検知したら無視するよ～ん
yuron	11:5a0d6f69e751	1731	else if((back_line_state == 40) \|\| (back_line_state == 80)) {
yuron	11:5a0d6f69e751	1732	if(trace_direction == 0) {
yuron	11:5a0d6f69e751	1733	line_state_pettern = front_trace;
yuron	11:5a0d6f69e751	1734	}
yuron	11:5a0d6f69e751	1735	else if(trace_direction == 1) {
yuron	11:5a0d6f69e751	1736	line_state_pettern = back_trace;
yuron	11:5a0d6f69e751	1737	}
yuron	10:b672aa81b226	1738
yuron	8:3df97287c825	1739	//それ以外
yuron	11:5a0d6f69e751	1740	} else {
yuron	8:3df97287c825	1741	line_state_pettern = stop;
yuron	7:7f16fb8b0192	1742	}
yuron	11:5a0d6f69e751	1743	}
yuron	5:167327a82430	1744	}
yuron	10:b672aa81b226	1745	}
yuron	8:3df97287c825	1746	//}
yuron	8:3df97287c825	1747	} else {
yuron	8:3df97287c825	1748	line_state_pettern = stop;
yuron	8:3df97287c825	1749	}
yuron	8:3df97287c825	1750
yuron	8:3df97287c825	1751	//lateral_fragが0(初期状態)の時
yuron	8:3df97287c825	1752	if(lateral_frag == 0) {
yuron	9:1359f0c813b1	1753	//ローラー回転2段高
yuron	9:1359f0c813b1	1754	table_fire = low_grade;
yuron	10:b672aa81b226	1755	firing_minimum_distamce = low_grade_minimum_distance;
yuron	10:b672aa81b226	1756	firing_maximum_distance = low_grade_maximum_distance;
yuron	9:1359f0c813b1	1757	//リミットONでlateral_frag = 1
yuron	8:3df97287c825	1758	if(limit.read() == 0) {
yuron	8:3df97287c825	1759	lateral_frag = 1;
yuron	8:3df97287c825	1760	}
yuron	8:3df97287c825	1761	}
yuron	8:3df97287c825	1762	//リミットがONの時(1回目)
yuron	8:3df97287c825	1763	if(lateral_frag == 1) {
yuron	9:1359f0c813b1	1764	//ローラー回転移動中
yuron	9:1359f0c813b1	1765	table_fire = low_table;
yuron	10:b672aa81b226	1766	firing_minimum_distamce = low_table_minimum_distance;
yuron	10:b672aa81b226	1767	firing_maximum_distance = low_table_maximum_distance;
yuron	9:1359f0c813b1	1768
yuron	8:3df97287c825	1769	//トレース方向の反転(前進)
yuron	8:3df97287c825	1770	trace_direction = 0;
yuron	8:3df97287c825	1771	back_timer1.start();
yuron	8:3df97287c825	1772	if(back_timer1.read() <= 1.5f && back_timer1.read() != 0.0f) {
yuron	8:3df97287c825	1773	if(side == blue) {
yuron	8:3df97287c825	1774	line_state_pettern = right_slow;
yuron	8:3df97287c825	1775	}
yuron	8:3df97287c825	1776	else if(side == red) {
yuron	8:3df97287c825	1777	line_state_pettern = left_slow;
yuron	8:3df97287c825	1778	}
yuron	8:3df97287c825	1779	}
yuron	8:3df97287c825	1780	else if(back_timer1.read() > 1.5f) {
yuron	8:3df97287c825	1781	back_timer1.reset();
yuron	8:3df97287c825	1782	lateral_frag = 2;
yuron	8:3df97287c825	1783	}
yuron	8:3df97287c825	1784	}
yuron	8:3df97287c825	1785	else if(lateral_frag == 2) {
yuron	8:3df97287c825	1786	if(limit.read() == 0) {
yuron	8:3df97287c825	1787	lateral_frag = 3;
yuron	8:3df97287c825	1788	}
yuron	8:3df97287c825	1789	}
yuron	8:3df97287c825	1790	//リミットがONの時(2回目)
yuron	8:3df97287c825	1791	else if(lateral_frag == 3) {
yuron	9:1359f0c813b1	1792	//ローラー回転移動中
yuron	9:1359f0c813b1	1793	table_fire = medium_table;
yuron	10:b672aa81b226	1794	firing_minimum_distamce = medium_table_minimum_distance;
yuron	10:b672aa81b226	1795	firing_maximum_distance = medium_table_maximum_distance;
yuron	9:1359f0c813b1	1796
yuron	8:3df97287c825	1797	trace_direction = 0;
yuron	8:3df97287c825	1798	back_timer2.start();
yuron	8:3df97287c825	1799	if(back_timer2.read() <= 0.8f && back_timer2.read() != 0.0f) {
yuron	8:3df97287c825	1800	if(side == blue) {
yuron	8:3df97287c825	1801	line_state_pettern = right_slow;
yuron	7:7f16fb8b0192	1802	}
yuron	8:3df97287c825	1803	else if(side == red) {
yuron	8:3df97287c825	1804	line_state_pettern = left_slow;
yuron	5:167327a82430	1805	}
yuron	8:3df97287c825	1806	}
yuron	8:3df97287c825	1807	else if(back_timer2.read() > 0.8f) {
yuron	8:3df97287c825	1808	back_timer2.reset();
yuron	8:3df97287c825	1809	lateral_frag = 4;
yuron	8:3df97287c825	1810	}
yuron	8:3df97287c825	1811	}
yuron	8:3df97287c825	1812	else if(lateral_frag == 4) {
yuron	8:3df97287c825	1813	if(limit.read() == 0) {
yuron	8:3df97287c825	1814	//上でback_timer3を使用しているためここでリセットをかける
yuron	8:3df97287c825	1815	//back_timer3.reset();
yuron	8:3df97287c825	1816	lateral_frag = 5;
yuron	8:3df97287c825	1817	}
yuron	8:3df97287c825	1818	}
yuron	8:3df97287c825	1819	//リミットがONの時(3回目)
yuron	8:3df97287c825	1820	else if(lateral_frag == 5) {
yuron	9:1359f0c813b1	1821	//ローラー回転移動高
yuron	9:1359f0c813b1	1822	table_fire = high_table;
yuron	10:b672aa81b226	1823	firing_minimum_distamce = high_table_minimum_distance;
yuron	10:b672aa81b226	1824	firing_maximum_distance = high_table_maximum_distance;
yuron	9:1359f0c813b1	1825
yuron	8:3df97287c825	1826	trace_direction = 0;
yuron	8:3df97287c825	1827	back_timer3.start();
yuron	8:3df97287c825	1828	if(back_timer3.read() <= 0.8f && back_timer3.read() != 0.8f) {
yuron	8:3df97287c825	1829	if(side == blue) {
yuron	8:3df97287c825	1830	line_state_pettern = right_slow;
yuron	8:3df97287c825	1831	}
yuron	8:3df97287c825	1832	else if(side == red) {
yuron	8:3df97287c825	1833	line_state_pettern = left_slow;
yuron	8:3df97287c825	1834	}
yuron	8:3df97287c825	1835	}
yuron	8:3df97287c825	1836	else if(back_timer3.read() > 0.8f) {
yuron	8:3df97287c825	1837	back_timer3.reset();
yuron	8:3df97287c825	1838	lateral_frag = 6;
yuron	8:3df97287c825	1839	}
yuron	8:3df97287c825	1840	}
yuron	8:3df97287c825	1841	else if(lateral_frag == 6) {
yuron	8:3df97287c825	1842	if(limit.read() == 0) {
yuron	8:3df97287c825	1843	lateral_frag = 7;
yuron	8:3df97287c825	1844	}
yuron	8:3df97287c825	1845	}
yuron	8:3df97287c825	1846	//リミットがONの時(4回目)
yuron	8:3df97287c825	1847	else if(lateral_frag == 7) {
yuron	9:1359f0c813b1	1848	//ローラー回転停止
yuron	9:1359f0c813b1	1849	table_fire = 0;
yuron	9:1359f0c813b1	1850
yuron	8:3df97287c825	1851	//スタートゾーンに近づいたら減速
yuron	8:3df97287c825	1852	if(abs(measure_pulse) < 1200) {
yuron	8:3df97287c825	1853	if(side == blue) {
yuron	8:3df97287c825	1854	line_state_pettern = left_super_slow;
yuron	6:5a051a378e42	1855	}
yuron	8:3df97287c825	1856	else if(side == red) {
yuron	8:3df97287c825	1857	line_state_pettern = right_super_slow;
yuron	8:3df97287c825	1858	}
yuron	8:3df97287c825	1859	}
yuron	10:b672aa81b226	1860	else if(abs(measure_pulse) < 500) {
yuron	8:3df97287c825	1861	line_state_pettern = stop;
yuron	8:3df97287c825	1862	} else {
yuron	8:3df97287c825	1863	if(side == blue) {
yuron	10:b672aa81b226	1864	line_state_pettern = left_fast;
yuron	8:3df97287c825	1865	}
yuron	8:3df97287c825	1866	else if(side == red) {
yuron	10:b672aa81b226	1867	line_state_pettern = right_fast;
yuron	5:167327a82430	1868	}
yuron	8:3df97287c825	1869	}
yuron	8:3df97287c825	1870	}
yuron	8:3df97287c825	1871	}
yuron	10:b672aa81b226	1872
yuron	9:1359f0c813b1	1873	//テーブルごとに目標値を変えてローラー回転
yuron	9:1359f0c813b1	1874	switch(table_fire) {
yuron	9:1359f0c813b1	1875	//2段低
yuron	9:1359f0c813b1	1876	case low_grade:
yuron	9:1359f0c813b1	1877	//ローラー回転開始
yuron	10:b672aa81b226	1878	//roller_PID(810, 683);
yuron	10:b672aa81b226	1879	roller_PID(815, 688);
yuron	9:1359f0c813b1	1880	i2c.write(0x20, front_roller_data, 1, false);
yuron	9:1359f0c813b1	1881	i2c.write(0x22, back_roller_data, 1, false);
yuron	9:1359f0c813b1	1882	wait_us(20);
yuron	9:1359f0c813b1	1883	break;
yuron	8:3df97287c825	1884
yuron	9:1359f0c813b1	1885	//2段高
yuron	9:1359f0c813b1	1886	case high_grade:
yuron	9:1359f0c813b1	1887	//ローラー回転開始
yuron	9:1359f0c813b1	1888	roller_PID(400, 400);
yuron	9:1359f0c813b1	1889	i2c.write(0x20, front_roller_data, 1, false);
yuron	9:1359f0c813b1	1890	i2c.write(0x22, back_roller_data, 1, false);
yuron	9:1359f0c813b1	1891	wait_us(20);
yuron	9:1359f0c813b1	1892	break;
yuron	8:3df97287c825	1893
yuron	9:1359f0c813b1	1894	//移動低
yuron	9:1359f0c813b1	1895	case low_table:
yuron	9:1359f0c813b1	1896	//ローラー回転開始
yuron	10:b672aa81b226	1897	roller_PID(820, 690);
yuron	9:1359f0c813b1	1898	i2c.write(0x20, front_roller_data, 1, false);
yuron	9:1359f0c813b1	1899	i2c.write(0x22, back_roller_data, 1, false);
yuron	9:1359f0c813b1	1900	wait_us(20);
yuron	9:1359f0c813b1	1901	break;
yuron	8:3df97287c825	1902
yuron	9:1359f0c813b1	1903	//移動中
yuron	9:1359f0c813b1	1904	case medium_table:
yuron	9:1359f0c813b1	1905	//ローラー回転開始
yuron	9:1359f0c813b1	1906	roller_PID(800, 800);
yuron	9:1359f0c813b1	1907	i2c.write(0x20, front_roller_data, 1, false);
yuron	9:1359f0c813b1	1908	i2c.write(0x22, back_roller_data, 1, false);
yuron	9:1359f0c813b1	1909	wait_us(20);
yuron	9:1359f0c813b1	1910	break;
yuron	8:3df97287c825	1911
yuron	9:1359f0c813b1	1912	//移動高
yuron	9:1359f0c813b1	1913	case high_table:
yuron	9:1359f0c813b1	1914	//ローラー回転開始
yuron	9:1359f0c813b1	1915	roller_PID(1000, 1000);
yuron	9:1359f0c813b1	1916	i2c.write(0x20, front_roller_data, 1, false);
yuron	9:1359f0c813b1	1917	i2c.write(0x22, back_roller_data, 1, false);
yuron	9:1359f0c813b1	1918	wait_us(20);
yuron	9:1359f0c813b1	1919	break;
yuron	9:1359f0c813b1	1920
yuron	9:1359f0c813b1	1921	//それ以外ショートブレーキ
yuron	9:1359f0c813b1	1922	default:
yuron	9:1359f0c813b1	1923	front_roller_data[0] = 0x80;
yuron	9:1359f0c813b1	1924	back_roller_data[0] = 0x80;
yuron	9:1359f0c813b1	1925	i2c.write(0x20, front_roller_data, 1, false);
yuron	9:1359f0c813b1	1926	i2c.write(0x22, back_roller_data, 1, false);
yuron	9:1359f0c813b1	1927	wait_us(20);
yuron	9:1359f0c813b1	1928	break;
yuron	4:df334779a69e	1929	}
yuron	8:3df97287c825	1930
yuron	5:167327a82430	1931	switch(line_state_pettern) {
yuron	5:167327a82430	1932	//青ゾーンでライン検知しないと低速右移動
yuron	5:167327a82430	1933	case right_slow:
yuron	8:3df97287c825	1934	//右前、右後ろ、左前、左後ろ
yuron	11:5a0d6f69e751	1935	right_PID_slow(50, 55, 50, 55);
yuron	5:167327a82430	1936	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	1937	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	1938	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	1939	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	1940	wait_us(20);
yuron	5:167327a82430	1941	break;
yuron	8:3df97287c825	1942
yuron	8:3df97287c825	1943	//赤ゾーンでライン検知しないと低速左移動
yuron	5:167327a82430	1944	case left_slow:
yuron	10:b672aa81b226	1945	left_PID_slow(50, 55, 50, 55);
yuron	8:3df97287c825	1946	//left_PID_slow(100, 110, 100, 110);
yuron	5:167327a82430	1947	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	1948	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	1949	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	1950	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	1951	wait_us(20);
yuron	5:167327a82430	1952	break;
yuron	5:167327a82430	1953
yuron	5:167327a82430	1954	//超低速右移動
yuron	5:167327a82430	1955	case right_super_slow:
yuron	8:3df97287c825	1956	right_PID_slow(5, 5, 5, 5);
yuron	5:167327a82430	1957	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	1958	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	1959	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	1960	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	1961	wait_us(20);
yuron	5:167327a82430	1962	break;
yuron	5:167327a82430	1963
yuron	5:167327a82430	1964	//超低速左移動
yuron	5:167327a82430	1965	case left_super_slow:
yuron	8:3df97287c825	1966	left_PID_slow(5, 5, 5, 5);
yuron	5:167327a82430	1967	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	1968	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	1969	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	1970	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	1971	wait_us(20);
yuron	5:167327a82430	1972	break;
yuron	5:167327a82430	1973
yuron	5:167327a82430	1974	//右旋回
yuron	5:167327a82430	1975	case turn_right:
yuron	8:3df97287c825	1976	turn_right_PID_slow(5, 5, 5, 5);
yuron	5:167327a82430	1977	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	1978	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	1979	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	1980	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	1981	wait_us(20);
yuron	5:167327a82430	1982	break;
yuron	5:167327a82430	1983
yuron	5:167327a82430	1984	//左旋回
yuron	5:167327a82430	1985	case turn_left:
yuron	8:3df97287c825	1986	turn_left_PID_slow(5, 5, 5, 5);
yuron	5:167327a82430	1987	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	1988	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	1989	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	1990	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	1991	wait_us(20);
yuron	5:167327a82430	1992	break;
yuron	5:167327a82430	1993
yuron	5:167327a82430	1994	//前進
yuron	5:167327a82430	1995	case front_trace:
yuron	5:167327a82430	1996	front_PID_slow(30, 30, 30, 30);
yuron	5:167327a82430	1997	//front_PID_slow(50, 50, 50, 50);
yuron	5:167327a82430	1998	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	1999	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2000	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2001	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	2002	wait_us(20);
yuron	5:167327a82430	2003	break;
yuron	8:3df97287c825	2004
yuron	5:167327a82430	2005	//後進
yuron	5:167327a82430	2006	case back_trace:
yuron	8:3df97287c825	2007	back_PID_slow(33, 31, 35, 35);
yuron	5:167327a82430	2008	//back_PID_slow(50, 50, 50, 50);
yuron	5:167327a82430	2009	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2010	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2011	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2012	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	2013	wait_us(20);
yuron	5:167327a82430	2014	break;
yuron	8:3df97287c825	2015
yuron	5:167327a82430	2016	//前前進後ろ右旋回
yuron	5:167327a82430	2017	case front_front_back_right:
yuron	8:3df97287c825	2018	front_front_back_right_PID(5, 5, 5, 5);
yuron	5:167327a82430	2019	//true_hidarimae_data[0] = 0x80;
yuron	5:167327a82430	2020	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2021	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2022	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2023	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	2024	wait_us(20);
yuron	5:167327a82430	2025	break;
yuron	5:167327a82430	2026
yuron	5:167327a82430	2027	//前前進後ろ左旋回
yuron	5:167327a82430	2028	case front_front_back_left:
yuron	8:3df97287c825	2029	front_front_back_left_PID(5, 5, 5, 5);
yuron	5:167327a82430	2030	//true_migimae_data[0] = 0x80;
yuron	5:167327a82430	2031	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2032	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2033	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2034	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	2035	wait_us(20);
yuron	5:167327a82430	2036	break;
yuron	5:167327a82430	2037
yuron	5:167327a82430	2038	//前右旋回後ろ前進
yuron	5:167327a82430	2039	case front_right_back_front:
yuron	8:3df97287c825	2040	front_right_back_front_PID(5, 5, 5, 5);
yuron	5:167327a82430	2041	//true_migiusiro_data[0] = 0x80;
yuron	5:167327a82430	2042	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2043	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2044	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2045	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	2046	wait_us(20);
yuron	5:167327a82430	2047	break;
yuron	5:167327a82430	2048
yuron	5:167327a82430	2049	//前左旋回後ろ前進
yuron	5:167327a82430	2050	case front_left_back_front:
yuron	8:3df97287c825	2051	front_left_back_front_PID(5, 5, 5, 5);
yuron	5:167327a82430	2052	//true_hidariusiro_data[0] = 0x80;
yuron	5:167327a82430	2053	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2054	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2055	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2056	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	2057	wait_us(20);
yuron	5:167327a82430	2058	break;
yuron	8:3df97287c825	2059
yuron	8:3df97287c825	2060	case right_fast:
yuron	8:3df97287c825	2061	//right_PID(300, 255, 300, 255);
yuron	10:b672aa81b226	2062	right_PID_slow(100, 107, 100, 107);
yuron	8:3df97287c825	2063	i2c.write(0x10, true_migimae_data, 1, false);
yuron	8:3df97287c825	2064	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	8:3df97287c825	2065	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	8:3df97287c825	2066	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	8:3df97287c825	2067	wait_us(20);
yuron	8:3df97287c825	2068	break;
yuron	8:3df97287c825	2069
yuron	8:3df97287c825	2070	case left_fast:
yuron	8:3df97287c825	2071	//left_PID(255, 300, 255, 300);
yuron	10:b672aa81b226	2072	left_PID_slow(107, 100, 107, 100);
yuron	8:3df97287c825	2073	i2c.write(0x10, true_migimae_data, 1, false);
yuron	8:3df97287c825	2074	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	8:3df97287c825	2075	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	8:3df97287c825	2076	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	8:3df97287c825	2077	wait_us(20);
yuron	8:3df97287c825	2078	break;
yuron	9:1359f0c813b1	2079
yuron	8:3df97287c825	2080	case stop:
yuron	8:3df97287c825	2081	true_migimae_data[0] = 0x80;
yuron	8:3df97287c825	2082	true_migiusiro_data[0] = 0x80;
yuron	8:3df97287c825	2083	true_hidarimae_data[0] = 0x80;
yuron	8:3df97287c825	2084	true_hidariusiro_data[0] = 0x80;
yuron	8:3df97287c825	2085	i2c.write(0x10, true_migimae_data, 1, false);
yuron	8:3df97287c825	2086	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	8:3df97287c825	2087	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	8:3df97287c825	2088	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	8:3df97287c825	2089	wait_us(20);
yuron	8:3df97287c825	2090	break;
yuron	8:3df97287c825	2091
yuron	5:167327a82430	2092	//それ以外ショートブレーキ
yuron	5:167327a82430	2093	default:
yuron	5:167327a82430	2094	true_migimae_data[0] = 0x80;
yuron	5:167327a82430	2095	true_migiusiro_data[0] = 0x80;
yuron	5:167327a82430	2096	true_hidarimae_data[0] = 0x80;
yuron	5:167327a82430	2097	true_hidariusiro_data[0] = 0x80;
yuron	5:167327a82430	2098	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2099	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2100	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2101	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	5:167327a82430	2102	wait_us(20);
yuron	5:167327a82430	2103	break;
yuron	7:7f16fb8b0192	2104	}
yuron	8:3df97287c825	2105
yuron	5:167327a82430	2106	/*
yuron	5:167327a82430	2107	//前進
yuron	5:167327a82430	2108	front_PID(300, 300, 300, 300);
yuron	4:df334779a69e	2109	i2c.write(0x10, true_migimae_data, 1, false);
yuron	4:df334779a69e	2110	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	4:df334779a69e	2111	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	4:df334779a69e	2112	i2c.write(0x16, true_hidariusiro_data, 1, false);
yuron	4:df334779a69e	2113	wait_us(20);
yuron	5:167327a82430	2114
yuron	5:167327a82430	2115	//後進
yuron	5:167327a82430	2116	back_PID(300, 300, 300, 300);
yuron	4:df334779a69e	2117	i2c.write(0x10, true_migimae_data, 1, false);
yuron	4:df334779a69e	2118	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	4:df334779a69e	2119	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	4:df334779a69e	2120	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	4:df334779a69e	2121	wait_us(20);
yuron	5:167327a82430	2122
yuron	5:167327a82430	2123	//右進行
yuron	5:167327a82430	2124	//right_PID(255, 300, 255, 300);
yuron	5:167327a82430	2125	right_PID_slow(93, 100, 93, 100);
yuron	5:167327a82430	2126	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2127	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2128	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2129	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	5:167327a82430	2130	wait_us(20);
yuron	8:3df97287c825	2131
yuron	5:167327a82430	2132	//左進行
yuron	7:7f16fb8b0192	2133	left_PID(300, 300, 300, 300);
yuron	5:167327a82430	2134	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2135	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2136	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2137	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	5:167327a82430	2138	wait_us(20);
yuron	8:3df97287c825	2139
yuron	5:167327a82430	2140	//斜め右前
yuron	5:167327a82430	2141	right_front_PID(300, 300);
yuron	4:df334779a69e	2142	i2c.write(0x10, true_migimae_data, 1, false);
yuron	4:df334779a69e	2143	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	4:df334779a69e	2144	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	4:df334779a69e	2145	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	4:df334779a69e	2146	wait_us(20);
yuron	5:167327a82430	2147
yuron	5:167327a82430	2148	//斜め右後
yuron	5:167327a82430	2149	right_back_PID(300, 300);
yuron	5:167327a82430	2150	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2151	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2152	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2153	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	5:167327a82430	2154	wait_us(20);
yuron	5:167327a82430	2155
yuron	5:167327a82430	2156	//斜め左前
yuron	5:167327a82430	2157	left_front_PID(300, 300);
yuron	4:df334779a69e	2158	i2c.write(0x10, true_migimae_data, 1, false);
yuron	4:df334779a69e	2159	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	4:df334779a69e	2160	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	4:df334779a69e	2161	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	4:df334779a69e	2162	wait_us(20);
yuron	5:167327a82430	2163
yuron	5:167327a82430	2164	//斜め左後
yuron	5:167327a82430	2165	left_back_PID(300, 300);
yuron	4:df334779a69e	2166	i2c.write(0x10, true_migimae_data, 1, false);
yuron	4:df334779a69e	2167	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	4:df334779a69e	2168	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	4:df334779a69e	2169	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	4:df334779a69e	2170	wait_us(20);
yuron	5:167327a82430	2171
yuron	5:167327a82430	2172	//右旋回
yuron	5:167327a82430	2173	turn_right_PID(300, 300, 300, 300);
yuron	5:167327a82430	2174	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2175	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2176	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2177	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	5:167327a82430	2178	wait_us(20);
yuron	5:167327a82430	2179
yuron	5:167327a82430	2180	//左旋回
yuron	5:167327a82430	2181	turn_left_PID(300, 300, 300, 300);
yuron	5:167327a82430	2182	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2183	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2184	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2185	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	5:167327a82430	2186	wait_us(20);
yuron	5:167327a82430	2187
yuron	5:167327a82430	2188	//前前進後ろ右旋回
yuron	5:167327a82430	2189	front_front_back_right_PID(30, 30, 30, 30);
yuron	4:df334779a69e	2190	true_hidarimae_data[0] = 0x80;
yuron	4:df334779a69e	2191	i2c.write(0x10, true_migimae_data, 1, false);
yuron	4:df334779a69e	2192	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	4:df334779a69e	2193	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	4:df334779a69e	2194	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	4:df334779a69e	2195	wait_us(20);
yuron	4:df334779a69e	2196
yuron	5:167327a82430	2197	//前前進後ろ左旋回
yuron	5:167327a82430	2198	front_front_back_left_PID(30, 30, 30, 30);
yuron	4:df334779a69e	2199	true_migimae_data[0] = 0x80;
yuron	4:df334779a69e	2200	i2c.write(0x10, true_migimae_data, 1, false);
yuron	4:df334779a69e	2201	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	4:df334779a69e	2202	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	4:df334779a69e	2203	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	4:df334779a69e	2204	wait_us(20);
yuron	4:df334779a69e	2205
yuron	5:167327a82430	2206	//前右旋回後ろ前進
yuron	5:167327a82430	2207	front_right_back_front_PID(30, 30, 30, 30);
yuron	4:df334779a69e	2208	true_migiusiro_data[0] = 0x80;
yuron	5:167327a82430	2209	i2c.write(0x10, true_migimae_data, 1, false);
yuron	5:167327a82430	2210	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	5:167327a82430	2211	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	5:167327a82430	2212	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	5:167327a82430	2213	wait_us(20);
yuron	5:167327a82430	2214
yuron	5:167327a82430	2215	//前左旋回後ろ前進
yuron	5:167327a82430	2216	front_left_back_front_PID(30, 30, 30, 30);
yuron	4:df334779a69e	2217	true_hidariusiro_data[0] = 0x80;
yuron	4:df334779a69e	2218	i2c.write(0x10, true_migimae_data, 1, false);
yuron	4:df334779a69e	2219	i2c.write(0x12, true_migiusiro_data, 1, false);
yuron	4:df334779a69e	2220	i2c.write(0x14, true_hidarimae_data, 1, false);
yuron	4:df334779a69e	2221	i2c.write(0x16, true_hidariusiro_data,1, false);
yuron	4:df334779a69e	2222	wait_us(20);
yuron	6:5a051a378e42	2223	*/
yuron	8:3df97287c825	2224
yuron	7:7f16fb8b0192	2225	/*
yuron	5:167327a82430	2226	//ローラーぐるぐる(max930rpm)
yuron	6:5a051a378e42	2227	pc.printf("%3d %3d %3d\n\r", abs(front_roller_rpm), abs(back_roller_rpm), distance);
yuron	7:7f16fb8b0192	2228	//このパラメータ(距離10cm, 移動1個め)で5~8割の確率で勃つ
yuron	7:7f16fb8b0192	2229	//roller_PID(814, 696);
yuron	7:7f16fb8b0192	2230	roller_PID(1006, 928);
yuron	4:df334779a69e	2231	i2c.write(0x20, front_roller_data, 1, false);
yuron	4:df334779a69e	2232	i2c.write(0x22, back_roller_data, 1, false);
yuron	5:167327a82430	2233	wait_us(20);
yuron	7:7f16fb8b0192	2234	*/
yuron	8:3df97287c825	2235
yuron	0:f73c1b076ae4	2236	/*
yuron	2:c32991ba628f	2237	//どんどん加速(逆転)
yuron	5:167327a82430	2238	for(send_data[0] = 0x84; send_data[0] < 0xFF; send_data[0]++){
yuron	5:167327a82430	2239	i2c.write(0x10, send_data, 1);
yuron	5:167327a82430	2240	i2c.write(0x12, send_data, 1);
yuron	5:167327a82430	2241	i2c.write(0x14, send_data, 1);
yuron	5:167327a82430	2242	i2c.write(0x16, send_data, 1);
yuron	0:f73c1b076ae4	2243	wait(0.1);
yuron	0:f73c1b076ae4	2244	}
yuron	2:c32991ba628f	2245	//だんだん減速(逆転)
yuron	5:167327a82430	2246	for(send_data[0] = 0x0C; send_data[0] > 0x7C; send_data[0]--){
yuron	0:f73c1b076ae4	2247	//ice(0x88, send_data);
yuron	0:f73c1b076ae4	2248	//ice(0xA2, send_data);
yuron	5:167327a82430	2249
yuron	5:167327a82430	2250	i2c.write(0x10, send_data, 1);
yuron	5:167327a82430	2251	i2c.write(0x12, send_data, 1);
yuron	5:167327a82430	2252	i2c.write(0x14, send_data, 1);
yuron	5:167327a82430	2253	i2c.write(0x16, send_data, 1);
yuron	0:f73c1b076ae4	2254	wait(0.1);
yuron	0:f73c1b076ae4	2255	}
yuron	0:f73c1b076ae4	2256	*/
yuron	0:f73c1b076ae4	2257	}
yuron	0:f73c1b076ae4	2258	}

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	11 Oct 2018
Imports:	1
Forks:	0
Commits:	15
Dependents:	0
Dependencies:	4
Followers:	7

main.cpp@11:5a0d6f69e751, 2018-10-10 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning