Bteam tactics plan A _ move table ver

Dependencies:   HCSR04 PID QEI mbed

Fork of UNKO_FINAL by NITICRobotClub 2018Team-B

Files at this revision

API Documentation at this revision

Comitter:
BIGBOSS04
Date:
Thu Oct 11 15:55:47 2018 +0000
Parent:
13:93321c73df60
Commit message:
Bteam tactics plan A _ center table ver;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 93321c73df60 -r 677e9f0785b8 main.cpp
--- a/main.cpp	Wed Oct 10 16:58:39 2018 +0000
+++ b/main.cpp	Thu Oct 11 15:55:47 2018 +0000
@@ -214,6 +214,7 @@
 
 Ticker get_rpm;
 Timer timer;
+Timer timer1;
 Timer back_timer1;
 Timer back_timer2;
 Timer back_timer3;
@@ -1466,7 +1467,7 @@
         }
 
         //6発発射したら弾倉切り替え
-        if(mouted_bottles == 10) {
+        if(mouted_bottles == 11) {
             if(photo_interrupter == 0) {
                 loading_data[0] = 0x0C;
                 i2c.write(0x30, loading_data, 1, false);
@@ -1482,8 +1483,11 @@
 
         //スタートボタンで動作開始
         if(start_flag == 1) {
+            timer1.start();
             //2段下段
             if(low_grade_flag == 0) {
+                timer1.start();
+                
                 //パルスがnを満たすまで無条件で横移動
                 //if(abs(measure_pulse) < 100) {
                     /*
@@ -1510,44 +1514,44 @@
                         trace_flag = 0;
                         //タイマスタート
                         timer.start();
-
+                
                         //発射距離に到達して1秒待って発射
                         if(timer.read() > 1.0f && timer.read() <= 2.5f) {
-                            cylinder_data[0] = 5;
-                            mouted_bottles = 5;
+                            cylinder_data[0] = 4;
+                            mouted_bottles = 4;
                             i2c.write(0x40, cylinder_data, 1);
                         }
-                        else if(timer.read() > 12.5f && timer.read() <= 15.0f) {
+                        else if(timer.read() > 10.5f && timer.read() <= 13.0f) {
                             cylinder_data[0] = 0x80;
                             i2c.write(0x40, cylinder_data, 1);
                         }
-                        else if(timer.read() > 15.0f && timer.read() <= 15.5f) {
+                        else if(timer.read() > 13.0f && timer.read() <= 13.5f) {
                             line_state_pettern = right_slow;
                         }
-                        else if(timer.read() > 15.5f && timer.read() <= 17.5f) {
+                        else if(timer.read() > 13.5f && timer.read() <= 15.5f) {
                             line_state_pettern = stop;
-                            cylinder_data[0] = 5;
-                            mouted_bottles = 10;
+                            cylinder_data[0] = 4;
+                            mouted_bottles = 8;
                             i2c.write(0x40, cylinder_data, 1);
                         }
-                        else if(timer.read() > 28.0f && timer.read() <= 30.0f) {
+                        else if(timer.read() > 26.0f && timer.read() <= 28.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) {
+                        else if(timer.read() > 28.0f && timer.read() <= 28.8f) {
                             line_state_pettern = left_slow;
                         }
-                        else if(timer.read() > 31.0f && timer.read() <= 33.0f) {
+                        else if(timer.read() > 28.8f && timer.read() <= 31.0f) {
                             line_state_pettern = stop;
-                            cylinder_data[0] = 5;
-                            mouted_bottles = 15;
+                            cylinder_data[0] = 4;
+                            mouted_bottles = 11;
                             i2c.write(0x40, cylinder_data, 1);
                         }
-                        else if(timer.read() > 33.0f && timer.read() <= 43.0f) {
+                        else if(timer.read() > 31.0f && timer.read() <= 41.0f) {
                             line_state_pettern = stop;
                         }
-                        else if(timer.read() > 43.0f && timer.read() <= 45.0f) {
+                        else if(timer.read() > 41.0f && timer.read() <= 43.0f) {
                             line_state_pettern = back_trace;
                             //ライントレースの復帰
                             //trace_flag = 1;
@@ -1556,7 +1560,7 @@
                             cylinder_data[0] = 0x80;
                             i2c.write(0x40, cylinder_data, 1);
                         }
-                        else if(timer.read() > 45.0f) {
+                        else if(timer.read() > 43.0f) {
                             line_state_pettern = right_slow;
                             //ライントレースの復帰
                             trace_flag = 1;
@@ -1724,7 +1728,8 @@
                 }
             }
             //リミットがONの時(1回目)
-            if(lateral_frag == 1) {
+            //if(lateral_frag == 1) {
+                /*
                 //ライントレース有効果
                 trace_flag = 1;
                 //ローラー回転移動中
@@ -1742,6 +1747,7 @@
                     else if(side == red) {
                         line_state_pettern  = left_slow;
                     }
+                
                 }
                 else if(back_timer1.read() > 1.5f) {
                     back_timer1.reset();
@@ -1752,9 +1758,10 @@
                 if(limit.read() == 0) {
                     lateral_frag = 3;
                 }
-            }
+            */
+            //}
             //リミットがONの時(2回目)
-            else if(lateral_frag == 3) {
+            if(lateral_frag == 1) {
                 
                 //ローラー回転停止
                 table_fire = 0;
@@ -1778,87 +1785,6 @@
                         line_state_pettern  = right_fast;
                     }
                 }
-                /*
-                //ローラー回転移動中
-                table_fire = medium_table;
-                firing_minimum_distamce = medium_table_minimum_distance;
-                firing_maximum_distance = medium_table_maximum_distance;
-                
-                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) {
-                    //上でback_timer3を使用しているためここでリセットをかける
-                    //back_timer3.reset();
-                    lateral_frag = 5;
-                }
-            }
-            //リミットがONの時(3回目)
-            else if(lateral_frag == 5) {
-                //ローラー回転移動高
-                table_fire = high_table;
-                firing_minimum_distamce = high_table_minimum_distance;
-                firing_maximum_distance = high_table_maximum_distance;
-
-                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) {
-                //ローラー回転停止
-                table_fire = 0;
-
-                //スタートゾーンに近づいたら減速
-                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) < 500) {
-                    line_state_pettern = stop;
-                } else {
-                    if(side == blue) {
-                        line_state_pettern  = left_fast;
-                    }
-                    else if(side == red) {
-                        line_state_pettern  = right_fast;
-                    }
-                }
-            }*/
         }
         
         //テーブルごとに目標値を変えてローラー回転
@@ -1866,8 +1792,8 @@
           //2段低
           case low_grade:
             //ローラー回転開始
-            //roller_PID(810, 683);
-            roller_PID(815, 688);
+            //roller_PID(825, 695);
+            roller_PID(818, 671);
             i2c.write(0x20, front_roller_data, 1, false);
             i2c.write(0x22, back_roller_data,  1, false);
             wait_us(20);
@@ -1923,7 +1849,7 @@
             //青ゾーンでライン検知しないと低速右移動
             case right_slow:
                 //右前、右後ろ、左前、左後ろ
-                right_PID_slow(50, 55, 50, 55);
+                right_PID_slow(50, 53, 56, 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);
@@ -2094,156 +2020,6 @@
                 break;
         }
 
-        /*
-        //前進
-        front_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);
-
-        //後進
-        back_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_PID(255, 300, 255, 300);
-        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);
-
-        //左進行
-        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);
-        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_back_PID(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);
-
-        //斜め左前
-        left_front_PID(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);
-
-        //斜め左後
-        left_back_PID(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);
-
-        //右旋回
-        turn_right_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);
-
-        //左旋回
-        turn_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);
-
-        //前前進後ろ右旋回
-        front_front_back_right_PID(30, 30, 30, 30);
-        true_hidarimae_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);
-
-        //前前進後ろ左旋回
-        front_front_back_left_PID(30, 30, 30, 30);
-        true_migimae_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);
-
-        //前右旋回後ろ前進
-        front_right_back_front_PID(30, 30, 30, 30);
-        true_migiusiro_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);
-
-        //前左旋回後ろ前進
-        front_left_back_front_PID(30, 30, 30, 30);
-        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);
-        */
-
-        /*
-        //ローラーぐるぐる(max930rpm)
-        pc.printf("%3d %3d %3d\n\r", abs(front_roller_rpm), abs(back_roller_rpm), distance);
-        //このパラメータ(距離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);
-        */
-
-        /*
-        //どんどん加速(逆転)
-        for(send_data[0] = 0x84; send_data[0] < 0xFF; send_data[0]++){
-            i2c.write(0x10, send_data, 1);
-            i2c.write(0x12, send_data, 1);
-            i2c.write(0x14, send_data, 1);
-            i2c.write(0x16, send_data, 1);
-            wait(0.1);
-        }
-        //だんだん減速(逆転)
-        for(send_data[0] = 0x0C; send_data[0] > 0x7C; send_data[0]--){
-            //ice(0x88, send_data);
-            //ice(0xA2, send_data);
-
-            i2c.write(0x10, send_data, 1);
-            i2c.write(0x12, send_data, 1);
-            i2c.write(0x14, send_data, 1);
-            i2c.write(0x16, send_data, 1);
-            wait(0.1);
-        }
-        */
     }
 }
+}
\ No newline at end of file