UNKO_FINAL_PlanA - Bteam tactics plan A _ move table ver

NITICRobotClub 2018Team-B » Code » UNKO_FINAL_PlanA

NITICRobotClub 2018Team-B / Mbed 2 deprecated UNKO_FINAL_PlanA

Bteam tactics plan A _ move table ver

Fork of UNKO_FINAL by NITICRobotClub 2018Team-B

main.cpp@9:1359f0c813b1, 2018-10-09 (annotated)

Committer:: yuron
Date:: Tue Oct 09 06:26:06 2018 +0000
Revision:: 9:1359f0c813b1
Parent:: 8:3df97287c825
Child:: 10:b672aa81b226

awda;

Who changed what in which revision?

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

Repository toolbox

Export to desktop IDE

Repository details

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

main.cpp@9:1359f0c813b1, 2018-10-09 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning