Alright folks, here is the deal, my team has looked over this code a zillion times but still cant find the error. When testing the robot it just goes in circles instead of following the course. The predefined course is four points and should turn when crossing a certain Latitude or Longitude. But instead it just sits and spins circles.
Hardware
Compass: http://www.sparkfun.com/products/7915
GPS: http://www.sparkfun.com/products/8975
Arduino: Arduino Uno - R3 SMD - DEV-11224 - SparkFun Electronics
Also using just a basic servo for steering and moving
#include "Wire.h" // include libraries for I2C, PWM servo control, and NMEA GPS parsing
#include "TinyGPS.h"
#include "Servo.h"
TinyGPS gps; // initialize objects and variables
Servo steering;
long lat, lon;
int dir, angle, i, j = 0;
void setup(){ // set up HW interfaces and put compass in continuous mode
steering.attach(9);
Serial.begin(57600);
Wire.begin();
Wire.beginTransmission(0x42 >> 1);
Wire.write("G");
Wire.write(0x74);
Wire.write(0x72);
Wire.endTransmission();
}
int gradient(char type, long S, long N, long E, long W){ // calculate course elements
if(lat > S && lat < N && lon > E && lon < W){
if(type == 'E') return (1800*(lat-S)/(N-S));
if(type == 'S') return 2700 - (1800*(lat-E)/(W-E));
if(type == 'W') return 3600 - (1800*(lat-S)/(N-S));
if(type == 'N') return (1800*(lat-E)/(W-E)) - 900;
}
else return 30000;
}
void loop(){
while(Serial.available()) gps.encode(Serial.read()); // get data from GPS and send it to NMEA parser
gps.get_position(&lat, &lon); // get data back from parser
lon = 0-lon; // invert longitude, since we're using all positive coordinate values
Wire.requestFrom(0x42 >> 1, 2); // get data from compass
dir = (Wire.read() << 8) + Wire.read();
// navigate around the building
i = gradient('E', 4006520, 4006532, 10520983, 10521057);
if(i != 30000) j = i;
i = gradient('S', 4006455, 4006540, 10520974, 10520983);
if(i != 30000) j = i;
i = gradient('W', 4006446, 4006455, 10520975, 10521043);
if(i != 30000) j = i;
i = gradient('N', 4006433, 4006520, 10521043, 10521052);
if(i != 30000) j = i;
angle = j - dir; // calculate steering angle
if(angle < -1800) angle = angle + 3600; // account for wraparounds of direction values
if(angle > 1800) angle = angle - 3600;
steering.write((angle/20) + 90); // write a steering angle to the servo HAL
}
I'm at a loss any feedback will help.