Problems Using Servo with GPS

Im tying to make an autonomous car. I finished my gps class to calculate a desired heading and I finished a class to take a heading and using a compass direct the steering servo toward that heading. Problem is when I merged the two the steering servo went bazerk. I tried commenting out bit by bit to identify the problem but nothing helped until commenting out the servo attach. As a test I just took my gps class and added
#include <Servo.h>
Servo steering;

and it already started making the servo go baser without any command even being sent to it…So here is the code that works great for heading degree calculation but screws up Servo. Anybody know what the problem is or how to solve it? The gps is the adafruit gps shield using pins 8,7 on top of arduino uno

#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>

SoftwareSerial mySerial(8, 7);
Adafruit_GPS GPS(&mySerial);
#define GPSECHO  false
boolean usingInterrupt = false;
void useInterrupt(boolean); // Func prototype keeps Arduino 0023 happy

int triggercountWaypoint;

long waypointlat[]= {40143318, 40142854, 40142450, 40142775, 40142306};
long waypointlon[]= {-83060232, -83059787, -83060160, -83060669, -83060700};

long currentlat;
long currentlon;
long londif;
long latdif;
int headingdegree;
long waypointDistance;

void setup() {
  // put your setup code here, to run once:

Serial.println("GPS heading each waypoint!");
GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);   // 1 Hz update rate

  // Ask for firmware version


  char c =;
#ifdef UDR0
  if (GPSECHO)
    if (c) UDR0 = c;  

void useInterrupt(boolean v) {
  if (v) {
    OCR0A = 0xAF;
    TIMSK0 |= _BV(OCIE0A);
    usingInterrupt = true;
  } else {
    TIMSK0 &= ~_BV(OCIE0A);
    usingInterrupt = false;

uint32_t timer = millis();

void loop() {
  // put your main code here, to run repeatedly:
long currentlat = (GPS.latitudeDegrees*1000000);
long currentlon = (GPS.longitudeDegrees*1000000);

if (GPS.newNMEAreceived()) {
    if (!GPS.parse(GPS.lastNMEA()))   // this also sets the newNMEAreceived() flag to false
      return;  // we can fail to parse a sentence in which case we should just wait for another

  if (timer > millis())  timer = millis();

  // approximately every 2 seconds or so, print out the current stats
  if (millis() - timer > 2000) { 
    timer = millis(); // reset the timer
  if (GPS.fix) {
      latdif = (waypointlat[triggercountWaypoint]- currentlat);
      londif = (waypointlon[triggercountWaypoint]- currentlon);
      waypointDistance = sqrt(latdif*latdif + londif*londif);
      Serial.print("Current location: ");
      Serial.print(", ");
      Serial.print("Current Waypoint: ");
      Serial.print(", ");

      Serial.print("Lat/Lon difference: ");
      Serial.print(", ");
headingdegree = round(atan2(londif, latdif)*180/3.14159265);
if (headingdegree < 0) headingdegree = headingdegree + 360;
  //if waypointDistance< 5; triggercountWaypoint++ 

while (!Serial.available());;{
triggercountWaypoint ++;

SoftwareSerial disables interrupts for long periods of time, so the servo probably won’t like that, and the Adafruit_GPS timer interrupt won’t help.

The Arduino PWM-capable pins (e.g., pin 10) can control a servo without interrupts. I hope you are using the Servo library in a way that allows the hardware to generates all the pulses, not with a timer interrupt and software (bit-banging). I don’t see any Servo code besides the attach call. You might need to set the min and max pulse width limits (read this).

**To replace SoftwareSerial: ** The best software serial library is AltSoftSerial, but it requires two specific pins (8 & 9 on an UNO). It also prevents using PWM on pin 10. If you could put your servo on a different PWM pin, you could use AltSoftSerial. Connect the GPS TX to pin 8 and GPS RX to pin 9. Since you have a shield, you’d have to do some soldering to make this connection.

The next best library is my NeoSWSerial. Use any two pins (e.g., 8 & 7) for a GPS @ 9600, 19200 or 38400. If the servo library disables interrupts for a long time (I don’t think it does), it could also interfere with NeoSWSerial.

Here’s a NeoGPS version of your sketch:

#include <NMEAGPS.h>
#include <NeoSWSerial.h>

NeoSWSerial gpsPort(8, 7);

NeoGPS::Location_t waypoints[] =
    { 401433180, -830602320 },
    { 401428540, -830597870 },
    { 401424500, -830601600 },
    { 401427750, -830606690 },
    { 401423060, -830607000 }

      int currentWaypoint = 0;
const int MAX_WAYPOINT    = sizeof(waypoints)/sizeof(waypoints[0]);

float waypointDistance = 0.0; // km
float headingdegree    = 0.0;

void setup()
  Serial.println( F("GPS heading each waypoint!") ); // F macro saves RAM

  GPS.send_P( &gpsPort, F("PMTK314,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0") ); // RMC only
  GPS.send_P( &gpsPort, F("PMTK220,1000") );   // 1 Hz update rate

void loop() {

  // Process GPS characters
  if (GPS.available( gpsPort )) {

    // A new fix structure is finally ready, get it.
    gps_fix fix =;

    if (fix.valid.location) {

      waypointDistance = fix.location.DistanceKm( waypoints[ currentWaypoint ] );
      headingdegree    = fix.location.BearingToDegrees( waypoints[ currentWaypoint ] );

      Serial.print( F("Current location: ") );
      Serial.print( fix.latitude(), 6 );
      Serial.print( F(", ") );
      Serial.println( fix.longitude(), 6 );

      Serial.print( F("Current Waypoint: ") );
      Serial.print( waypoints[ currentWaypoint ].latF(), 6 );
      Serial.print( F(", ") );
      Serial.println( waypoints[ currentWaypoint ].lonF(), 6 );

      Serial.println( headingdegree );
      Serial.println( waypointDistance );

      //if waypointDistance< 5; currentWaypoint++

      // Step to the next waypoint when user types something
      if (Serial.available()) {;
        currentWaypoint ++;
        if (currentWaypoint >= MAX_WAYPOINT)
          currentWaypoint = 0; // start over...

You can see that it’s much shorter and easier to read. It also shows the C++ needed to declare and use an array of NeoGPS::Location_t elements. The major difference is the loop structure. In the NeoGPS version, it processes GPS characters until a complete fix is ready. That happens once per second.

When a new fix is finally ready, it calculates the distance and bearing and prints a few things. Notice how there are no delays that keep other things from working. The rest of the time, loop just runs over and over, checking if gps.available().

Your original sketch uses 12140 bytes of program space and 997 bytes of RAM (almost half of it!).
The NeoGPS version uses 11076 bytes of program space and 395 bytes of RAM.


Not all heroes wear capes! Wow i appreciate that so much man! I think you answered a question of mine the other day as well. I really appreciate it man! You’re the real mvp

How accurate can the waypoint distance be? Its printing out like .06 km for example which isn’t super accurate. Can this be adjusted to become more accurate to within a few feet. If not i will probably just do something like

if currentLon<(waypointLon+10) && currentLon>(waypointLon-10) && currentLat<(waypointLat+10) && currentLat>(waypointLat-10)

next waypoint

Idk the exact numbers but you get the idea

How accurate can the waypoint distance be? Its printing out like .06 km

The DistanceKm return value has 6 significant digits. You are doing the default print of a float, which only prints 2 digits after the decimal:

      Serial.println( waypointDistance );

You can ask it to print more digits after the decimal point:

      Serial.println( waypointDistance, 7 );

And you can convert the distance from kilometres to other units:

      waypointDistance *= 3280.84; // feet per kilometer

Then, printing it with this statement:

      Serial.println( waypointDistance );

...would show feet to the hundredths. For example, if DistanceKm returns 0.0653213km, the above statement would display 214.31 (feet).

Although this is an accurate distance between two Location_t variables, you must remember that the GPS location may only be accurate to 10m. The distance can't be more accurate than that, even though it was computed very accurately.

On a good day, you might get a GPS accuracy of 3m. If you average locations for an hour or more, with good reception, you might get sub-meter accuracy. The NMEAaverage.ino example shows one way to do that.

Cheers, /dev