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_20 ... 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:
Bottom:
Code:
Code: Select all
/* 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;
};