2019_A_ver6-2_fixed_phase1 - ver6_2の修正版変更点 phase1のバグ kaisyu関数 tyokudo関数

2019NHK_teamA » Code » 2019_A_ver6-2_fixed_phase1

2019NHK_teamA / Mbed 2 deprecated 2019_A_ver6-2_fixed_phase1

ver6_2の修正版変更点 phase1のバグ kaisyu関数 tyokudo関数

Dependencies: mbed QEI PID

main.cpp@8:3df97287c825, 2018-10-08 (annotated)

Committer:: yuron
Date:: Mon Oct 08 07:21:45 2018 +0000
Revision:: 8:3df97287c825
Parent:: 7:7f16fb8b0192
Child:: 9:1359f0c813b1

auto robo 10/8

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

Repository toolbox

Export to desktop IDE

Repository details

Type:	Library
Created:	19 Sep 2019
Imports:	0
Forks:	0
Commits:	22
Dependents:	0
Dependencies:	3
Followers:	7

The code in this repository is Apache licensed.

main.cpp@8:3df97287c825, 2018-10-08 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning