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:
- 11:5a0d6f69e751
- Parent:
- 10:b672aa81b226
- Child:
- 12:1a22b9797004
--- a/main.cpp Wed Oct 10 08:20:24 2018 +0000
+++ b/main.cpp Wed Oct 10 11:27:09 2018 +0000
@@ -81,9 +81,9 @@
#define high_table 5
//2段低最小距離
-#define low_grade_minimum_distance 10
+#define low_grade_minimum_distance 7
//2段低最大距離
-#define low_grade_maximum_distance 15
+#define low_grade_maximum_distance 16
//2段高最小距離
//#define high_grade_minimum_distance 1
@@ -221,6 +221,7 @@
bool roller_flag = 0;
bool start_flag = 0;
+bool trace_flag = 1;
//2段下段乗ったかなフラグ
bool low_grade_flag = 0;
@@ -1477,9 +1478,9 @@
}
*/
- 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("%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());
+ pc.printf("%3d %3d %3d %3lf\n\r", trace_flag, line_state_pettern, distance, timer.read());
//スタートボタンで動作開始
if(start_flag == 1) {
@@ -1506,7 +1507,10 @@
//} else {
//距離が11cm~29cmだったらトレースせずに停止
- if(distance >= firing_minimum_distamce && distance <= firing_maximum_distance) {
+ if(distance >= firing_minimum_distamce && distance <= firing_maximum_distance && distance != 0) {
+ //ライントレースの停止
+ trace_flag = 0;
+
//タイマスタート
timer.start();
@@ -1523,8 +1527,8 @@
*/
//発射距離に到達して1秒待って発射
- if(timer.read() > 1.0f && timer.read() <= 3.5f) {
- cylinder_data[0] = 4;
+ if(timer.read() > 1.0f && timer.read() <= 2.5f) {
+ cylinder_data[0] = 5;
i2c.write(0x40, cylinder_data, 1);
mouted_bottles++;
}
@@ -1537,15 +1541,66 @@
mouted_bottles++;
}
*/
-
+ else if(timer.read() > 12.5f && timer.read() <= 15.0f) {
+ cylinder_data[0] = 0x80;
+ i2c.write(0x40, cylinder_data, 1);
+ //line_state_pettern = right_super_slow;
+ }
+ else if(timer.read() > 15.0f && timer.read() <= 15.5f) {
+ line_state_pettern = right_slow;
+ }
+ else if(timer.read() > 15.5f && timer.read() <= 18.0f) {
+ line_state_pettern = stop;
+ cylinder_data[0] = 5;
+ i2c.write(0x40, cylinder_data, 1);
+ mouted_bottles++;
+ }
+ else if(timer.read() > 28.0f && timer.read() <= 30.0f) {
+ line_state_pettern = stop;
+ cylinder_data[0] = 0x80;
+ i2c.write(0x40, cylinder_data, 1);
+ }
+ else if(timer.read() > 30.0f && timer.read() <= 31.0f) {
+ line_state_pettern = left_slow;
+ }
+ else if(timer.read() > 31.0f && timer.read() <= 33.0f) {
+ line_state_pettern = stop;
+ cylinder_data[0] = 5;
+ i2c.write(0x40, cylinder_data, 1);
+ mouted_bottles++;
+ }
+ else if(timer.read() > 33.0f && timer.read() <= 43.0f) {
+ line_state_pettern = stop;
+ }
+ else if(timer.read() > 43.0f && timer.read() <= 43.5f) {
+ line_state_pettern = back_trace;
+ //ライントレースの復帰
+ trace_flag = 1;
+ //トレース方向の反転
+ trace_direction = 1;
+ cylinder_data[0] = 0x80;
+ i2c.write(0x40, cylinder_data, 1);
+ }
+ else if(timer.read() > 45.0f) {
+ line_state_pettern = right_slow;
+ //ライントレースの復帰
+ trace_flag = 1;
+ //トレース方向の反転
+ trace_direction = 1;
+ cylinder_data[0] = 0x80;
+ i2c.write(0x40, cylinder_data, 1);
+
+
+ /*
//発射して0.5秒たったら後進開始
- else if(timer.read() > 8.0f) {
+ else if(timer.read() > 15.0f) {
//トレース方向の反転
trace_direction = 1;
line_state_pettern = back_trace;
cylinder_data[0] = 0x80;
i2c.write(0x40, cylinder_data, 1);
+ */
} else {
line_state_pettern = stop;
cylinder_data[0] = 0x80;
@@ -1554,6 +1609,8 @@
//上記以外の距離でライントレースするよん
} else {
+ if(trace_flag == 1) {
+
//タイマリセット
timer.reset();
@@ -1661,19 +1718,29 @@
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((front_line_state == 40) || (front_line_state == 80)) {
+ if(trace_direction == 0) {
+ line_state_pettern = front_trace;
+ }
+ else if(trace_direction == 1) {
+ line_state_pettern = back_trace;
+ }
}
- //前で白線の長いの検知したら無視するよ～ん
- else if((back_line_state == 40) || (back_line_state == 40)) {
- line_state_pettern = front_trace;
+ //後で白線の長いの検知したら無視するよ～ん
+ else if((back_line_state == 40) || (back_line_state == 80)) {
+ if(trace_direction == 0) {
+ line_state_pettern = front_trace;
+ }
+ else if(trace_direction == 1) {
+ line_state_pettern = back_trace;
+ }
//それ以外
- }*/ else {
+ } else {
line_state_pettern = stop;
}
+ }
}
}
//}
@@ -1865,7 +1932,7 @@
//青ゾーンでライン検知しないと低速右移動
case right_slow:
//右前、右後ろ、左前、左後ろ
- right_PID_slow(55, 50, 55, 50);
+ right_PID_slow(50, 55, 50, 55);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);