diff --git a/Cooler/Cooler.ino b/Cooler/Cooler.ino deleted file mode 100644 index 98db17c..0000000 --- a/Cooler/Cooler.ino +++ /dev/null @@ -1,401 +0,0 @@ -#define BLYNK_USE_DIRECT_CONNECT - -// Imports -#include -#include -#include -#include -#include -#include -#include "./TinyGPS.h" // Use local version of this library -#include "./CoolerDefinitions.h" - -// GPS -TinyGPS gps; - -// Lid -Servo lidServo; -CoolerLid lidState = CLOSED; - -// Master Enable -bool enabled = false; - -//WidgetTerminal terminal(V3); - -// Serial components -SoftwareSerial bluetoothSerial(BLUETOOTH_TX_PIN, BLUETOOTH_RX_PIN); -SoftwareSerial nss(GPS_TX_PIN, 255); // TXD to digital pin 6 - -/* Compass */ -Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345); - -GeoLoc checkGPS() { - Serial.println("Reading onboard GPS: "); - bool newdata = false; - unsigned long start = millis(); - while (millis() - start < GPS_UPDATE_INTERVAL) { - if (feedgps()) - newdata = true; - } - if (newdata) { - return gpsdump(gps); - } - - GeoLoc coolerLoc; - coolerLoc.lat = 0.0; - coolerLoc.lon = 0.0; - - return coolerLoc; -} - -// Get and process GPS data -GeoLoc gpsdump(TinyGPS &gps) { - float flat, flon; - unsigned long age; - - gps.f_get_position(&flat, &flon, &age); - - GeoLoc coolerLoc; - coolerLoc.lat = flat; - coolerLoc.lon = flon; - - Serial.print(coolerLoc.lat, 7); Serial.print(", "); Serial.println(coolerLoc.lon, 7); - - return coolerLoc; -} - -// Feed data as it becomes available -bool feedgps() { - while (nss.available()) { - if (gps.encode(nss.read())) - return true; - } - return false; -} - -// Lid Hook -BLYNK_WRITE(V0) { - switch (lidState) { - case OPENED: - setServo(SERVO_LID_CLOSE); - lidState = CLOSED; - break; - case CLOSED: - setServo(SERVO_LID_OPEN); - lidState = OPENED; - break; - } -} - -// Killswitch Hook -BLYNK_WRITE(V1) { - enabled = !enabled; - - //Stop the wheels - stop(); -} - -// GPS Streaming Hook -BLYNK_WRITE(V2) { - GpsParam gps(param); - - Serial.println("Received remote GPS: "); - - // Print 7 decimal places for Lat - Serial.print(gps.getLat(), 7); Serial.print(", "); Serial.println(gps.getLon(), 7); - - GeoLoc phoneLoc; - phoneLoc.lat = gps.getLat(); - phoneLoc.lon = gps.getLon(); - - driveTo(phoneLoc, GPS_STREAM_TIMEOUT); -} - -// Terminal Hook -BLYNK_WRITE(V3) { - Serial.print("Received Text: "); - Serial.println(param.asStr()); - - String rawInput(param.asStr()); - int colonIndex; - int commaIndex; - - do { - commaIndex = rawInput.indexOf(','); - colonIndex = rawInput.indexOf(':'); - - if (commaIndex != -1) { - String latStr = rawInput.substring(0, commaIndex); - String lonStr = rawInput.substring(commaIndex+1); - - if (colonIndex != -1) { - lonStr = rawInput.substring(commaIndex+1, colonIndex); - } - - float lat = latStr.toFloat(); - float lon = lonStr.toFloat(); - - if (lat != 0 && lon != 0) { - GeoLoc waypoint; - waypoint.lat = lat; - waypoint.lon = lon; - - Serial.print("Waypoint found: "); Serial.print(lat); Serial.println(lon); - driveTo(waypoint, GPS_WAYPOINT_TIMEOUT); - } - } - - rawInput = rawInput.substring(colonIndex + 1); - - } while (colonIndex != -1); -} - -void displayCompassDetails(void) -{ - sensor_t sensor; - mag.getSensor(&sensor); - Serial.println("------------------------------------"); - Serial.print ("Sensor: "); Serial.println(sensor.name); - Serial.print ("Driver Ver: "); Serial.println(sensor.version); - Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id); - Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" uT"); - Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" uT"); - Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" uT"); - Serial.println("------------------------------------"); - Serial.println(""); - delay(500); -} - -#ifndef DEGTORAD -#define DEGTORAD 0.0174532925199432957f -#define RADTODEG 57.295779513082320876f -#endif - -float geoBearing(struct GeoLoc &a, struct GeoLoc &b) { - float y = sin(b.lon-a.lon) * cos(b.lat); - float x = cos(a.lat)*sin(b.lat) - sin(a.lat)*cos(b.lat)*cos(b.lon-a.lon); - return atan2(y, x) * RADTODEG; -} - -float geoDistance(struct GeoLoc &a, struct GeoLoc &b) { - const float R = 6371000; // km - float p1 = a.lat * DEGTORAD; - float p2 = b.lat * DEGTORAD; - float dp = (b.lat-a.lat) * DEGTORAD; - float dl = (b.lon-a.lon) * DEGTORAD; - - float x = sin(dp/2) * sin(dp/2) + cos(p1) * cos(p2) * sin(dl/2) * sin(dl/2); - float y = 2 * atan2(sqrt(x), sqrt(1-x)); - - return R * y; -} - -float geoHeading() { - /* Get a new sensor event */ - sensors_event_t event; - mag.getEvent(&event); - - // Hold the module so that Z is pointing 'up' and you can measure the heading with x&y - // Calculate heading when the magnetometer is level, then correct for signs of axis. - float heading = atan2(event.magnetic.y, event.magnetic.x); - - // Offset - heading -= DECLINATION_ANGLE; - heading -= COMPASS_OFFSET; - - // Correct for when signs are reversed. - if(heading < 0) - heading += 2*PI; - - // Check for wrap due to addition of declination. - if(heading > 2*PI) - heading -= 2*PI; - - // Convert radians to degrees for readability. - float headingDegrees = heading * 180/M_PI; - - // Map to -180 - 180 - while (headingDegrees < -180) headingDegrees += 360; - while (headingDegrees > 180) headingDegrees -= 360; - - return headingDegrees; -} - -void setServo(int pos) { - lidServo.attach(SERVO_PIN); - lidServo.write(pos); - delay(2000); - lidServo.detach(); -} - -void setSpeedMotorA(int speed) { - digitalWrite(MOTOR_A_IN_1_PIN, LOW); - digitalWrite(MOTOR_A_IN_2_PIN, HIGH); - - // set speed to 200 out of possible range 0~255 - analogWrite(MOTOR_A_EN_PIN, speed + MOTOR_A_OFFSET); -} - -void setSpeedMotorB(int speed) { - digitalWrite(MOTOR_B_IN_1_PIN, LOW); - digitalWrite(MOTOR_B_IN_2_PIN, HIGH); - - // set speed to 200 out of possible range 0~255 - analogWrite(MOTOR_B_EN_PIN, speed + MOTOR_B_OFFSET); -} - -void setSpeed(int speed) -{ - // this function will run the motors in both directions at a fixed speed - // turn on motor A - setSpeedMotorA(speed); - - // turn on motor B - setSpeedMotorB(speed); -} - -void stop() { - // now turn off motors - digitalWrite(MOTOR_A_IN_1_PIN, LOW); - digitalWrite(MOTOR_A_IN_2_PIN, LOW); - digitalWrite(MOTOR_B_IN_1_PIN, LOW); - digitalWrite(MOTOR_B_IN_2_PIN, LOW); -} - -void drive(int distance, float turn) { - int fullSpeed = 230; - int stopSpeed = 0; - - // drive to location - int s = fullSpeed; - if ( distance < 8 ) { - int wouldBeSpeed = s - stopSpeed; - wouldBeSpeed *= distance / 8.0f; - s = stopSpeed + wouldBeSpeed; - } - - int autoThrottle = constrain(s, stopSpeed, fullSpeed); - autoThrottle = 230; - - float t = turn; - while (t < -180) t += 360; - while (t > 180) t -= 360; - - Serial.print("turn: "); - Serial.println(t); - Serial.print("original: "); - Serial.println(turn); - - float t_modifier = (180.0 - abs(t)) / 180.0; - float autoSteerA = 1; - float autoSteerB = 1; - - if (t < 0) { - autoSteerB = t_modifier; - } else if (t > 0){ - autoSteerA = t_modifier; - } - - Serial.print("steerA: "); Serial.println(autoSteerA); - Serial.print("steerB: "); Serial.println(autoSteerB); - - int speedA = (int) (((float) autoThrottle) * autoSteerA); - int speedB = (int) (((float) autoThrottle) * autoSteerB); - - setSpeedMotorA(speedA); - setSpeedMotorB(speedB); -} - -void driveTo(struct GeoLoc &loc, int timeout) { - nss.listen(); - GeoLoc coolerLoc = checkGPS(); - bluetoothSerial.listen(); - - if (coolerLoc.lat != 0 && coolerLoc.lon != 0 && enabled) { - float d = 0; - //Start move loop here - do { - nss.listen(); - coolerLoc = checkGPS(); - bluetoothSerial.listen(); - - d = geoDistance(coolerLoc, loc); - float t = geoBearing(coolerLoc, loc) - geoHeading(); - - Serial.print("Distance: "); - Serial.println(geoDistance(coolerLoc, loc)); - - Serial.print("Bearing: "); - Serial.println(geoBearing(coolerLoc, loc)); - - Serial.print("heading: "); - Serial.println(geoHeading()); - - drive(d, t); - timeout -= 1; - } while (d > 3.0 && enabled && timeout>0); - - stop(); - } -} - -void setupCompass() { - /* Initialise the compass */ - if(!mag.begin()) - { - /* There was a problem detecting the HMC5883 ... check your connections */ - Serial.println("Ooops, no HMC5883 detected ... Check your wiring!"); - while(1); - } - - /* Display some basic information on this sensor */ - displayCompassDetails(); -} - -void setup() -{ - // Compass - setupCompass(); - - // Motor pins - pinMode(MOTOR_A_EN_PIN, OUTPUT); - pinMode(MOTOR_B_EN_PIN, OUTPUT); - pinMode(MOTOR_A_IN_1_PIN, OUTPUT); - pinMode(MOTOR_A_IN_2_PIN, OUTPUT); - pinMode(MOTOR_B_IN_1_PIN, OUTPUT); - pinMode(MOTOR_B_IN_2_PIN, OUTPUT); - - pinMode(LED_BUILTIN, OUTPUT); - digitalWrite(LED_BUILTIN, HIGH); - - //Debugging via serial - Serial.begin(4800); - - //GPS - nss.begin(9600); - - //Bluetooth - bluetoothSerial.begin(9600); - Blynk.begin(bluetoothSerial, auth); -} - -// Testing -void testDriveNorth() { - float heading = geoHeading(); - int testDist = 10; - Serial.println(heading); - - while(!(heading < 5 && heading > -5)) { - drive(testDist, heading); - heading = geoHeading(); - Serial.println(heading); - delay(500); - } - - stop(); -} - -void loop() -{ - Blynk.run(); -} diff --git a/Cooler/CoolerDefinitions.h b/Cooler/CoolerDefinitions.h deleted file mode 100644 index 434251d..0000000 --- a/Cooler/CoolerDefinitions.h +++ /dev/null @@ -1,57 +0,0 @@ -// Blynk Auth -char auth[] = "blynk-token"; - -// Pin variables -#define SERVO_PIN 3 - -#define GPS_TX_PIN 6 - -#define BLUETOOTH_TX_PIN 10 -#define BLUETOOTH_RX_PIN 11 - -#define MOTOR_A_EN_PIN 5 -#define MOTOR_B_EN_PIN 9 -#define MOTOR_A_IN_1_PIN 7 -#define MOTOR_A_IN_2_PIN 8 -#define MOTOR_B_IN_1_PIN 12 -#define MOTOR_B_IN_2_PIN 4 - -// If one motor tends to spin faster than the other, add offset -#define MOTOR_A_OFFSET 20 -#define MOTOR_B_OFFSET 0 - -// You must then add your 'Declination Angle' to the compass, which is the 'Error' of the magnetic field in your location. -// Find yours here: http://www.magnetic-declination.com/ -// Mine is: 13° 24' E (Positive), which is ~13 Degrees, or (which we need) 0.23 radians -#define DECLINATION_ANGLE 0.23f - -// The offset of the mounting position to true north -// It would be best to run the /examples/magsensor sketch and compare to the compass on your smartphone -#define COMPASS_OFFSET 0.0f - -// How often the GPS should update in MS -// Keep this above 1000 -#define GPS_UPDATE_INTERVAL 1000 - -// Number of changes in movement to timeout for GPS streaming -// Keeps the cooler from driving away if there is a problem -#define GPS_STREAM_TIMEOUT 18 - -// Number of changes in movement to timeout for GPS waypoints -// Keeps the cooler from driving away if there is a problem -#define GPS_WAYPOINT_TIMEOUT 45 - -// PWM write for servo locations -#define SERVO_LID_OPEN 20 -#define SERVO_LID_CLOSE 165 - -// Definitions (don't edit these) -struct GeoLoc { - float lat; - float lon; -}; - -enum CoolerLid { - OPENED, - CLOSED -}; diff --git a/Cooler/TinyGPS.cpp b/Cooler/TinyGPS.cpp deleted file mode 100644 index 61b58f5..0000000 --- a/Cooler/TinyGPS.cpp +++ /dev/null @@ -1,306 +0,0 @@ -/* - TinyGPS - a small GPS library for Arduino providing basic NMEA parsing - Based on work by and "distance_to" courtesy of Maarten Lamers. - Copyright (C) 2008-2011 Mikal Hart - All rights reserved. - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - - **This is an unofficial version. It is altered to allow use in Arduino 1.0** - -*/ - -#include "Arduino.h" -#include "TinyGPS.h" - -#define _GPRMC_TERM "GPRMC" -#define _GPGGA_TERM "GPGGA" - -TinyGPS::TinyGPS() -: _time(GPS_INVALID_TIME) -, _date(GPS_INVALID_DATE) -, _latitude(GPS_INVALID_ANGLE) -, _longitude(GPS_INVALID_ANGLE) -, _altitude(GPS_INVALID_ALTITUDE) -, _speed(GPS_INVALID_SPEED) -, _course(GPS_INVALID_ANGLE) -, _last_time_fix(GPS_INVALID_FIX_TIME) -, _last_position_fix(GPS_INVALID_FIX_TIME) -, _parity(0) -, _is_checksum_term(false) -, _sentence_type(_GPS_SENTENCE_OTHER) -, _term_number(0) -, _term_offset(0) -, _gps_data_good(false) -#ifndef _GPS_NO_STATS -, _encoded_characters(0) -, _good_sentences(0) -, _failed_checksum(0) -#endif -{ - _term[0] = '\0'; -} - -// -// public methods -// - -bool TinyGPS::encode(char c) -{ - bool valid_sentence = false; - - ++_encoded_characters; - switch(c) - { - case ',': // term terminators - _parity ^= c; - case '\r': - case '\n': - case '*': - if (_term_offset < sizeof(_term)) - { - _term[_term_offset] = 0; - valid_sentence = term_complete(); - } - ++_term_number; - _term_offset = 0; - _is_checksum_term = c == '*'; - return valid_sentence; - - case '$': // sentence begin - _term_number = _term_offset = 0; - _parity = 0; - _sentence_type = _GPS_SENTENCE_OTHER; - _is_checksum_term = false; - _gps_data_good = false; - return valid_sentence; - } - - // ordinary characters - if (_term_offset < sizeof(_term) - 1) - _term[_term_offset++] = c; - if (!_is_checksum_term) - _parity ^= c; - - return valid_sentence; -} - -#ifndef _GPS_NO_STATS -void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs) -{ - if (chars) *chars = _encoded_characters; - if (sentences) *sentences = _good_sentences; - if (failed_cs) *failed_cs = _failed_checksum; -} -#endif - -// -// internal utilities -// -int TinyGPS::from_hex(char a) -{ - if (a >= 'A' && a <= 'F') - return a - 'A' + 10; - else if (a >= 'a' && a <= 'f') - return a - 'a' + 10; - else - return a - '0'; -} - -unsigned long TinyGPS::parse_decimal() -{ - char *p = _term; - bool isneg = *p == '-'; - if (isneg) ++p; - unsigned long ret = 100UL * gpsatol(p); - while (gpsisdigit(*p)) ++p; - if (*p == '.') - { - if (gpsisdigit(p[1])) - { - ret += 10 * (p[1] - '0'); - if (gpsisdigit(p[2])) - ret += p[2] - '0'; - } - } - return isneg ? -ret : ret; -} - -unsigned long TinyGPS::parse_degrees() -{ - char *p; - unsigned long left = gpsatol(_term); - unsigned long tenk_minutes = (left % 100UL) * 10000UL; - for (p=_term; gpsisdigit(*p); ++p); - if (*p == '.') - { - unsigned long mult = 1000; - while (gpsisdigit(*++p)) - { - tenk_minutes += mult * (*p - '0'); - mult /= 10; - } - } - return (left / 100) * 100000 + tenk_minutes / 6; -} - -// Processes a just-completed term -// Returns true if new sentence has just passed checksum test and is validated -bool TinyGPS::term_complete() -{ - if (_is_checksum_term) - { - byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]); - if (checksum == _parity) - { - if (_gps_data_good) - { -#ifndef _GPS_NO_STATS - ++_good_sentences; -#endif - _last_time_fix = _new_time_fix; - _last_position_fix = _new_position_fix; - - switch(_sentence_type) - { - case _GPS_SENTENCE_GPRMC: - _time = _new_time; - _date = _new_date; - _latitude = _new_latitude; - _longitude = _new_longitude; - _speed = _new_speed; - _course = _new_course; - break; - case _GPS_SENTENCE_GPGGA: - _altitude = _new_altitude; - _time = _new_time; - _latitude = _new_latitude; - _longitude = _new_longitude; - break; - } - - return true; - } - } - -#ifndef _GPS_NO_STATS - else - ++_failed_checksum; -#endif - return false; - } - - // the first term determines the sentence type - if (_term_number == 0) - { - if (!gpsstrcmp(_term, _GPRMC_TERM)) - _sentence_type = _GPS_SENTENCE_GPRMC; - else if (!gpsstrcmp(_term, _GPGGA_TERM)) - _sentence_type = _GPS_SENTENCE_GPGGA; - else - _sentence_type = _GPS_SENTENCE_OTHER; - return false; - } - - if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) - switch((_sentence_type == _GPS_SENTENCE_GPGGA ? 200 : 100) + _term_number) - { - case 101: // Time in both sentences - case 201: - _new_time = parse_decimal(); - _new_time_fix = millis(); - break; - case 102: // GPRMC validity - _gps_data_good = _term[0] == 'A'; - break; - case 103: // Latitude - case 202: - _new_latitude = parse_degrees(); - _new_position_fix = millis(); - break; - case 104: // N/S - case 203: - if (_term[0] == 'S') - _new_latitude = -_new_latitude; - break; - case 105: // Longitude - case 204: - _new_longitude = parse_degrees(); - break; - case 106: // E/W - case 205: - if (_term[0] == 'W') - _new_longitude = -_new_longitude; - break; - case 107: // Speed (GPRMC) - _new_speed = parse_decimal(); - break; - case 108: // Course (GPRMC) - _new_course = parse_decimal(); - break; - case 109: // Date (GPRMC) - _new_date = gpsatol(_term); - break; - case 206: // Fix data (GPGGA) - _gps_data_good = _term[0] > '0'; - break; - case 209: // Altitude (GPGGA) - _new_altitude = parse_decimal(); - break; - } - - return false; -} - -long TinyGPS::gpsatol(const char *str) -{ - long ret = 0; - while (gpsisdigit(*str)) - ret = 10 * ret + *str++ - '0'; - return ret; -} - -int TinyGPS::gpsstrcmp(const char *str1, const char *str2) -{ - while (*str1 && *str1 == *str2) - ++str1, ++str2; - return *str1; -} - -/* static */ -float TinyGPS::distance_between (float lat1, float long1, float lat2, float long2) -{ - // returns distance in meters between two positions, both specified - // as signed decimal-degrees latitude and longitude. Uses great-circle - // distance computation for hypothetical sphere of radius 6372795 meters. - // Because Earth is no exact sphere, rounding errors may be up to 0.5%. - // Courtesy of Maarten Lamers - float delta = radians(long1-long2); - float sdlong = sin(delta); - float cdlong = cos(delta); - lat1 = radians(lat1); - lat2 = radians(lat2); - float slat1 = sin(lat1); - float clat1 = cos(lat1); - float slat2 = sin(lat2); - float clat2 = cos(lat2); - delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); - delta = sq(delta); - delta += sq(clat2 * sdlong); - delta = sqrt(delta); - float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); - delta = atan2(delta, denom); - return delta * 6372795; -} diff --git a/Cooler/TinyGPS.h b/Cooler/TinyGPS.h deleted file mode 100644 index a252aa3..0000000 --- a/Cooler/TinyGPS.h +++ /dev/null @@ -1,167 +0,0 @@ -/* - TinyGPS - a small GPS library for Arduino providing basic NMEA parsing - Based on work by and "distance_to" courtesy of Maarten Lamers. - Copyright (C) 2008-2011 Mikal Hart - All rights reserved. - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - This library is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU - Lesser General Public License for more details. - - You should have received a copy of the GNU Lesser General Public - License along with this library; if not, write to the Free Software - Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA - - **This is an unofficial version. It is altered to allow use in Arduino 1.0** -*/ - -#ifndef TinyGPS_h -#define TinyGPS_h - -#include "Arduino.h" - -#define _GPS_VERSION 10 // software version of this library -#define _GPS_MPH_PER_KNOT 1.15077945 -#define _GPS_MPS_PER_KNOT 0.51444444 -#define _GPS_KMPH_PER_KNOT 1.852 -#define _GPS_MILES_PER_METER 0.00062137112 -#define _GPS_KM_PER_METER 0.001 -//#define _GPS_NO_STATS - -class TinyGPS -{ - public: - TinyGPS(); - bool encode(char c); // process one character received from GPS - TinyGPS &operator << (char c) {encode(c); return *this;} - - // lat/long in hundred thousandths of a degree and age of fix in milliseconds - inline void get_position(long *latitude, long *longitude, unsigned long *fix_age = 0) - { - if (latitude) *latitude = _latitude; - if (longitude) *longitude = _longitude; - if (fix_age) *fix_age = _last_position_fix == GPS_INVALID_FIX_TIME ? - GPS_INVALID_AGE : millis() - _last_position_fix; - } - - // date as ddmmyy, time as hhmmsscc, and age in milliseconds - inline void get_datetime(unsigned long *date, unsigned long *time, unsigned long *fix_age = 0) - { - if (date) *date = _date; - if (time) *time = _time; - if (fix_age) *fix_age = _last_time_fix == GPS_INVALID_FIX_TIME ? - GPS_INVALID_AGE : millis() - _last_time_fix; - } - - // signed altitude in centimeters (from GPGGA sentence) - inline long altitude() { return _altitude; } - - // course in last full GPRMC sentence in 100th of a degree - inline unsigned long course() { return _course; } - - // speed in last full GPRMC sentence in 100ths of a knot - unsigned long speed() { return _speed; } - -#ifndef _GPS_NO_STATS - void stats(unsigned long *chars, unsigned short *good_sentences, unsigned short *failed_cs); -#endif - - inline void f_get_position(float *latitude, float *longitude, unsigned long *fix_age = 0) - { - long lat, lon; - get_position(&lat, &lon, fix_age); - *latitude = lat / 100000.0; - *longitude = lon / 100000.0; - } - - inline void crack_datetime(int *year, byte *month, byte *day, - byte *hour, byte *minute, byte *second, byte *hundredths = 0, unsigned long *fix_age = 0) - { - unsigned long date, time; - get_datetime(&date, &time, fix_age); - if (year) - { - *year = date % 100; - *year += *year > 80 ? 1900 : 2000; - } - if (month) *month = (date / 100) % 100; - if (day) *day = date / 10000; - if (hour) *hour = time / 1000000; - if (minute) *minute = (time / 10000) % 100; - if (second) *second = (time / 100) % 100; - if (hundredths) *hundredths = time % 100; - } - - inline float f_altitude() { return altitude() / 100.0; } - inline float f_course() { return course() / 100.0; } - inline float f_speed_knots() { return speed() / 100.0; } - inline float f_speed_mph() { return _GPS_MPH_PER_KNOT * f_speed_knots(); } - inline float f_speed_mps() { return _GPS_MPS_PER_KNOT * f_speed_knots(); } - inline float f_speed_kmph() { return _GPS_KMPH_PER_KNOT * f_speed_knots(); } - - static int library_version() { return _GPS_VERSION; } - - enum {GPS_INVALID_AGE = 0xFFFFFFFF, GPS_INVALID_ANGLE = 999999999, GPS_INVALID_ALTITUDE = 999999999, GPS_INVALID_DATE = 0, - GPS_INVALID_TIME = 0xFFFFFFFF, GPS_INVALID_SPEED = 999999999, GPS_INVALID_FIX_TIME = 0xFFFFFFFF}; - - - static float distance_between (float lat1, float long1, float lat2, float long2); - -private: - enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_OTHER}; - - // properties - unsigned long _time, _new_time; - unsigned long _date, _new_date; - long _latitude, _new_latitude; - long _longitude, _new_longitude; - long _altitude, _new_altitude; - unsigned long _speed, _new_speed; - unsigned long _course, _new_course; - - unsigned long _last_time_fix, _new_time_fix; - unsigned long _last_position_fix, _new_position_fix; - - // parsing state variables - byte _parity; - bool _is_checksum_term; - char _term[15]; - byte _sentence_type; - byte _term_number; - byte _term_offset; - bool _gps_data_good; - -#ifndef _GPS_NO_STATS - // statistics - unsigned long _encoded_characters; - unsigned short _good_sentences; - unsigned short _failed_checksum; - unsigned short _passed_checksum; -#endif - - // internal utilities - int from_hex(char a); - unsigned long parse_decimal(); - unsigned long parse_degrees(); - bool term_complete(); - bool gpsisdigit(char c) { return c >= '0' && c <= '9'; } - long gpsatol(const char *str); - int gpsstrcmp(const char *str1, const char *str2); -}; - -// Arduino 0012 workaround -#undef int -#undef char -#undef long -#undef byte -#undef float -#undef abs -#undef round - -#endif diff --git a/README.md b/README.md index 9aa0bf6..48a0c42 100644 --- a/README.md +++ b/README.md @@ -1,100 +1,2 @@ # Autonomous-Follow-Me-Cooler -Our autonomous cooler has two modes: GPS Streaming and GPS Waypoint. While in GPS Streaming mode, the cooler will follow you by streaming coordinates from your phone. In GPS Waypoint mode, you can send a predetermined set of coordinates that the cooler will travel to. - -**Disclaimer: When testing the cooler, we noticed that the GPS coordinates streamed from Blynk were often inaccurate. While it works for some locations, it doesn't for others. If you want high precision, we highly suggest creating your own Android app to stream GPS coordinates.** - -## Installation -All files for this project are contained in the `/cooler` directory. Open the directory and click the `cooler.ino`. The Arduino IDE should open with all of the files in that directory. - -### Dependencies -You need to make sure to install the following dependencies through the Arduino library manager (Sketch->Include Library->Manage Libraries...) -- [Adafruit Sensor](https://learn.adafruit.com/adafruit-hmc5883l-breakout-triple-axis-magnetometer-compass-sensor/wiring-and-test) -- Blynk -- Adafruit_HMC5883_Unified - - -### Setup -You'll need to configure a few variable definitions for the code to work with your configuration. `CoolerDefinitions.h` contains variables that you can edit to switch pins, tweak parameters, and configure your Blynk app. - -#### Blynk -The most notable is the `char auth[]` variable. Replace the auth string (inside the quotes) with the auth token generated by your Blynk app. - -#### Pin Configuration -Most of the components on this robot are connected via Digital I/O pins. These pins can be changed by adjusting the following variables in `CoolerDefinitions.h`. - -``` -#define SERVO_PIN 3 - -#define GPS_TX_PIN 6 - -#define BLUETOOTH_TX_PIN 10 -#define BLUETOOTH_RX_PIN 11 - -#define MOTOR_A_EN_PIN 5 -#define MOTOR_B_EN_PIN 9 -#define MOTOR_A_IN_1_PIN 7 -#define MOTOR_A_IN_2_PIN 8 -#define MOTOR_B_IN_1_PIN 12 -#define MOTOR_B_IN_2_PIN 4 -``` - -Make sure that the following pins are connected to PWM ports (we had no available PWM ports left when everything was attached): - -``` -#define SERVO_PIN 3 -#define GPS_TX_PIN 6 -#define BLUETOOTH_TX_PIN 10 -#define BLUETOOTH_RX_PIN 11 -#define MOTOR_A_EN_PIN 5 -#define MOTOR_B_EN_PIN 9 -``` - -#### Parameters -The following variables were used for configuration. Adjust them as you see fit. - -``` -// If one motor tends to spin faster than the other, add offset -#define MOTOR_A_OFFSET 20 -#define MOTOR_B_OFFSET 0 - -// You must then add your 'Declination Angle' to the compass, which is the 'Error' of the magnetic field in your location. -// Find yours here: http://www.magnetic-declination.com/ -// Mine is: 13° 24' E (Positive), which is ~13 Degrees, or (which we need) 0.23 radians -#define DECLINATION_ANGLE 0.23f - -// The offset of the mounting position to true north -// It would be best to run the /examples/magsensor sketch and compare to the compass on your smartphone -#define COMPASS_OFFSET 0.0f - -// How often the GPS should update in MS -// Keep this above 1000 -#define GPS_UPDATE_INTERVAL 1000 - -// Number of changes in movement to timeout for GPS streaming -// Keeps the cooler from driving away if there is a problem -#define GPS_STREAM_TIMEOUT 18 - -// Number of changes in movement to timeout for GPS waypoints -// Keeps the cooler from driving away if there is a problem -#define GPS_WAYPOINT_TIMEOUT 45 -``` - -In order for our servo-driven lid to work, we had to fine tune the start and stop angle so that we could get the right leverage on the lid. They can be given a value between 0 and 180. - -``` -// PWM write for servo locations -#define SERVO_LID_OPEN 20 -#define SERVO_LID_CLOSE 165 -``` - -## Running the Code -Once you have everything configured property, verify and upload the code to your Arduino. If you are outside, it will take a few seconds for the GPS to acquire a satellite lock. Once it does, it will begin flashing. - -Make sure your android device is paired with HC-05 bluetooth module. If it asks for a password, it should be default `1234`. Open Blynk and press play! - -If your computer is connected via USB, you can also open the serial monitor and observe events on 4800 baud rate. - -If you have any problems, please post an issue and we'll try to help solve it. - - - +Cooler that follows you by navigating via GPS