Diff: main.cpp

Revision:

12:88432f588cfd

Parent:

8:3266cc9eda81

--- a/main.cpp	Thu Dec 08 21:21:38 2016 +0000
+++ b/main.cpp	Thu Dec 08 22:22:45 2016 +0000
@@ -35,6 +35,10 @@
 int volatile current_screen = 0;
 bool volatile screen_change = false;
 void timeToNextClass();
+float displacement(float diffLat, float diffLong);// calculates the distance to destination
+float calc_eta(float dis, float speed);//(miles,mph) returns in hours
+bool late(float eta);// returns 1 if late
+
 
 void left_callback(void)
 {
@@ -139,7 +143,21 @@
             currSecond = myGPS.seconds;
             nextClass = getNextClass(courseVec);
             timeToNextClass();
-
+            
+            destinationLat = courseVec[nextClass].getLat();
+            destinationLong = courseVec[nextClass].getLong();
+            float diffLat = destinationLat - myLat;
+            float diffLong = destinationLong - myLong;
+            float distToDest = displacement(diffLat,diffLong);//(miles)
+            float speed = 3.1;// (mph)Source: taken from google
+            float eta = calc_eta(distToDest, speed*60);
+            if(late(eta)==1)
+            {
+                uLCD.background_color(RED);
+            }
+            else{
+                uLCD.background_color(BLACK);
+            }
             switch(current_screen) {
                 case 0:
                     wait(.2);
@@ -158,10 +176,6 @@
                     IMU.readMag();
                     h = calculateHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
                     pc.printf("Heading: %f\n", h);
-                    destinationLat = courseVec[nextClass].getLat();
-                    destinationLong = courseVec[nextClass].getLong();
-                    float diffLat = destinationLat - myLat;
-                    float diffLong = destinationLong - myLong;
                     angleToDest = computeAngleToDestination(diffLat, diffLong);
                     h = angleToDest - h;
                     h = h - 90;
@@ -343,6 +357,19 @@
     return angle;
 }
 
+float displacement(float diffLat, float diffLong)// calculates the distance to destination
+{
+    float d = sqrt(diffLat*diffLat+diffLong*diffLong);
+    return d;
+}
+
+
+float calc_eta(float dis, float speed)
+{
+    float eta = dis/speed; // for displacement. take average speed // convert to minutes later
+    eta += 0.25*eta; //adding delay for distance vs displacement and traffic lights
+    return eta;//hours
+}
 int getNextClass(vector<Course>& cVec)
 {
     int numIterations = 0;
@@ -411,4 +438,16 @@
     temp = temp / .6;
     longInDec = floor(longInDec) + temp;
     return -1 * longInDec;
+}
+
+bool late(float eta)// returns 1 if late
+{   
+    float totalTime =  hoursToNextClass*60+ minutesToNextClass+ secondsToNextClass/60;
+    if(eta<totalTime)
+    {   
+        return 1;
+    }else
+    {
+        return 0;
+    }
 }
\ No newline at end of file