Diff: main.cpp

Revision:

7:7f16fb8b0192

Parent:

6:5a051a378e42

Child:

8:3df97287c825

--- 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);
+        */
         
         /*
         //どんどん加速(逆転)