File content as of revision 17:1de531cd3d4a:

// Class Scheduler
#include "mbed.h"
#include "uLCD_4DGL.h"
#include <string>
#include <vector>
#include "Course.h"
#include "SDFileSystem.h"
#include "MBed_Adafruit_GPS.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);
uLCD_4DGL uLCD(p13,p14,p15); // serial tx, serial rx, reset pin;
PinDetect left(p20);
PinDetect right(p18);
PinDetect center(p19);
Serial * gps_Serial;

int update;
vector<Course> courseVec;
void readClassFile(vector<Course>& cVec);
int getNextClass(vector<Course>& cVec);
void displayCourseVec();
float calculateHeading(float mx, float my), computeAngleToDestination(float diffLat, float diffLong);
float longToDecimal(float longitudeInDDMM, char eastOrWest), latToDecimal(float latitudeInDDMM, char northOrSouth);
int xEnd, yEnd = 0.0;
int currHour, currMinute, currSecond = 0;
int hoursToNextClass, minutesToNextClass, secondsToNextClass = 0;
int nextClass = 0;
int currClass = -1;
const int refresh_Time = 1000; //refresh time in ms
bool screen_refreshed = false;
int volatile current_screen = 0;
bool volatile screen_change = false;
void timeToNextClass();
float displacement(float diffLat,float diffLong);
float calc_eta(float displacement, float speed);
bool late(float eta);
Timer t2;

bool volatile snooze = 0;

void center_callback(void)
{
    snooze=1;
}

void left_callback(void)
{
    if (courseVec.size() != 0) {
        current_screen = (3 + current_screen - 1)%3;
        screen_change = true;
    } else
        current_screen = 0;
    currClass = -1;
    uLCD.cls();
}

void right_callback(void)
{
    if (courseVec.size() != 0) {
        current_screen = (3 + current_screen + 1)%3;
        screen_change = true;
    } else
        current_screen = 0;
    currClass = -1;
    uLCD.cls();
}

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);

    left.mode(PullUp);
    right.mode(PullUp);
    center.mode(PullUp);
    left.attach_deasserted(&left_callback);
    right.attach_deasserted(&right_callback);
    center.attach_deasserted(&center_callback);
    left.setSampleFrequency();
    right.setSampleFrequency();
    center.setSampleFrequency();
    readClassFile(courseVec);

    gps_Serial = new Serial(p28,p27); //serial object for use w/ GPS
    Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
    char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
    Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
    const int refresh_Time = 2000; //refresh time in ms

    myGPS.begin(9600);  //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
    //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf

    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    myGPS.sendCommand(PGCMD_ANTENNA);

    float myLat = 33.775991;
    float myLong = -84.397128;

    float destinationLat = courseVec[1].getLat();
    float destinationLong = courseVec[1].getLong();

    screen_change = true;
    pc.baud(9600);
    c = myGPS.read();   //queries the GPS
    currHour = myGPS.hour - 5; //for eastern time zone
    if (currHour < 0)
        currHour += 24;
    currMinute = myGPS.minute;
    currSecond = myGPS.seconds;
    nextClass = getNextClass(courseVec);

    refresh_Timer.start();  //starts the clock on the timer
    while(1) {
        c = myGPS.read();   //queries the GPS

        //if (c) { pc.printf("%c", c); } //this line will echo the GPS data if not paused

        //check if we recieved a new message from GPS, if so, attempt to parse it,
        if ( myGPS.newNMEAreceived() ) {
            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
                continue;
            }
        }

        if (refresh_Timer.read_ms() >= refresh_Time) {
            refresh_Timer.reset();
            if (myGPS.fix) {
                myLat = latToDecimal(myGPS.latitude, myGPS.lat);
                myLong = longToDecimal(myGPS.longitude, myGPS.lon);
            }

            currHour = myGPS.hour - 5; //for eastern time zone
            if (currHour < 0)
                currHour += 24;
            currMinute = myGPS.minute;
            currSecond = myGPS.seconds;
            nextClass = getNextClass(courseVec);
            timeToNextClass();
            destinationLat = courseVec[nextClass].getLat();
            destinationLong = courseVec[nextClass].getLong();
            float diffLat = destinationLat - myLat;
            float diffLong = destinationLong - myLong;
            float dis = displacement(diffLat,diffLong);
            float speed = 3.1; //mph. Source:https://lmgtfy.com/?q=average+human+walking+speed
            float eta = calc_eta(dis,speed)*60;//(miles, mph)*60
            if((late(eta)==1)&&(snooze==0)) {
                uLCD.background_color(RED);
                uLCD.textbackground_color(RED);
            } else if((late(eta)==1)&&(snooze==1)) {
                t2.start();
                if(t2.read()<120) { //2 minute snooze
                    uLCD.background_color(BLACK);
                    uLCD.textbackground_color(BLACK);
                } else {
                    t2.stop();
                    snooze = 0;
                }
            } else {
                uLCD.background_color(BLACK);
                uLCD.textbackground_color(BLACK);
            }

            switch(current_screen) {
                case 0:
                    wait(.2);
                    if (screen_change) {
                        displayCourseVec();
                        screen_change = false;
                    }
                    break;
                case 1:
                    IMU.readMag();
                    h = calculateHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
                    pc.printf("Heading: %f\n", h);
                    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.cls();
                    uLCD.line(63, 63, xEnd, yEnd, WHITE);
                    break;
                case 2:
                    //uLCD.cls();
                    //uLCD.text_mode(TRANSPARENT);
                    uLCD.locate(0,2);
                    uLCD.printf("                ");
                    uLCD.locate(0,3);
                    uLCD.printf("Time to next class");
                    uLCD.locate(0, 4);
                    uLCD.printf("%02dhrs %02dminutes", hoursToNextClass, minutesToNextClass);
                    uLCD.locate(0, 5);
                    uLCD.printf("%02dseconds", secondsToNextClass);
                    uLCD.locate(0,7);
                    uLCD.printf("ETA: (min)");
                    uLCD.locate(0,8);
                    uLCD.printf("%f",eta);
                    uLCD.locate(0,10);
                    uLCD.printf("Displacement:");
                    uLCD.locate(0,11);
                    uLCD.printf("%f",dis);
                    uLCD.locate(0, 0);
                    uLCD.printf("Next class        ");
                    if (nextClass != currClass) {
                        uLCD.locate(0, 1);
                        uLCD.printf("                     ");
                        uLCD.locate(0, 1);
                        uLCD.printf("%s", courseVec[nextClass].getDisplayString());
                    }
                    currClass = nextClass;
                    break;
            }
        }
    }
}

void readClassFile(vector<Course>& cVec)
{
    cVec.clear();

    FILE *readFp = fopen("/sd/classdir/classes.txt", "r");
    char line[15];
    char buildingBuf[4];
    char hourBuf[3];
    int hour;
    char minuteBuf[3];
    int minute;
    char ampmBuf[3];
    uLCD.cls();
    uLCD.locate(0, 1);
    uLCD.printf("Reading class file...");

    memset(buildingBuf, 0, sizeof(buildingBuf));
    memset(hourBuf, 0, sizeof(hourBuf));
    memset(minuteBuf, 0, sizeof(minuteBuf));
    memset(ampmBuf, 0, sizeof(ampmBuf));
    memset(line, 0, sizeof(line));

    if (readFp == NULL)
        return;
    else {
        while (!feof(readFp)) {
            fgets(line, 15, readFp);
            if(line[8] == NULL)
                continue;
            memcpy(buildingBuf, line, 3);
            memcpy(hourBuf, &line[4], 2);
            memcpy(minuteBuf, &line[7], 2);
            memcpy(ampmBuf, &line[10], 2);

            string building = buildingBuf;
            hour = atoi(hourBuf);
            minute = atoi(minuteBuf);
            string ampm = ampmBuf;

            Course temp(building, hour, minute, ampm);
            cVec.push_back(temp);
        }
    }
    fclose(readFp);
    return;
}

void displayCourseVec()
{
    //uLCD.text_mode(TRANSPARENT);
    if (courseVec.size() == 0) {
        uLCD.cls();
        uLCD.locate(0,0);
        uLCD.printf("No classes input!");
        uLCD.locate(0,1);
    } else {
        uLCD.cls();
        uLCD.locate(0,1);
        for (int i = 0; i < courseVec.size(); i++) {
            uLCD.locate(0, i);
            uLCD.printf("%s", courseVec[i].getDisplayString());
        }
    }
}

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;
}

int getNextClass(vector<Course>& cVec)
{
    int numIterations = 0;
    for (int i = 0; i < cVec.size(); i++) {
        numIterations++;
        pc.printf("Iteration : %i\ncurrHour: %i\ncourseHour: %i\n", numIterations, currHour, cVec[i].getHour_inMilitaryTime());
        if (courseVec[i].getHour_inMilitaryTime() < currHour)
            continue;
        else if (courseVec[i].getHour_inMilitaryTime() > currHour)
            return i;
        else if (courseVec[i].getHour_inMilitaryTime() == currHour) {
            if (courseVec[i].getMinute() <= currMinute)
                continue;
            else if (courseVec[i].getMinute() > currMinute) {
                return i;
            }
        }
        if (numIterations == cVec.size())
            return 0;
    }
    return 0;
}

void timeToNextClass()
{
    hoursToNextClass = courseVec[nextClass].getHour_inMilitaryTime() - currHour;
    if (hoursToNextClass < 0)
        hoursToNextClass += 24;

    minutesToNextClass = courseVec[nextClass].getMinute() - currMinute;
    if (minutesToNextClass < 0) {
        hoursToNextClass--;
        minutesToNextClass += 60;
    }

    secondsToNextClass = 60 - currSecond;
    minutesToNextClass--;

    if (minutesToNextClass < 0) {
        hoursToNextClass--;
        minutesToNextClass += 60;
    }

    if (hoursToNextClass < 0)
        hoursToNextClass += 24;
}

// convert from DDMM.mmmm to decimal
float latToDecimal(float latitudeInDDMM, char northOrSouth)
{
    float latInDec = 0.0;
    float temp = 0.0;
    latInDec = latitudeInDDMM / 100;
    temp = latInDec - floor(latInDec);
    temp = temp / .6;
    latInDec = floor(latInDec) + temp;
    if (northOrSouth == 'S')
        return -1 * latInDec;
    return latInDec;
}

float displacement(float diffLat,float diffLong)
{
    float d = sqrt(diffLat*diffLat+diffLong*diffLong);
    return d;
}
float calc_eta(float displacement, float speed)
{
    float estTimeArr = displacement/speed; // for displacement. take average speed // convert to minutes later
    estTimeArr = estTimeArr+ 0.25*estTimeArr; //adding delay for distance vs displacement and traffic lights
    return estTimeArr;
}

float longToDecimal(float longitudeInDDMM, char eastOrWest)
{
    float longInDec = 0.0;
    float temp = 0.0;
    longInDec = longitudeInDDMM / 100;
    temp = longInDec - floor(longInDec);
    temp = temp / .6;
    longInDec = floor(longInDec) + temp;
    if (eastOrWest == 'E')
        return longInDec;
    return -1 * longInDec;
}

bool late(float eta)
{
    float totalMinutes = hoursToNextClass*60+ minutesToNextClass+ secondsToNextClass/60;
    if(eta>totalMinutes) {
        return 1;
    } else {
        return 0;
    }
}