#define BLYNK_USE_DIRECT_CONNECT
// Imports
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#include <Servo.h>
#include <SoftwareSerial.h>
#include <BlynkSimpleSerialBLE.h>
#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() {
[Link]("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;
[Link] = 0.0;
[Link] = 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;
[Link] = flat;
[Link] = flon;
[Link]([Link], 7); [Link](", "); [Link]([Link],
7);
return coolerLoc;
}
// Feed data as it becomes available
bool feedgps() {
while ([Link]()) {
if ([Link]([Link]()))
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);
[Link]("Received remote GPS: ");
// Print 7 decimal places for Lat
[Link]([Link](), 7); [Link](", "); [Link]([Link](),
7);
GeoLoc phoneLoc;
[Link] = [Link]();
[Link] = [Link]();
driveTo(phoneLoc, GPS_STREAM_TIMEOUT);
}
// Terminal Hook
BLYNK_WRITE(V3) {
[Link]("Received Text: ");
[Link]([Link]());
String rawInput([Link]());
int colonIndex;
int commaIndex;
do {
commaIndex = [Link](',');
colonIndex = [Link](':');
if (commaIndex != -1) {
String latStr = [Link](0, commaIndex);
String lonStr = [Link](commaIndex+1);
if (colonIndex != -1) {
lonStr = [Link](commaIndex+1, colonIndex);
}
float lat = [Link]();
float lon = [Link]();
if (lat != 0 && lon != 0) {
GeoLoc waypoint;
[Link] = lat;
[Link] = lon;
[Link]("Waypoint found: "); [Link](lat); [Link](lon);
driveTo(waypoint, GPS_WAYPOINT_TIMEOUT);
}
}
rawInput = [Link](colonIndex + 1);
} while (colonIndex != -1);
}
void displayCompassDetails(void)
{
sensor_t sensor;
[Link](&sensor);
[Link]("------------------------------------");
[Link] ("Sensor: "); [Link]([Link]);
[Link] ("Driver Ver: "); [Link]([Link]);
[Link] ("Unique ID: "); [Link](sensor.sensor_id);
[Link] ("Max Value: "); [Link](sensor.max_value);
[Link](" uT");
[Link] ("Min Value: "); [Link](sensor.min_value);
[Link](" uT");
[Link] ("Resolution: "); [Link]([Link]);
[Link](" uT");
[Link]("------------------------------------");
[Link]("");
delay(500);
}
#ifndef DEGTORAD
#define DEGTORAD 0.0174532925199432957f
#define RADTODEG 57.295779513082320876f
#endif
float geoBearing(struct GeoLoc &a, struct GeoLoc &b) {
float y = sin([Link]) * cos([Link]);
float x = cos([Link])*sin([Link]) - sin([Link])*cos([Link])*cos([Link]);
return atan2(y, x) * RADTODEG;
}
float geoDistance(struct GeoLoc &a, struct GeoLoc &b) {
const float R = 6371000; // km
float p1 = [Link] * DEGTORAD;
float p2 = [Link] * DEGTORAD;
float dp = ([Link]) * DEGTORAD;
float dl = ([Link]) * 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;
[Link](&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([Link].y, [Link].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) {
[Link](SERVO_PIN);
[Link](pos);
delay(2000);
[Link]();
}
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;
[Link]("turn: ");
[Link](t);
[Link]("original: ");
[Link](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;
}
[Link]("steerA: "); [Link](autoSteerA);
[Link]("steerB: "); [Link](autoSteerB);
int speedA = (int) (((float) autoThrottle) * autoSteerA);
int speedB = (int) (((float) autoThrottle) * autoSteerB);
setSpeedMotorA(speedA);
setSpeedMotorB(speedB);
}
void driveTo(struct GeoLoc &loc, int timeout) {
[Link]();
GeoLoc coolerLoc = checkGPS();
[Link]();
if ([Link] != 0 && [Link] != 0 && enabled) {
float d = 0;
//Start move loop here
do {
[Link]();
coolerLoc = checkGPS();
[Link]();
d = geoDistance(coolerLoc, loc);
float t = geoBearing(coolerLoc, loc) - geoHeading();
[Link]("Distance: ");
[Link](geoDistance(coolerLoc, loc));
[Link]("Bearing: ");
[Link](geoBearing(coolerLoc, loc));
[Link]("heading: ");
[Link](geoHeading());
drive(d, t);
timeout -= 1;
} while (d > 3.0 && enabled && timeout>0);
stop();
}
}
void setupCompass() {
/* Initialise the compass */
if(![Link]())
{
/* There was a problem detecting the HMC5883 ... check your connections */
[Link]("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
[Link](4800);
//GPS
[Link](9600);
//Bluetooth
[Link](9600);
[Link](bluetoothSerial, auth);
}
// Testing
void testDriveNorth() {
float heading = geoHeading();
int testDist = 10;
[Link](heading);
while(!(heading < 5 && heading > -5)) {
drive(testDist, heading);
heading = geoHeading();
[Link](heading);
delay(500);
}
stop();
}
void loop()
{
[Link]();
}