Diff: main.cpp

Revision:

9:1359f0c813b1

Parent:

8:3df97287c825

Child:

10:b672aa81b226

--- a/main.cpp	Mon Oct 08 07:21:45 2018 +0000
+++ b/main.cpp	Tue Oct 09 06:26:06 2018 +0000
@@ -69,6 +69,17 @@
 #define right_fast 13
 #define left_fast 14
 
+//2段低
+#define low_grade 1
+//2段高
+#define high_grade 2
+//移動低
+#define low_table 3
+//移動中
+#define medium_table 4
+//移動高
+#define high_table 5
+
 //赤ゾーン
 #define red  1
 //青ゾーン
@@ -196,6 +207,19 @@
 //移動(高)乗ったかなフラグ
 bool high_table_flag = 0;
 
+int table_fire = 0;
+/*
+//2段下段撃ったかなフラグ
+bool low_grade_fire_flag = 0;
+//2段上段撃ったかなフラグ
+bool high_grade_fire_flag = 0;
+//移動(低)撃ったかなフラグ
+bool low_table_fire_flag = 0;
+//移動(中)撃ったかなフラグ
+bool medium_table_fire_flag = 0;
+//移動(高)撃ったかなフラグ
+bool high_table_fire_flag = 0;
+*/
 
 static int mouted_bottles = 0;
 int loading_state = 0;
@@ -208,6 +232,11 @@
 static int i = 0;
 int lateral_frag = 0;
 
+/*
+int bottles_flag[20] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                        0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+*/
+
 int migimae_rpm;
 int migiusiro_rpm;
 int hidarimae_rpm;
@@ -1386,35 +1415,40 @@
             myled5 = 1;
         }
 
+        /*
+        //2段高ローラー回転
+        if(high_grade_fire_flag == 0) {
+          //ローラー回転開始
+          roller_PID(1006, 928);
+          i2c.write(0x20, front_roller_data, 1, false);
+          i2c.write(0x22, back_roller_data,  1, false);
+          wait_us(20);
+        }
+        */
+
         //フォトインタラプタ検知で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);
-
+                i2c.write(0x30, loading_data, 1, false);
             } else {
                 loading_data[0] = 0x80;
-                i2c.write(0x30, loading_data, 1);
+                i2c.write(0x30, loading_data, 1, false);
             }
         }
 
         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());
 
         //スタートボタンで動作開始
         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);
-
             //2段下段
             if(low_grade_flag == 0) {
                 //パルスがnを満たすまで無条件で横移動
@@ -1434,31 +1468,45 @@
                     //else if(start_timer.read() > 2.0f) {
                     */
                 //一定の距離を超えたらゆっくり移動してライン見つけたら(ry
-                //} else {  
+                //} else {
+
                     //距離が11cm~29cmだったらトレースせずに停止
-                    if(distance >= 20 && distance <= 25) {
+                    if(distance >= 20 && distance <= 30) {
                         //タイマスタート
                         timer.start();
                         //トレース方向の反転
                         trace_direction = 1;
-
-                        //発射距離に到達して1秒待って発射
-                        if(timer.read() > 1.0f && timer.read() <= 1.5f) {
+                        /*
+                        if(timer.read() < 1.0f) {
+                            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);
-                            mouted_bottles = mouted_bottles + 1;
+                            mouted_bottles++;
+                        }
+
+                        */
+                        //発射距離に到達して1秒待って発射
+                        if(timer.read() > 1.0f && timer.read() <= 1.01f) {
+                            cylinder_data[0] = 0x0F;
+                            i2c.write(0x40, cylinder_data, 1);
+                            //2段下段撃ったよ-
+                            //high_grade_fire_flag = 1;
+                            mouted_bottles++;
                         }
                         //発射して1秒たったら後進開始
-                        else if(timer.read() > 1.5f) {
+                        else if(timer.read() > 1.01f) {
                             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 {
                         //タイマリセット
@@ -1538,7 +1586,7 @@
                                         line_state_pettern = back_trace;
                                     }
                                     //25cmより遠くて前進
-                                    else if(distance > 25) {
+                                    else if(distance > 30) {
                                         line_state_pettern = front_trace;
                                     } else {
                                         line_state_pettern = front_trace;
@@ -1581,13 +1629,18 @@
 
             //lateral_fragが0(初期状態)の時
             if(lateral_frag == 0) {
-            //リミットONでlateral_frag = 1
+                //ローラー回転2段高
+                table_fire = low_grade;
+                //リミットONでlateral_frag = 1
                 if(limit.read() == 0) {
                     lateral_frag = 1;
                 }
             }
             //リミットがONの時(1回目)
             if(lateral_frag == 1) {
+                //ローラー回転移動中
+                table_fire = low_table;
+
                 //トレース方向の反転(前進)
                 trace_direction = 0;
                 back_timer1.start();
@@ -1611,6 +1664,9 @@
             }
             //リミットがONの時(2回目)
             else if(lateral_frag == 3) {
+                //ローラー回転移動中
+                table_fire = medium_table;
+
                 trace_direction = 0;
                 back_timer2.start();
                 if(back_timer2.read() <= 0.8f && back_timer2.read() != 0.0f) {
@@ -1635,6 +1691,9 @@
             }
             //リミットがONの時(3回目)
             else if(lateral_frag == 5) {
+                //ローラー回転移動高
+                table_fire = high_table;
+
                 trace_direction = 0;
                 back_timer3.start();
                 if(back_timer3.read() <= 0.8f && back_timer3.read() != 0.8f) {
@@ -1657,6 +1716,9 @@
             }
             //リミットがONの時(4回目)
             else if(lateral_frag == 7) {
+                //ローラー回転停止
+                table_fire = 0;
+
                 //スタートゾーンに近づいたら減速
                 if(abs(measure_pulse) < 1200) {
                     if(side == blue) {
@@ -1678,250 +1740,62 @@
                 }
             }
         }
-
-        /*
-        //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--;
-                        }
-                        //発射して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 {
-                        //タイマリセット
-                        timer.reset();
+        //テーブルごとに目標値を変えてローラー回転
+        switch(table_fire) {
+          //2段低
+          case low_grade:
+            //ローラー回転開始
+            roller_PID(200, 200);
+            i2c.write(0x20, front_roller_data, 1, false);
+            i2c.write(0x22, back_roller_data,  1, false);
+            wait_us(20);
+            break;
 
-                        //ライン検知するまで右移動
-                        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)) {
-                                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)) {
+          //2段高
+          case high_grade:
+            //ローラー回転開始
+            roller_PID(400, 400);
+            i2c.write(0x20, front_roller_data, 1, false);
+            i2c.write(0x22, back_roller_data,  1, false);
+            wait_us(20);
+            break;
 
-                            //前が真ん中で後ろが右寄りの時前前進後ろ右旋回
-                            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)) {
+          //移動低
+          case low_table:
+            //ローラー回転開始
+            roller_PID(600, 600);
+            i2c.write(0x20, front_roller_data, 1, false);
+            i2c.write(0x22, back_roller_data,  1, false);
+            wait_us(20);
+            break;
 
-                            //前が左寄りで後ろが右寄りの時右旋回
-                            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;
-            }
+            //移動中
+          case medium_table:
+            //ローラー回転開始
+            roller_PID(800, 800);
+            i2c.write(0x20, front_roller_data, 1, false);
+            i2c.write(0x22, back_roller_data,  1, false);
+            wait_us(20);
+            break;
 
-            //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(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) {
-                    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(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;
-                    }
-                }
-            }
+          //移動高
+          case high_table:
+            //ローラー回転開始
+            roller_PID(1000, 1000);
+            i2c.write(0x20, front_roller_data, 1, false);
+            i2c.write(0x22, back_roller_data,  1, false);
+            wait_us(20);
+            break;
+
+          //それ以外ショートブレーキ
+          default:
+            front_roller_data[0] = 0x80;
+            back_roller_data[0]  = 0x80;
+            i2c.write(0x20, front_roller_data, 1, false);
+            i2c.write(0x22, back_roller_data,  1, false);
+            wait_us(20);
+            break;
         }
-        */
 
         switch(line_state_pettern) {
             //青ゾーンでライン検知しないと低速右移動
@@ -2071,6 +1945,7 @@
                 i2c.write(0x16, true_hidariusiro_data, 1, false);
                 wait_us(20);
                 break;
+
             case stop:
               true_migimae_data[0]     = 0x80;
               true_migiusiro_data[0]   = 0x80;