Diff: main.cpp

Revision:

3:0ca91f8afec5

Parent:

2:c708e2027970

Child:

4:93a4b415fe6c

--- a/main.cpp	Thu Dec 08 01:42:35 2016 +0000
+++ b/main.cpp	Thu Dec 08 14:15:25 2016 +0000
@@ -5,19 +5,21 @@
 #include <vector>
 #include "Course.h"
 #include "SDFileSystem.h"
+#include "LSM9DS1.h"
 #include "PinDetect.h"
+#define PI 3.14159
+#define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
 
 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
 Serial pc(USBTX, USBRX);
 Serial esp(p28, p27); // tx, rx
-uLCD_4DGL uLCD(p9,p10,p11); // serial tx, serial rx, reset pin;
-//Nav_Switch myNav( p18, p15, p16, p14, p17);
-PinDetect left(p16);
-PinDetect right(p14);
-PinDetect center(p17);
+uLCD_4DGL uLCD(p13,p14,p15); // serial tx, serial rx, reset pin;
+PinDetect left(p20);
+PinDetect right(p18);
+PinDetect center(p19);
 
-char ssid[32] = "iPhone";     // enter WiFi router ssid inside the quotes
-char pwd [32] = "cdc4012zarus"; // enter WiFi router password inside the quotes
+char ssid[32] = "";     // enter WiFi router ssid inside the quotes
+char pwd [32] = ""; // enter WiFi router password inside the quotes
 
 // for sending/receiving data over serial
 volatile int tx_in=0;
@@ -40,9 +42,12 @@
 vector<Course> courseVec;
 void readClassFile(vector<Course>& cVec), writeClassFile(vector<Course>& cVec);
 void displayCourseVec();
+float calculateHeading(float mx, float my), computeAngleToDestination(float diffLat, float diffLong);
+int xEnd, yEnd, lastXEnd, lastYEnd = 0.0;
 char rx_line[512];
 string ip_line;
 bool ip_found = false;
+bool screen_refreshed = false;
 int volatile current_screen = 0;
 bool volatile screen_change = false;
 int port        =80;  // set server port
@@ -50,23 +55,52 @@
 
 void left_callback(void)
 {
-    current_screen = (3 + current_screen - 1)%3;
-    screen_change = true;
+    if (courseVec.size() != 0) {
+        current_screen = (3 + current_screen - 1)%3;
+        screen_change = true;
+    } else
+        current_screen = 0;
 }
 
 void right_callback(void)
 {
-    current_screen = (3 + current_screen + 1)%3;
-    screen_change = true;
+    if (courseVec.size() != 0) {
+        current_screen = (3 + current_screen + 1)%3;
+        screen_change = true;
+    } else
+        current_screen = 0;
 }
 
 void center_callback(void)
 {
-
+    if (current_screen == 0) {
+        courseVec.clear();
+        update = 1;
+    }
 }
 
 int main()
 {
+    LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
+    float h = 0.0;
+    float angleToDest = 0.0;
+    IMU.begin();
+    if (!IMU.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1.\n");
+    }
+    uLCD.cls();
+    uLCD.printf("Place IMU flat");
+    IMU.calibrate(1);
+    uLCD.cls();
+    uLCD.printf("Rotate IMU 360\ndegrees in \nhorizontal plane");
+    IMU.calibrateMag(0);
+
+    float myLat = 33.776346;
+    float myLong = -84.378927;
+
+    float destinationLat = 33.772778;
+    float destinationLong = -84.379685;
+
     left.mode(PullUp);
     right.mode(PullUp);
     center.mode(PullUp);
@@ -80,10 +114,6 @@
 
     readClassFile(courseVec);
 
-    courseVec.clear();
-
-    displayCourseVec();
-
     pc.baud(9600);
     esp.baud(9600);
     // Setup a serial interrupt function to receive data
@@ -94,31 +124,86 @@
     DataRX=0;
 
     while(1) {
-        uLCD.locate(0, 8);
-        uLCD.printf("%i", current_screen);
         switch(current_screen) {
             case 0:
                 if (screen_change) {
                     displayCourseVec();
+                    screen_refreshed = true;
                     screen_change = false;
                 }
                 if(DataRX==1) {
                     ReadWebData();
                     esp.attach(&Rx_interrupt, Serial::RxIrq);
-                    displayCourseVec();
+                    if (!screen_refreshed) {
+                        displayCourseVec();
+                        screen_refreshed = true;
+                    }
                 }
                 if(update==1) {
                     writeClassFile(courseVec);
-                    displayCourseVec();
+                    if (!screen_refreshed) {
+                        displayCourseVec();
+                    }
                     update=0;
                 }
+                screen_refreshed = false;
                 break;
             case 1:
                 if (screen_change) {
-                    uLCD.cls();
                     uLCD.printf("Compass screen");
                     screen_change = false;
                 }
+
+                IMU.readMag();
+                h = calculateHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
+                destinationLat = courseVec[0].getLat();
+                destinationLong = courseVec[0].getLong();
+                float diffLat = destinationLat - myLat;
+                float diffLong = destinationLong - myLong;
+                angleToDest = computeAngleToDestination(diffLat, diffLong);
+                h = angleToDest - h;
+                h = h - 90;
+                if (h < 0)
+                    h = h + 360;
+                if (h > 360)
+                    h = h - 360;
+                xEnd = 0;
+                yEnd = 0;
+                float rads = 0.0;
+                //uLCD.cls();
+                if (h < 90) {
+                    rads = h * PI / 180;
+                    xEnd = floor(63 * sin(rads) + .5);
+                    yEnd = floor(63 * cos(rads) + .5);
+                    xEnd = 63 + xEnd;
+                    yEnd = 63 + yEnd;
+                } else if (90 < h < 180) {
+                    h = h - 90;
+                    rads = h * PI / 180;
+                    xEnd = floor(63 * cos(rads) + .5);
+                    yEnd = floor(63 * sin(rads) + .5);
+                    xEnd = 63 + xEnd;
+                    yEnd = 63 - yEnd;
+                } else if (180 < h < 270) {
+                    h = h - 180;
+                    rads = h * PI / 180;
+                    xEnd = floor(63 * sin(rads) + .5);
+                    yEnd = floor(63 * cos(rads) + .5);
+                    xEnd = 63 - xEnd;
+                    yEnd = 63 - yEnd;
+                } else if (270 < h) {
+                    h = h - 270;
+                    rads = h * PI / 180;
+                    xEnd = floor(63 * cos(rads) + .5);
+                    yEnd = floor(63 * sin(rads) + .5);
+                    xEnd = 63 - xEnd;
+                    yEnd = 63 + yEnd;
+                }
+                //uLCD.line(63, 63, lastXEnd, lastYEnd, BLACK);
+                uLCD.cls();
+                uLCD.line(63, 63, xEnd, yEnd, WHITE);
+                lastXEnd = xEnd; lastYEnd = yEnd;
+                wait(.1);
                 break;
             case 2:
                 if (screen_change) {
@@ -140,12 +225,16 @@
     DataRX=0;
 
     if (!ip_found) {
-        char* ip_loc = strstr(rx_buffer, "172");
-        if (ip_loc != NULL) {
-            char ip_buf[16];
-            memset(ip_buf, '\0', sizeof(ip_buf));
-            memcpy(ip_buf, ip_loc, 15);
-            ip_line = ip_buf;
+        char* ip_loc = strstr(rx_buffer, "IP Address:");
+        if ((ip_loc != NULL)) {
+            ip_loc = strstr(&ip_loc[10], "IP Address:");
+            if (ip_loc != NULL) {
+                char ip_buf[16];
+                memset(ip_buf, '\0', sizeof(ip_buf));
+                memcpy(ip_buf, &ip_loc[12], 15);
+                ip_line = ip_buf;
+                ip_found = true;
+            }
         }
     }
 
@@ -203,6 +292,9 @@
 // Starts webserver
 void startserver()
 {
+    uLCD.cls();
+    uLCD.locate(0,0);
+    uLCD.printf("Starting server...");
     pc.printf("++++++++++ Resetting ESP ++++++++++\r\n");
     strcpy(cmdbuff,"node.restart()\r\n");
     SendCMD();
@@ -608,5 +700,60 @@
             uLCD.locate(0, i);
             uLCD.printf("%s", courseVec[i].getDisplayString());
         }
+        uLCD.locate(0, 8);
+        uLCD.printf("To add courses \ngo to");
+        uLCD.locate(0,10);
+        uLCD.printf("%s", ip_line);
+        uLCD.locate(0,12);
+        uLCD.printf("To reset courses\npress center");
     }
+}
+
+float calculateHeading(float mx, float my)
+{
+    float heading = 0.0;
+    if (my == 0.0)
+        heading = (mx < 0.0) ? 180.0 : 0.0;
+    else
+        heading = atan2(mx, my)*360.0/(2.0*PI);
+    //pc.printf("heading atan=%f \n\r",heading);
+    heading -= DECLINATION; //correct for geo location
+    if(heading>180.0) heading = heading - 360.0;
+    else if(heading<-180.0) heading = 360.0 + heading;
+    else if(heading<0.0) heading = 360.0  + heading;
+
+    // Convert everything from radians to degrees:
+    //heading *= 180.0 / PI;
+
+    //pc.printf("Magnetic Heading: %f degress\n\r",heading);
+    return heading;
+}
+
+float computeAngleToDestination(float diffLat, float diffLong)
+{
+    float angle = 0.0;
+
+    if (diffLat > 0) {
+        if (diffLong > 0) {
+            // in quadrant 1
+            angle = 180*atan2(diffLat,diffLong)/PI;
+        } else {
+            // in quadrant 2
+            angle = 180*atan2(diffLat,-1 * diffLong)/PI;
+            angle = 180 - angle;
+        }
+    } else {
+        if (diffLong > 0) {
+            // in quadrant 4
+            angle = 180*atan2(-1*diffLat, diffLong)/PI;
+            angle = 360 - angle;
+        } else {
+            // in quadrant 3
+            angle = 180*atan2(-1*diffLat, -1*diffLong)/PI;
+            angle = 180 + angle;
+        }
+    }
+
+    //pc.printf("Angle to Destination: %f degress\n\r",angle);
+    return angle;
 }
\ No newline at end of file