2019NHK_teamA
/
2019_A_ver8
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 10:b672aa81b226, committed 2018-10-10
- Comitter:
- yuron
- Date:
- Wed Oct 10 08:20:24 2018 +0000
- Parent:
- 9:1359f0c813b1
- Child:
- 11:5a0d6f69e751
- Commit message:
- ?????????
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Tue Oct 09 06:26:06 2018 +0000
+++ b/main.cpp Wed Oct 10 08:20:24 2018 +0000
@@ -80,6 +80,32 @@
//移動高
#define high_table 5
+//2段低最小距離
+#define low_grade_minimum_distance 10
+//2段低最大距離
+#define low_grade_maximum_distance 15
+
+//2段高最小距離
+//#define high_grade_minimum_distance 1
+//2段高最大距離
+//#define high_grade_maximum_distance 1
+
+//移動低最小距離
+#define low_table_minimum_distance 10
+//移動低最大距離
+#define low_table_maximum_distance 15
+
+//移動中最小距離
+#define medium_table_minimum_distance 20
+//移動中最大距離
+#define medium_table_maximum_distance 30
+
+//移動高最小距離
+#define high_table_minimum_distance 30
+//移動高最大距離
+#define high_table_maximum_distance 40
+
+
//赤ゾーン
#define red 1
//青ゾーン
@@ -237,6 +263,9 @@
0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
*/
+int firing_minimum_distamce = 0;
+int firing_maximum_distance = 0;
+
int migimae_rpm;
int migiusiro_rpm;
int hidarimae_rpm;
@@ -287,7 +316,7 @@
char front_roller_data[1];
char back_roller_data[1];
char loading_data[1];
-char cylinder_data[1];
+char cylinder_data[1] = {0x80};
char true_migimae_data[1];
char true_migiusiro_data[1];
@@ -1332,6 +1361,8 @@
pc.printf("HelloWorld\n");
//緊急停止用信号ピンをLow
emergency = 0;
+ //スタートボタンのLEDをHigh
+ start_LED = 1;
//回転数取得関数のタイマ割り込み(40ms)
get_rpm.attach_us(&flip, 40000);
@@ -1343,6 +1374,7 @@
//サイドチェンジボタンをPullDownに設定
side.mode(PullDown);
limit.mode(PullDown);
+
//超音波センサの温度設定
// setTemp関数は、HCSR04のメンバ変数temperature(気温を保持する変数)を引数として受け取った値に変更します。
sonic.setTemp(25);
@@ -1369,7 +1401,6 @@
//超音波取得関数の呼び出し
ultrasonic();
- start_LED = 1;
//赤ゾーン選択
if(side == red){
@@ -1433,6 +1464,7 @@
myled4 = 0;
}
+ /*
//6発発射したら弾倉切り替え
if(mouted_bottles == 6) {
if(photo_interrupter == 0) {
@@ -1443,9 +1475,11 @@
i2c.write(0x30, loading_data, 1, false);
}
}
+ */
pc.printf("%3d %3d %3d %3d %3d %3d\n\r", front_line_state, back_line_state, line_state_pettern, abs(measure_pulse), distance, mouted_bottles);
//pc.printf("%d %3lf\n\r", mouted_bottles, timer.read());
+ //pc.printf("%3d %3lf\n\r", cylinder_data[0], timer.read());
//スタートボタンで動作開始
if(start_flag == 1) {
@@ -1466,16 +1500,16 @@
}
} else {
//else if(start_timer.read() > 2.0f) {
+ }
*/
//一定の距離を超えたらゆっくり移動してライン見つけたら(ry
//} else {
//距離が11cm~29cmだったらトレースせずに停止
- if(distance >= 20 && distance <= 30) {
+ if(distance >= firing_minimum_distamce && distance <= firing_maximum_distance) {
//タイマスタート
timer.start();
- //トレース方向の反転
- trace_direction = 1;
+
/*
if(timer.read() < 1.0f) {
cylinder_data[0] = 0x80;
@@ -1489,15 +1523,26 @@
*/
//発射距離に到達して1秒待って発射
- if(timer.read() > 1.0f && timer.read() <= 1.01f) {
- cylinder_data[0] = 0x0F;
+ if(timer.read() > 1.0f && timer.read() <= 3.5f) {
+ cylinder_data[0] = 4;
+ i2c.write(0x40, cylinder_data, 1);
+ mouted_bottles++;
+ }
+ /*
+ if(timer.read() > 1.0f && timer.read() <= 9.0f) {
+ cylinder_data[0] = 4;
i2c.write(0x40, cylinder_data, 1);
//2段下段撃ったよ-
//high_grade_fire_flag = 1;
mouted_bottles++;
}
- //発射して1秒たったら後進開始
- else if(timer.read() > 1.01f) {
+ */
+
+ //発射して0.5秒たったら後進開始
+ else if(timer.read() > 8.0f) {
+ //トレース方向の反転
+ trace_direction = 1;
+
line_state_pettern = back_trace;
cylinder_data[0] = 0x80;
i2c.write(0x40, cylinder_data, 1);
@@ -1582,11 +1627,11 @@
else if((back_line_state >= 6) && (back_line_state <= 12)) {
if(trace_direction == 0) {
//20cm未満で後進
- if(distance < 20 && distance != 0) {
+ if(distance < firing_minimum_distamce && distance != 0) {
line_state_pettern = back_trace;
}
//25cmより遠くて前進
- else if(distance > 30) {
+ else if(distance > firing_maximum_distance) {
line_state_pettern = front_trace;
} else {
line_state_pettern = front_trace;
@@ -1615,13 +1660,22 @@
//前が左よりで後ろも左寄りの時左移動
else if((back_line_state <= 5) && (back_line_state != 0)) {
line_state_pettern = left_super_slow;
-
+ }
+ /*
+ //前で白線の長いの検知したら無視するよ～ん
+ else if((front_line_state == 40) || (front_line_state == 40)) {
+ line_state_pettern = front_trace;
+ }
+ //前で白線の長いの検知したら無視するよ～ん
+ else if((back_line_state == 40) || (back_line_state == 40)) {
+ line_state_pettern = front_trace;
+
//それ以外
- } else {
+ }*/ else {
line_state_pettern = stop;
}
}
- }
+ }
//}
} else {
line_state_pettern = stop;
@@ -1631,6 +1685,8 @@
if(lateral_frag == 0) {
//ローラー回転2段高
table_fire = low_grade;
+ firing_minimum_distamce = low_grade_minimum_distance;
+ firing_maximum_distance = low_grade_maximum_distance;
//リミットONでlateral_frag = 1
if(limit.read() == 0) {
lateral_frag = 1;
@@ -1640,6 +1696,8 @@
if(lateral_frag == 1) {
//ローラー回転移動中
table_fire = low_table;
+ firing_minimum_distamce = low_table_minimum_distance;
+ firing_maximum_distance = low_table_maximum_distance;
//トレース方向の反転(前進)
trace_direction = 0;
@@ -1666,6 +1724,8 @@
else if(lateral_frag == 3) {
//ローラー回転移動中
table_fire = medium_table;
+ firing_minimum_distamce = medium_table_minimum_distance;
+ firing_maximum_distance = medium_table_maximum_distance;
trace_direction = 0;
back_timer2.start();
@@ -1693,6 +1753,8 @@
else if(lateral_frag == 5) {
//ローラー回転移動高
table_fire = high_table;
+ firing_minimum_distamce = high_table_minimum_distance;
+ firing_maximum_distance = high_table_maximum_distance;
trace_direction = 0;
back_timer3.start();
@@ -1728,24 +1790,26 @@
line_state_pettern = right_super_slow;
}
}
- else if(abs(measure_pulse) < 800) {
+ else if(abs(measure_pulse) < 500) {
line_state_pettern = stop;
} else {
if(side == blue) {
- line_state_pettern = left_slow;
+ line_state_pettern = left_fast;
}
else if(side == red) {
- line_state_pettern = right_slow;
+ line_state_pettern = right_fast;
}
}
}
}
+
//テーブルごとに目標値を変えてローラー回転
switch(table_fire) {
//2段低
case low_grade:
//ローラー回転開始
- roller_PID(200, 200);
+ //roller_PID(810, 683);
+ roller_PID(815, 688);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
@@ -1763,7 +1827,7 @@
//移動低
case low_table:
//ローラー回転開始
- roller_PID(600, 600);
+ roller_PID(820, 690);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
@@ -1801,7 +1865,7 @@
//青ゾーンでライン検知しないと低速右移動
case right_slow:
//右前、右後ろ、左前、左後ろ
- right_PID_slow(55, 55, 50, 50);
+ right_PID_slow(55, 50, 55, 50);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
@@ -1811,7 +1875,7 @@
//赤ゾーンでライン検知しないと低速左移動
case left_slow:
- left_PID_slow(55, 55, 50, 59);
+ left_PID_slow(50, 55, 50, 55);
//left_PID_slow(100, 110, 100, 110);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -1928,7 +1992,7 @@
case right_fast:
//right_PID(300, 255, 300, 255);
- right_PID_slow(100, 100, 100, 100);
+ right_PID_slow(100, 107, 100, 107);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
@@ -1938,7 +2002,7 @@
case left_fast:
//left_PID(255, 300, 255, 300);
- left_PID_slow(100, 100, 100, 100);
+ left_PID_slow(107, 100, 107, 100);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);