Compass with GPS

Projects Programming
1

Hi All,

I'm making an autonomous car with an Arduino Uno and a GPS module. I also want to use a compass to get a more accurate reading of the direction of the car. I was wondering if anyone could provide a modification of my code that would allow me to attach a compass and have it work in conjunction with the rest of the code and my GPS. I'll attach the code below. Thanks,
Chris

[quote]
[color=#7E7E7E]/* GPS Car v0.1 by Eric Barch (ttjcrew.com) */[/color]

#include <nmea.h>
#include <[color=#CC6600]Servo[/color].h>
#undef [color=#CC6600]abs[/color]
#undef [color=#CC6600]round[/color]
[color=#CC6600]int[/color] wpt = 0;
[color=#CC6600]float[/color] dest_latitude;
[color=#CC6600]float[/color] dest_longitude;
[color=#CC6600]NMEA[/color] gps([color=#006699]GPRMC[/color]);  [color=#7E7E7E]// GPS data connection to GPRMC sentence type[/color]



[color=#7E7E7E]/* BEGIN EDITABLE CONSTANTS SECTION */[/color]

[color=#7E7E7E]//These define the positions for your steering servo[/color]
#define CENTER_SERVO 95
#define MAX_LEFT_SERVO 85
#define MAX_RIGHT_SERVO 108

[color=#7E7E7E]//When the car is within this range (meters), move to the next waypoint[/color]
#define WPT_IN_RANGE_M 12

[color=#7E7E7E]//These pins are your h-bridge output. If the car is running in reverse, swap these[/color]
#define MOTOR_OUTPUT_ONE 3
#define MOTOR_OUTPUT_TWO 4

[color=#7E7E7E]/* DEFINE GPS WPTS HERE - Create more cases as needed */[/color]
[color=#CC6600]void[/color] trackWpts() {
  [color=#CC6600]switch[/color](wpt) {
    [color=#CC6600]case[/color] 0:
      dest_latitude = 37.764209;
      dest_longitude = -122.428447;
      [color=#CC6600]break[/color];
    [color=#CC6600]case[/color] 1:
      dest_latitude = 37.775206;
      dest_longitude = -122.419209;
      [color=#CC6600]break[/color];
    [color=#CC6600]default[/color]:
      dest_latitude = 0;
      dest_longitude = 0;
      [color=#CC6600]break[/color];
  }
  [color=#CC6600]if[/color] (gps.[color=#CC6600]gprmc_distance_to[/color](dest_latitude,dest_longitude,[color=#006699]MTR[/color]) < WPT_IN_RANGE_M)
    wpt++;
}



[color=#7E7E7E]/* END EDITABLE CONSTANTS SECTION */[/color]




[color=#CC6600]Servo[/color] steering;
[color=#CC6600]float[/color] dir;          [color=#7E7E7E]// relative direction to destination[/color]
[color=#CC6600]int[/color] servo_pos = CENTER_SERVO;

[color=#CC6600]void[/color] [color=#CC6600][b]setup[/b][/color]() {
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]begin[/color](4800);
  [color=#CC6600]pinMode[/color](MOTOR_OUTPUT_ONE, [color=#006699]OUTPUT[/color]);
  [color=#CC6600]pinMode[/color](MOTOR_OUTPUT_TWO, [color=#006699]OUTPUT[/color]);
  steering.[color=#CC6600]attach[/color](9);
}

[color=#CC6600]void[/color] [color=#CC6600][b]loop[/b][/color]() {
  trackWpts();
  trackGPS();
}

[color=#CC6600]void[/color] trackGPS() {
  [color=#CC6600]if[/color] (dest_latitude != 0 && dest_longitude != 0)
  {
    [color=#CC6600]if[/color] ([color=#CC6600][b]Serial[/b][/color].[color=#CC6600]available[/color]() > 0 ) {
      [color=#CC6600]char[/color] c = [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]read[/color]();

      [color=#CC6600]if[/color] (gps.[color=#CC6600]decode[/color](c)) {
          
        [color=#7E7E7E]// check if GPS positioning was active[/color]
        [color=#CC6600]if[/color] (gps.[color=#CC6600]gprmc_status[/color]() == [color=#006699]'A'[/color]) {
          [color=#7E7E7E]// calculate relative direction to destination[/color]
          dir = gps.[color=#CC6600]gprmc_course_to[/color](dest_latitude, dest_longitude) - gps.[color=#CC6600]gprmc_course[/color]();
          
          [color=#CC6600]if[/color] (dir < 0)
            dir += 360;
          [color=#CC6600]if[/color] (dir > 180)
            dir -= 360;
            
          [color=#CC6600]if[/color] (dir < -75)
            hardLeft();
          [color=#CC6600]else[/color] [color=#CC6600]if[/color] (dir > 75)
            hardRight();
          [color=#CC6600]else[/color]
            driveToTarget();
          
        }
        [color=#CC6600]else[/color] [color=#7E7E7E]//No GPS Fix...Wait for signal[/color]
          [color=#CC6600]stop[/color]();
            
        }
      }
  }
  [color=#CC6600]else[/color]
    [color=#CC6600]stop[/color]();
}

[color=#CC6600]void[/color] driveStraight() {
  steering.[color=#CC6600]write[/color](CENTER_SERVO);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_ONE, [color=#006699]LOW[/color]);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_TWO, [color=#006699]HIGH[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]println[/color]([color=#006699]"Driving straight..."[/color]);
}

[color=#CC6600]void[/color] driveToTarget() {
  servo_pos = [color=#CC6600]map[/color](dir, -75, 75, MAX_LEFT_SERVO, MAX_RIGHT_SERVO);
  steering.[color=#CC6600]write[/color](servo_pos);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_ONE, [color=#006699]LOW[/color]);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_TWO, [color=#006699]HIGH[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"Driving at "[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color](dir);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]". - "[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color](gps.[color=#CC6600]gprmc_distance_to[/color](dest_latitude,dest_longitude,[color=#006699]MTR[/color]));
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"m to go"[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"\n"[/color]);
}

[color=#CC6600]void[/color] hardLeft() {
  steering.[color=#CC6600]write[/color](MAX_LEFT_SERVO);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_ONE, [color=#006699]LOW[/color]);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_TWO, [color=#006699]HIGH[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"Driving hard left. - "[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color](gps.[color=#CC6600]gprmc_distance_to[/color](dest_latitude,dest_longitude,[color=#006699]MTR[/color]));
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"m to go"[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"\n"[/color]);
}

[color=#CC6600]void[/color] hardRight() {
  steering.[color=#CC6600]write[/color](MAX_RIGHT_SERVO);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_ONE, [color=#006699]LOW[/color]);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_TWO, [color=#006699]HIGH[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"Driving hard right. - "[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color](gps.[color=#CC6600]gprmc_distance_to[/color](dest_latitude,dest_longitude,[color=#006699]MTR[/color]));
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"m to go"[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"\n"[/color]);
}

[color=#CC6600]void[/color] [color=#CC6600]stop[/color]() {
  steering.[color=#CC6600]write[/color](CENTER_SERVO);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_ONE, [color=#006699]HIGH[/color]);
  [color=#CC6600]digitalWrite[/color](MOTOR_OUTPUT_TWO, [color=#006699]HIGH[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"Stopped."[/color]);
  [color=#CC6600][b]Serial[/b][/color].[color=#CC6600]print[/color]([color=#006699]"\n"[/color]);
}

[/quote]
2

Thats not code its a web page ???

To calculate bearing and compass you need your locaion and the next way point

you use
Haversine
formula: a = sin²(??/2) + cos ?1 ? cos ?2 ? sin²(??/2)
c = 2 ? atan2( ?a, ?(1?a) )
d = R ? c
where ? is latitude, ? is longitude, R is earth’s radius (mean radius = 6,371km);
note that angles need to be in radians to pass to trig functions!

Good luck, if you can't post real code I guess this is beyond you ..
8) 8)

3

:cold_sweat:

I had copied it using the "copy for forum" option

/* GPS Car v0.1 by Eric Barch (ttjcrew.com) */

#include <nmea.h>
#include <Servo.h>
#undef abs
#undef round
int wpt = 0;
float dest_latitude;
float dest_longitude;
NMEA gps(GPRMC);  // GPS data connection to GPRMC sentence type



/* BEGIN EDITABLE CONSTANTS SECTION */

//These define the positions for your steering servo
#define CENTER_SERVO 95
#define MAX_LEFT_SERVO 85
#define MAX_RIGHT_SERVO 108

//When the car is within this range (meters), move to the next waypoint
#define WPT_IN_RANGE_M 12

//These pins are your h-bridge output. If the car is running in reverse, swap these
#define MOTOR_OUTPUT_ONE 3
#define MOTOR_OUTPUT_TWO 4

/* DEFINE GPS WPTS HERE - Create more cases as needed */
void trackWpts() {
  switch(wpt) {
    case 0:
      dest_latitude = 37.764209;
      dest_longitude = -122.428447;
      break;
    case 1:
      dest_latitude = 37.775206;
      dest_longitude = -122.419209;
      break;
    default:
      dest_latitude = 0;
      dest_longitude = 0;
      break;
  }
  if (gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR) < WPT_IN_RANGE_M)
    wpt++;
}



/* END EDITABLE CONSTANTS SECTION */




Servo steering;
float dir;          // relative direction to destination
int servo_pos = CENTER_SERVO;

void setup() {
  Serial.begin(4800);
  pinMode(MOTOR_OUTPUT_ONE, OUTPUT);
  pinMode(MOTOR_OUTPUT_TWO, OUTPUT);
  steering.attach(9);
}

void loop() {
  trackWpts();
  trackGPS();
}

void trackGPS() {
  if (dest_latitude != 0 && dest_longitude != 0)
  {
    if (Serial.available() > 0 ) {
      char c = Serial.read();

      if (gps.decode(c)) {
          
        // check if GPS positioning was active
        if (gps.gprmc_status() == 'A') {
          // calculate relative direction to destination
          dir = gps.gprmc_course_to(dest_latitude, dest_longitude) - gps.gprmc_course();
          
          if (dir < 0)
            dir += 360;
          if (dir > 180)
            dir -= 360;
            
          if (dir < -75)
            hardLeft();
          else if (dir > 75)
            hardRight();
          else
            driveToTarget();
          
        }
        else //No GPS Fix...Wait for signal
          stop();
            
        }
      }
  }
  else
    stop();
}

void driveStraight() {
  steering.write(CENTER_SERVO);
  digitalWrite(MOTOR_OUTPUT_ONE, LOW);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.println("Driving straight...");
}

void driveToTarget() {
  servo_pos = map(dir, -75, 75, MAX_LEFT_SERVO, MAX_RIGHT_SERVO);
  steering.write(servo_pos);
  digitalWrite(MOTOR_OUTPUT_ONE, LOW);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.print("Driving at ");
  Serial.print(dir);
  Serial.print(". - ");
  Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
  Serial.print("m to go");
  Serial.print("\n");
}

void hardLeft() {
  steering.write(MAX_LEFT_SERVO);
  digitalWrite(MOTOR_OUTPUT_ONE, LOW);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.print("Driving hard left. - ");
  Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
  Serial.print("m to go");
  Serial.print("\n");
}

void hardRight() {
  steering.write(MAX_RIGHT_SERVO);
  digitalWrite(MOTOR_OUTPUT_ONE, LOW);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.print("Driving hard right. - ");
  Serial.print(gps.gprmc_distance_to(dest_latitude,dest_longitude,MTR));
  Serial.print("m to go");
  Serial.print("\n");
}

void stop() {
  steering.write(CENTER_SERVO);
  digitalWrite(MOTOR_OUTPUT_ONE, HIGH);
  digitalWrite(MOTOR_OUTPUT_TWO, HIGH);
  Serial.print("Stopped.");
  Serial.print("\n");
}
4

I was wondering if anyone could provide a modification of my code that would allow me to attach a compass and have it work in conjunction with the rest of the code and my GPS.

Without knowing which compass, and how you want to use the data, no.