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:
- 8:3df97287c825
- Parent:
- 7:7f16fb8b0192
- Child:
- 9:1359f0c813b1
--- a/main.cpp Wed Oct 03 12:03:03 2018 +0000
+++ b/main.cpp Mon Oct 08 07:21:45 2018 +0000
@@ -36,7 +36,8 @@
#define Kd_slow 0.0
//搭載ボトル数
-#define bottles 15
+//1弾倉当たり9発、残り3発になったら弾倉切り替えしよう
+#define bottles 14
//停止
#define stop 0
@@ -65,6 +66,9 @@
//前左旋回後ろ前進
#define front_left_back_front 12
+#define right_fast 13
+#define left_fast 14
+
//赤ゾーン
#define red 1
//青ゾーン
@@ -142,6 +146,7 @@
//スタートLED
DigitalOut start_LED(PC_13);
//DigitalOut start_LED(PA_8);
+
//LED1
DigitalOut myled1(PC_6);
//LED2
@@ -175,10 +180,24 @@
Timer back_timer1;
Timer back_timer2;
Timer back_timer3;
+Timer start_timer;
bool roller_flag = 0;
bool start_flag = 0;
-int mouted_bottles = bottles;
+
+//2段下段乗ったかなフラグ
+bool low_grade_flag = 0;
+//2段上段乗ったかなフラグ
+bool high_grade_flag = 0;
+//移動(低)乗ったかなフラグ
+bool low_table_flag = 0;
+//移動(中)乗ったかなフラグ
+bool medium_table_flag = 0;
+//移動(高)乗ったかなフラグ
+bool high_table_flag = 0;
+
+
+static int mouted_bottles = 0;
int loading_state = 0;
int line_state = 0;
int front_line_state = 0;
@@ -1261,7 +1280,7 @@
//7bit目が1だったら7bit目を0に戻す
if((0b10000000 & line_state) == 0b10000000) {
back_line_state = 0b01111111 & line_state;
-
+
} else {
front_line_state = line_state;
}
@@ -1311,12 +1330,18 @@
i2c.write(0x40, send_data, 1);
wait(0.1);
+ myled1 = 0;
+ myled2 = 0;
+ myled3 = 0;
+ myled4 = 0;
+ myled5 = 0;
+
while(1) {
//超音波取得関数の呼び出し
ultrasonic();
start_LED = 1;
-
+
//赤ゾーン選択
if(side == red){
red_side = 1;
@@ -1329,328 +1354,591 @@
//スタートボタン押したらエアシリ伸びる
if(start_button == 0){
+ start_flag = 1;
+ }
+ //2段下段スイッチでフラグ立てる
+ if(user_switch1 == 0) {
+ low_grade_flag = 1;
myled1 = 1;
- start_flag = 1;
- cylinder_data[0] = 0x0F;
- i2c.write(0x40, cylinder_data, 1);
- } else {
- myled1 = 0;
- cylinder_data[0] = 0x80;
- i2c.write(0x40, cylinder_data, 1);
}
- if(user_switch2 == 0 && user_switch3 == 1){
+ //2段上段スイッチでフラグ立てる
+ if(user_switch2 == 0) {
+ high_grade_flag = 1;
myled2 = 1;
- loading_data[0] = 0x0C;
- i2c.write(0x30, loading_data, 1);
}
- else if(user_switch3 == 0 && user_switch2 == 1){
+ //移動(低)スイッチでフラグ立てる
+ if(user_switch3 == 0) {
+ low_table_flag = 1;
myled3 = 1;
- loading_data[0] = 0xFF;
- i2c.write(0x30, loading_data, 1);
- } else {
- myled2 = 0;
- myled3 = 0;
- loading_data[0] = 0x80;
- i2c.write(0x30, loading_data, 1);
}
- //ここからプロトタイプ移動プログラム
- //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));
-
- //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);
+ //移動(中)スイッチでフラグ立てる
+ /*
+ if(user_switch4 == 0) {
+ medium_table_flag = 1;
+ myled4 = 1;
+ }
+ */
+
+ //移動(高)スイッチでフラグ立てる
+ if(user_switch5 == 0) {
+ high_table_flag = 1;
+ myled5 = 1;
+ }
+
+ //フォトインタラプタ検知で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);
+
+ } else {
+ loading_data[0] = 0x80;
+ i2c.write(0x30, loading_data, 1);
+ }
+ }
+
+ 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);
//スタートボタンで動作開始
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");
+
+ //2段下段
+ if(low_grade_flag == 0) {
+ //パルスがnを満たすまで無条件で横移動
+ //if(abs(measure_pulse) < 100) {
+ /*
+ start_timer.start();
+ if(start_timer.read() <= 1.0f) {
+ //青ゾーンの時指定のパルス値まで無条件高速右移動
+ if(side == blue) {
+ line_state_pettern = right_fast;
+ //赤ゾーンの時指定のパルス値まで無条件高速左移動
+ }
+ else if(side == red) {
+ line_state_pettern = left_fast;
+ }
+ } else {
+ //else if(start_timer.read() > 2.0f) {
+ */
+ //一定の距離を超えたらゆっくり移動してライン見つけたら(ry
+ //} else {
+ //距離が11cm~29cmだったらトレースせずに停止
+ if(distance >= 20 && distance <= 25) {
+ //タイマスタート
+ 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 = mouted_bottles + 1;
+ }
+ //発射して1秒たったら後進開始
+ else if(timer.read() > 1.5f) {
+ 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 {
- line_state_pettern = stop;
- cylinder_data[0] = 0x0F;
- i2c.write(0x40, cylinder_data, 1);
- }
+ //タイマリセット
+ 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)) {
+ if(side == red) {
+ line_state_pettern = left_super_slow;
+ }
+ else if(side == blue) {
+ line_state_pettern = right_super_slow;
+ }
+ }
+ //前はライン検知できないけど後ろは検知してる時左移動
+ else if((front_line_state == 0) && (back_line_state >= 1)) {
+ if(side == red) {
+ line_state_pettern = left_super_slow;
+ }
+ else if(side == blue) {
+ line_state_pettern = right_super_slow;
+ }
+ }
+
+ //前が右寄りの時
+ else if((front_line_state <= 5) && (front_line_state != 0)) {
- //上記以外の距離でライントレースするよん
- } else {
- //タイマリセット
- timer.reset();
-
- //ライン検知するまで右移動
- if(back_line_state == 0) {
- //青ゾーンの時ライン検知するまで右移動
- if(side == blue) {
- line_state_pettern = right_slow;
- //赤ゾーンの時ライン検知するまで左移動
+ //前も後ろも右寄りの時右移動
+ if((back_line_state >= 13) && (back_line_state <= 17)) {
+ 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 <= 5) && (back_line_state != 0)) {
+ line_state_pettern = turn_left;
+ }
+
+ }
+
+ //前が真ん中寄りの時
+ else if((front_line_state >= 6) && (front_line_state <= 12)) {
+
+ //前が真ん中で後ろが右寄りの時前前進後ろ右旋回
+ if((back_line_state >= 13) && (back_line_state <= 17)) {
+ line_state_pettern = front_front_back_right;
+ }
+ //前も後ろも真ん中の時前進
+ else if((back_line_state >= 6) && (back_line_state <= 12)) {
+ if(trace_direction == 0) {
+ //20cm未満で後進
+ if(distance < 20 && distance != 0) {
+ line_state_pettern = back_trace;
+ }
+ //25cmより遠くて前進
+ else if(distance > 25) {
+ line_state_pettern = front_trace;
+ } else {
+ line_state_pettern = front_trace;
+ }
+ }
+ else if(trace_direction == 1) {
+ line_state_pettern = back_trace;
+ }
+ }
+ //前が真ん中で後ろが左寄りの時前前進後ろ左旋回
+ else if((back_line_state <= 5) && (back_line_state != 0)) {
+ line_state_pettern = front_front_back_left;
+ }
+ }
+ //前が左寄りの時
+ else if((front_line_state >= 13) && (front_line_state <= 17)) {
+
+ //前が左寄りで後ろが右寄りの時右旋回
+ if((back_line_state >= 13) && (back_line_state <= 17)) {
+ 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 <= 5) && (back_line_state != 0)) {
+ line_state_pettern = left_super_slow;
+
+ //それ以外
} else {
- line_state_pettern = left_slow;
+ line_state_pettern = stop;
}
}
-
- /*
- //ライン検知するまで右移動
- if((front_line_state == 0) && (back_line_state == 0)) {
- //青ゾーンの時ライン検知するまで右移動
- if(side == blue) {
- line_state_pettern = right_slow;
- //赤ゾーンの時ライン検知するまで左移動
- } else {
- line_state_pettern = left_slow;
- }
+ }
+ //}
+ } else {
+ line_state_pettern = stop;
+ }
+
+ //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((front_line_state >= 1) && (back_line_state == 0)) {
- line_state_pettern = right_super_slow;
+ else if(side == red) {
+ line_state_pettern = left_slow;
}
- //前はライン検知できないけど後ろは検知してる時右移動
- else if((front_line_state == 0) && (back_line_state >= 1)) {
- line_state_pettern = right_super_slow;
+ }
+ else if(back_timer2.read() > 0.8f) {
+ back_timer2.reset();
+ lateral_frag = 4;
+ }
+ }
+ else if(lateral_frag == 4) {
+ if(limit.read() == 0) {
+ //上でback_timer3を使用しているためここでリセットをかける
+ //back_timer3.reset();
+ 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((back_line_state <= 5) && (back_line_state != 0)) {
- 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;
}
- //後ろが真ん中の時直進
- else if((back_line_state >= 6) && (back_line_state <= 12)) {
- if(trace_direction == 0) {
- line_state_pettern = front_trace;
+ }
+ }
+ }
+
+ /*
+ //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--;
}
- else if(trace_direction == 1) {
+ //発射して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 if((back_line_state >= 13) && (back_line_state <= 17)) {
- 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 {
+ //タイマリセット
+ 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((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((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((back_line_state >= 6) && (back_line_state <= 12)) {
- if(trace_direction == 0) {
- line_state_pettern = front_trace;
+ //前が真ん中寄りの時
+ 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(trace_direction == 1) {
- line_state_pettern = back_trace;
+ //前も後ろも真ん中の時前進
+ 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((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((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((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), distance);
-
- /*
- //lateral_fragが0(初期状態)の時
- if(lateral_frag == 0) {
- //リミットONでlateral_frag = 1
- if(limit.read() == 0) {
- lateral_frag = 1;
+ } else {
+ line_state_pettern = stop;
}
- }
- //リミットが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;
+
+ //lateral_fragが0(初期状態)の時
+ if(lateral_frag == 0) {
+ //リミットONでlateral_frag = 1
+ if(limit.read() == 0) {
+ lateral_frag = 1;
}
}
- 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の時(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;
+ }
}
- }
- //リミットが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(lateral_frag == 2) {
+ if(limit.read() == 0) {
+ lateral_frag = 3;
}
}
- 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の時(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;
+ }
}
- }
- //リミットが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(lateral_frag == 4) {
+ if(limit.read() == 0) {
+ lateral_frag = 5;
}
}
- 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;
+ //リミットが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(side == red) {
- line_state_pettern = right_super_slow;
+ else if(back_timer3.read() > 0.8f) {
+ back_timer3.reset();
+ lateral_frag = 6;
}
}
- else if(abs(measure_pulse) < 800) {
- line_state_pettern = stop;
- } else {
- if(side == blue) {
- line_state_pettern = left_slow;
+ else if(lateral_frag == 6) {
+ if(limit.read() == 0) {
+ lateral_frag = 7;
}
- else if(side == red) {
- line_state_pettern = right_slow;
+ }
+ //リミットが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;
+ }
}
}
}
*/
-
+
switch(line_state_pettern) {
-
//青ゾーンでライン検知しないと低速右移動
case right_slow:
- right_PID_slow(60, 50, 60, 50);
+ //右前、右後ろ、左前、左後ろ
+ right_PID_slow(55, 55, 50, 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);
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
break;
-
- //赤ゾーンでライン検知しないと低速左移動
+
+ //赤ゾーンでライン検知しないと低速左移動
case left_slow:
- left_PID_slow(50, 60, 50, 60);
+ left_PID_slow(55, 55, 50, 59);
+ //left_PID_slow(100, 110, 100, 110);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
@@ -1660,7 +1948,7 @@
//超低速右移動
case right_super_slow:
- right_PID_slow(10, 10, 10, 10);
+ right_PID_slow(5, 5, 5, 5);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
@@ -1670,7 +1958,7 @@
//超低速左移動
case left_super_slow:
- left_PID_slow(10, 10, 10, 10);
+ left_PID_slow(5, 5, 5, 5);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
@@ -1680,7 +1968,7 @@
//右旋回
case turn_right:
- turn_right_PID_slow(10, 10, 10, 10);
+ turn_right_PID_slow(5, 5, 5, 5);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
@@ -1690,7 +1978,7 @@
//左旋回
case turn_left:
- turn_left_PID_slow(10, 10, 10, 10);
+ turn_left_PID_slow(5, 5, 5, 5);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
i2c.write(0x14, true_hidarimae_data, 1, false);
@@ -1708,9 +1996,10 @@
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
break;
+
//後進
case back_trace:
- back_PID_slow(30, 30, 30, 30);
+ back_PID_slow(33, 31, 35, 35);
//back_PID_slow(50, 50, 50, 50);
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -1718,10 +2007,10 @@
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
break;
-
+
//前前進後ろ右旋回
case front_front_back_right:
- front_front_back_right_PID(30, 30, 30, 30);
+ front_front_back_right_PID(5, 5, 5, 5);
//true_hidarimae_data[0] = 0x80;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -1732,7 +2021,7 @@
//前前進後ろ左旋回
case front_front_back_left:
- front_front_back_left_PID(30, 30, 30, 30);
+ front_front_back_left_PID(5, 5, 5, 5);
//true_migimae_data[0] = 0x80;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -1743,7 +2032,7 @@
//前右旋回後ろ前進
case front_right_back_front:
- front_right_back_front_PID(30, 30, 30, 30);
+ front_right_back_front_PID(5, 5, 5, 5);
//true_migiusiro_data[0] = 0x80;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -1754,7 +2043,7 @@
//前左旋回後ろ前進
case front_left_back_front:
- front_left_back_front_PID(30, 30, 30, 30);
+ front_left_back_front_PID(5, 5, 5, 5);
//true_hidariusiro_data[0] = 0x80;
i2c.write(0x10, true_migimae_data, 1, false);
i2c.write(0x12, true_migiusiro_data, 1, false);
@@ -1762,7 +2051,38 @@
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
break;
-
+
+ case right_fast:
+ //right_PID(300, 255, 300, 255);
+ right_PID_slow(100, 100, 100, 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);
+ break;
+
+ case left_fast:
+ //left_PID(255, 300, 255, 300);
+ left_PID_slow(100, 100, 100, 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);
+ break;
+ case stop:
+ true_migimae_data[0] = 0x80;
+ true_migiusiro_data[0] = 0x80;
+ true_hidarimae_data[0] = 0x80;
+ true_hidariusiro_data[0] = 0x80;
+ 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);
+ break;
+
//それ以外ショートブレーキ
default:
true_migimae_data[0] = 0x80;
@@ -1776,7 +2096,7 @@
wait_us(20);
break;
}
-
+
/*
//前進
front_PID(300, 300, 300, 300);
@@ -1802,7 +2122,7 @@
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data,1, false);
wait_us(20);
-
+
//左進行
left_PID(300, 300, 300, 300);
i2c.write(0x10, true_migimae_data, 1, false);
@@ -1810,7 +2130,7 @@
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);
@@ -1895,7 +2215,7 @@
i2c.write(0x16, true_hidariusiro_data,1, false);
wait_us(20);
*/
-
+
/*
//ローラーぐるぐる(max930rpm)
pc.printf("%3d %3d %3d\n\r", abs(front_roller_rpm), abs(back_roller_rpm), distance);
@@ -1906,7 +2226,7 @@
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
*/
-
+
/*
//どんどん加速(逆転)
for(send_data[0] = 0x84; send_data[0] < 0xFF; send_data[0]++){
@@ -1930,8 +2250,3 @@
*/
}
}
-
-
-
-
-