next topic »

[on:]

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
}

[Reply #1 on:]

Code:

float headingInt = headingSum / 10;

Why do you have a float called headingInt? Perhaps that should be in int called headingFloat.

Code:

  if (OrientationDiff == 0)            //if the difference equals 0

Exact comparisons involving floats is never a good idea.

Code:

  else if ((OrientationDiff < 180) && (OrientationDiff > 0))  //if the difference is greater than 0 and less than 180

Is it possible for the compass reading to be negative? It is possible for the difference between the current heading and the desired heading to be negative. Your code doesn't seem to account for this possibility.

[Reply #2 on:]

hi Paul,

Thank you very much for replying

1) to the first question, that part was taken from an example and it works fine without attaching the servo (just using the example to use serial to show the heading)

2) Do you think it would be a smarter idea to use a range of plus or minus 10?  It really is jittery when at zero and is never exact

3) I think this might be part (or the main part) of the problem; I have the difference printing to serial and I didn't think anything of it......I don't know why I didn't think of that earlier...

also, If the difference is negative, how would I account for this in my code?

Thank you very much!!

[Reply #3 on:]

also, If the difference is negative, how would I account for this in my code?

You have code to do something if the difference is between 0 and 180. Have some more code to do something if the difference is between 0 and -180.

[Reply #4 on:]

would just an

Code:

else

at the end of my code work?

instead of

Code:

else if ((OrientationDiff < 180) && (OrientationDiff > 0))  //if the difference is greater than 0 and less than 180

?

thank you very much for the help so far!!

[Reply #5 on:]

would just an else at the end of my code work?

It's generally faster to comment out a bit of code and test, than to ask on the forum for someone else to do it for you.

[Reply #6 on:]

Hi-- sorry about that

But I did some tests and I'm still getting very odd results....Is there any way to calibrate the compass?  Some times I get sways of 40 degrees in either direction.

Also, would this make sense in my code to have a range of about 10 degrees for the servo to be able to stay straight?

Code:

if (((OrientationDiff > 355) && (OrientationDiff <359.9)) && ((OrientationDiff < 5) && (OrientationDiff > 0)))           //if the difference is greater than 355 and less than 360 and less than 5 and greater than 0
    Steering.write(50);                //go straight

Thank you!!!

[Reply #7 on:]

Code:

if (((OrientationDiff > 355) && (OrientationDiff <359.9)) && ((OrientationDiff < 5) && (OrientationDiff > 0)))

Let's try some values in that if statement. Let's try 180. 180 is not greater than 355, so the test fails, and we do nothing.

Let's try 360 (probably unlikely to happen, but let's be inclusive). That is greater than 355. It is not less than 359.9, so the test fails, and we do nothing.

Let's try 358. That is greater than 355. It is less than 359.9. It is not less than 5, though, so the test fails, and we do nothing.

Hmmm. I can't really think of a value that is greater than 355 while also being less than 5.

Perhaps you wanted an OR (||) in the middle, rather than an AND (&&).

I'd also consider using >= and <= rather than > and <. Also, consider arranging the tests in logical order. "Greater than 355 and less than 360" makes sense, but "less than 5 and greater than 0" requires more thought than "greater than 0 and less than 5".

[Reply #8 on:]

Oh!!!!!

Wow I feel like an idiot.......That makes so much more sense now....

One other thing still--

when the car turns right to a certain extent past "0", it turns left like it should; but if it goes maybe another 60 degrees, it turns right again

Do you know why this might be happening?  I can't seem to debug that in my code.....

And is there a way to calibrate the compass?

[Reply #9 on:]

I can't seem to debug that in my code.....

I'd think that printing the actual heading and the desired heading would provide the information needed to see which way the robot should turn. Printing out which way it actually turned would confirm whether the right, or wrong, decision was made.

[Reply #10 on:]

And is there a way to calibrate the compass?

The datasheet contains details on calibrating the compass.