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@6:5a051a378e42, 2018-09-28 (annotated)

Committer:: yuron
Date:: Fri Sep 28 08:13:44 2018 +0000
Revision:: 6:5a051a378e42
Parent:: 5:167327a82430
Child:: 7:7f16fb8b0192

?????(2018/09/28)

Who changed what in which revision?

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

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@6:5a051a378e42, 2018-09-28 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning