Diff: main.cpp

Revision:

4:0e6997707f43

Parent:

3:f1ddc26da601

diff -r f1ddc26da601 -r 0e6997707f43 main.cpp
--- a/main.cpp	Wed Feb 08 15:53:58 2017 +0000
+++ b/main.cpp	Wed Mar 29 03:40:18 2017 +0000
@@ -3,62 +3,96 @@
 #include "m3pi.h"
 #include "S11059.h"
 
+//必要なオブジェクトを作る
 VL6180x rf(p28, p27); //I2C sda and scl
 Serial pc(USBTX, USBRX); //USB serial
 S11059 col(p28,p27);
 m3pi m3pi;
 Timer t;
+
 // Minimum and maximum motor speeds
-#define MAX 1
-#define MIN 0
+#define MAX 1 //速度の最大値
+#define MIN 0 //速度の最小値（バックはしません）
 
-// PID terms
+// PID terms ライントレースに必要なPID項
+//このプログラムでは車両の下に存在する黒いテープの車両の中心からのズレを状態として、PIDで制御します。
 #define P_TERM 1
 #define I_TERM 0
 #define D_TERM 20
 
-// PID terms for collision
+// PID terms 車両が衝突回避を行っているときのライントレースのPID項（速度がおそくなるから値を変えてます）
 #define P_TERM_COL 1
 #define I_TERM_COL 0
 #define D_TERM_COL 20
 
+//リニア信号機，前方車両，ライントレースから計算される速度から車両の速度（左右のタイヤにかかる電圧）を決定するための最も遅い速度のインデックスを返す関数
+int min_index(float a,float b,float c,float d,float e,float f){
+    float sum1 = a + b;
+    float sum2 = c + d;
+    float sum3 = e + f;
+
+    if(sum1<sum2 && sum1<sum3){
+        return 1;
+    }else if(sum1>sum2 && sum3>sum2){
+        return 2;
+    }else{
+        return 3;
+    }
+}
 
 int main() {
     m3pi.cls();
     //t_for_music.start();
-    m3pi.locate(0,0);
-    m3pi.printf("Battery");
+    //一秒間スクリーンに文字を表示する
+    m3pi.locate(0,0); //指定した位置（0,0）に
+    m3pi.printf("Battery"); //指定した文字"Battery"を表示する
     m3pi.locate(0,1);
     float battery = m3pi.battery();
     m3pi.printf("%.0fmV",battery*1000);
-    wait(3);
+    wait(1);
     
     
     // distance sensor
-    int reading;
-    float time[2];
-    m3pi.cls();
+    int reading; //前方車両との距離を格納する変数
+    float time[2]; //サンプリング時間を一定にするための配列
+    //おまじない
+    m3pi.cls(); 
     rf.VL6180xInit();
     rf.VL6180xDefautSettings();
-    int check = 1;
-    int Sensor_num = 0;
-    int reading_history[3];
+    
+    int check = 1; //サンプリリング時間が乱れた時にシステムを止めるための変数
+    int Sensor_num = 0;//距離センサは取得に時間がかかるため3ステップに一回値を取得することにするので、そのための値
+    int reading_history[3]; //ノイズがあるため前方車両との距離は最新の3つの値の平均をとるのでそのための配列
     int i;
+    //
     for(i = 0;i<3;i++){
-        reading_history[i] = 255;
+        reading_history[i] = 255;//初期値はセンサの最大値
         }
-    float ave_reading;
+    float ave_reading = 255;
 
-    
+    //おまじない
     rf.triggerDistance();
+    m3pi.sensor_auto_calibrate();
     
     
-    m3pi.sensor_auto_calibrate();
     // variables for PID
-    float right;
-    float left;
-    float current_pos_of_line = 0.0;
-    float previous_pos_of_line = 0.0;
+    //左右のタイヤにかかる電圧を格納する変数
+    float right;//右のタイヤの最終的な電圧（下記の三つの値から最終的にきめる）
+    float left;//左のタイヤの最終的な電圧（下記の三つの値から最終的にきめる）
+    
+    float right1;//ライントレースからの右のタイヤの電圧
+    float left1;//ライントレースからの左のタイヤの電圧
+    float right2;//前方車両から計算される右のタイヤの電圧
+    float left2;//前方車両から計算される左のタイヤの電圧
+    float right3;//リニア信号機から受け取る右のタイヤの電圧
+    float left3;//リニア信号機から受け取る左のタイヤの電圧
+    
+    int mode = 1;
+    
+    
+    //ライントレースに必要な変数
+    float current_pos_of_line = 0.0;//現在の車両のライン上のポジション
+    float previous_pos_of_line = 0.0;//過去の車両のライン上のポジション
     float derivative,proportional,integral = 0;
     float power;
     float power_collision;
@@ -68,20 +102,32 @@
     //for color sensor
     int bl=0;
     int gr=0;
-    int re=0;
+    int re=0;   
 
-    t.start();
+    t.start();//サンプリング時間を一定に保つためタイマーを使い時間を計るので、タイマーをスタートさせる。
+    time[0] = t.read();//スタート時間を格納する
 
     while(1) {
-        Sensor_num = Sensor_num + 1;
+        
+        Sensor_num = Sensor_num + 1;//距離センサは取得に時間がかかるため3ステップに一回値を取得することにするので、そのための値
         m3pi.cls();
         //t.start();
         //time[0] = t.read();
         col.update();
+        //カラーセンサからのRGB値をそれぞれの変数に格納する。
         bl=col.b;
         gr=col.g;
         re=col.r;
         
+        //左右のモータの電圧を初期化
+        right1 = MAX;
+        right2 = MAX;
+        right3 = MAX;
+        left1 = MAX;
+        left2 = MAX;
+        left3 = MAX;
+        
+        //ライントレースするための左右のモータの電圧の計算
         // Get the position of the line.
         current_pos_of_line = m3pi.line_position();        
         proportional = current_pos_of_line;
@@ -100,23 +146,23 @@
         power_collision = (proportional * (P_TERM_COL) ) + (integral*(I_TERM_COL)) + (derivative*(D_TERM_COL)) ;
         
         // Compute new speeds if there is nothing ahead   
-        if(ave_reading >= 190 && (gr+re) <= 7000){
-            right = speed+power;
-            left  = speed-power;
+        if(ave_reading >= 190 && (gr+re) <= 10000){
+            right1 = speed+power;
+            left1  = speed-power;
             
             // limit checks
-            if (right < MIN)
-                right = MIN;
-            else if (right > MAX)
-                right = MAX;
-                
-            if (left < MIN)
-                left = MIN;
-            else if (left > MAX)
-                left = MAX;
+            if (right1 < MIN)
+                right1 = MIN;
+            else if (right1 > MAX)
+                right1 = MAX;
+            if (left1 < MIN)
+                left1 = MIN;
+            else if (left1 > MAX)
+                left1 = MAX;
         }
         
         // get distance once every three times because of the processing speed
+        //3ステップに一回、車両は前方車両との距離を取得する。
         if((Sensor_num %3)== 1){    
             reading = rf.pollDistance();
             rf.triggerDistance();
@@ -125,69 +171,89 @@
                 m3pi.printf("%dmm",reading);
                 wait(10);*/
                 reading = 90;
-            }else if(reading < 0.1){
+            }else if(reading < 0.1){//たまにセンサの不具合により距離0を返すときがあるのでそれを除外。
                 reading = 255;
                 }
             reading_history[0] = reading_history[1];
             reading_history[1] = reading_history[2];
             reading_history[2] = reading;
-            ave_reading = float(reading_history[0]+reading_history[1]+reading_history[2])/3;
+            ave_reading = float(reading_history[0]+reading_history[1]+reading_history[2])/3;//三回取得した平均値を前方車両との距離とする
         }
         
        // if distance is too close, change the speed
+       //前方車両との距離から左右のモータの電圧を計算
         if(ave_reading < 190){
-            right = ((ave_reading - 90)/100) + ((ave_reading - 90)/100) * power_collision;
-            left = ((ave_reading - 90)/100) - ((ave_reading - 90)/100) * power_collision;
+            right2 = ((ave_reading - 90)/100) + ((ave_reading - 90)/100) * power_collision;
+            left2 = ((ave_reading - 90)/100) - ((ave_reading - 90)/100) * power_collision;
        // limit checks
-            if (right < MIN)
-                right = MIN;
-            else if (right > MAX)
-                right = MAX;
+            if (right2 < MIN)
+                right2 = MIN;
+            else if (right2 > MAX)
+                right2 = MAX;
                 
-            if (left < MIN)
-                left = MIN;
-            else if (left > MAX)
-                left = MAX;
+            if (left2 < MIN)
+                left2 = MIN;
+            else if (left2 > MAX)
+                left2 = MAX;
        }
        
        //if light is on its left side, use light to control 
-        
-        if((gr+re) > 7000 && ave_reading > 190){
+        //カラーセンサから得られるRGB値から左右のモータの電圧を計算する。
+        if((gr+re) > 10000){
            // if(Sensor_num%3 == 2){
                 double light_position = (((double)gr)/((double) (re+gr)));
                 double light_speed = light_position*(10.0/7.0)-(2.0/7.0);
-                right = light_speed + light_speed*power_collision;
-                left = light_speed - light_speed*power_collision;
+                right3 = light_speed + light_speed*power_collision;
+                left3 = light_speed - light_speed*power_collision;
                 
                 // limit checks
-                if (right < MIN)
-                    right = MIN;
-                else if (right > MAX)
-                    right = MAX;
+                if (right3 < MIN)
+                    right3 = MIN;
+                else if (right3 > MAX)
+                    right3 = MAX;
                     
-                if (left < MIN)
-                    left = MIN;
-                else if (left > MAX)
-                    left = MAX;
+                if (left3 < MIN)
+                    left3 = MIN;
+                else if (left3 > MAX)
+                    left3 = MAX;
            // }
         }
-       
-       
+        
+        //上記で計算された左右のモータにかかる電圧（３つ）から最終的な電圧を計算する。
+        mode = min_index(right1,left1,right2,left2,right3,left3);
+        
+        if(mode == 1){//（ライントレースから求められた値）
+            right = right1;
+            left = left1;
+        }else if(mode == 2){//（前方車両との距離から求められた値）
+            right = right2;
+            left = left2;
+        }else{//（カラーセンサのRGB値より求められた値）
+            right = right3;
+            left = left3;
+        }
+        
+        //求められた左右のモータの電圧を入力
         m3pi.left_motor(left);
         m3pi.right_motor(right);
         //wait_ms(1);
+        
+        //サンプル時間が一定に保たれているかチェック
         while(t.read() - time[0] < 0.005){
             check = 0;
             }
-        if(check == 1){
+        if(check == 1){//保たれていなければストップ
             m3pi.locate(0,0);
             m3pi.printf("%dmm",reading);
             wait(1);
             m3pi.stop();
             }
         check = 1;
+        
+        //タイマーストップ
         t.stop();
+        //タイマースタート（次のステップのサンプル時間をチェックするため）
         t.start();
-        time[0] = t.read();
+        time[0] = t.read();//（スタート時間をチェック）
     }
 }
\ No newline at end of file