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:
- 7:7f16fb8b0192
- Parent:
- 6:5a051a378e42
- Child:
- 8:3df97287c825
diff -r 5a051a378e42 -r 7f16fb8b0192 main.cpp
--- a/main.cpp Fri Sep 28 08:13:44 2018 +0000
+++ b/main.cpp Wed Oct 03 12:03:03 2018 +0000
@@ -35,6 +35,9 @@
#define Ki_slow 0.03
#define Kd_slow 0.0
+//搭載ボトル数
+#define bottles 15
+
//停止
#define stop 0
//低速右移動
@@ -173,13 +176,14 @@
Timer back_timer2;
Timer back_timer3;
-bool roller_flag = 0;
-bool start_flag = 0;
-int loading_state = 0;
-int line_state = 0;
+bool roller_flag = 0;
+bool start_flag = 0;
+int mouted_bottles = bottles;
+int loading_state = 0;
+int line_state = 0;
int front_line_state = 0;
int back_line_state = 0;
-int line_state_pettern = 0;
+int line_state_pettern = 0;
unsigned int distance;
int trace_direction = 0;
static int i = 0;
@@ -1326,6 +1330,7 @@
//スタートボタン押したらエアシリ伸びる
if(start_button == 0){
myled1 = 1;
+ start_flag = 1;
cylinder_data[0] = 0x0F;
i2c.write(0x40, cylinder_data, 1);
} else {
@@ -1353,95 +1358,105 @@
//pc.printf("pulse: %d, front: %d, back: %d\n\r", abs(measure_pulse), front_line_state, back_line_state);
//pc.printf("FR: %d, BR: %d, FL: %d, BL: %d\n\r", abs(migimae_pulse), abs(migiusiro_pulse), abs(hidarimae_pulse), abs(hidariusiro_pulse));
- /*
- //パルスが0以上10000未満の間高速右移動
- 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);
- }
- //パルスが10000以上になったら低速右移動開始
- else if(abs(measure_pulse) >= 10000) {
- right_PID_slow(93, 100, 93, 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);
- i2c.write(0x16, true_hidariusiro_data, 1, false);
- wait_us(20);
- */
-
- /*
- //距離が11cm~29cmだったらトレースせずに停止
- if(distance > 10 && distance < 30) {
- //タイマスタート
- timer.start();
- //トレース方向の反転
- trace_direction = 1;
- //テーブル検知して1秒たったら後進開始
- if(timer.read() > 1.0f) {
- line_state_pettern = back_trace;
- pc.printf("start_back!\n\r");
- } else {
- line_state_pettern = stop;
- }
+ //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);
- //上記以外の距離でライントレースするよん
- } else {
- //タイマリセット
- timer.reset();
+ //スタートボタンで動作開始
+ 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);
+ /*
+ //パルスが0以上10000未満の間高速右移動
+ 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);
+ }
+ //パルスが10000以上になったら低速右移動開始
+ else if(abs(measure_pulse) >= 10000) {
+ right_PID_slow(93, 100, 93, 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);
+ i2c.write(0x16, true_hidariusiro_data, 1, false);
+ wait_us(20);
+ */
+
+ //距離が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);
+ }
- //ライン検知するまで右移動
- if((front_line_state == 0) && (back_line_state == 0)) {
- //青ゾーンの時ライン検知するまで右移動
- if(side == blue) {
- line_state_pettern = right_slow;
- //赤ゾーンの時ライン検知するまで左移動
- } else {
- line_state_pettern = left_slow;
+ //上記以外の距離でライントレースするよん
+ } else {
+ //タイマリセット
+ timer.reset();
+
+ //ライン検知するまで右移動
+ 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)) {
+ /*
+ //前はライン検知してるけど後ろは検知できない時右移動
+ else if((front_line_state >= 1) && (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 == 0) && (back_line_state >= 1)) {
+ line_state_pettern = right_super_slow;
}
-
- }
- //前が真ん中寄りの時
- else if((front_line_state >= 6) && (front_line_state <= 12)) {
-
- //前が真ん中で後ろが右寄りの時前前進後ろ右旋回
- if((back_line_state <= 5) && (back_line_state != 0)) {
- line_state_pettern = front_front_back_right;
+ */
+
+ //後ろが左よりの時左移動
+ else if((back_line_state <= 5) && (back_line_state != 0)) {
+ line_state_pettern = left_super_slow;
}
- //前も後ろも真ん中の時前進
+ //後ろが真ん中の時直進
else if((back_line_state >= 6) && (back_line_state <= 12)) {
if(trace_direction == 0) {
line_state_pettern = front_trace;
@@ -1450,36 +1465,79 @@
line_state_pettern = back_trace;
}
}
- //前が真ん中で後ろが左寄りの時前前進後ろ左旋回
+ //後ろが右よりの時右移動
else if((back_line_state >= 13) && (back_line_state <= 17)) {
- line_state_pettern = front_front_back_left;
+ 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)) {
+
+ //前が真ん中で後ろが右寄りの時前前進後ろ右旋回
+ 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)) {
+
+ //前が左寄りで後ろが右寄りの時右旋回
+ 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 if((front_line_state >= 13) && (front_line_state <= 17)) {
-
- //前が左寄りで後ろが右寄りの時右旋回
- 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;
+ }
- 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);
+ 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);
+ /*
//lateral_fragが0(初期状態)の時
if(lateral_frag == 0) {
//リミットONでlateral_frag = 1
@@ -1576,12 +1634,13 @@
}
}
}
-
+ */
+
switch(line_state_pettern) {
//青ゾーンでライン検知しないと低速右移動
case right_slow:
- right_PID_slow(50, 51, 50, 51);
+ right_PID_slow(60, 50, 60, 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);
@@ -1591,7 +1650,7 @@
//赤ゾーンでライン検知しないと低速左移動
case left_slow:
- left_PID_slow(50, 50, 50, 50);
+ left_PID_slow(50, 60, 50, 60);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
@@ -1704,7 +1763,6 @@
wait_us(20);
break;
-
//それ以外ショートブレーキ
default:
true_migimae_data[0] = 0x80;
@@ -1717,8 +1775,8 @@
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
break;
- }*/
-
+ }
+
/*
//前進
front_PID(300, 300, 300, 300);
@@ -1744,15 +1802,15 @@
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data,1, false);
wait_us(20);
-
+
//左進行
- left_PID(300, 255, 300, 255);
+ left_PID(300, 300, 300, 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);
-
+
//斜め右前
right_front_PID(300, 300);
i2c.write(0x10, true_migimae_data, 1, false);
@@ -1838,13 +1896,16 @@
wait_us(20);
*/
+ /*
//ローラーぐるぐる(max930rpm)
pc.printf("%3d %3d %3d\n\r", abs(front_roller_rpm), abs(back_roller_rpm), distance);
- //このパラメータ(距離10cm, 中央下段)で3~6割の確率で勃つ
- roller_PID(789, 671);
+ //このパラメータ(距離10cm, 移動1個め)で5~8割の確率で勃つ
+ //roller_PID(814, 696);
+ roller_PID(1006, 928);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
+ */
/*
//どんどん加速(逆転)