Diff: main.cpp

Revision:

6:5a051a378e42

Parent:

5:167327a82430

Child:

7:7f16fb8b0192

--- 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]++){