Diff: main.cpp

Revision:

3:f1ddc26da601

Parent:

2:3ebca956fd36

Child:

4:0e6997707f43

--- a/main.cpp	Tue Jul 19 03:51:32 2016 +0000
+++ b/main.cpp	Wed Feb 08 15:53:58 2017 +0000
@@ -8,44 +8,186 @@
 S11059 col(p28,p27);
 m3pi m3pi;
 Timer t;
+// Minimum and maximum motor speeds
+#define MAX 1
+#define MIN 0
+
+// PID terms
+#define P_TERM 1
+#define I_TERM 0
+#define D_TERM 20
+
+// PID terms for collision
+#define P_TERM_COL 1
+#define I_TERM_COL 0
+#define D_TERM_COL 20
+
 
 int main() {
     m3pi.cls();
+    //t_for_music.start();
     m3pi.locate(0,0);
-    m3pi.printf("Sensor");
+    m3pi.printf("Battery");
     m3pi.locate(0,1);
-    m3pi.printf("Check");
-    wait(1);
+    float battery = m3pi.battery();
+    m3pi.printf("%.0fmV",battery*1000);
+    wait(3);
+    
+    
+    // distance sensor
     int reading;
     float time[2];
-    int bl=0;
-    char buf[255];
     m3pi.cls();
     rf.VL6180xInit();
     rf.VL6180xDefautSettings();
+    int check = 1;
+    int Sensor_num = 0;
+    int reading_history[3];
+    int i;
+    for(i = 0;i<3;i++){
+        reading_history[i] = 255;
+        }
+    float ave_reading;
+
     
+    rf.triggerDistance();
+    
+    
+    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 derivative,proportional,integral = 0;
+    float power;
+    float power_collision;
+    float speed = MAX;
+    
+    
+    //for color sensor
+    int bl=0;
+    int gr=0;
+    int re=0;
+
+    t.start();
+
     while(1) {
+        Sensor_num = Sensor_num + 1;
         m3pi.cls();
+        //t.start();
+        //time[0] = t.read();
+        col.update();
+        bl=col.b;
+        gr=col.g;
+        re=col.r;
+        
+        // Get the position of the line.
+        current_pos_of_line = m3pi.line_position();        
+        proportional = current_pos_of_line;
+        
+        // Compute the derivative
+        derivative = current_pos_of_line - previous_pos_of_line;
+        
+        // Compute the integral
+        integral += proportional;
+        
+        // Remember the last position.
+        previous_pos_of_line = current_pos_of_line;
+        
+        // Compute the power
+        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
+        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;
+            
+            // limit checks
+            if (right < MIN)
+                right = MIN;
+            else if (right > MAX)
+                right = MAX;
+                
+            if (left < MIN)
+                left = MIN;
+            else if (left > MAX)
+                left = MAX;
+        }
+        
+        // get distance once every three times because of the processing speed
+        if((Sensor_num %3)== 1){    
+            reading = rf.pollDistance();
+            rf.triggerDistance();
+            if(reading < 90 && reading > 0.1){
+                /*m3pi.locate(0,0);
+                m3pi.printf("%dmm",reading);
+                wait(10);*/
+                reading = 90;
+            }else if(reading < 0.1){
+                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;
+        }
+        
+       // 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;
+       // 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 light is on its left side, use light to control 
+        
+        if((gr+re) > 7000 && ave_reading > 190){
+           // 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;
+                
+                // limit checks
+                if (right < MIN)
+                    right = MIN;
+                else if (right > MAX)
+                    right = MAX;
+                    
+                if (left < MIN)
+                    left = MIN;
+                else if (left > MAX)
+                    left = MAX;
+           // }
+        }
+       
+       
+        m3pi.left_motor(left);
+        m3pi.right_motor(right);
+        //wait_ms(1);
+        while(t.read() - time[0] < 0.005){
+            check = 0;
+            }
+        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();
-        rf.triggerDistance();
-        reading = rf.pollDistance();
-        t.stop();
-        time[1] = t.read();
-        col.update();
-        bl = col.b;     
-        int len = 0;
-        m3pi.locate(0,0);     
-        m3pi.printf("%dmm",reading);
-
-//        m3pi.locate(0,1);
-//        m3pi.printf("%d",bl);
-
-        m3pi.locate(0,1);
-        m3pi.printf("%.3fms",(time[1]-time[0])*1000);
-
-        pc.printf("Read %d mm\n\r", reading);
-        m3pi.print(buf,len);
-        wait_ms(100);
     }
 }
\ No newline at end of file