Micro Servo has a mind of it's own!
Moderators: adafruit_support_bill, adafruit

Micro Servo has a mind of it's own!

by kenton86 on Tue Nov 06, 2012 2:26 pm

Hellofriends- I need some help.
I've got a TPro Micro servo ( https://www.adafruit.com/products/169 ) attached to a project I'm working on that seems to acting with a mind of it's own. I've stared at the code for hours and I can't see why it is performing the way it is.

The project is a reverse geocache and the servo unlocks the door when it reaches the destination. However, the servo keeps rotating back and forth from some other angle and I don't know why!

Here's a video of problem:
https://dl.dropbox.com/u/9137682/VID_20121106_121941.mp4

If I just take the servo parts of the code and put it into a new sketch, the servo works perfectly. So something else in the sketch is messing up the servo. I'm guessing that it is the interrupt used for parsing the GPS output. However, I have no idea how that interrupt works (looks like C code and there's no explanation for it in the GPS libraries) I just copied the code from the parsing example.

Any help??

Digital pins 2 and 3 are used for the Adafruit GPS breakout.
4 is a led indicating GPS fix (haven't soldered this on yet).
6 is the servo
7-12 is a 16x2 standard LCD.

Top view:
Image

Bottom:
Image

Code:
Code: Select all | TOGGLE FULL SIZE
/* Engagement Box   by Kenton Harris
Reverse Geocache Engagement Ring Box
This program unlocks a box that has reached a certain location.

The device uses the following products from Adafruit Industries:
Arduino Uno: https://www.adafruit.com/products/50
Ultimate GPS (version1): http://www.adafruit.com/products/746
16x2 Character LCD: https://www.adafruit.com/products/181
TPro Micro Servo SG90: https://www.adafruit.com/products/169
Waterproof LED Switch: https://www.adafruit.com/products/915
Half Sized Perma proto: https://www.adafruit.com/products/571

Tutorials for these products found on learn.adafruit.com helped with much of this.
Copyright (c) 2012, Adafruit Industries
All rights reserved.

Thanks to bnordlund9 for much of the code. This is  simplified verison of his Geobox found here:
http://www.youtube.com/watch?v=g0060tcuofg
Credit to Mikal Hart of http://arduiniana.org/ for the original idea of Reverse Geocache.

*/

#include <math.h>
#include <LiquidCrystal.h>
#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>
SoftwareSerial mySerial(3,2);
Adafruit_GPS GPS(&mySerial);
#define GPSECHO false
boolean usingInterrupt = false;
void useInterrupt(boolean);

//Servo
#include <Servo.h>
Servo servoLatch;

//Declarations
const float deg2rad = 0.01745329251994;
const float rEarth = 6370000.0;                                           //can replace with 3958.75 mi, 6370.0 km, or 3440.06 NM
float range;                                                             // distance from HERE to THERE
String here;                                                             // read from GPS

// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(7, 8, 9, 10, 11, 12);

int gpsWasFixed = HIGH;                                                  // did the GPS have a fix?
int ledFix = 4;                                                          // pin for fix LED
int servoPin = 6;                                                           // pin for servo
int servoLock = 110;                                                      // angle (deg) of "locked" servo
int servoUnlock = 0;                                                   // angle (deg) of "unlocked" servo

String there = "N38 53.366, W077 2.102";                                 //Desired Location (Washington Monument)

void setup()
{
  servoLatch.attach(servoPin);
  servoLatch.write(servoLock);
  delay(15);
  lcd.begin(16, 2);


  //Serial.begin(115200);
  //Serial.println("Debug GPS Test:");

  GPS.begin(9600);
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);                          // RMC (recommended minimum) and GGA (fix data) including altitude
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);                             // 1 Hz update rate
  useInterrupt(true);                                                    // reads the steaming data in a background
  delay(1000);
 

}

void loop(){
  // Parse GPS and recalculate RANGE
  if (GPS.newNMEAreceived()) {
    if (!GPS.parse(GPS.lastNMEA()))                                      // 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 (GPS.fix) {
    gpsWasFixed = HIGH;
    digitalWrite(ledFix, HIGH);
    //Serial.print("Location: ");                                        //for GPS debug
    //Serial.print(GPS.latitude, 4); Serial.print(GPS.lat);
    //Serial.print(", ");
    //Serial.print(GPS.longitude, 4); Serial.println(GPS.lon);
 
    here = gps2string ((String) GPS.lat, GPS.latitude, (String) GPS.lon, GPS.longitude);
    range = haversine(string2lat(here), string2lon(here), string2lat(there), string2lon(there));
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Distance to goal");
    //lcd.setCursor(0,1);
    //lcd.print("                ");
    lcd.setCursor(0,1);
    lcd.print(range);
    delay(500);
  }
  else {                                                              //No GPS fix- take box outside
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Hello Jenny!");
    lcd.setCursor(0,1);
    lcd.print("Take me outside!");
  }
 
  if (range < 50){
    //servoLatch.write(servoUnlock);
    delay(5000);
  }
}


SIGNAL(TIMER0_COMPA_vect) {
  // Interrupt is called once a millisecond, looks for any new GPS data, and stores it
  char c = GPS.read();
  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;
  }
}

String int2fw (int x, int n) {
  // returns a string of length n (fixed-width)
  String s = (String) x;
  while (s.length() < n) {
    s = "0" + s;
  }
  return s;
}

String gps2string (String lat, float latitude, String lon, float longitude) {
  // returns "Ndd mm.mmm, Wddd mm.mmm";
  int dd = (int) latitude/100;
  int mm = (int) latitude % 100;
  int mmm = (int) round(1000 * (latitude - floor(latitude)));
  String gps2lat = lat + int2fw(dd, 2) + " " + int2fw(mm, 2) + "." + int2fw(mmm, 3);
  dd = (int) longitude/100;
  mm = (int) longitude % 100;
  mmm = (int) round(1000 * (longitude - floor(longitude)));
  String gps2lon = lon + int2fw(dd, 3) + " " + int2fw(mm, 2) + "." + int2fw(mmm, 3);
  String myString = gps2lat + ", " + gps2lon;
  return myString;
};

float string2radius (String myString) {
  // returns a floating-point number: e.g. String myString = "Radius: 005.1 NM";
  float r = ((myString.charAt(8) - '0') * 100.0) + ((myString.charAt(9) - '0') * 10.0) + ((myString.charAt(10) - '0') * 1.0) + ((myString.charAt(12) - '0') * 0.10);
  return r;
};

float string2lat (String myString) {
  // returns radians: e.g. String myString = "N38 58.892, W076 29.177";
  float lat = ((myString.charAt(1) - '0') * 10.0) + (myString.charAt(2) - '0') * 1.0 + ((myString.charAt(4) - '0') / 6.0) + ((myString.charAt(5) - '0') / 60.0) + ((myString.charAt(7) - '0') / 600.0) + ((myString.charAt(8) - '0') / 6000.0) + ((myString.charAt(9) - '0') / 60000.0);
  lat *= deg2rad;
  if (myString.charAt(0) == 'S')
    lat *= -1;                                                           // Correct for hemisphere
  return lat;
};

float string2lon (String myString) {
  // returns radians: e.g. String myString = "N38 58.892, W076 29.177";
  float lon = ((myString.charAt(13) - '0') * 100.0) + ((myString.charAt(14) - '0') * 10.0) + (myString.charAt(15) - '0') * 1.0 + ((myString.charAt(17) - '0') / 6.0) + ((myString.charAt(18) - '0') / 60.0) + ((myString.charAt(20) - '0') / 600.0) + ((myString.charAt(21) - '0') / 6000.0) + ((myString.charAt(22) - '0') / 60000.0);
  lon *= deg2rad;
  if (myString.charAt(12) == 'W')
    lon *= -1;                                                           // Correct for hemisphere
  return lon;
};


float haversine (float lat1, float lon1, float lat2, float lon2) {
  // returns the great-circle distance between two points (radians) on a sphere
  float h = sq((sin((lat1 - lat2) / 2.0))) + (cos(lat1) * cos(lat2) * sq((sin((lon1 - lon2) / 2.0))));
  float d = 2.0 * rEarth * asin (sqrt(h));
  return d;
};

 
 
 
 
kenton86
 
Posts: 14
Joined: Thu Nov 10, 2011 6:05 pm

Re: Micro Servo has a mind of it's own!

by adafruit_support_bill on Tue Nov 06, 2012 3:53 pm

The servo twitch looks pretty regular at about 1 second. Your GPS communication lines are fairly close to the servo control line. You could be picking up some spurious pulse on the control line that the servo mistakes for a position pulse. Try re-routing the servo signal line.
User avatar
adafruit_support_bill
 
Posts: 30721
Joined: Sat Feb 07, 2009 10:11 am

Re: Micro Servo has a mind of it's own!

by kenton86 on Tue Nov 06, 2012 5:10 pm

I rerouted the servo signal line and the GPS TX line the best I could but I'm still getting problems with the servo.

Image

I combined the sweep servo example with the Adafruit_GPS echo example, and I'm able to replicate the problem.. the servo sweeps but twitches in time with the GPS data.

Might it have something to do with the timers they are using?

Code: Select all | TOGGLE FULL SIZE
// Sweepmod
// by BARRAGAN <http://barraganstudio.com>
// This example code is in the public domain.
//modified to include GPS Echo example

#include <Servo.h>
#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>
SoftwareSerial mySerial(3,2);
Adafruit_GPS GPS(&mySerial);
#define GPSECHO  true
boolean usingInterrupt = false;
void useInterrupt(boolean); // Func prototype keeps Arduino 0023 happy

Servo servoLatch;  // create servo object to control a servo
                // a maximum of eight servo objects can be created
 
int servoPin = 5;                                                           // pin for servo
int servoLock = 110;                                                      // angle (deg) of "locked" servo
int servoUnlock = 0;                                                   // angle (deg) of "unlocked" servo
int pos = 0;
void setup()
{
  Serial.begin(115200);
  Serial.println("Adafruit GPS library basic test!");
  GPS.begin(9600);
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
  useInterrupt(true);
 
  delay(1000);
  servoLatch.attach(servoPin);
  servoLatch.write(servoLock);
  delay(15);}
 
 
void loop()
{
  for(pos = 0; pos < 180; pos += 1)  // goes from 0 degrees to 180 degrees
  {                                  // in steps of 1 degree
    servoLatch.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);    // waits 15ms for the servo to reach the position
   }
 

  for(pos = 180; pos>=1; pos-=1)     // goes from 180 degrees to 0 degrees
  {                               
    servoLatch.write(pos);              // tell servo to go to position in variable 'pos'
    delay(15);                       // waits 15ms for the servo to reach the position
  }
 
  servoLatch.write(servoLock);
  delay(1000);
  servoLatch.write(servoUnlock);
  delay(1000);
}

SIGNAL(TIMER0_COMPA_vect) {
  char c = GPS.read();
  // if you want to debug, this is a good time to do it!
  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.
}

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;
  }
}

kenton86
 
Posts: 14
Joined: Thu Nov 10, 2011 6:05 pm

Re: Micro Servo has a mind of it's own!

by adafruit_support_bill on Tue Nov 06, 2012 5:35 pm

The Servo Library uses timer 1. I think your code is using timer 0. I didn't see any obvious place where you had interrupts disabled. If you were to disable them for longer than 25-30 mS, you would likely get some twitching as well.
User avatar
adafruit_support_bill
 
Posts: 30721
Joined: Sat Feb 07, 2009 10:11 am

Re: Micro Servo has a mind of it's own!

by kenton86 on Tue Nov 06, 2012 6:38 pm

I found this forum posting regarding the Servo and SoftSerial:
http://arduino.cc/forum/index.php/topic,28456.0.html
this identifies a problem with SoftwareSerial with the Servo library, but has no solution and is quite out of date.

I found more recent problems here: viewtopic.php?f=25&t=29695

and here: http://arduino.cc/forum/index.php/topic,125804.0.html

But once again it doesn't look like there's a solution. Should I try using a Servo2 library? It looks like others have tried but haven't had good results, and I'd have to modify my board to put the servo on Pin 9 or 10 and move one of the LCD pins elsewhere.

FYI I'm using arduino version 1.0.1.

Interrupts confuse me!
kenton86
 
Posts: 14
Joined: Thu Nov 10, 2011 6:05 pm

Re: Micro Servo has a mind of it's own!

by adafruit_support_bill on Tue Nov 06, 2012 7:30 pm

Servo2 might work. But it is not supported.
Another approach would be to use one of these: http://www.adafruit.com/products/815
That eliminates dependence on Arduino timers for controlling servos.
User avatar
adafruit_support_bill
 
Posts: 30721
Joined: Sat Feb 07, 2009 10:11 am

Re: Micro Servo has a mind of it's own!

by kenton86 on Wed Nov 07, 2012 10:21 am

Fixed!!
For anyone else having this problem in the future:

I found a workaround by installing the old Servo library that uses PWM instead of the interrupts in the current servo library.
That library is found here: http://arduiniana.org/libraries/pwmservo/

I had to move the servo pin to pin 9 and change my lcd pin 9 to pin 6, but it works!

I'll try to post a writeup about my project in the next couple of weeks: it's a pretty easy one that uses a lot of great adafruit components! Lesson Learned: Softserial and Servo do not play nice with each other.

Thanks to gukropina for the debugging!
kenton86
 
Posts: 14
Joined: Thu Nov 10, 2011 6:05 pm