I am currently working on a robot that drives via GPS to a predefined place the robot is rather going wacko and I think the problem is in the code. Here is my code:
//pins
//only pwm and dir need to be connected.
int aPwm = 5;
int bPwm = 6;
int aDir = 7;
int bDir = 8;
//system vars
int low = 150;
int med = 200;
int high = 250;
int myHeading;
double destHeading;
float headThresh;
int a;
int b;
//compass
#include <Wire.h>
int HMC6352Address = 0x42;
// This is calculated in the setup() function
int slaveAddress;
int ledPin = 13;
boolean ledState = false;
byte headingData[2];
int i, headingValue;
//gps
#include <SoftwareSerial.h>
#include <TinyGPS++.h>
#define RXPin 2
#define TXPin 3
#define GPSBaud 9600
#define ConsoleBaud 9600
// The serial connection to the GPS device
SoftwareSerial ss(RXPin, TXPin);
// The TinyGPS++ object
TinyGPSPlus gps;
unsigned long lastUpdateTime = 0;
float dest[] = {
42.35376, -82.57171};
#define destLat dest[0]
#define destLong dest[1]
void setup()
{delay(30000);//wait for the gos to get a fix.
pinMode(aPwm, OUTPUT);
pinMode(bPwm, OUTPUT);
pinMode(aDir, OUTPUT);
pinMode(bDir, OUTPUT);
digitalWrite(aDir, HIGH);
digitalWrite(bDir, HIGH);
//compass
// Shift the device's documented slave address (0x42) 1 bit right
// This compensates for how the TWI library only wants the
// 7 most significant bits (with the high bit padded with 0)
slaveAddress = HMC6352Address >> 1; // This results in 0x21 as the address to pass to TWI
Serial.begin(115200);
pinMode(ledPin, OUTPUT); // Set the LED pin as output
Wire.begin();
//gps
ss.begin(GPSBaud);
}
void loop()
{
//compass
// Send a "A" command to the HMC6352
// This requests the current heading data
Wire.beginTransmission(slaveAddress);
Wire.write("A"); // The "Get Data" command
Wire.endTransmission();
delay(10); // The HMC6352 needs at least a 70us (microsecond) delay
// after this command. Using 10ms just makes it safe
// Read the 2 heading bytes, MSB first
// The resulting 16bit word is the compass heading in 10th's of a degree
// For example: a heading of 1345 would be 134.5 degrees
Wire.requestFrom(slaveAddress, 2); // Request the 2 byte heading (MSB comes first)
i = 0;
while(Wire.available() && i < 2)
{
headingData[i] = Wire.read();
i++;
}
headingValue = headingData[0]*256 + headingData[1]; // Put the MSB and LSB together
myHeading = headingValue / 10;
//Serial.print("Current heading: ");
//Serial.print(int (headingValue / 10)); // The whole number part of the heading
//gps
// If any characters have arrived from the GPS,
// send them to the TinyGPS++ object
while (ss.available() > 0)
gps.encode(ss.read());
// Every 5 seconds, do an update.
if (millis() - lastUpdateTime >= 1)
{
lastUpdateTime = millis();
Serial.println();
// Establish our current status
double distanceToDestination = TinyGPSPlus::distanceBetween(
gps.location.lat(), gps.location.lng(),destLat, destLong);
destHeading = TinyGPSPlus::courseTo(
gps.location.lat(), gps.location.lng(), destLat, destLong);
const char *directionToDestination = TinyGPSPlus::cardinal(destHeading);
int courseChangeNeeded = (int)(360 + destHeading - gps.course.deg()) % 360;
}
if(/*gps.location.isValid()*/true){
// movement
if(myHeading < destHeading + headThresh && myHeading > destHeading - headThresh){// Our heading is within 5 degrees of the target course
a = med;
b = med;
}
if(myHeading > destHeading + headThresh){
a = low;
b = high;
}
if(myHeading < destHeading - headThresh){
a = high;
b = low;
}
applyVals(a, b);
}else{a = 0; b = 0;Serial.println("FAIL");applyVals(a, b); delay(5000);
}
double distanceToDestination = TinyGPSPlus::distanceBetween(
gps.location.lat(), gps.location.lng(),destLat, destLong);
if (distanceToDestination <= 5.0)
{
Serial.println("CONGRATULATIONS: You've arrived!");
exit(1);
}
}
void applyVals(int A, int B){
analogWrite(aPwm, A);
analogWrite(bPwm, B);
Serial.print("A");
Serial.print(A);
Serial.print("B");
Serial.print(B);
Serial.println("\n");
}
The code should drive the robot to the coordinates but the robot just turns in circles. I am using two motor controllers, a linksprite GPS shield and a parallax #29323 compass. All help is appreciated and I will gladly provide more details if needed!
Thanks,
L