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:
- 9:1359f0c813b1
- Parent:
- 8:3df97287c825
- Child:
- 10:b672aa81b226
--- a/main.cpp Mon Oct 08 07:21:45 2018 +0000
+++ b/main.cpp Tue Oct 09 06:26:06 2018 +0000
@@ -69,6 +69,17 @@
#define right_fast 13
#define left_fast 14
+//2段低
+#define low_grade 1
+//2段高
+#define high_grade 2
+//移動低
+#define low_table 3
+//移動中
+#define medium_table 4
+//移動高
+#define high_table 5
+
//赤ゾーン
#define red 1
//青ゾーン
@@ -196,6 +207,19 @@
//移動(高)乗ったかなフラグ
bool high_table_flag = 0;
+int table_fire = 0;
+/*
+//2段下段撃ったかなフラグ
+bool low_grade_fire_flag = 0;
+//2段上段撃ったかなフラグ
+bool high_grade_fire_flag = 0;
+//移動(低)撃ったかなフラグ
+bool low_table_fire_flag = 0;
+//移動(中)撃ったかなフラグ
+bool medium_table_fire_flag = 0;
+//移動(高)撃ったかなフラグ
+bool high_table_fire_flag = 0;
+*/
static int mouted_bottles = 0;
int loading_state = 0;
@@ -208,6 +232,11 @@
static int i = 0;
int lateral_frag = 0;
+/*
+int bottles_flag[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+*/
+
int migimae_rpm;
int migiusiro_rpm;
int hidarimae_rpm;
@@ -1386,35 +1415,40 @@
myled5 = 1;
}
+ /*
+ //2段高ローラー回転
+ if(high_grade_fire_flag == 0) {
+ //ローラー回転開始
+ roller_PID(1006, 928);
+ i2c.write(0x20, front_roller_data, 1, false);
+ i2c.write(0x22, back_roller_data, 1, false);
+ wait_us(20);
+ }
+ */
+
//フォトインタラプタ検知でLチカ
if(photo_interrupter == 1) {
myled4 = 1;
} else {
myled4 = 0;
}
+
//6発発射したら弾倉切り替え
if(mouted_bottles == 6) {
if(photo_interrupter == 0) {
loading_data[0] = 0x0C;
- i2c.write(0x30, loading_data, 1);
-
+ i2c.write(0x30, loading_data, 1, false);
} else {
loading_data[0] = 0x80;
- i2c.write(0x30, loading_data, 1);
+ 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());
//スタートボタンで動作開始
if(start_flag == 1) {
-
- //ローラー回転開始
- roller_PID(1006, 928);
- i2c.write(0x20, front_roller_data, 1, false);
- i2c.write(0x22, back_roller_data, 1, false);
- wait_us(20);
-
//2段下段
if(low_grade_flag == 0) {
//パルスがnを満たすまで無条件で横移動
@@ -1434,31 +1468,45 @@
//else if(start_timer.read() > 2.0f) {
*/
//一定の距離を超えたらゆっくり移動してライン見つけたら(ry
- //} else {
+ //} else {
+
//距離が11cm~29cmだったらトレースせずに停止
- if(distance >= 20 && distance <= 25) {
+ if(distance >= 20 && distance <= 30) {
//タイマスタート
timer.start();
//トレース方向の反転
trace_direction = 1;
-
- //発射距離に到達して1秒待って発射
- if(timer.read() > 1.0f && timer.read() <= 1.5f) {
+ /*
+ if(timer.read() < 1.0f) {
+ cylinder_data[0] = 0x80;
+ i2c.write(0x40, cylinder_data, 1);
+ } else {
+ line_state_pettern = stop;
cylinder_data[0] = 0x0F;
i2c.write(0x40, cylinder_data, 1);
- mouted_bottles = mouted_bottles + 1;
+ mouted_bottles++;
+ }
+
+ */
+ //発射距離に到達して1秒待って発射
+ if(timer.read() > 1.0f && timer.read() <= 1.01f) {
+ cylinder_data[0] = 0x0F;
+ i2c.write(0x40, cylinder_data, 1);
+ //2段下段撃ったよ-
+ //high_grade_fire_flag = 1;
+ mouted_bottles++;
}
//発射して1秒たったら後進開始
- else if(timer.read() > 1.5f) {
+ else if(timer.read() > 1.01f) {
line_state_pettern = back_trace;
cylinder_data[0] = 0x80;
i2c.write(0x40, cylinder_data, 1);
- pc.printf("start_back!\n\r");
} else {
line_state_pettern = stop;
cylinder_data[0] = 0x80;
i2c.write(0x40, cylinder_data, 1);
}
+
//上記以外の距離でライントレースするよん
} else {
//タイマリセット
@@ -1538,7 +1586,7 @@
line_state_pettern = back_trace;
}
//25cmより遠くて前進
- else if(distance > 25) {
+ else if(distance > 30) {
line_state_pettern = front_trace;
} else {
line_state_pettern = front_trace;
@@ -1581,13 +1629,18 @@
//lateral_fragが0(初期状態)の時
if(lateral_frag == 0) {
- //リミットONでlateral_frag = 1
+ //ローラー回転2段高
+ table_fire = low_grade;
+ //リミットONでlateral_frag = 1
if(limit.read() == 0) {
lateral_frag = 1;
}
}
//リミットがONの時(1回目)
if(lateral_frag == 1) {
+ //ローラー回転移動中
+ table_fire = low_table;
+
//トレース方向の反転(前進)
trace_direction = 0;
back_timer1.start();
@@ -1611,6 +1664,9 @@
}
//リミットがONの時(2回目)
else if(lateral_frag == 3) {
+ //ローラー回転移動中
+ table_fire = medium_table;
+
trace_direction = 0;
back_timer2.start();
if(back_timer2.read() <= 0.8f && back_timer2.read() != 0.0f) {
@@ -1635,6 +1691,9 @@
}
//リミットがONの時(3回目)
else if(lateral_frag == 5) {
+ //ローラー回転移動高
+ table_fire = high_table;
+
trace_direction = 0;
back_timer3.start();
if(back_timer3.read() <= 0.8f && back_timer3.read() != 0.8f) {
@@ -1657,6 +1716,9 @@
}
//リミットがONの時(4回目)
else if(lateral_frag == 7) {
+ //ローラー回転停止
+ table_fire = 0;
+
//スタートゾーンに近づいたら減速
if(abs(measure_pulse) < 1200) {
if(side == blue) {
@@ -1678,250 +1740,62 @@
}
}
}
-
- /*
- //2段上段
- if(high_grade_flag == 0) {
-
- //パルスがnを満たすまで無条件で横移動
- if(abs(measure_pulse) < 10000 && abs(measure_pulse) >= 0) {
- right_PID(255, 300, 255, 300);
- i2c.write(0x10, true_migimae_data, 1, false);
- i2c.write(0x12, true_migiusiro_data, 1, false);
- i2c.write(0x14, true_hidarimae_data, 1, false);
- i2c.write(0x16, true_hidariusiro_data, 1, false);
- wait_us(20);
-
- //一定の距離を超えたらゆっくり移動してライン見つけたら(ry
- } else {
-
- //距離が11cm~29cmだったらトレースせずに停止
- if(distance > 10 && distance < 15) {
- //タイマスタート
- timer.start();
- //トレース方向の反転
- trace_direction = 1;
-
- //発射距離に到達して1秒待って発射
- if(timer.read() > 1.0f && timer.read() <= 1.5f) {
- cylinder_data[0] = 0x0F;
- i2c.write(0x40, cylinder_data, 1);
- mouted_bottles--;
- }
- //発射して1秒たったら後進開始
- else if(timer.read() > 1.5f) {
- line_state_pettern = back_trace;
- cylinder_data[0] = 0x0F;
- i2c.write(0x40, cylinder_data, 1);
- pc.printf("start_back!\n\r");
- } else {
- line_state_pettern = stop;
- cylinder_data[0] = 0x0F;
- i2c.write(0x40, cylinder_data, 1);
- }
-
- //上記以外の距離でライントレースするよん
- } else {
- //タイマリセット
- timer.reset();
+ //テーブルごとに目標値を変えてローラー回転
+ switch(table_fire) {
+ //2段低
+ case low_grade:
+ //ローラー回転開始
+ roller_PID(200, 200);
+ i2c.write(0x20, front_roller_data, 1, false);
+ i2c.write(0x22, back_roller_data, 1, false);
+ wait_us(20);
+ break;
- //ライン検知するまで右移動
- if(back_line_state == 0) {
- //青ゾーンの時ライン検知するまで右移動
- if(side == blue) {
- line_state_pettern = right_slow;
- //赤ゾーンの時ライン検知するまで左移動
- } else {
- line_state_pettern = left_slow;
- }
- }
-
- //ライン検知するまで右移動
- if((front_line_state == 0) && (back_line_state == 0)) {
- //青ゾーンの時ライン検知するまで右移動
- if(side == blue) {
- line_state_pettern = right_slow;
- //赤ゾーンの時ライン検知するまで左移動
- } else {
- line_state_pettern = left_slow;
- }
- }
- */
-
- /*
- //前はライン検知してるけど後ろは検知できない時右移動
- else if((front_line_state >= 1) && (back_line_state == 0)) {
- line_state_pettern = right_super_slow;
- }
- //前はライン検知できないけど後ろは検知してる時右移動
- else if((front_line_state == 0) && (back_line_state >= 1)) {
- line_state_pettern = right_super_slow;
- }
- */
-
- /*
- //前が右寄りの時
- else if((front_line_state <= 5) && (front_line_state != 0)) {
-
- //前も後ろも右寄りの時右移動
- if((back_line_state <= 5) && (back_line_state != 0)) {
- line_state_pettern = right_super_slow;
- }
- //前が右寄りで後ろが真ん中の時前右旋回後ろ前進
- else if((back_line_state >= 6) && (back_line_state <= 12)) {
- line_state_pettern = front_right_back_front;
- }
- //前が右寄りで後ろが左寄りの時左旋回
- else if((back_line_state >= 13) && (back_line_state <= 17)) {
- line_state_pettern = turn_left;
- }
-
- }
- //前が真ん中寄りの時
- else if((front_line_state >= 6) && (front_line_state <= 12)) {
+ //2段高
+ case high_grade:
+ //ローラー回転開始
+ roller_PID(400, 400);
+ i2c.write(0x20, front_roller_data, 1, false);
+ i2c.write(0x22, back_roller_data, 1, false);
+ wait_us(20);
+ break;
- //前が真ん中で後ろが右寄りの時前前進後ろ右旋回
- if((back_line_state <= 5) && (back_line_state != 0)) {
- line_state_pettern = front_front_back_right;
- }
- //前も後ろも真ん中の時前進
- else if((back_line_state >= 6) && (back_line_state <= 12)) {
- if(trace_direction == 0) {
- line_state_pettern = front_trace;
- }
- else if(trace_direction == 1) {
- line_state_pettern = back_trace;
- }
- }
- //前が真ん中で後ろが左寄りの時前前進後ろ左旋回
- else if((back_line_state >= 13) && (back_line_state <= 17)) {
- line_state_pettern = front_front_back_left;
- }
- }
- //前が左寄りの時
- else if((front_line_state >= 13) && (front_line_state <= 17)) {
+ //移動低
+ case low_table:
+ //ローラー回転開始
+ roller_PID(600, 600);
+ i2c.write(0x20, front_roller_data, 1, false);
+ i2c.write(0x22, back_roller_data, 1, false);
+ wait_us(20);
+ break;
- //前が左寄りで後ろが右寄りの時右旋回
- if((back_line_state <= 5) && (back_line_state != 0)) {
- line_state_pettern = turn_right;
- }
- //前が左寄りで後ろが真ん中の時前左旋回後ろ前進
- else if((back_line_state >= 6) && (back_line_state <= 12)) {
- line_state_pettern = front_left_back_front;
- }
- //前が左よりで後ろも左寄りの時左移動
- else if((back_line_state >= 13) && (back_line_state <= 17)) {
- line_state_pettern = left_super_slow;
- }
-
- //それ以外
- } else {
- line_state_pettern = stop;
- }
- }
- }
- } else {
- line_state_pettern = stop;
- }
+ //移動中
+ case medium_table:
+ //ローラー回転開始
+ roller_PID(800, 800);
+ i2c.write(0x20, front_roller_data, 1, false);
+ i2c.write(0x22, back_roller_data, 1, false);
+ wait_us(20);
+ break;
- //lateral_fragが0(初期状態)の時
- if(lateral_frag == 0) {
- //リミットONでlateral_frag = 1
- if(limit.read() == 0) {
- lateral_frag = 1;
- }
- }
- //リミットがONの時(1回目)
- if(lateral_frag == 1) {
- //トレース方向の反転(前進)
- trace_direction = 0;
- back_timer1.start();
- if(back_timer1.read() <= 1.5f && back_timer1.read() != 0.0f) {
- if(side == blue) {
- line_state_pettern = right_slow;
- }
- else if(side == red) {
- line_state_pettern = left_slow;
- }
- }
- else if(back_timer1.read() > 1.5f) {
- back_timer1.reset();
- lateral_frag = 2;
- }
- }
- else if(lateral_frag == 2) {
- if(limit.read() == 0) {
- lateral_frag = 3;
- }
- }
- //リミットがONの時(2回目)
- else if(lateral_frag == 3) {
- trace_direction = 0;
- back_timer2.start();
- if(back_timer2.read() <= 0.8f && back_timer2.read() != 0.0f) {
- if(side == blue) {
- line_state_pettern = right_slow;
- }
- else if(side == red) {
- line_state_pettern = left_slow;
- }
- }
- else if(back_timer2.read() > 0.8f) {
- back_timer2.reset();
- lateral_frag = 4;
- }
- }
- else if(lateral_frag == 4) {
- if(limit.read() == 0) {
- lateral_frag = 5;
- }
- }
- //リミットがONの時(3回目)
- else if(lateral_frag == 5) {
- trace_direction = 0;
- back_timer3.start();
- if(back_timer3.read() <= 0.8f && back_timer3.read() != 0.8f) {
- if(side == blue) {
- line_state_pettern = right_slow;
- }
- else if(side == red) {
- line_state_pettern = left_slow;
- }
- }
- else if(back_timer3.read() > 0.8f) {
- back_timer3.reset();
- lateral_frag = 6;
- }
- }
- else if(lateral_frag == 6) {
- if(limit.read() == 0) {
- lateral_frag = 7;
- }
- }
- //リミットがONの時(4回目)
- else if(lateral_frag == 7) {
- //スタートゾーンに近づいたら減速
- if(abs(measure_pulse) < 1200) {
- if(side == blue) {
- line_state_pettern = left_super_slow;
- }
- else if(side == red) {
- line_state_pettern = right_super_slow;
- }
- }
- else if(abs(measure_pulse) < 800) {
- line_state_pettern = stop;
- } else {
- if(side == blue) {
- line_state_pettern = left_slow;
- }
- else if(side == red) {
- line_state_pettern = right_slow;
- }
- }
- }
+ //移動高
+ case high_table:
+ //ローラー回転開始
+ roller_PID(1000, 1000);
+ i2c.write(0x20, front_roller_data, 1, false);
+ i2c.write(0x22, back_roller_data, 1, false);
+ wait_us(20);
+ break;
+
+ //それ以外ショートブレーキ
+ default:
+ front_roller_data[0] = 0x80;
+ back_roller_data[0] = 0x80;
+ i2c.write(0x20, front_roller_data, 1, false);
+ i2c.write(0x22, back_roller_data, 1, false);
+ wait_us(20);
+ break;
}
- */
switch(line_state_pettern) {
//青ゾーンでライン検知しないと低速右移動
@@ -2071,6 +1945,7 @@
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
break;
+
case stop:
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;