Diff: main.cpp

Revision:

0:4d1ae7eb0070

Child:

1:936e329e840a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri May 22 13:34:01 2015 +0000
@@ -0,0 +1,212 @@
+#include "mbed.h"
+#include "m3pi_ng.h"
+#include "time.h"
+#include "btbee.h"
+
+m3pi m3pi;
+btbee btbee;
+DigitalIn m3pi_pb(p21);
+DigitalIn m3pi_IN[] = {(p12),(p21)};
+DigitalOut mbed_led[] = {(LED1), (LED2),(LED3), (LED4)};
+DigitalOut m3pi_led[] = {(p13), (p14), (p15), (p16), (p17), (p18), (p19), (p20)};
+
+// Minimum and maximum motor speeds
+#define MAX 1.0
+#define MIN 0
+
+// PID terms
+#define P_TERM 1.5
+#define I_TERM 0
+#define D_TERM 25
+
+int main() {
+    
+
+    
+    btbee.reset();
+    m3pi_pb.mode(PullUp);
+    
+    m3pi.printf("Wait 4");
+    m3pi.locate(0,1);
+    m3pi.printf("PC");
+    
+    while(m3pi_pb) {
+        m3pi_led[0]=!m3pi_led[0];
+        wait(3);
+        btbee.printf("\n");
+        btbee.printf("PC connected. Press the button \n");
+    }
+    
+    
+    
+    Timer LapTimer;
+    char Bat[] = {'V','o','l','t',' ','i','s'};
+  m3pi.print(Bat,7);
+  wait(0.75);
+  m3pi.cls();
+   float batteryvoltage = m3pi.battery();
+   char* str = new char[30];
+   sprintf(str, "%.4g", batteryvoltage);
+   m3pi.print(str,6);
+   btbee.printf("Battery voltage is %f \n", batteryvoltage);
+   wait(0.75);
+   m3pi.cls();
+   
+    if (batteryvoltage < 4.2)
+     {
+    char low[] = {'L','o','w',' ','b','a','t'};
+    m3pi.print(low,7);
+    char ExitSound[]={'V','1','5','O','6','E','4','O','5','E','4'};
+    m3pi.playtune(ExitSound,11);
+    btbee.printf("Battery voltage is too low. Stopping program");
+    exit(1);
+     }
+
+    m3pi.locate(0,1);
+    m3pi.printf("Line PID");
+
+    wait(2.0);
+
+    btbee.printf("Now calibrating \n");
+
+    m3pi.sensor_auto_calibrate();
+    
+    btbee.printf("Finished calibrating \n");
+    
+    if (m3pi_IN [0] == 0) {
+    btbee.printf("Obstacle detected. Stopping program");
+    exit(1);
+    }
+
+    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 speed = MAX;
+    int LapTest[5];
+    int s1, s2, s3, s4, s5;
+    int counter = -1;
+    int checkvar = 0;
+    double Time1, Time2, Time3, Time4, Time5, TimeAve;
+    btbee.printf("\n");
+    btbee.printf("Now starting \n");
+    
+    
+    while (1) {
+    
+
+        // 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)) ;
+        
+        // Compute new speeds   
+        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;
+            
+       // set speed 
+        m3pi.left_motor(left);
+        m3pi.right_motor(right);
+        
+    m3pi.calibrated_sensor(LapTest);
+    
+    s1 = LapTest[0];
+    s2 = LapTest[1];
+    s3 = LapTest[2];
+    s4 = LapTest[3];
+    s5 = LapTest[4];
+    
+    if (s1 > 500 and s5 > 500) {
+        checkvar = 1;
+        }
+        
+    if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
+        LapTimer.start();
+        }
+        
+    if (s1 < 500 and s5 < 500 and checkvar == 1) {
+        counter = counter + 1;
+        m3pi.cls();
+        m3pi.printf("%d", counter);
+        checkvar = 0;
+        switch (counter) {
+            case 1: {
+                Time1 = LapTimer.read();
+                LapTimer.reset();
+                LapTimer.start();
+                btbee.printf("Lap 1: %f \n", Time1);
+                break;
+                }
+            case 2: {
+                Time2 = LapTimer.read();
+                LapTimer.reset();
+                LapTimer.start();
+                btbee.printf("Lap 2: %f \n", Time2);
+                break;
+                }
+            case 3: {
+                Time3 = LapTimer.read();
+                LapTimer.reset();
+                LapTimer.start();
+                btbee.printf("Lap 3: %f \n", Time3);
+                break;
+                }
+            case 4: {
+                Time4 = LapTimer.read();
+                LapTimer.reset();
+                LapTimer.start();
+                btbee.printf("Lap 4: %f \n", Time4);
+                break;
+                }
+            case 5: {
+                Time5 = LapTimer.read();
+                LapTimer.reset();
+                LapTimer.start();
+                btbee.printf("Lap 5: %f \n", Time5);
+                break;
+                }
+            }
+                
+        
+        }
+        
+    if (counter == 5) {
+        m3pi.stop();
+        m3pi.cls();
+        m3pi.locate(0,0);
+        m3pi.printf("Ave time");
+        TimeAve = (Time1 + Time2 + Time3 + Time4 + Time5)/5.0;
+        m3pi.locate(0,1);
+        m3pi.printf("%f", TimeAve);
+        btbee.printf("Average time per lap: %f ", TimeAve);
+        exit(1);
+        }
+
+
+    }
+    
+}
\ No newline at end of file