main.cpp

00001 // Class Scheduler
00002 #include "mbed.h"
00003 #include "uLCD_4DGL.h"
00004 #include <string>
00005 #include <vector>
00006 #include "Course.h"
00007 #include "SDFileSystem.h"
00008 #include "MBed_Adafruit_GPS.h"
00009 #include "LSM9DS1.h"
00010 #include "PinDetect.h"
00011 #define PI 3.14159
00012 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00013 
00014 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00015 Serial pc(USBTX, USBRX);
00016 uLCD_4DGL uLCD(p13,p14,p15); // serial tx, serial rx, reset pin;
00017 PinDetect left(p20);
00018 PinDetect right(p18);
00019 PinDetect center(p19);
00020 Serial * gps_Serial;
00021 
00022 int update;
00023 vector<Course> courseVec;
00024 void readClassFile(vector<Course>& cVec);
00025 int getNextClass(vector<Course>& cVec);
00026 void displayCourseVec();
00027 float calculateHeading(float mx, float my), computeAngleToDestination(float diffLat, float diffLong);
00028 float longToDecimal(float longitudeInDDMM, char eastOrWest), latToDecimal(float latitudeInDDMM, char northOrSouth);
00029 int xEnd, yEnd = 0.0;
00030 int currHour, currMinute, currSecond = 0;
00031 int hoursToNextClass, minutesToNextClass, secondsToNextClass = 0;
00032 int nextClass = 0;
00033 int currClass = -1;
00034 const int refresh_Time = 1000; //refresh time in ms
00035 bool screen_refreshed = false;
00036 int volatile current_screen = 0;
00037 bool volatile screen_change = false;
00038 void timeToNextClass();
00039 float displacement(float diffLat,float diffLong);
00040 float calc_eta(float displacement, float speed);
00041 bool late(float eta);
00042 Timer t2;
00043 
00044 bool volatile snooze = 0;
00045 
00046 void center_callback(void)
00047 {
00048     snooze=1;
00049 }
00050 
00051 void left_callback(void)
00052 {
00053     if (courseVec.size() != 0) {
00054         current_screen = (3 + current_screen - 1)%3;
00055         screen_change = true;
00056     } else
00057         current_screen = 0;
00058     currClass = -1;
00059     uLCD.cls();
00060 }
00061 
00062 void right_callback(void)
00063 {
00064     if (courseVec.size() != 0) {
00065         current_screen = (3 + current_screen + 1)%3;
00066         screen_change = true;
00067     } else
00068         current_screen = 0;
00069     currClass = -1;
00070     uLCD.cls();
00071 }
00072 
00073 int main()
00074 {
00075     LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
00076     float h = 0.0;
00077     float angleToDest = 0.0;
00078     IMU.begin();
00079     if (!IMU.begin()) {
00080         pc.printf("Failed to communicate with LSM9DS1.\n");
00081     }
00082     uLCD.cls();
00083     uLCD.printf("Place IMU flat");
00084     IMU.calibrate(1);
00085     uLCD.cls();
00086     uLCD.printf("Rotate IMU 360\ndegrees in \nhorizontal plane");
00087     IMU.calibrateMag(0);
00088 
00089     left.mode(PullUp);
00090     right.mode(PullUp);
00091     center.mode(PullUp);
00092     left.attach_deasserted(&left_callback);
00093     right.attach_deasserted(&right_callback);
00094     center.attach_deasserted(&center_callback);
00095     left.setSampleFrequency();
00096     right.setSampleFrequency();
00097     center.setSampleFrequency();
00098     readClassFile(courseVec);
00099 
00100     gps_Serial = new Serial(p28,p27); //serial object for use w/ GPS
00101     Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
00102     char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
00103     Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
00104     const int refresh_Time = 2000; //refresh time in ms
00105 
00106     myGPS.begin(9600);  //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
00107     //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
00108 
00109     myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
00110     myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
00111     myGPS.sendCommand(PGCMD_ANTENNA);
00112 
00113     float myLat = 33.775991;
00114     float myLong = -84.397128;
00115 
00116     float destinationLat = courseVec[1].getLat();
00117     float destinationLong = courseVec[1].getLong();
00118 
00119     screen_change = true;
00120     pc.baud(9600);
00121     c = myGPS.read();   //queries the GPS
00122     currHour = myGPS.hour - 5; //for eastern time zone
00123     if (currHour < 0)
00124         currHour += 24;
00125     currMinute = myGPS.minute;
00126     currSecond = myGPS.seconds;
00127     nextClass = getNextClass(courseVec);
00128 
00129     refresh_Timer.start();  //starts the clock on the timer
00130     while(1) {
00131         c = myGPS.read();   //queries the GPS
00132 
00133         //if (c) { pc.printf("%c", c); } //this line will echo the GPS data if not paused
00134 
00135         //check if we recieved a new message from GPS, if so, attempt to parse it,
00136         if ( myGPS.newNMEAreceived() ) {
00137             if ( !myGPS.parse(myGPS.lastNMEA()) ) {
00138                 continue;
00139             }
00140         }
00141 
00142         if (refresh_Timer.read_ms() >= refresh_Time) {
00143             refresh_Timer.reset();
00144             if (myGPS.fix) {
00145                 myLat = latToDecimal(myGPS.latitude, myGPS.lat);
00146                 myLong = longToDecimal(myGPS.longitude, myGPS.lon);
00147             }
00148 
00149             currHour = myGPS.hour - 5; //for eastern time zone
00150             if (currHour < 0)
00151                 currHour += 24;
00152             currMinute = myGPS.minute;
00153             currSecond = myGPS.seconds;
00154             nextClass = getNextClass(courseVec);
00155             timeToNextClass();
00156             destinationLat = courseVec[nextClass].getLat();
00157             destinationLong = courseVec[nextClass].getLong();
00158             float diffLat = destinationLat - myLat;
00159             float diffLong = destinationLong - myLong;
00160             float dis = displacement(diffLat,diffLong);
00161             float speed = 3.1; //mph. Source:https://lmgtfy.com/?q=average+human+walking+speed
00162             float eta = calc_eta(dis,speed)*60;//(miles, mph)*60
00163             if((late(eta)==1)&&(snooze==0)) {
00164                 uLCD.background_color(RED);
00165                 uLCD.textbackground_color(RED);
00166             } else if((late(eta)==1)&&(snooze==1)) {
00167                 t2.start();
00168                 if(t2.read()<120) { //2 minute snooze
00169                     uLCD.background_color(BLACK);
00170                     uLCD.textbackground_color(BLACK);
00171                 } else {
00172                     t2.stop();
00173                     snooze = 0;
00174                 }
00175             } else {
00176                 uLCD.background_color(BLACK);
00177                 uLCD.textbackground_color(BLACK);
00178             }
00179 
00180             switch(current_screen) {
00181                 case 0:
00182                     wait(.2);
00183                     if (screen_change) {
00184                         displayCourseVec();
00185                         screen_change = false;
00186                     }
00187                     break;
00188                 case 1:
00189                     IMU.readMag();
00190                     h = calculateHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
00191                     pc.printf("Heading: %f\n", h);
00192                     angleToDest = computeAngleToDestination(diffLat, diffLong);
00193                     h = angleToDest - h;
00194                     h = h - 90;
00195                     if (h < 0)
00196                         h = h + 360;
00197                     if (h > 360)
00198                         h = h - 360;
00199                     xEnd = 0;
00200                     yEnd = 0;
00201                     float rads = 0.0;
00202                     //uLCD.cls();
00203                     if (h < 90) {
00204                         rads = h * PI / 180;
00205                         xEnd = floor(63 * sin(rads) + .5);
00206                         yEnd = floor(63 * cos(rads) + .5);
00207                         xEnd = 63 + xEnd;
00208                         yEnd = 63 + yEnd;
00209                     } else if (90 < h < 180) {
00210                         h = h - 90;
00211                         rads = h * PI / 180;
00212                         xEnd = floor(63 * cos(rads) + .5);
00213                         yEnd = floor(63 * sin(rads) + .5);
00214                         xEnd = 63 + xEnd;
00215                         yEnd = 63 - yEnd;
00216                     } else if (180 < h < 270) {
00217                         h = h - 180;
00218                         rads = h * PI / 180;
00219                         xEnd = floor(63 * sin(rads) + .5);
00220                         yEnd = floor(63 * cos(rads) + .5);
00221                         xEnd = 63 - xEnd;
00222                         yEnd = 63 - yEnd;
00223                     } else if (270 < h) {
00224                         h = h - 270;
00225                         rads = h * PI / 180;
00226                         xEnd = floor(63 * cos(rads) + .5);
00227                         yEnd = floor(63 * sin(rads) + .5);
00228                         xEnd = 63 - xEnd;
00229                         yEnd = 63 + yEnd;
00230                     }
00231                     uLCD.cls();
00232                     uLCD.line(63, 63, xEnd, yEnd, WHITE);
00233                     break;
00234                 case 2:
00235                     //uLCD.cls();
00236                     //uLCD.text_mode(TRANSPARENT);
00237                     uLCD.locate(0,2);
00238                     uLCD.printf("                ");
00239                     uLCD.locate(0,3);
00240                     uLCD.printf("Time to next class");
00241                     uLCD.locate(0, 4);
00242                     uLCD.printf("%02dhrs %02dminutes", hoursToNextClass, minutesToNextClass);
00243                     uLCD.locate(0, 5);
00244                     uLCD.printf("%02dseconds", secondsToNextClass);
00245                     uLCD.locate(0,7);
00246                     uLCD.printf("ETA: (min)");
00247                     uLCD.locate(0,8);
00248                     uLCD.printf("%f",eta);
00249                     uLCD.locate(0,10);
00250                     uLCD.printf("Displacement:");
00251                     uLCD.locate(0,11);
00252                     uLCD.printf("%f",dis);
00253                     uLCD.locate(0, 0);
00254                     uLCD.printf("Next class        ");
00255                     if (nextClass != currClass) {
00256                         uLCD.locate(0, 1);
00257                         uLCD.printf("                     ");
00258                         uLCD.locate(0, 1);
00259                         uLCD.printf("%s", courseVec[nextClass].getDisplayString());
00260                     }
00261                     currClass = nextClass;
00262                     break;
00263             }
00264         }
00265     }
00266 }
00267 
00268 void readClassFile(vector<Course>& cVec)
00269 {
00270     cVec.clear();
00271 
00272     FILE *readFp = fopen("/sd/classdir/classes.txt", "r");
00273     char line[15];
00274     char buildingBuf[4];
00275     char hourBuf[3];
00276     int hour;
00277     char minuteBuf[3];
00278     int minute;
00279     char ampmBuf[3];
00280     uLCD.cls();
00281     uLCD.locate(0, 1);
00282     uLCD.printf("Reading class file...");
00283 
00284     memset(buildingBuf, 0, sizeof(buildingBuf));
00285     memset(hourBuf, 0, sizeof(hourBuf));
00286     memset(minuteBuf, 0, sizeof(minuteBuf));
00287     memset(ampmBuf, 0, sizeof(ampmBuf));
00288     memset(line, 0, sizeof(line));
00289 
00290     if (readFp == NULL)
00291         return;
00292     else {
00293         while (!feof(readFp)) {
00294             fgets(line, 15, readFp);
00295             if(line[8] == NULL)
00296                 continue;
00297             memcpy(buildingBuf, line, 3);
00298             memcpy(hourBuf, &line[4], 2);
00299             memcpy(minuteBuf, &line[7], 2);
00300             memcpy(ampmBuf, &line[10], 2);
00301 
00302             string building = buildingBuf;
00303             hour = atoi(hourBuf);
00304             minute = atoi(minuteBuf);
00305             string ampm = ampmBuf;
00306 
00307             Course temp(building, hour, minute, ampm);
00308             cVec.push_back(temp);
00309         }
00310     }
00311     fclose(readFp);
00312     return;
00313 }
00314 
00315 void displayCourseVec()
00316 {
00317     //uLCD.text_mode(TRANSPARENT);
00318     if (courseVec.size() == 0) {
00319         uLCD.cls();
00320         uLCD.locate(0,0);
00321         uLCD.printf("No classes input!");
00322         uLCD.locate(0,1);
00323     } else {
00324         uLCD.cls();
00325         uLCD.locate(0,1);
00326         for (int i = 0; i < courseVec.size(); i++) {
00327             uLCD.locate(0, i);
00328             uLCD.printf("%s", courseVec[i].getDisplayString());
00329         }
00330     }
00331 }
00332 
00333 float calculateHeading(float mx, float my)
00334 {
00335     float heading = 0.0;
00336     if (my == 0.0)
00337         heading = (mx < 0.0) ? 180.0 : 0.0;
00338     else
00339         heading = atan2(mx, my)*360.0/(2.0*PI);
00340     //pc.printf("heading atan=%f \n\r",heading);
00341     heading -= DECLINATION; //correct for geo location
00342     if(heading>180.0) heading = heading - 360.0;
00343     else if(heading<-180.0) heading = 360.0 + heading;
00344     else if(heading<0.0) heading = 360.0  + heading;
00345 
00346     // Convert everything from radians to degrees:
00347     //heading *= 180.0 / PI;
00348 
00349     //pc.printf("Magnetic Heading: %f degress\n\r",heading);
00350     return heading;
00351 }
00352 
00353 float computeAngleToDestination(float diffLat, float diffLong)
00354 {
00355     float angle = 0.0;
00356 
00357     if (diffLat > 0) {
00358         if (diffLong > 0) {
00359             // in quadrant 1
00360             angle = 180*atan2(diffLat,diffLong)/PI;
00361         } else {
00362             // in quadrant 2
00363             angle = 180*atan2(diffLat,-1 * diffLong)/PI;
00364             angle = 180 - angle;
00365         }
00366     } else {
00367         if (diffLong > 0) {
00368             // in quadrant 4
00369             angle = 180*atan2(-1*diffLat, diffLong)/PI;
00370             angle = 360 - angle;
00371         } else {
00372             // in quadrant 3
00373             angle = 180*atan2(-1*diffLat, -1*diffLong)/PI;
00374             angle = 180 + angle;
00375         }
00376     }
00377 
00378     //pc.printf("Angle to Destination: %f degress\n\r",angle);
00379     return angle;
00380 }
00381 
00382 int getNextClass(vector<Course>& cVec)
00383 {
00384     int numIterations = 0;
00385     for (int i = 0; i < cVec.size(); i++) {
00386         numIterations++;
00387         pc.printf("Iteration : %i\ncurrHour: %i\ncourseHour: %i\n", numIterations, currHour, cVec[i].getHour_inMilitaryTime());
00388         if (courseVec[i].getHour_inMilitaryTime() < currHour)
00389             continue;
00390         else if (courseVec[i].getHour_inMilitaryTime() > currHour)
00391             return i;
00392         else if (courseVec[i].getHour_inMilitaryTime() == currHour) {
00393             if (courseVec[i].getMinute() <= currMinute)
00394                 continue;
00395             else if (courseVec[i].getMinute() > currMinute) {
00396                 return i;
00397             }
00398         }
00399         if (numIterations == cVec.size())
00400             return 0;
00401     }
00402     return 0;
00403 }
00404 
00405 void timeToNextClass()
00406 {
00407     hoursToNextClass = courseVec[nextClass].getHour_inMilitaryTime() - currHour;
00408     if (hoursToNextClass < 0)
00409         hoursToNextClass += 24;
00410 
00411     minutesToNextClass = courseVec[nextClass].getMinute() - currMinute;
00412     if (minutesToNextClass < 0) {
00413         hoursToNextClass--;
00414         minutesToNextClass += 60;
00415     }
00416 
00417     secondsToNextClass = 60 - currSecond;
00418     minutesToNextClass--;
00419 
00420     if (minutesToNextClass < 0) {
00421         hoursToNextClass--;
00422         minutesToNextClass += 60;
00423     }
00424 
00425     if (hoursToNextClass < 0)
00426         hoursToNextClass += 24;
00427 }
00428 
00429 // convert from DDMM.mmmm to decimal
00430 float latToDecimal(float latitudeInDDMM, char northOrSouth)
00431 {
00432     float latInDec = 0.0;
00433     float temp = 0.0;
00434     latInDec = latitudeInDDMM / 100;
00435     temp = latInDec - floor(latInDec);
00436     temp = temp / .6;
00437     latInDec = floor(latInDec) + temp;
00438     if (northOrSouth == 'S')
00439         return -1 * latInDec;
00440     return latInDec;
00441 }
00442 
00443 float displacement(float diffLat,float diffLong)
00444 {
00445     float d = sqrt(diffLat*diffLat+diffLong*diffLong);
00446     return d;
00447 }
00448 float calc_eta(float displacement, float speed)
00449 {
00450     float estTimeArr = displacement/speed; // for displacement. take average speed // convert to minutes later
00451     estTimeArr = estTimeArr+ 0.25*estTimeArr; //adding delay for distance vs displacement and traffic lights
00452     return estTimeArr;
00453 }
00454 
00455 float longToDecimal(float longitudeInDDMM, char eastOrWest)
00456 {
00457     float longInDec = 0.0;
00458     float temp = 0.0;
00459     longInDec = longitudeInDDMM / 100;
00460     temp = longInDec - floor(longInDec);
00461     temp = temp / .6;
00462     longInDec = floor(longInDec) + temp;
00463     if (eastOrWest == 'E')
00464         return longInDec;
00465     return -1 * longInDec;
00466 }
00467 
00468 bool late(float eta)
00469 {
00470     float totalMinutes = hoursToNextClass*60+ minutesToNextClass+ secondsToNextClass/60;
00471     if(eta>totalMinutes) {
00472         return 1;
00473     } else {
00474         return 0;
00475     }
00476 }
00477