Hi all, Matt here--
I am in the Authentic Science Research Program at my high school and one of my projects is creating a scale AGV and I need a little help debuging my code to make a servo change direction based on the orientation of an on board compass. I have written a simple program to attempt to do this, only to have mixed results. When I have the compass oriented at 0 degrees, the wheels should straighten (controlled by the servo). When the compass is anywhere between 1 and 180, the wheels should turn right. When the compass is between 181 and 359, the servo should turn the wheels to the left.
Turning to the right works fine, and straightening is ok, but I might make the range between 355 and 5. When it should turn left, it does until it is rotated to a little more than 270 degrees and it turns right again. Can anyone help me debug the program below? Thanks!!
And by the way, I am using an Arduino Mega 2560.
/*Code to make the Arduino turn the servo in the "target" direction
based on the current orientation
Created on July 17th, 2012
*/
//Begining code is to initialize the compass and find the current orientation
#include <Wire.h> //includes the wire library for compass
#include <Servo.h> //includes the servo library to PWM the servo
int HMC6352SlaveAddress = 0x42;
int HMC6352ReadAddress = 0x41; //"A" in hex, A command is:
float GOAL = 90; //Taget of the compass (where to orient to)
Servo Steering; //defines the servo to be used to steer the car
int headingValue;
void setup(){
Steering.attach(10); //attaches the servo to pin 10
// "The Wire library uses 7 bit addresses throughout.
//If you have a datasheet or sample code that uses 8 bit address,
//you'll want to drop the low bit (i.e. shift the value one bit to the right),
//yielding an address between 0 and 127."
HMC6352SlaveAddress = HMC6352SlaveAddress >> 1; // I know 0x42 is less than 127, but this is still required
Serial.begin(9600);
Wire.begin();
}
void loop(){
//"Get Data. Compensate and Calculate New Heading"
Wire.beginTransmission(HMC6352SlaveAddress);
Wire.write(HMC6352ReadAddress); // The "Get Data" command
Wire.endTransmission();
//time delays required by HMC6352 upon receipt of the command
//Get Data. Compensate and Calculate New Heading : 6ms
delay(6);
Wire.requestFrom(HMC6352SlaveAddress, 2); //get the two data bytes, MSB and LSB
//"The heading output data will be the value in tenths of degrees
//from zero to 3599 and provided in binary format over the two bytes."
byte MSB = Wire.read();
byte LSB = Wire.read();
float headingSum = (MSB << 8) + LSB; //(MSB / LSB sum)
float headingInt = headingSum / 10;
//Serial.print(headingInt);
//Serial.println(" degrees");
float OrientationDiff = GOAL - headingInt;
//defining OrientationDiff as the target minus the current orientaion
Serial.print(OrientationDiff);
Serial.println(" degrees");
if (OrientationDiff == 0) //if the difference equals 0
Steering.write(90); //go straight
else if ((OrientationDiff < 180) && (OrientationDiff > 0)) //if the difference is greater than 0 and less than 180
Steering.write(180); //turn right
else //anything else (if the difference is less than 0 and greater than 180
Steering.write(0); //turn left
}