R@mbln Reks / Mbed 2 deprecated class_finder

Guides the user to their classes

Dependencies:   4DGL-uLCD-SE Course SDFileSystem mbed PinDetect LSM9DS1_Library_cal MBed_Adafruit-GPS-Library

main.cpp@17:1de531cd3d4a, 2016-12-09 (annotated)

Committer:
nkela6
Date:
Fri Dec 09 00:39:19 2016 +0000
Revision:
17:1de531cd3d4a
Parent:
16:4b4e6cf66e7c 
Final commit after fixing timer errors (Still limited to 1 snooze call)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kkizirian 0:ce60014510e5 1 // Class Scheduler
kkizirian 0:ce60014510e5 2 #include "mbed.h"
kkizirian 0:ce60014510e5 3 #include "uLCD_4DGL.h"
kkizirian 0:ce60014510e5 4 #include <string>
kkizirian 0:ce60014510e5 5 #include <vector>
kkizirian 0:ce60014510e5 6 #include "Course.h"
kkizirian 0:ce60014510e5 7 #include "SDFileSystem.h"
kkizirian 5:430f44669f94 8 #include "MBed_Adafruit_GPS.h"
kkizirian 3:0ca91f8afec5 9 #include "LSM9DS1.h"
kkizirian 2:c708e2027970 10 #include "PinDetect.h"
kkizirian 3:0ca91f8afec5 11 #define PI 3.14159
kkizirian 3:0ca91f8afec5 12 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
kkizirian 0:ce60014510e5 13
kkizirian 0:ce60014510e5 14 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
kkizirian 0:ce60014510e5 15 Serial pc(USBTX, USBRX);
kkizirian 3:0ca91f8afec5 16 uLCD_4DGL uLCD(p13,p14,p15); // serial tx, serial rx, reset pin;
kkizirian 3:0ca91f8afec5 17 PinDetect left(p20);
kkizirian 3:0ca91f8afec5 18 PinDetect right(p18);
nkela6 16:4b4e6cf66e7c 19 PinDetect center(p19);
kkizirian 5:430f44669f94 20 Serial * gps_Serial;
kkizirian 0:ce60014510e5 21
kkizirian 0:ce60014510e5 22 int update;
kkizirian 0:ce60014510e5 23 vector<Course> courseVec;
kkizirian 4:93a4b415fe6c 24 void readClassFile(vector<Course>& cVec);
kkizirian 5:430f44669f94 25 int getNextClass(vector<Course>& cVec);
kkizirian 2:c708e2027970 26 void displayCourseVec();
kkizirian 3:0ca91f8afec5 27 float calculateHeading(float mx, float my), computeAngleToDestination(float diffLat, float diffLong);
kkizirian 9:5b871fb9f554 28 float longToDecimal(float longitudeInDDMM, char eastOrWest), latToDecimal(float latitudeInDDMM, char northOrSouth);
kkizirian 4:93a4b415fe6c 29 int xEnd, yEnd = 0.0;
kkizirian 5:430f44669f94 30 int currHour, currMinute, currSecond = 0;
kkizirian 5:430f44669f94 31 int hoursToNextClass, minutesToNextClass, secondsToNextClass = 0;
kkizirian 5:430f44669f94 32 int nextClass = 0;
kkizirian 5:430f44669f94 33 int currClass = -1;
kkizirian 5:430f44669f94 34 const int refresh_Time = 1000; //refresh time in ms
kkizirian 3:0ca91f8afec5 35 bool screen_refreshed = false;
kkizirian 2:c708e2027970 36 int volatile current_screen = 0;
kkizirian 2:c708e2027970 37 bool volatile screen_change = false;
kkizirian 5:430f44669f94 38 void timeToNextClass();
nkela6 13:0eb9fbd63fb2 39 float displacement(float diffLat,float diffLong);
nkela6 13:0eb9fbd63fb2 40 float calc_eta(float displacement, float speed);
nkela6 13:0eb9fbd63fb2 41 bool late(float eta);
nkela6 17:1de531cd3d4a 42 Timer t2;
nkela6 17:1de531cd3d4a 43
nkela6 17:1de531cd3d4a 44 bool volatile snooze = 0;
nkela6 16:4b4e6cf66e7c 45
nkela6 17:1de531cd3d4a 46 void center_callback(void)
nkela6 17:1de531cd3d4a 47 {
nkela6 17:1de531cd3d4a 48 snooze=1;
nkela6 17:1de531cd3d4a 49 }
kkizirian 0:ce60014510e5 50
kkizirian 2:c708e2027970 51 void left_callback(void)
kkizirian 2:c708e2027970 52 {
kkizirian 3:0ca91f8afec5 53 if (courseVec.size() != 0) {
kkizirian 3:0ca91f8afec5 54 current_screen = (3 + current_screen - 1)%3;
kkizirian 3:0ca91f8afec5 55 screen_change = true;
kkizirian 3:0ca91f8afec5 56 } else
kkizirian 3:0ca91f8afec5 57 current_screen = 0;
kkizirian 5:430f44669f94 58 currClass = -1;
kkizirian 5:430f44669f94 59 uLCD.cls();
kkizirian 2:c708e2027970 60 }
kkizirian 2:c708e2027970 61
kkizirian 2:c708e2027970 62 void right_callback(void)
kkizirian 2:c708e2027970 63 {
kkizirian 3:0ca91f8afec5 64 if (courseVec.size() != 0) {
kkizirian 3:0ca91f8afec5 65 current_screen = (3 + current_screen + 1)%3;
kkizirian 3:0ca91f8afec5 66 screen_change = true;
kkizirian 3:0ca91f8afec5 67 } else
kkizirian 3:0ca91f8afec5 68 current_screen = 0;
kkizirian 5:430f44669f94 69 currClass = -1;
kkizirian 5:430f44669f94 70 uLCD.cls();
kkizirian 2:c708e2027970 71 }
kkizirian 2:c708e2027970 72
kkizirian 0:ce60014510e5 73 int main()
kkizirian 0:ce60014510e5 74 {
kkizirian 3:0ca91f8afec5 75 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
kkizirian 3:0ca91f8afec5 76 float h = 0.0;
kkizirian 3:0ca91f8afec5 77 float angleToDest = 0.0;
kkizirian 3:0ca91f8afec5 78 IMU.begin();
kkizirian 3:0ca91f8afec5 79 if (!IMU.begin()) {
kkizirian 3:0ca91f8afec5 80 pc.printf("Failed to communicate with LSM9DS1.\n");
kkizirian 3:0ca91f8afec5 81 }
kkizirian 3:0ca91f8afec5 82 uLCD.cls();
kkizirian 3:0ca91f8afec5 83 uLCD.printf("Place IMU flat");
kkizirian 3:0ca91f8afec5 84 IMU.calibrate(1);
kkizirian 3:0ca91f8afec5 85 uLCD.cls();
kkizirian 3:0ca91f8afec5 86 uLCD.printf("Rotate IMU 360\ndegrees in \nhorizontal plane");
kkizirian 3:0ca91f8afec5 87 IMU.calibrateMag(0);
kkizirian 3:0ca91f8afec5 88
kkizirian 2:c708e2027970 89 left.mode(PullUp);
kkizirian 2:c708e2027970 90 right.mode(PullUp);
nkela6 17:1de531cd3d4a 91 center.mode(PullUp);
kkizirian 2:c708e2027970 92 left.attach_deasserted(&left_callback);
kkizirian 2:c708e2027970 93 right.attach_deasserted(&right_callback);
nkela6 17:1de531cd3d4a 94 center.attach_deasserted(&center_callback);
kkizirian 2:c708e2027970 95 left.setSampleFrequency();
kkizirian 2:c708e2027970 96 right.setSampleFrequency();
nkela6 17:1de531cd3d4a 97 center.setSampleFrequency();
kkizirian 0:ce60014510e5 98 readClassFile(courseVec);
kkizirian 5:430f44669f94 99
kkizirian 5:430f44669f94 100 gps_Serial = new Serial(p28,p27); //serial object for use w/ GPS
kkizirian 5:430f44669f94 101 Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
kkizirian 5:430f44669f94 102 char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
kkizirian 5:430f44669f94 103 Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
kkizirian 5:430f44669f94 104 const int refresh_Time = 2000; //refresh time in ms
kkizirian 5:430f44669f94 105
kkizirian 5:430f44669f94 106 myGPS.begin(9600); //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
kkizirian 5:430f44669f94 107 //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
kkizirian 5:430f44669f94 108
kkizirian 5:430f44669f94 109 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
kkizirian 5:430f44669f94 110 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
kkizirian 5:430f44669f94 111 myGPS.sendCommand(PGCMD_ANTENNA);
kkizirian 5:430f44669f94 112
kkizirian 4:93a4b415fe6c 113 float myLat = 33.775991;
kkizirian 4:93a4b415fe6c 114 float myLong = -84.397128;
kkizirian 2:c708e2027970 115
kkizirian 5:430f44669f94 116 float destinationLat = courseVec[1].getLat();
kkizirian 5:430f44669f94 117 float destinationLong = courseVec[1].getLong();
kkizirian 5:430f44669f94 118
kkizirian 4:93a4b415fe6c 119 screen_change = true;
kkizirian 0:ce60014510e5 120 pc.baud(9600);
kkizirian 5:430f44669f94 121 c = myGPS.read(); //queries the GPS
kkizirian 5:430f44669f94 122 currHour = myGPS.hour - 5; //for eastern time zone
kkizirian 5:430f44669f94 123 if (currHour < 0)
kkizirian 5:430f44669f94 124 currHour += 24;
kkizirian 5:430f44669f94 125 currMinute = myGPS.minute;
kkizirian 5:430f44669f94 126 currSecond = myGPS.seconds;
kkizirian 5:430f44669f94 127 nextClass = getNextClass(courseVec);
kkizirian 2:c708e2027970 128
kkizirian 5:430f44669f94 129 refresh_Timer.start(); //starts the clock on the timer
kkizirian 0:ce60014510e5 130 while(1) {
kkizirian 5:430f44669f94 131 c = myGPS.read(); //queries the GPS
kkizirian 5:430f44669f94 132
kkizirian 5:430f44669f94 133 //if (c) { pc.printf("%c", c); } //this line will echo the GPS data if not paused
kkizirian 5:430f44669f94 134
kkizirian 5:430f44669f94 135 //check if we recieved a new message from GPS, if so, attempt to parse it,
kkizirian 5:430f44669f94 136 if ( myGPS.newNMEAreceived() ) {
kkizirian 5:430f44669f94 137 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
kkizirian 5:430f44669f94 138 continue;
kkizirian 5:430f44669f94 139 }
kkizirian 5:430f44669f94 140 }
kkizirian 5:430f44669f94 141
kkizirian 5:430f44669f94 142 if (refresh_Timer.read_ms() >= refresh_Time) {
kkizirian 5:430f44669f94 143 refresh_Timer.reset();
kkizirian 6:ceb6a5426add 144 if (myGPS.fix) {
kkizirian 9:5b871fb9f554 145 myLat = latToDecimal(myGPS.latitude, myGPS.lat);
kkizirian 9:5b871fb9f554 146 myLong = longToDecimal(myGPS.longitude, myGPS.lon);
kkizirian 6:ceb6a5426add 147 }
kkizirian 9:5b871fb9f554 148
kkizirian 5:430f44669f94 149 currHour = myGPS.hour - 5; //for eastern time zone
kkizirian 5:430f44669f94 150 if (currHour < 0)
kkizirian 5:430f44669f94 151 currHour += 24;
kkizirian 5:430f44669f94 152 currMinute = myGPS.minute;
kkizirian 5:430f44669f94 153 currSecond = myGPS.seconds;
kkizirian 5:430f44669f94 154 nextClass = getNextClass(courseVec);
kkizirian 5:430f44669f94 155 timeToNextClass();
nkela6 13:0eb9fbd63fb2 156 destinationLat = courseVec[nextClass].getLat();
nkela6 13:0eb9fbd63fb2 157 destinationLong = courseVec[nextClass].getLong();
nkela6 13:0eb9fbd63fb2 158 float diffLat = destinationLat - myLat;
nkela6 13:0eb9fbd63fb2 159 float diffLong = destinationLong - myLong;
nkela6 13:0eb9fbd63fb2 160 float dis = displacement(diffLat,diffLong);
nkela6 13:0eb9fbd63fb2 161 float speed = 3.1; //mph. Source:https://lmgtfy.com/?q=average+human+walking+speed
nkela6 13:0eb9fbd63fb2 162 float eta = calc_eta(dis,speed)*60;//(miles, mph)*60
nkela6 16:4b4e6cf66e7c 163 if((late(eta)==1)&&(snooze==0)) {
nkela6 14:d8a6134cafa3 164 uLCD.background_color(RED);
nkela6 16:4b4e6cf66e7c 165 uLCD.textbackground_color(RED);
nkela6 17:1de531cd3d4a 166 } else if((late(eta)==1)&&(snooze==1)) {
nkela6 16:4b4e6cf66e7c 167 t2.start();
nkela6 17:1de531cd3d4a 168 if(t2.read()<120) { //2 minute snooze
nkela6 16:4b4e6cf66e7c 169 uLCD.background_color(BLACK);
nkela6 16:4b4e6cf66e7c 170 uLCD.textbackground_color(BLACK);
nkela6 17:1de531cd3d4a 171 } else {
nkela6 17:1de531cd3d4a 172 t2.stop();
nkela6 17:1de531cd3d4a 173 snooze = 0;
nkela6 16:4b4e6cf66e7c 174 }
nkela6 17:1de531cd3d4a 175 } else {
nkela6 14:d8a6134cafa3 176 uLCD.background_color(BLACK);
nkela6 16:4b4e6cf66e7c 177 uLCD.textbackground_color(BLACK);
nkela6 13:0eb9fbd63fb2 178 }
kkizirian 5:430f44669f94 179
kkizirian 5:430f44669f94 180 switch(current_screen) {
kkizirian 5:430f44669f94 181 case 0:
kkizirian 5:430f44669f94 182 wait(.2);
kkizirian 5:430f44669f94 183 if (screen_change) {
kkizirian 5:430f44669f94 184 displayCourseVec();
kkizirian 5:430f44669f94 185 screen_change = false;
kkizirian 5:430f44669f94 186 }
kkizirian 5:430f44669f94 187 break;
kkizirian 5:430f44669f94 188 case 1:
kkizirian 5:430f44669f94 189 IMU.readMag();
kkizirian 5:430f44669f94 190 h = calculateHeading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
kkizirian 5:430f44669f94 191 pc.printf("Heading: %f\n", h);
kkizirian 5:430f44669f94 192 angleToDest = computeAngleToDestination(diffLat, diffLong);
kkizirian 5:430f44669f94 193 h = angleToDest - h;
kkizirian 3:0ca91f8afec5 194 h = h - 90;
kkizirian 5:430f44669f94 195 if (h < 0)
kkizirian 5:430f44669f94 196 h = h + 360;
kkizirian 5:430f44669f94 197 if (h > 360)
kkizirian 5:430f44669f94 198 h = h - 360;
kkizirian 5:430f44669f94 199 xEnd = 0;
kkizirian 5:430f44669f94 200 yEnd = 0;
kkizirian 5:430f44669f94 201 float rads = 0.0;
kkizirian 5:430f44669f94 202 //uLCD.cls();
kkizirian 5:430f44669f94 203 if (h < 90) {
kkizirian 5:430f44669f94 204 rads = h * PI / 180;
kkizirian 5:430f44669f94 205 xEnd = floor(63 * sin(rads) + .5);
kkizirian 5:430f44669f94 206 yEnd = floor(63 * cos(rads) + .5);
kkizirian 5:430f44669f94 207 xEnd = 63 + xEnd;
kkizirian 5:430f44669f94 208 yEnd = 63 + yEnd;
kkizirian 5:430f44669f94 209 } else if (90 < h < 180) {
kkizirian 5:430f44669f94 210 h = h - 90;
kkizirian 5:430f44669f94 211 rads = h * PI / 180;
kkizirian 5:430f44669f94 212 xEnd = floor(63 * cos(rads) + .5);
kkizirian 5:430f44669f94 213 yEnd = floor(63 * sin(rads) + .5);
kkizirian 5:430f44669f94 214 xEnd = 63 + xEnd;
kkizirian 5:430f44669f94 215 yEnd = 63 - yEnd;
kkizirian 5:430f44669f94 216 } else if (180 < h < 270) {
kkizirian 5:430f44669f94 217 h = h - 180;
kkizirian 5:430f44669f94 218 rads = h * PI / 180;
kkizirian 5:430f44669f94 219 xEnd = floor(63 * sin(rads) + .5);
kkizirian 5:430f44669f94 220 yEnd = floor(63 * cos(rads) + .5);
kkizirian 5:430f44669f94 221 xEnd = 63 - xEnd;
kkizirian 5:430f44669f94 222 yEnd = 63 - yEnd;
kkizirian 5:430f44669f94 223 } else if (270 < h) {
kkizirian 5:430f44669f94 224 h = h - 270;
kkizirian 5:430f44669f94 225 rads = h * PI / 180;
kkizirian 5:430f44669f94 226 xEnd = floor(63 * cos(rads) + .5);
kkizirian 5:430f44669f94 227 yEnd = floor(63 * sin(rads) + .5);
kkizirian 5:430f44669f94 228 xEnd = 63 - xEnd;
kkizirian 5:430f44669f94 229 yEnd = 63 + yEnd;
kkizirian 5:430f44669f94 230 }
kkizirian 2:c708e2027970 231 uLCD.cls();
kkizirian 5:430f44669f94 232 uLCD.line(63, 63, xEnd, yEnd, WHITE);
kkizirian 5:430f44669f94 233 break;
kkizirian 5:430f44669f94 234 case 2:
kkizirian 5:430f44669f94 235 //uLCD.cls();
nkela6 16:4b4e6cf66e7c 236 //uLCD.text_mode(TRANSPARENT);
kkizirian 7:e4d667beaf33 237 uLCD.locate(0,2);
kkizirian 8:3266cc9eda81 238 uLCD.printf(" ");
kkizirian 6:ceb6a5426add 239 uLCD.locate(0,3);
kkizirian 5:430f44669f94 240 uLCD.printf("Time to next class");
kkizirian 6:ceb6a5426add 241 uLCD.locate(0, 4);
kkizirian 5:430f44669f94 242 uLCD.printf("%02dhrs %02dminutes", hoursToNextClass, minutesToNextClass);
kkizirian 6:ceb6a5426add 243 uLCD.locate(0, 5);
kkizirian 5:430f44669f94 244 uLCD.printf("%02dseconds", secondsToNextClass);
nkela6 15:75a121b14e5d 245 uLCD.locate(0,7);
nkela6 15:75a121b14e5d 246 uLCD.printf("ETA: (min)");
nkela6 15:75a121b14e5d 247 uLCD.locate(0,8);
nkela6 15:75a121b14e5d 248 uLCD.printf("%f",eta);
nkela6 15:75a121b14e5d 249 uLCD.locate(0,10);
nkela6 15:75a121b14e5d 250 uLCD.printf("Displacement:");
nkela6 15:75a121b14e5d 251 uLCD.locate(0,11);
nkela6 15:75a121b14e5d 252 uLCD.printf("%f",dis);
kkizirian 6:ceb6a5426add 253 uLCD.locate(0, 0);
kkizirian 7:e4d667beaf33 254 uLCD.printf("Next class ");
kkizirian 5:430f44669f94 255 if (nextClass != currClass) {
kkizirian 6:ceb6a5426add 256 uLCD.locate(0, 1);
kkizirian 5:430f44669f94 257 uLCD.printf(" ");
kkizirian 6:ceb6a5426add 258 uLCD.locate(0, 1);
kkizirian 5:430f44669f94 259 uLCD.printf("%s", courseVec[nextClass].getDisplayString());
kkizirian 5:430f44669f94 260 }
kkizirian 5:430f44669f94 261 currClass = nextClass;
kkizirian 5:430f44669f94 262 break;
kkizirian 5:430f44669f94 263 }
kkizirian 0:ce60014510e5 264 }
kkizirian 0:ce60014510e5 265 }
kkizirian 0:ce60014510e5 266 }
kkizirian 0:ce60014510e5 267
kkizirian 2:c708e2027970 268 void readClassFile(vector<Course>& cVec)
kkizirian 2:c708e2027970 269 {
kkizirian 0:ce60014510e5 270 cVec.clear();
kkizirian 0:ce60014510e5 271
kkizirian 0:ce60014510e5 272 FILE *readFp = fopen("/sd/classdir/classes.txt", "r");
kkizirian 0:ce60014510e5 273 char line[15];
kkizirian 0:ce60014510e5 274 char buildingBuf[4];
kkizirian 0:ce60014510e5 275 char hourBuf[3];
kkizirian 0:ce60014510e5 276 int hour;
kkizirian 0:ce60014510e5 277 char minuteBuf[3];
kkizirian 0:ce60014510e5 278 int minute;
kkizirian 0:ce60014510e5 279 char ampmBuf[3];
kkizirian 0:ce60014510e5 280 uLCD.cls();
kkizirian 0:ce60014510e5 281 uLCD.locate(0, 1);
kkizirian 0:ce60014510e5 282 uLCD.printf("Reading class file...");
kkizirian 2:c708e2027970 283
kkizirian 0:ce60014510e5 284 memset(buildingBuf, 0, sizeof(buildingBuf));
kkizirian 0:ce60014510e5 285 memset(hourBuf, 0, sizeof(hourBuf));
kkizirian 0:ce60014510e5 286 memset(minuteBuf, 0, sizeof(minuteBuf));
kkizirian 0:ce60014510e5 287 memset(ampmBuf, 0, sizeof(ampmBuf));
kkizirian 0:ce60014510e5 288 memset(line, 0, sizeof(line));
kkizirian 0:ce60014510e5 289
kkizirian 0:ce60014510e5 290 if (readFp == NULL)
kkizirian 0:ce60014510e5 291 return;
kkizirian 0:ce60014510e5 292 else {
kkizirian 0:ce60014510e5 293 while (!feof(readFp)) {
kkizirian 0:ce60014510e5 294 fgets(line, 15, readFp);
kkizirian 0:ce60014510e5 295 if(line[8] == NULL)
kkizirian 0:ce60014510e5 296 continue;
kkizirian 0:ce60014510e5 297 memcpy(buildingBuf, line, 3);
kkizirian 0:ce60014510e5 298 memcpy(hourBuf, &line[4], 2);
kkizirian 0:ce60014510e5 299 memcpy(minuteBuf, &line[7], 2);
kkizirian 0:ce60014510e5 300 memcpy(ampmBuf, &line[10], 2);
kkizirian 0:ce60014510e5 301
kkizirian 0:ce60014510e5 302 string building = buildingBuf;
kkizirian 0:ce60014510e5 303 hour = atoi(hourBuf);
kkizirian 0:ce60014510e5 304 minute = atoi(minuteBuf);
kkizirian 0:ce60014510e5 305 string ampm = ampmBuf;
kkizirian 0:ce60014510e5 306
kkizirian 0:ce60014510e5 307 Course temp(building, hour, minute, ampm);
kkizirian 0:ce60014510e5 308 cVec.push_back(temp);
kkizirian 0:ce60014510e5 309 }
kkizirian 0:ce60014510e5 310 }
kkizirian 0:ce60014510e5 311 fclose(readFp);
kkizirian 0:ce60014510e5 312 return;
kkizirian 2:c708e2027970 313 }
kkizirian 2:c708e2027970 314
kkizirian 2:c708e2027970 315 void displayCourseVec()
kkizirian 2:c708e2027970 316 {
nkela6 16:4b4e6cf66e7c 317 //uLCD.text_mode(TRANSPARENT);
kkizirian 2:c708e2027970 318 if (courseVec.size() == 0) {
kkizirian 2:c708e2027970 319 uLCD.cls();
kkizirian 2:c708e2027970 320 uLCD.locate(0,0);
kkizirian 2:c708e2027970 321 uLCD.printf("No classes input!");
kkizirian 2:c708e2027970 322 uLCD.locate(0,1);
kkizirian 2:c708e2027970 323 } else {
kkizirian 2:c708e2027970 324 uLCD.cls();
kkizirian 2:c708e2027970 325 uLCD.locate(0,1);
kkizirian 2:c708e2027970 326 for (int i = 0; i < courseVec.size(); i++) {
kkizirian 2:c708e2027970 327 uLCD.locate(0, i);
kkizirian 2:c708e2027970 328 uLCD.printf("%s", courseVec[i].getDisplayString());
kkizirian 2:c708e2027970 329 }
kkizirian 2:c708e2027970 330 }
kkizirian 3:0ca91f8afec5 331 }
kkizirian 3:0ca91f8afec5 332
kkizirian 3:0ca91f8afec5 333 float calculateHeading(float mx, float my)
kkizirian 3:0ca91f8afec5 334 {
kkizirian 3:0ca91f8afec5 335 float heading = 0.0;
kkizirian 3:0ca91f8afec5 336 if (my == 0.0)
kkizirian 3:0ca91f8afec5 337 heading = (mx < 0.0) ? 180.0 : 0.0;
kkizirian 3:0ca91f8afec5 338 else
kkizirian 3:0ca91f8afec5 339 heading = atan2(mx, my)*360.0/(2.0*PI);
kkizirian 3:0ca91f8afec5 340 //pc.printf("heading atan=%f \n\r",heading);
kkizirian 3:0ca91f8afec5 341 heading -= DECLINATION; //correct for geo location
kkizirian 3:0ca91f8afec5 342 if(heading>180.0) heading = heading - 360.0;
kkizirian 3:0ca91f8afec5 343 else if(heading<-180.0) heading = 360.0 + heading;
kkizirian 3:0ca91f8afec5 344 else if(heading<0.0) heading = 360.0 + heading;
kkizirian 3:0ca91f8afec5 345
kkizirian 3:0ca91f8afec5 346 // Convert everything from radians to degrees:
kkizirian 3:0ca91f8afec5 347 //heading *= 180.0 / PI;
kkizirian 3:0ca91f8afec5 348
kkizirian 3:0ca91f8afec5 349 //pc.printf("Magnetic Heading: %f degress\n\r",heading);
kkizirian 3:0ca91f8afec5 350 return heading;
kkizirian 3:0ca91f8afec5 351 }
kkizirian 3:0ca91f8afec5 352
kkizirian 3:0ca91f8afec5 353 float computeAngleToDestination(float diffLat, float diffLong)
kkizirian 3:0ca91f8afec5 354 {
kkizirian 3:0ca91f8afec5 355 float angle = 0.0;
kkizirian 3:0ca91f8afec5 356
kkizirian 3:0ca91f8afec5 357 if (diffLat > 0) {
kkizirian 3:0ca91f8afec5 358 if (diffLong > 0) {
kkizirian 3:0ca91f8afec5 359 // in quadrant 1
kkizirian 3:0ca91f8afec5 360 angle = 180*atan2(diffLat,diffLong)/PI;
kkizirian 3:0ca91f8afec5 361 } else {
kkizirian 3:0ca91f8afec5 362 // in quadrant 2
kkizirian 3:0ca91f8afec5 363 angle = 180*atan2(diffLat,-1 * diffLong)/PI;
kkizirian 3:0ca91f8afec5 364 angle = 180 - angle;
kkizirian 3:0ca91f8afec5 365 }
kkizirian 3:0ca91f8afec5 366 } else {
kkizirian 3:0ca91f8afec5 367 if (diffLong > 0) {
kkizirian 3:0ca91f8afec5 368 // in quadrant 4
kkizirian 3:0ca91f8afec5 369 angle = 180*atan2(-1*diffLat, diffLong)/PI;
kkizirian 3:0ca91f8afec5 370 angle = 360 - angle;
kkizirian 3:0ca91f8afec5 371 } else {
kkizirian 3:0ca91f8afec5 372 // in quadrant 3
kkizirian 3:0ca91f8afec5 373 angle = 180*atan2(-1*diffLat, -1*diffLong)/PI;
kkizirian 3:0ca91f8afec5 374 angle = 180 + angle;
kkizirian 3:0ca91f8afec5 375 }
kkizirian 3:0ca91f8afec5 376 }
kkizirian 3:0ca91f8afec5 377
kkizirian 3:0ca91f8afec5 378 //pc.printf("Angle to Destination: %f degress\n\r",angle);
kkizirian 3:0ca91f8afec5 379 return angle;
kkizirian 5:430f44669f94 380 }
kkizirian 5:430f44669f94 381
kkizirian 5:430f44669f94 382 int getNextClass(vector<Course>& cVec)
kkizirian 5:430f44669f94 383 {
kkizirian 5:430f44669f94 384 int numIterations = 0;
kkizirian 5:430f44669f94 385 for (int i = 0; i < cVec.size(); i++) {
kkizirian 5:430f44669f94 386 numIterations++;
kkizirian 5:430f44669f94 387 pc.printf("Iteration : %i\ncurrHour: %i\ncourseHour: %i\n", numIterations, currHour, cVec[i].getHour_inMilitaryTime());
kkizirian 5:430f44669f94 388 if (courseVec[i].getHour_inMilitaryTime() < currHour)
kkizirian 5:430f44669f94 389 continue;
kkizirian 5:430f44669f94 390 else if (courseVec[i].getHour_inMilitaryTime() > currHour)
kkizirian 5:430f44669f94 391 return i;
kkizirian 5:430f44669f94 392 else if (courseVec[i].getHour_inMilitaryTime() == currHour) {
kkizirian 5:430f44669f94 393 if (courseVec[i].getMinute() <= currMinute)
kkizirian 5:430f44669f94 394 continue;
kkizirian 5:430f44669f94 395 else if (courseVec[i].getMinute() > currMinute) {
kkizirian 5:430f44669f94 396 return i;
kkizirian 5:430f44669f94 397 }
kkizirian 5:430f44669f94 398 }
kkizirian 5:430f44669f94 399 if (numIterations == cVec.size())
kkizirian 5:430f44669f94 400 return 0;
kkizirian 5:430f44669f94 401 }
kkizirian 5:430f44669f94 402 return 0;
kkizirian 5:430f44669f94 403 }
kkizirian 5:430f44669f94 404
kkizirian 5:430f44669f94 405 void timeToNextClass()
kkizirian 6:ceb6a5426add 406 {
kkizirian 5:430f44669f94 407 hoursToNextClass = courseVec[nextClass].getHour_inMilitaryTime() - currHour;
kkizirian 5:430f44669f94 408 if (hoursToNextClass < 0)
kkizirian 5:430f44669f94 409 hoursToNextClass += 24;
kkizirian 5:430f44669f94 410
kkizirian 5:430f44669f94 411 minutesToNextClass = courseVec[nextClass].getMinute() - currMinute;
kkizirian 5:430f44669f94 412 if (minutesToNextClass < 0) {
kkizirian 5:430f44669f94 413 hoursToNextClass--;
kkizirian 5:430f44669f94 414 minutesToNextClass += 60;
kkizirian 5:430f44669f94 415 }
kkizirian 6:ceb6a5426add 416
kkizirian 5:430f44669f94 417 secondsToNextClass = 60 - currSecond;
kkizirian 5:430f44669f94 418 minutesToNextClass--;
kkizirian 6:ceb6a5426add 419
kkizirian 5:430f44669f94 420 if (minutesToNextClass < 0) {
kkizirian 5:430f44669f94 421 hoursToNextClass--;
kkizirian 5:430f44669f94 422 minutesToNextClass += 60;
kkizirian 5:430f44669f94 423 }
kkizirian 6:ceb6a5426add 424
kkizirian 5:430f44669f94 425 if (hoursToNextClass < 0)
kkizirian 5:430f44669f94 426 hoursToNextClass += 24;
kkizirian 6:ceb6a5426add 427 }
kkizirian 6:ceb6a5426add 428
kkizirian 6:ceb6a5426add 429 // convert from DDMM.mmmm to decimal
kkizirian 9:5b871fb9f554 430 float latToDecimal(float latitudeInDDMM, char northOrSouth)
kkizirian 6:ceb6a5426add 431 {
kkizirian 6:ceb6a5426add 432 float latInDec = 0.0;
kkizirian 6:ceb6a5426add 433 float temp = 0.0;
kkizirian 6:ceb6a5426add 434 latInDec = latitudeInDDMM / 100;
kkizirian 6:ceb6a5426add 435 temp = latInDec - floor(latInDec);
kkizirian 6:ceb6a5426add 436 temp = temp / .6;
kkizirian 6:ceb6a5426add 437 latInDec = floor(latInDec) + temp;
kkizirian 10:5579618161ef 438 if (northOrSouth == 'S')
kkizirian 9:5b871fb9f554 439 return -1 * latInDec;
kkizirian 6:ceb6a5426add 440 return latInDec;
kkizirian 6:ceb6a5426add 441 }
kkizirian 6:ceb6a5426add 442
nkela6 13:0eb9fbd63fb2 443 float displacement(float diffLat,float diffLong)
nkela6 13:0eb9fbd63fb2 444 {
nkela6 14:d8a6134cafa3 445 float d = sqrt(diffLat*diffLat+diffLong*diffLong);
nkela6 14:d8a6134cafa3 446 return d;
nkela6 13:0eb9fbd63fb2 447 }
nkela6 13:0eb9fbd63fb2 448 float calc_eta(float displacement, float speed)
nkela6 13:0eb9fbd63fb2 449 {
nkela6 15:75a121b14e5d 450 float estTimeArr = displacement/speed; // for displacement. take average speed // convert to minutes later
nkela6 15:75a121b14e5d 451 estTimeArr = estTimeArr+ 0.25*estTimeArr; //adding delay for distance vs displacement and traffic lights
nkela6 15:75a121b14e5d 452 return estTimeArr;
nkela6 14:d8a6134cafa3 453 }
nkela6 13:0eb9fbd63fb2 454
kkizirian 9:5b871fb9f554 455 float longToDecimal(float longitudeInDDMM, char eastOrWest)
kkizirian 6:ceb6a5426add 456 {
kkizirian 6:ceb6a5426add 457 float longInDec = 0.0;
kkizirian 6:ceb6a5426add 458 float temp = 0.0;
kkizirian 6:ceb6a5426add 459 longInDec = longitudeInDDMM / 100;
kkizirian 6:ceb6a5426add 460 temp = longInDec - floor(longInDec);
kkizirian 6:ceb6a5426add 461 temp = temp / .6;
kkizirian 6:ceb6a5426add 462 longInDec = floor(longInDec) + temp;
kkizirian 9:5b871fb9f554 463 if (eastOrWest == 'E')
kkizirian 9:5b871fb9f554 464 return longInDec;
kkizirian 6:ceb6a5426add 465 return -1 * longInDec;
nkela6 13:0eb9fbd63fb2 466 }
nkela6 13:0eb9fbd63fb2 467
nkela6 13:0eb9fbd63fb2 468 bool late(float eta)
nkela6 13:0eb9fbd63fb2 469 {
nkela6 13:0eb9fbd63fb2 470 float totalMinutes = hoursToNextClass*60+ minutesToNextClass+ secondsToNextClass/60;
nkela6 17:1de531cd3d4a 471 if(eta>totalMinutes) {
nkela6 13:0eb9fbd63fb2 472 return 1;
nkela6 14:d8a6134cafa3 473 } else {
nkela6 13:0eb9fbd63fb2 474 return 0;
nkela6 13:0eb9fbd63fb2 475 }
nkela6 14:d8a6134cafa3 476 }
nkela6 17:1de531cd3d4a 477