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@7:7f16fb8b0192, 2018-10-03 (annotated)

Committer:: yuron
Date:: Wed Oct 03 12:03:03 2018 +0000
Revision:: 7:7f16fb8b0192
Parent:: 6:5a051a378e42
Child:: 8:3df97287c825

?????????????(?????????????????????????)

Who changed what in which revision?

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

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@7:7f16fb8b0192, 2018-10-03 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning