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

2019NHK_teamA » Code » 2019_A_ver6-2_fixed_phase1

2019NHK_teamA / Mbed 2 deprecated 2019_A_ver6-2_fixed_phase1

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

Dependencies: mbed QEI PID

main.cpp@5:167327a82430, 2018-09-23 (annotated)

Committer:: yuron
Date:: Sun Sep 23 17:02:06 2018 +0000
Revision:: 5:167327a82430
Parent:: 4:df334779a69e
Child:: 6:5a051a378e42

??????????????????????????????????????

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

Repository toolbox

Export to desktop IDE

Repository details

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

The code in this repository is Apache licensed.

main.cpp@5:167327a82430, 2018-09-23 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning