UPDATE: I SOLVED THIS. THEIR IS AN ISSUE USING #include <Servo.h> AND <SoftwareSerial.h> TOGETHER SO I SWITCHED <Servo.h> TO THE PWMSERVO USING GitHub - PaulStoffregen/PWMServo: Control RC Servo motors with interrupt-resilient PWM
I am using an Arduino UNO with Adafruit Ultimate GPS, an L3M303 magnetometer, and a Tower Pro SG92R servo.
I use the GPS to calculate the heading from my current location to a set of GPS coordinates (bearing) and then I use the magnetometer to calculate my current heading (adjHeading).
Everything works except my servo integration. I am trying to indicate that if the difference between the bearing and adjHeading is <3 degrees, have the servo point straight. Otherwise, turn to the left. I would like to build it to a point that it says turn right or left to get to the right heading, than go straight.
Currently, the code I am using to control the servo is:
{
if (adjHeading <= bearing + 3 && adjHeading >= bearing - 3) {
myservo.write(90);
}
else myservo.write(180);
}
When the bearing and adjHeading are within 3 degrees, it 'freaks out' alternating between 180 and 90 degrees on the servo every second or so. When the difference is greater than 3 degrees, it sets in the 180 position but I can still hear it clicking every second or so.
How do I get it to stay in position and not move if it is on the right course?
The whole code base is below.
//Magnometer setup: GND -->; VIN --> 5V; SCL --> SCL (Top pin) and SDA to SDA (Second pin from top)
//Connect the GPS and Magnometer power to 5V
//Connect the GPS Magnometer ground to ground
#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303_U.h>
SoftwareSerial mySerial(3, 2); // Connect the GPS TX (transmit) pin to Digital 3 and GPS RX (receive) to Digital 2
Adafruit_GPS GPS(&mySerial);
#define GPSECHO false
#include <Servo.h>
#include <SoftwareSerial.h>
//Servo setup
Servo myservo;
int pos = 0;
int servoPin = 5; //define Servo output pin
//Compass setup
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(12345);
boolean usingInterrupt = false;
void useInterrupt(boolean);
float pi = 3.141592653589793238462;
float EarthRadius = 6371000; //radius in meters
double gpsLatitude; //Current latitude, in degrees, from GPS
double gpsLongitude; //Current longitude, in degrees, from GPS
double radiansCurrentLatitude; //Current latitude calculated as radians
double radiansCurrentLongitude; //Current longitude calculated as radians
double degreesDestinationLatitude = ; //Destination latitude in degrees
double degreesDestinationLongitude = ; // Destination longitude in degrees
double radiansDestinationLatitude = degreesDestinationLatitude * (pi / 180); //Calculates destination latitude in radians
double radiansDestinationLongitude = degreesDestinationLongitude * (pi / 180); //Calculates destination longitude in radians
double DeltaLatitude; //radiansCurrentLatitude - radiansDestinationLatitude
double DeltaLongitude;//radiansCurrentLongitude - radiansDestinationLongitude
//Variables used to break Haversine Formula into steps
double a;
double b;
double c;
double d;
double Distance; //Distance output between current GPS location and destination coordinates
//Variables used to break bearing formula into steps
double e;
double f;
double g;
double h;
double bearing; //Bearing from current GPS location to destination coordinates
//Variables to output compass heading
float adjHeading;
void setup()
{
//Setup GPS
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); // 1 Hz update rate
useInterrupt(true);
delay(1000);
mySerial.println(PMTK_Q_RELEASE);
//Setup Magnetometer
Serial.println("Magnetometer Test");
Serial.println("");
//Initalise the sensor
if (!mag.begin())
{
Serial.println("No magnometer detected. Check wiring.");
while (1);
}
//Setup Servo
myservo.attach(5);
}
SIGNAL(TIMER0_COMPA_vect) {
char c = GPS.read();
#ifdef UDR0
if (GPSECHO)
if (c) UDR0 = c;
#endif
}
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()
{
//Code for getting heading data from magnetometer
sensors_event_t event;
mag.getEvent(&event);
//Calculate heading
float heading = (atan2(event.magnetic.y, event.magnetic.x) * 180) / pi;
//Normalize to 0-360 degrees
if (heading < 0)
{
heading = 360 + heading;
}
//Adjust heading based on compass calibration sketch values
/*
CorrectedValue = (((RawValue – RawLow) * ReferenceRange) / RawRange) + ReferenceLow
RawValue = heading
ReferenceRange = 360
RawLow = 1.33
RawHigh = 359.96
RawRange = RawHigh - RawLow = 358.63
ReferenceLow = 0
*/
float adjHeading = (((heading - 1.33) * 360) / 358.63) + 0;
if (adjHeading < 0)
{
adjHeading = 360 + adjHeading;
}
//Code for getting GPS data
if (! usingInterrupt) {
char c = GPS.read();
if (GPSECHO)
if (c) Serial.print(c);
}
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
Serial.print("Fix: "); Serial.print((int)GPS.fix);
Serial.print(" quality: "); Serial.println((int)GPS.fixquality);
if (GPS.fix) {
{
{
{
float gpsLatitude = GPS.latitudeDegrees; //get current GPS latitude in degrees
float gpsLongitude = GPS.longitudeDegrees; //get current GPS longitude in degrees
radiansCurrentLatitude = (gpsLatitude * (pi / 180)); //convert current GPS latitude to radians
radiansCurrentLongitude = (gpsLongitude * (pi / 180)); //convert current GPS longitude to radians
DeltaLatitude = radiansCurrentLatitude - radiansDestinationLatitude; //calculate difference between current and destination latitudes
DeltaLongitude = radiansCurrentLongitude - radiansDestinationLongitude; //calculate difference between current and destination longitudes
}
//calculate distance, in KM, from current location to destination GPS coordinates using Haversine Formula
{
a = (sin(DeltaLatitude / 2) * (sin(DeltaLatitude / 2)));
b = (cos(radiansCurrentLatitude)) * (cos(radiansDestinationLatitude)) * (sin(DeltaLongitude / 2) * (sin(DeltaLongitude / 2)));
c = a + b;
d = (2 * atan2(sqrt(c), sqrt(1 - c)));
Distance = (EarthRadius * d) / 1000; //distance as measured in kilometers
}
//Calculates bearing to destination GPS coordinates
{
e = sin(DeltaLongitude) * cos(radiansDestinationLatitude);
f = cos(radiansCurrentLatitude) * sin(radiansDestinationLatitude) - sin(radiansCurrentLatitude) * cos(radiansDestinationLatitude) * cos(DeltaLongitude);
g = atan2(e, f);
h = g * (180 / pi); //Convert radians back to degrees
bearing = 360 - ( ((int)h + 360) % 360 ); //Calcuates bearing on 360 degree compass
}
}
}
}
}
{
{
if (adjHeading <= bearing + 3 && adjHeading >= bearing - 3) {
myservo.write(90);
}
else myservo.write(180);
}
if (Distance < 0.005) {
Serial.print ("You have arrived!");
} else {
Serial.print("Current Location: ");
Serial.print(GPS.latitudeDegrees, 10);
Serial.print(", ");
Serial.print(GPS.longitudeDegrees, 10);
Serial.print(", ");
Serial.print("Distance to ");
Serial.print(degreesDestinationLatitude, 10);
Serial.print(", ");
Serial.print(degreesDestinationLongitude, 10);
Serial.print(" is ");
Serial.print(Distance, 5);
Serial.print(" KM");
Serial.print(", ");
Serial.print("Follow heading: ");
Serial.println(bearing, 3);
Serial.print(", ");
Serial.print("Current Heading: ");
Serial.println(adjHeading);
}
delay (2500);
}
}