NITICRobotClub 2018Team-B
Bteam tactics plan A _ move table ver
Dependencies: HCSR04 PID QEI mbed
Fork of
UNKO_FINAL
by 
NITICRobotClub 2018Team-B
Diff: main.cpp
- Revision:
- 10:b672aa81b226
- Parent:
- 9:1359f0c813b1
- Child:
- 11:5a0d6f69e751
diff -r 1359f0c813b1 -r b672aa81b226 main.cpp
--- 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);