Revision 20:e844f2e48691, committed 2015-05-29

Comitter:

crmackey

Date:

Fri May 29 14:08:07 2015 +0000

Parent:

18:1dc7fdc248aa

Commit message:

Works now for real

Changed in this revision

--- a/main.cpp	Fri May 29 13:41:58 2015 +0000
+++ b/main.cpp	Fri May 29 14:08:07 2015 +0000
@@ -2,16 +2,6 @@
 #include "m3pi_ng.h"
 #include "time.h"
 #include "btbee.h"
-#include <string>
-#include <sstream>
-
-using namespace std;
-
-//For USB writing
-#include "MSCFileSystem.h"
-#define FSNAME "msc"
-MSCFileSystem msc(FSNAME);
-
 
 m3pi m3pi;
 btbee btbee;
@@ -25,25 +15,14 @@
 #define MIN 0
 
 // PID terms
-double P_TERM = 1.5;
-double I_TERM = 0;
-double D_TERM = 25;
-char arr_read[30];
-int  chars_read;
+#define P_TERM 1.5
+#define I_TERM 0
+#define D_TERM 25
 
 int main() {
     
-    DIR *d;
-    struct dirent *p;
-   
-    d = opendir("/" FSNAME);
     
-    FILE *fp = fopen( "/" FSNAME "/Fertig.txt", "w");
-    if ( fp == NULL )
-    {
-        m3pi.leds(1);
-    }
-    
+while(1) {
     
     btbee.reset();
     m3pi_pb.mode(PullUp);
@@ -62,7 +41,6 @@
     m3pi.cls();
     
     Timer LapTimer;
-    Timer LoopTime;
     char Bat[] = {'V','o','l','t',' ','i','s'};
   m3pi.print(Bat,7);
   wait(0.75);
@@ -89,7 +67,7 @@
     m3pi.printf("Line PID");
 
     wait(2.0);
- 
+
     btbee.printf("Now calibrating \n");
 
     m3pi.sensor_auto_calibrate();
@@ -109,33 +87,26 @@
     int counter = -1;
     int checkvar = 0;
     double Time1, Time2, Time3, Time4, Time5, TimeAve;
-    bool IR = 1;
-    double timer;
     btbee.printf("\n");
     btbee.printf("Now starting \n");
     
+    
+    
+    
     while (1) {
         
-    LoopTime.start();
-        
-    IR = m3pi_IN [0];
-        
-    if(m3pi_IN [0] == 0 and IR == 0) {
-        while (m3pi_IN [0] == 0 and IR == 0) {
-    m3pi.cls();
-    btbee.printf("Obstacle detected. Ending program");
-    m3pi.printf("ABORT");
-    m3pi.stop();
-    exit(1);
-    }
-}
-
+    //if(m3pi_IN [0] == 0) {
+//        while (m3pi_IN [0] == 0) {
+//    btbee.printf("Obstacle detected. Please remove to continue");
+//    m3pi.stop();
+//    wait(3.0);
+//    }
+//}
         
        // 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;
         
@@ -167,159 +138,83 @@
         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);
         }
-       
-//    if (s1 < 500 and s5 < 500 and checkvar == 1 and counter == -1) {
-//        LapTimer.reset();
-//        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 \n", TimeAve);
-//        btbee.printf("\n");
-//        btbee.printf("Enter P value, I value, and D value separately \n");
-//                
-//                while(1) {
-//                    
-//                        
-//                        if (btbee.readable()) {
-//                            
-//                          btbee.printf("Enter P valure: \n");
-//            
-//            
-//                          btbee.read_all(arr_read, 30, &chars_read);
-//                           P_TERM = atof(arr_read);
-//            
-//                          btbee.printf("P term is %f \n", P_TERM);
-//            
-//                          wait(1.0);
-//            
-//            break;
-//            
-//                                             }
-//            
-//                          }
-//            
-//            while(1) {
-//                            
-//            if(btbee.readable()) {
-//                
-//                btbee.printf("Enter I value: \n");
-//            
-//                btbee.read_all(arr_read, 30, &chars_read);
-//                I_TERM = atof(arr_read);
-//            
-//                btbee.printf("I term is %f \n", I_TERM);
-//            
-//                 wait(1.0);
-//            
-//                 break;
-//            
-//                                    }
-//                                    
-//                        }
-//                        
-//            while(1)  {
-//                
-//                if(btbee.readable()) {
-//                    
-//                    btbee.printf("Enter D value: \n");
-//            
-//                    btbee.read_all(arr_read, 30, &chars_read);
-//                    D_TERM = atof(arr_read);
-//            
-//                    btbee.printf("D term is %f \n", D_TERM);
-//                    
-//                    wait(1);
-//                    
-//                    btbee.printf("Starting again with new parameters");
-//            
-//                    counter = -1;
-//            
-//            
-//                     break;
-//            
-//                                    }
-//            
-//                      }
-//              
-//        }
-                        
-                                        
-        
 
-    //if(timer < 0.005) {
-//        wait(0.005 - timer);
-//        }
-//    timer = 0;
-//    LoopTime.reset();
 
-    fprintf(fp,"LOL");
-    fclose(fp); 
     }
     
 }
+
+}