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:
- 6:5a051a378e42
- Parent:
- 5:167327a82430
- Child:
- 7:7f16fb8b0192
diff -r 167327a82430 -r 5a051a378e42 main.cpp
--- a/main.cpp Sun Sep 23 17:02:06 2018 +0000
+++ b/main.cpp Fri Sep 28 08:13:44 2018 +0000
@@ -24,9 +24,12 @@
#define Ki 0.02
#define Kd 0.0
//PIDGain of rollers
-#define roller_Kp 4.0
-#define roller_Ki 0.04
-#define roller_Kd 0.0
+#define front_roller_Kp 1.3
+#define front_roller_Ki 0.19
+#define front_roller_Kd 0.0
+#define back_roller_Kp 1.3
+#define back_roller_Ki 0.1
+#define back_roller_Kd 0.0
//PIDGain of wheels(slow_mode)
#define Kp_slow 0.6
#define Ki_slow 0.03
@@ -83,9 +86,9 @@
PID hidariusiro_slow(Kp_slow, Ki_slow, Kd_slow, 0.001);
//前ローラー
-PID front_roller(roller_Kp, roller_Ki, roller_Kd, 0.001);
+PID front_roller(front_roller_Kp, front_roller_Ki, front_roller_Kd, 0.001);
//後ローラー
-PID back_roller (roller_Kp, roller_Ki, roller_Kd, 0.001);
+PID back_roller (back_roller_Kp, back_roller_Ki, back_roller_Kd, 0.001);
//MDとの通信ポート
I2C i2c(PB_9, PB_8); //SDA, SCL
@@ -134,7 +137,8 @@
//青ゾーンLED
DigitalOut red_side(PC_3);
//スタートLED
-DigitalOut start_LED(PA_8);
+DigitalOut start_LED(PC_13);
+//DigitalOut start_LED(PA_8);
//LED1
DigitalOut myled1(PC_6);
//LED2
@@ -163,8 +167,7 @@
//左後オムニ
QEI hidariusiro_wheel(PB_14, PB_13, NC, 624);
-Ticker get_rps;
-Ticker get_distance;
+Ticker get_rpm;
Timer timer;
Timer back_timer1;
Timer back_timer2;
@@ -180,8 +183,7 @@
unsigned int distance;
int trace_direction = 0;
static int i = 0;
-int right_flag = 0;
-
+int lateral_frag = 0;
int migimae_rpm;
int migiusiro_rpm;
@@ -1255,8 +1257,8 @@
//7bit目が1だったら7bit目を0に戻す
if((0b10000000 & line_state) == 0b10000000) {
back_line_state = 0b01111111 & line_state;
- }
- else {
+
+ } else {
front_line_state = line_state;
}
//4byte以上は出力できないよ
@@ -1279,7 +1281,7 @@
//緊急停止用信号ピンをLow
emergency = 0;
//回転数取得関数のタイマ割り込み(40ms)
- get_rps.attach_us(&flip, 40000);
+ get_rpm.attach_us(&flip, 40000);
//フォトセンサ制御基板との通信ボーレートの設定(115200)
photo.baud(115200);
@@ -1309,7 +1311,8 @@
//超音波取得関数の呼び出し
ultrasonic();
-
+ start_LED = 1;
+
//赤ゾーン選択
if(side == red){
red_side = 1;
@@ -1320,9 +1323,8 @@
blue_side = 1;
}
- //スタートボタン押したらエアシリ伸びる&start_flag立つ
+ //スタートボタン押したらエアシリ伸びる
if(start_button == 0){
- //start_flag = 1;
myled1 = 1;
cylinder_data[0] = 0x0F;
i2c.write(0x40, cylinder_data, 1);
@@ -1370,6 +1372,8 @@
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
*/
+
+ /*
//距離が11cm~29cmだったらトレースせずに停止
if(distance > 10 && distance < 30) {
//タイマスタート
@@ -1388,208 +1392,191 @@
} else {
//タイマリセット
timer.reset();
- /*
- //リミットスイッチが押されたら
- if(limit.read() == 0) {
- //トレース方向の反転
- trace_direction = 0;
- //タイマスタート
- timer.start();
- //1.5秒間右移動
- if(timer.read() < 1.5f) {
+
+ //ライン検知するまで右移動
+ if((front_line_state == 0) && (back_line_state == 0)) {
+ //青ゾーンの時ライン検知するまで右移動
+ if(side == blue) {
line_state_pettern = right_slow;
+ //赤ゾーンの時ライン検知するまで左移動
} else {
- timer.reset();
+ 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)) {
+ }
+ */
+
+ /*
+ //前はライン検知してるけど後ろは検知できない時右移動
+ 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((front_line_state <= 5) && (front_line_state != 0)) {
+ //前が右寄りで後ろが真ん中の時前右旋回後ろ前進
+ 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;
+ }
- //前も後ろも右寄りの時右移動
- 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((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((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;
- /*
- //リミットスイッチが押されたら
- if(limit.read() == 0) {
- //トレース方向の反転
- trace_direction = 0;
- right_flag = 1;
- //timer.start();
- //1.5秒間右移動
- //if(timer.read() < 1.5f) {
- //line_state_pettern = right_slow;
- //} else {
- //timer.reset();
- //}
- }
- */
- }
- }
- //前が真ん中で後ろが左寄りの時前前進後ろ左旋回
- else if((back_line_state >= 13) && (back_line_state <= 17)) {
- line_state_pettern = front_front_back_left;
+ 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)) {
+ 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;
- }
+ //前が左寄りで後ろが右寄りの時右旋回
+ 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 = back_trace;
- }*/
+ 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), right_flag);
+
+ 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);
- //right_flagが0(初期状態)の時
- if(right_flag == 0) {
- //リミットONでright_flag = 1
+ //lateral_fragが0(初期状態)の時
+ if(lateral_frag == 0) {
+ //リミットONでlateral_frag = 1
if(limit.read() == 0) {
- right_flag = 1;
+ lateral_frag = 1;
}
}
//リミットがONの時(1回目)
- if(right_flag == 1) {
+ if(lateral_frag == 1) {
//トレース方向の反転(前進)
trace_direction = 0;
back_timer1.start();
if(back_timer1.read() <= 1.5f && back_timer1.read() != 0.0f) {
- line_state_pettern = right_slow;
+ 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();
- right_flag = 2;
+ lateral_frag = 2;
}
}
- else if(right_flag == 2) {
+ else if(lateral_frag == 2) {
if(limit.read() == 0) {
- right_flag = 3;
+ lateral_frag = 3;
}
}
//リミットがONの時(2回目)
- else if(right_flag == 3) {
+ else if(lateral_frag == 3) {
trace_direction = 0;
back_timer2.start();
if(back_timer2.read() <= 0.8f && back_timer2.read() != 0.0f) {
- line_state_pettern = right_slow;
+ 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();
- right_flag = 4;
+ lateral_frag = 4;
}
}
- else if(right_flag == 4) {
+ else if(lateral_frag == 4) {
if(limit.read() == 0) {
- right_flag = 5;
+ lateral_frag = 5;
}
}
//リミットがONの時(3回目)
- else if(right_flag == 5) {
+ else if(lateral_frag == 5) {
trace_direction = 0;
back_timer3.start();
if(back_timer3.read() <= 0.8f && back_timer3.read() != 0.8f) {
- line_state_pettern = right_slow;
+ 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();
- right_flag = 6;
+ lateral_frag = 6;
}
}
- else if(right_flag == 6) {
+ else if(lateral_frag == 6) {
if(limit.read() == 0) {
- right_flag = 7;
+ lateral_frag = 7;
}
}
//リミットがONの時(4回目)
- else if(right_flag == 7) {
+ else if(lateral_frag == 7) {
//スタートゾーンに近づいたら減速
if(abs(measure_pulse) < 1200) {
- line_state_pettern = left_super_slow;
+ if(side == blue) {
+ line_state_pettern = left_super_slow;
+ }
+ else if(side == red) {
+ line_state_pettern = right_super_slow;
+ }
}
- else if(abs(measure_pulse) < 500) {
+ else if(abs(measure_pulse) < 800) {
line_state_pettern = stop;
} else {
- line_state_pettern = left_slow;
+ if(side == blue) {
+ line_state_pettern = left_slow;
+ }
+ else if(side == red) {
+ line_state_pettern = right_slow;
+ }
}
}
-
- /*
- if(right_flag == 1) {
- line_state_pettern = front_trace;
- wait(2);
- line_state_pettern = right_slow;
- wait(2);
- right_flag = 2;
- }
- */
-
+
switch(line_state_pettern) {
//青ゾーンでライン検知しないと低速右移動
@@ -1720,16 +1707,6 @@
//それ以外ショートブレーキ
default:
- /*
- front_PID_slow(30, 30, 30, 30);
- 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;
- */
-
true_migimae_data[0] = 0x80;
true_migiusiro_data[0] = 0x80;
true_hidarimae_data[0] = 0x80;
@@ -1740,8 +1717,7 @@
i2c.write(0x16, true_hidariusiro_data, 1, false);
wait_us(20);
break;
-
- }
+ }*/
/*
//前進
@@ -1860,32 +1836,16 @@
i2c.write(0x14, true_hidarimae_data, 1, false);
i2c.write(0x16, true_hidariusiro_data,1, false);
wait_us(20);
-
+ */
+
//ローラーぐるぐる(max930rpm)
- roller_PID(440, 400);
+ pc.printf("%3d %3d %3d\n\r", abs(front_roller_rpm), abs(back_roller_rpm), distance);
+ //このパラメータ(距離10cm, 中央下段)で3~6割の確率で勃つ
+ roller_PID(789, 671);
i2c.write(0x20, front_roller_data, 1, false);
i2c.write(0x22, back_roller_data, 1, false);
wait_us(20);
- */
-
- /*
- if(front_roller_rpm > 500) {
- cylinder_data[0] = 0x0F;
- i2c.write(0x40, cylinder_data, 1);
- myled5 = 1;
- wait(0.5);
-
- cylinder_data[0] = 0x80;
- i2c.write(0x40, cylinder_data, 1);
- myled5 = 0;
- wait(0.5);
- } else {
- cylinder_data[0] = 0x80;
- i2c.write(0x40, cylinder_data, 1);
- myled5 = 0;
- wait(0.5);
- }
- */
+
/*
//どんどん加速(逆転)
for(send_data[0] = 0x84; send_data[0] < 0xFF; send_data[0]++){