Mission 4

Mission 4

Home Mission 1 Mission 2 Mission 3 Mission 4

Advanced GPS Navigation

Criteria:

  1. Navigate to specified GPS waypoint that will be provided to you. You may hardcode the GPS waypoint coordinates for this mission. The rover should stop within 15 feet of the marked GPS waypoint.
  2. The rover should read the GPS coordinates from an RF transmitter beacon placed at the starting location, place a physical marker within 15 feet of the specified waypoint, and return to original start location. The distance between the physical marker and the marked waypoint will be measured along with the distance between start and end positions of the rover. The cumulative error should not exceed 25 feet.

Mission 4A Psuedo Code:

  1. Determine the latitude and longitude of the rover's starting point
  2. Drive forward a certain distance
  3. Determine the latitude and longitude of the rover's new position
  4. Calculate the heading angle
  5. Compare the rover's heading to the heading angle for the destination point
  6. If the angle is less than 180 degrees, have the rover turn left the angle of the heading
  7. If the angle is greater than 180 degrees, have the rover turn right (360 degrees minus the angle of the heading)
  8. Calculate the distance between the coordinate points, then travel that far
  9. Repeat all steps a second time to increase accuracy

Mission 4B Psuedo Code:

  1. Determine the latitude and longitude of the rover's starting point
  2. Store these coordinates
  3. Drive forward a certain distance
  4. Determine the latitude and longitude of the rover's new position
  5. Calculate the heading angle
  6. Compare the rover's heading to the heading angle for the destination point
  7. If the angle is less than 180 degrees, have the rover turn left the angle of the heading
  8. If the angle is greater than 180 degrees, have the rover turn right (360 degrees minus the angle of the heading)
  9. Calculate the distance between the coordinate points, then travel that far
  10. Repeat all steps a second time to increase accuracy
  11. Then, repeat all steps two more times to travel to the stored starting position coordinates

Mission 4A Arduino Code:

#include SoftwareSerial.h
#include TinyGPS++.h // Include TinyGPS++ library
#include AFMotor.h // Include AFMotor library
#define RXPin A1
#define TXPin A0
#define GPSBaud 9600
#define ConsoleBaud 115200
TinyGPSPlus gps; // Create a TinyGPS++ object for the GPS device
AF_DCMotor dcMotor1(1); // Create an AFMotor object for the motor driver (assuming you have connected the motor to the first channel of the motor driver)
AF_DCMotor dcMotor2(2); // Create an AFMotor object for the motor driver (assuming you have connected the motor to the second channel of the motor driver)
SoftwareSerial ss(RXPin, TXPin);
float roverLatitude1 = 45;
float roverLongitude1;
float roverLatitude2 = 45;
float roverLongitude2;
float roverHeading3;
unsigned long timestamp = 0;
void setup() {
Serial.begin(ConsoleBaud); // Initialize serial communication with the GPS device
ss.begin(GPSBaud);
dcMotor1.setSpeed(255); // Set the speed of the first motor to 255 (full speed)
dcMotor2.setSpeed(255); // Set the speed of the second motor to 255 (full speed)
}
void loop() {
while(roverLatitude1 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a valid fix on the location
if (gps.location.isValid()) {
roverLatitude1 = gps.location.lat();
roverLongitude1 = gps.location.lng();
Serial.print("Latitude1: ");
Serial.print(roverLatitude1, 6);
Serial.print(", ");
Serial.print("Longitude1: ");
Serial.print(roverLongitude1, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //go straight 6'
dcMotor2.run(FORWARD);
delay(10000);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
while(roverLatitude2 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a new fix on the location
if (gps.location.isUpdated()) {
roverLatitude2 = gps.location.lat();
roverLongitude2 = gps.location.lng();
Serial.print("Latitude2: ");
Serial.print(roverLatitude2, 6);
Serial.print(", ");
Serial.print("Longitude2: ");
Serial.print(roverLongitude2, 6);
Serial.println(" ");
}
}
float roverHeading1 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude1, roverLongitude1, roverLatitude2, roverLongitude2);
Serial.print("Heading1: ");
Serial.print(roverHeading1);
Serial.println(" ");
float roverHeading2 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude2, roverLongitude2, 39.080430, -108.560791);
Serial.print("Heading2: ");
Serial.print(roverHeading2);
Serial.println(" ");
if (roverHeading2 > roverHeading1){
roverHeading3 = roverHeading2 - roverHeading1;
if (roverHeading3 > 180){
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else {
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
if (roverHeading2 < roverHeading1){
roverHeading3 = roverHeading1 - roverHeading2;
if(roverHeading3 < 180){
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else{
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
Serial.print("Heading3: ");
Serial.print(roverHeading3);
Serial.println(" ");
float distanceToDestination = TinyGPSPlus::distanceBetween( //39.080635, 108.560577
roverLatitude2, roverLongitude2, 39.080430, -108.560791); //39.08067, 108.56046
Serial.print("Distance: ");
Serial.print(distanceToDestination, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //drive forward distance calculated
dcMotor2.run(FORWARD);
delay(distanceToDestination * 1500); //870
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
roverLatitude1 = 45;
roverLatitude2 = 45;
while(roverLatitude1 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a valid fix on the location
if (gps.location.isValid()) {
roverLatitude1 = gps.location.lat();
roverLongitude1 = gps.location.lng();
Serial.print("Latitude1: ");
Serial.print(roverLatitude1, 6);
Serial.print(", ");
Serial.print("Longitude1: ");
Serial.print(roverLongitude1, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //go straight 6'
dcMotor2.run(FORWARD);
delay(10000);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
while(roverLatitude2 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a new fix on the location
if (gps.location.isUpdated()) {
roverLatitude2 = gps.location.lat();
roverLongitude2 = gps.location.lng();
Serial.print("Latitude2: ");
Serial.print(roverLatitude2, 6);
Serial.print(", ");
Serial.print("Longitude2: ");
Serial.print(roverLongitude2, 6);
Serial.println(" ");
}
}
roverHeading1 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude1, roverLongitude1, roverLatitude2, roverLongitude2);
Serial.print("Heading1: ");
Serial.print(roverHeading1);
Serial.println(" ");
roverHeading2 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude2, roverLongitude2, 39.080430, -108.560791);
Serial.print("Heading2: ");
Serial.print(roverHeading2);
Serial.println(" ");
if (roverHeading2 > roverHeading1){
roverHeading3 = roverHeading2 - roverHeading1;
if (roverHeading3 > 180){
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else {
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
if (roverHeading2 < roverHeading1){
roverHeading3 = roverHeading1 - roverHeading2;
if(roverHeading3 < 180){
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else{
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
Serial.print("Heading3: ");
Serial.print(roverHeading3);
Serial.println(" ");
distanceToDestination = TinyGPSPlus::distanceBetween( //39.080635, 108.560577
roverLatitude2, roverLongitude2, 39.080430, -108.560791); //39.08067, 108.56046
Serial.print("Distance: ");
Serial.print(distanceToDestination, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //drive forward distance calculated
dcMotor2.run(FORWARD);
delay(distanceToDestination * 2000); //870
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
delay(100000000);
}

Mission 4B Arduino Code:

#include SoftwareSerial.h
#include TinyGPS++.h // Include TinyGPS++ library
#include AFMotor.h // Include AFMotor library
#define RXPin A1
#define TXPin A0
#define GPSBaud 9600
#define ConsoleBaud 115200
TinyGPSPlus gps; // Create a TinyGPS++ object for the GPS device
AF_DCMotor dcMotor1(1); // Create an AFMotor object for the motor driver (assuming you have connected the motor to the first channel of the motor driver)
AF_DCMotor dcMotor2(2); // Create an AFMotor object for the motor driver (assuming you have connected the motor to the second channel of the motor driver)
SoftwareSerial ss(RXPin, TXPin);
float roverLatitude1 = 45;
float roverLongitude1;
float roverLatitude2 = 45;
float roverLongitude2;
float roverLatitude0 = 45;
float roverLongitude0;
float roverHeading3;
unsigned long timestamp = 0;
void setup() {
Serial.begin(ConsoleBaud); // Initialize serial communication with the GPS device
ss.begin(GPSBaud);
dcMotor1.setSpeed(255); // Set the speed of the first motor to 255 (full speed)
dcMotor2.setSpeed(255); // Set the speed of the second motor to 255 (full speed)
}
void loop() {
while(roverLatitude0 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a valid fix on the location
if (gps.location.isValid()) {
roverLatitude0 = gps.location.lat();
roverLongitude0 = gps.location.lng();
Serial.print("Latitude0: ");
Serial.print(roverLatitude0, 6);
Serial.print(", ");
Serial.print("Longitude0: ");
Serial.print(roverLongitude0, 6);
Serial.println(" ");
}
}
while(roverLatitude1 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a valid fix on the location
if (gps.location.isValid()) {
roverLatitude1 = gps.location.lat();
roverLongitude1 = gps.location.lng();
Serial.print("Latitude1: ");
Serial.print(roverLatitude1, 6);
Serial.print(", ");
Serial.print("Longitude1: ");
Serial.print(roverLongitude1, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //go straight 6'
dcMotor2.run(FORWARD);
delay(10000);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
while(roverLatitude2 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a new fix on the location
if (gps.location.isUpdated()) {
roverLatitude2 = gps.location.lat();
roverLongitude2 = gps.location.lng();
Serial.print("Latitude2: ");
Serial.print(roverLatitude2, 6);
Serial.print(", ");
Serial.print("Longitude2: ");
Serial.print(roverLongitude2, 6);
Serial.println(" ");
}
}
float roverHeading1 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude1, roverLongitude1, roverLatitude2, roverLongitude2);
Serial.print("Heading1: ");
Serial.print(roverHeading1);
Serial.println(" ");
float roverHeading2 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude2, roverLongitude2, 39.080430, -108.560791);
Serial.print("Heading2: ");
Serial.print(roverHeading2);
Serial.println(" ");
if (roverHeading2 > roverHeading1){
roverHeading3 = roverHeading2 - roverHeading1;
if (roverHeading3 > 180){
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else {
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
if (roverHeading2 < roverHeading1){
roverHeading3 = roverHeading1 - roverHeading2;
if(roverHeading3 < 180){
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else{
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
Serial.print("Heading3: ");
Serial.print(roverHeading3);
Serial.println(" ");
float distanceToDestination = TinyGPSPlus::distanceBetween( //39.080635, 108.560577
roverLatitude2, roverLongitude2, 39.080430, -108.560791); //39.08067, 108.56046
Serial.print("Distance: ");
Serial.print(distanceToDestination, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //drive forward distance calculated
dcMotor2.run(FORWARD);
delay(distanceToDestination * 1500); //870
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
roverLatitude1 = 45;
roverLatitude2 = 45;
while(roverLatitude1 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a valid fix on the location
if (gps.location.isValid()) {
roverLatitude1 = gps.location.lat();
roverLongitude1 = gps.location.lng();
Serial.print("Latitude1: ");
Serial.print(roverLatitude1, 6);
Serial.print(", ");
Serial.print("Longitude1: ");
Serial.print(roverLongitude1, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //go straight 6'
dcMotor2.run(FORWARD);
delay(10000);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
while(roverLatitude2 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a new fix on the location
if (gps.location.isUpdated()) {
roverLatitude2 = gps.location.lat();
roverLongitude2 = gps.location.lng();
Serial.print("Latitude2: ");
Serial.print(roverLatitude2, 6);
Serial.print(", ");
Serial.print("Longitude2: ");
Serial.print(roverLongitude2, 6);
Serial.println(" ");
}
}
roverHeading1 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude1, roverLongitude1, roverLatitude2, roverLongitude2);
Serial.print("Heading1: ");
Serial.print(roverHeading1);
Serial.println(" ");
roverHeading2 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude2, roverLongitude2, 39.080430, -108.560791);
Serial.print("Heading2: ");
Serial.print(roverHeading2);
Serial.println(" ");
if (roverHeading2 > roverHeading1){
roverHeading3 = roverHeading2 - roverHeading1;
if (roverHeading3 > 180){
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else {
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
if (roverHeading2 < roverHeading1){
roverHeading3 = roverHeading1 - roverHeading2;
if(roverHeading3 < 180){
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else{
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
Serial.print("Heading3: ");
Serial.print(roverHeading3);
Serial.println(" ");
distanceToDestination = TinyGPSPlus::distanceBetween( //39.080635, 108.560577
roverLatitude2, roverLongitude2, 39.080430, -108.560791); //39.08067, 108.56046
Serial.print("Distance: ");
Serial.print(distanceToDestination, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //drive forward distance calculated
dcMotor2.run(FORWARD);
delay(distanceToDestination * 2000); //870
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
roverHeading2 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude2, roverLongitude2, roverLatitude0, roverLongitude0);
Serial.print("Heading2: ");
Serial.print(roverHeading2);
Serial.println(" ");
if (roverHeading2 > roverHeading1){
roverHeading3 = roverHeading2 - roverHeading1;
if (roverHeading3 > 180){
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else {
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
if (roverHeading2 < roverHeading1){
roverHeading3 = roverHeading1 - roverHeading2;
if(roverHeading3 < 180){
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else{
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
Serial.print("Heading3: ");
Serial.print(roverHeading3);
Serial.println(" ");
distanceToDestination = TinyGPSPlus::distanceBetween(
roverLatitude2, roverLongitude2, roverLatitude0, roverLongitude0);
Serial.print("Distance: ");
Serial.print(distanceToDestination, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //drive forward distance calculated
dcMotor2.run(FORWARD);
delay(distanceToDestination * 870); //870
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
roverLatitude1 = 45;
roverLatitude2 = 45;
while(roverLatitude1 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a valid fix on the location
if (gps.location.isValid()) {
roverLatitude1 = gps.location.lat();
roverLongitude1 = gps.location.lng();
Serial.print("Latitude1: ");
Serial.print(roverLatitude1, 6);
Serial.print(", ");
Serial.print("Longitude1: ");
Serial.print(roverLongitude1, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //go straight 6'
dcMotor2.run(FORWARD);
delay(10000); //10000
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
while(roverLatitude2 > 40){
// Check if there is new data available from the GPS device
while (ss.available() > 0) {
gps.encode(ss.read()); // Decode the new data
}
// Check if the GPS device has a new fix on the location
if (gps.location.isUpdated()) {
roverLatitude2 = gps.location.lat();
roverLongitude2 = gps.location.lng();
Serial.print("Latitude2: ");
Serial.print(roverLatitude2, 6);
Serial.print(", ");
Serial.print("Longitude2: ");
Serial.print(roverLongitude2, 6);
Serial.println(" ");
}
}
roverHeading1 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude1, roverLongitude1, roverLatitude2, roverLongitude2);
Serial.print("Heading1: ");
Serial.print(roverHeading1);
Serial.println(" ");
roverHeading2 = TinyGPSPlus::courseTo( //assign heading the the variable roverHeading
roverLatitude2, roverLongitude2, roverLatitude0, roverLongitude0);
Serial.print("Heading2: ");
Serial.print(roverHeading2);
Serial.println(" ");
if (roverHeading2 > roverHeading1){
roverHeading3 = roverHeading2 - roverHeading1;
if (roverHeading3 > 180){
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else {
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
if (roverHeading2 < roverHeading1){
roverHeading3 = roverHeading1 - roverHeading2;
if(roverHeading3 < 180){
float turnTime = roverHeading3 * 8.2222; //compute the duration of the turn
dcMotor1.run(FORWARD); //turn left
dcMotor2.run(BACKWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
else{
float turnTime = (360 - roverHeading3) * 8.2222; //compute the duration of the turn
dcMotor1.run(BACKWARD); //turn right
dcMotor2.run(FORWARD);
delay(turnTime);
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
}
}
Serial.print("Heading3: ");
Serial.print(roverHeading3);
Serial.println(" ");
distanceToDestination = TinyGPSPlus::distanceBetween( //39.080635, 108.560577
roverLatitude2, roverLongitude2, roverLatitude0, roverLongitude0); //39.08067, 108.56046
Serial.print("Distance: ");
Serial.print(distanceToDestination, 6);
Serial.println(" ");
dcMotor1.run(FORWARD); //drive forward distance calculated
dcMotor2.run(FORWARD);
delay(distanceToDestination * 1000); //870
dcMotor1.run(RELEASE);
dcMotor2.run(RELEASE);
delay(100000000);
}

Mission 4A Results:


Mission 4B Results: