0

GPS with servo problems
Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.

GPS with servo problems

by clayton on Thu Mar 31, 2016 3:20 pm

I have encountered a problem that appears to be a conflict between the Adafruit_GPS library and the Adruino Servo library. Debugging libraries is beyond my programming ability so I am asking for help.

I have started a project with my 10 year old granddaughter hoping it was about 6th grad science project level. This would be her first programming experience. The project is to attach a small solar cell to a pan-tilt and have it always point at the sun. Our hardware is:

UNO R3
Adafruit Ultimate GPS Logger Shield
Adafruit Mini Pan-Tilt Kit-Assembled with Servos

We are using the GPS shield to obtain the longitude, latitude, and UTC time to determine the local elevation and azimuth of the sun and then convert that to pan-tilt input. We have written a program for the UNO to make all the calculations and this part works fine. We then added the Servo.h library, the attach commands, and the write commands to operate the pan-tilt. The UNO continues to correctly calculate the locations, but both axis of the pan-tilt oscillates between random locations.

The random oscillations occur:
with or without the servo write statements (with only the include and the attach statements),
regardless of the pins used to control the servos (I have used three different combinations).

I noted the oscillations occurred at about 1Hz. This was also the GPS update rate. I changed that to 5Hz and the pan-tilt tried to oscillate at 5Hz. It appears there is a conflict between the Adafruit_GPS and the Servo libraries.

The pan-tilt works fine by itself with a small program to make it run between extremes.

Code: Select all | TOGGLE FULL SIZE
/*

 Soloar_Tracker_1

 Track the sun from any location.
 Get time, latitude, and logitude from a GPS shield.
 Calculate elevation and azymith of sun for current time.

 created 23 March 2016
 by Mark Morris

*/

#include <SPI.h>
#include <math.h>
#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>
#include <Servo.h>

// If you're using the Adafruit GPS shield, use
// SoftwareSerial mySerial(8, 7);
// and make sure the switch is set to SoftSerial

SoftwareSerial mySerial(8, 7); // If you're using the Adafruit GPS shield
Servo myservo_tilt;            // create servo object to control the tilt servo
Servo myservo_pan;             // create servo object to control the pan servo
Adafruit_GPS GPS(&mySerial);   // creat GPS instance

// this keeps track of whether we're using the interrupt
// off by default!
boolean usingInterrupt = true;
void useInterrupt(boolean); // Func prototype keeps Arduino 0023 happy


typedef struct date_time
{
  unsigned int _year;
  unsigned int _month;
  unsigned int _day;
  unsigned int _hour;
  unsigned int _minute;
  unsigned int _second;
};

typedef struct lat_long_rad
{
  double lat_rad;
  double long_rad;
};

typedef struct elev_azi_rad
{
   double elevation_rad;
   double azimuth_rad;
};

date_time Current_time;
lat_long_rad Current_location;
elev_azi_rad EA;

int tilt_pin = 2;
int pan_pin = 3;
int pan_south = 95;        // set pan south
int pan_max = 180;         // set pan maximum
int pan_min = 0;           // set pan minimum
int tilt_hor = 77;         // set tilt hor
int tilt_min = 0;          // set tilt minimum
int tilt_max = tilt_hor;   // set tilt maximum

double azi_deg;
double elev_deg;
int azi_deg_int;
int elev_deg_int;
int pan_from_true_south;
int pan_tilt_azi;
int pan_tilt_elev;
int tilt_from_hor;

uint32_t timer = millis();

void setup()
{
  myservo_tilt.attach(tilt_pin);     // attach pin to servo
  myservo_pan.attach(pan_pin);       // attach pin to servo
 
  // connect at 115200 so we can read the GPS fast enough and echo without dropping chars
  Serial.begin(115200);
  while (!Serial) { ; }// wait for serial port to connect.

  // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
  GPS.begin(9600);

  // turn on RMC (recommended minimum) and GGA (fix data) including altitude
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);

  // Set the update rate
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);   // 1 Hz update rate

  // Request updates on antenna status, comment out to keep quiet
  //GPS.sendCommand(PGCMD_ANTENNA);

  useInterrupt(true);

  delay(1000);
  // Ask for firmware version
  mySerial.println(PMTK_Q_RELEASE);        // send a command to GPS
 
  Serial.println("Date (m/d/y)    Time      Elev     Azi  Altitude  Sat   El   Azi FTS   PTA  TFH  PTE");
  Serial.println("---------------------------------------------------------------------------------------");
} // void setup

// Interrupt is called once a millisecond, looks for any new GPS data, and stores it
SIGNAL(TIMER0_COMPA_vect) {
  char c = GPS.read();
  // if you want to debug, this is a good time to do it!
/* 
#ifdef UDR0
  if (GPSECHO)
    if (c) UDR0 = c; 
    // writing direct to UDR0 is much much faster than Serial.print
    // but only one character can be written at a time.
#endif
*/
} // function SIGNAL

void useInterrupt(boolean v) {
  if (v) {
    // Timer0 is already used for millis() - we'll just interrupt somewhere
    // in the middle and call the "Compare A" function above
    OCR0A = 0xAF;
    TIMSK0 |= _BV(OCIE0A);
    usingInterrupt = true;
  } else {
    // do not call the interrupt function COMPA anymore
    TIMSK0 &= ~_BV(OCIE0A);
    usingInterrupt = false;
  }
} // void UseInterrupt

void loop()
{
  // if a sentence is received, we can check the checksum, parse it...
  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 millis() or timer wraps around, we'll just reset it
  if (timer > millis())  timer = millis();

  // approximately every 5 seconds or so, print out the current stats
  if (millis() - timer > 5000) {
    timer = millis(); // reset the timer

    Current_time = { 2000+GPS.year, GPS.month, GPS.day, GPS.hour, GPS.minute, GPS.seconds};
    Current_location = {GPS.latitudeDegrees*M_PI/180.0, GPS.longitudeDegrees*M_PI/180.0};
    EA = GetElevAzi( Current_time, Current_location);      // get elevation and azimuth
    elev_deg = EA.elevation_rad * 180.0/M_PI;              // convert to degrees
    azi_deg = EA.azimuth_rad * 180.0/M_PI;                 // convert to degrees

    // print date
    if (GPS.month < 10) {
      // In the first 10 months we'll want a leading '0'
      Serial.print('0');
    }
    Serial.print(GPS.month, DEC); Serial.print("/");
    if (GPS.day < 10) {
      // In the first 10 days we'll want a leading '0'
      Serial.print('0');
    }
    Serial.print(GPS.day, DEC); Serial.print("/");
    Serial.print(2000+GPS.year, DEC);
    Serial.print("    ");
   
    // print time
    if (GPS.hour < 10) {
      // In the first 10 hours we'll want a leading '0'
      Serial.print('0');
    }
    Serial.print(GPS.hour, DEC); Serial.print(':');
    if (GPS.minute < 10) {
      // In the first 10 minutes we'll want a leading '0'
      Serial.print('0');
    }
    Serial.print(GPS.minute, DEC); Serial.print(':');
    if (GPS.seconds < 10) {
      // In the first 10 seconds we'll want a leading '0'
      Serial.print('0');
    }
    Serial.print(GPS.seconds, DEC); Serial.print("   ");
 
    Serial.print(elev_deg); Serial.print("   ");
    Serial.print(azi_deg); Serial.print("   ");
    Serial.print(GPS.altitude*3.28084); Serial.print("   ");
    if (GPS.satellites < 10) {
      // In the first 10 seconds we'll want a leading '0'
      Serial.print('0');
    }
    Serial.print(GPS.satellites); Serial.print("   ");   

    // find pan and tilt input
    azi_deg_int = (azi_deg_int>0 ? int(azi_deg+0.5) : int(azi_deg-0.5));      // convert to integer
    elev_deg_int = (elev_deg_int>0 ? int(elev_deg+0.5) : int(elev_deg-0.5));  // convert to integer
    Serial.print(elev_deg_int); Serial.print("   ");
    Serial.print(azi_deg_int); Serial.print("  ");
    pan_from_true_south = 180 - azi_deg_int;                   // degrees from true south, east positive
    pan_tilt_azi = pan_south + pan_from_true_south;            // convert to pan tilt input
    if (pan_tilt_azi > pan_max) {pan_tilt_azi = pan_max;}      // do not exceed pan tilt max
    if (pan_tilt_azi < pan_min) {pan_tilt_azi = pan_min;}      // do not exceed pan tilt min
    Serial.print(pan_from_true_south); Serial.print("   ");
    Serial.print(pan_tilt_azi); Serial.print("   ");
    tilt_from_hor = 90 - tilt_hor + elev_deg_int;              // degrees from tilt horizontal
    pan_tilt_elev = 90 - tilt_from_hor;                        // convert to pan tilt input
    if (pan_tilt_elev > tilt_max) {pan_tilt_elev = tilt_max;}  // do not exceed pan tilt max
    if (pan_tilt_elev < tilt_min) {pan_tilt_elev = tilt_min;}  // do not exceed pan tilt min   
    Serial.print(tilt_from_hor); Serial.print("   ");
    Serial.print(pan_tilt_elev); Serial.print("   ");   
    Serial.println();

    // adjust servos
    //myservo_tilt.write(tilt_from_hor);
    //myservo_pan.write(pan_tilt_elev);
       
  } // if millis
} // void loop

struct elev_azi_rad GetElevAzi(struct date_time T, struct lat_long_rad L) {    // calculate elevation and azimuth
  double earth_tilt = 24.44 * M_PI / 180.0;     // earth's axial tilt in radians
  elev_azi_rad EAR;
  EAR.elevation_rad = 0.0;
  EAR.azimuth_rad = 0.0; 

  double days_since_newyear;
  switch (T._month) {
    case  1 : days_since_newyear = 0.0; break;
    case  2 : days_since_newyear = 31.0; break;
    case  3 : days_since_newyear = 59.0; break;
    case  4 : days_since_newyear = 90.0; break;
    case  5 : days_since_newyear = 120.0; break;
    case  6 : days_since_newyear = 151.0; break;
    case  7 : days_since_newyear = 181.0; break;
    case  8 : days_since_newyear = 212.0; break;
    case  9 : days_since_newyear = 243.0; break;
    case 10 : days_since_newyear = 273.0; break;
    case 11 : days_since_newyear = 304.0; break;
    case 12 : days_since_newyear = 334.0; break;
  }

  //Serial.print("days_since_newyear = ");Serial.println(days_since_newyear);
  days_since_newyear = days_since_newyear+float(T._day)+float(T._hour/24.0)+(T._minute/1440.0)+(T._month > 2 ? ((T._year % 4) == 0 ? 1 : 0) : 0); // days since new year
  //Serial.print("days_since_newyear = ");Serial.println(days_since_newyear);

  // get B
  double big_B = (days_since_newyear - 81.0)*(360.0/365.25); // degrees along ecliptic
  //Serial.print("big_B deg = ");Serial.println(big_B);
  big_B = M_PI * big_B / 180.0;  // radians along ecliptic
  //Serial.print("big_B rad = ");Serial.println(big_B);

  // get EoT
  double EoT = 9.87 * sin(2.0*big_B) - 7.53 * cos(big_B) - 1.5 * sin(big_B);  // equation of time
  //Serial.print("EoT = ");Serial.println(EoT);

  // get sun declination
  double declination_of_sun_rad = asin(sin(big_B)*sin(earth_tilt)); //declination of sun in radians
  //Serial.print("declination_of_sun_rad = ");Serial.println(declination_of_sun_rad);

  // get hours from UTC
  double hours_from_UTC = (L.long_rad*12.0/M_PI);
  //Serial.print("hours_from_UTC = ");Serial.println(hours_from_UTC);

  // get local logitude time LLT
  double local_logitude_time = hours_from_UTC + T._hour+(T._minute/60.0)+(T._second/3600.0);
  if (local_logitude_time <0.0) local_logitude_time = local_logitude_time + 24.0;
  //Serial.print("local_logitude_time = ");Serial.println(local_logitude_time);

  // get LLT + EoT
  double LLTpEoT = local_logitude_time + EoT/60.0;
  //Serial.print("LLTpEoT = ");Serial.println(LLTpEoT);

  // get hour angle HRA
  double HRA_deg = (LLTpEoT-12.0) * 15.0;  // HRA in degrees
  double HRA_rad = M_PI * HRA_deg / 180.0;  // HRA radians
  //Serial.print("HRA_deg = ");Serial.println(HRA_deg);

  // get sun elevation
  double elevation_rad = asin(sin(declination_of_sun_rad)*sin(L.lat_rad) + cos(declination_of_sun_rad)*cos(L.lat_rad)*cos(HRA_rad));
  //Serial.print("elevation_rad = ");Serial.println(elevation_rad);

  // get sun elevation
  double azimuth_rad = acos((sin(declination_of_sun_rad)*cos(L.lat_rad) - cos(declination_of_sun_rad)*sin(L.lat_rad)*cos(HRA_rad))/sin((M_PI/2)-elevation_rad));
  //Serial.print("azimuth_rad = ");Serial.println(azimuth_rad);
  if (HRA_rad > 0.0) {
    azimuth_rad = 2.0*M_PI - azimuth_rad;
  }
  //Serial.println();

  EAR.elevation_rad = elevation_rad;
  EAR.azimuth_rad = azimuth_rad;
  return EAR;

} // function GetElevAzi

clayton
 
Posts: 19
Joined: Wed Apr 18, 2012 1:20 pm

Re: GPS with servo problems

by adafruit_support_bill on Thu Mar 31, 2016 3:51 pm

The problem is most likely interrupt contention. The Servo library makes use of timer interrupts. The Serial and SoftwareSerial libraries also make use of interrupts. Since the UNO has only one level of interrupt handling, it is not possible to handle a timer interrupt while processing a character receive interrupt. This ends up distorting the servo pulse timing and causes erratic servo behavior.

One solution is to use something like the Servo Shield which has a dedicated PWM processor to generate the servo pulses.

adafruit_support_bill
 
Posts: 74951
Joined: Sat Feb 07, 2009 10:11 am

Re: GPS with servo problems

by clayton on Thu Mar 31, 2016 6:44 pm

Thanks. I got the servo shield for the project but then realized I did not need it to work the servos. I will give it a try.

clayton
 
Posts: 19
Joined: Wed Apr 18, 2012 1:20 pm

Re: GPS with servo problems

by clayton on Thu Mar 31, 2016 8:13 pm

I installed the motor shield and looked up the instructions for its use on the Adafruit web site. The instructions stated the motor shield simply redirects the Arduino pins 9 and 10 to the RC servo header pins on the shield and does not actually do anything. The Arduino servo library is still used.

When I connected the pan-tilt it still responds as described above. This would indicate to me the RC servos cannot be used when a GPS device is connected to the Arduino. Would that be correct?

clayton
 
Posts: 19
Joined: Wed Apr 18, 2012 1:20 pm

Re: GPS with servo problems

by franklin97355 on Thu Mar 31, 2016 8:39 pm

It seems that there is a contention when used on the Uno. The Servo shield is not the same as the Motor shield and takes the servo control off the Uno and handles it onboard the shield.

franklin97355
 
Posts: 21300
Joined: Mon Apr 21, 2008 2:33 pm
Location: Lacomb, OR.

Re: GPS with servo problems

by adafruit_support_bill on Thu Mar 31, 2016 9:15 pm

I installed the motor shield

You want the Servo shield - not the Motor shield. The Servo shield has a dedicated PWM chip to handle the timing. This should eliminate the contention.
https://www.adafruit.com/products/1411

adafruit_support_bill
 
Posts: 74951
Joined: Sat Feb 07, 2009 10:11 am

Re: GPS with servo problems

by clayton on Thu Mar 31, 2016 9:58 pm

Okay, servo shield it is. I'll give it a try.

clayton
 
Posts: 19
Joined: Wed Apr 18, 2012 1:20 pm

Please be positive and constructive with your questions and comments.