SparkFun Forums

[sora62896]

I still don't understand---what are the "i" and "D" terms? I'm still getting an error message even with Joeisi's update! Isn't there suppose to be something after each if statement?? I'm really getting confused here.....

[fll-freak]

Your first code was a "bang bang" or binary controller. The sample untested code I gave you was a Proportional controller (P). This should work better once you get the bugs out of it. I was just telling you that adding an Integral (I) and a differential term (D) might be even better. But if you are having problems getting rid of syntax errors, I would hold off on the I and D terms.

[sora62896]

so that means take out the

Code: Select all

if (ang >  180.0) (ang -= 360.0;)
  if (ang < -180.0) (ang += 360.0;) 

?

what is that suppose to do?? There are two if statements--aren't if statements suppose to reflect certain actions? and here, none are specified!!

[Mee_n_Mac]

so that means take out the

Code: Select all

if (ang >  180.0) (ang -= 360.0;)
  if (ang < -180.0) (ang += 360.0;) 

?
what is that suppose to do?? There are two if statements--aren't if statements suppose to reflect certain actions? and here, none are specified!!

No, the part above just limits the angle to be between +180 deg and -180 deg. Look at your code where you determine the steering correction.

Code: Select all

if ((OrientationDiff >= -15) && (OrientationDiff <= 15))  //if difference ia greater or equal to -15 and less than or equal to 15
    Steering.write(105);                //go straight
  else if ((OrientationDiff < -15) && (OrientationDiff > -215))  //if the difference is greater than 0 and less than 180
    Steering.write(50);               //turn left
  else                                 //anything else (if the difference is less than 0 and greater than 180
     Steering.write(140);                //turn right

If the difference (the heading error) between the compass reading (which is the measured heading) and the goal (the desired heading) is less than +/- some amount, the correction is 0 (you go straight). If the error is off by more than 15 deg and to the left, you issue a fixed amount of steering to the right. If the error is off by more than the 15 deg threshold and to the right, you issue a fixed amount of steering to the left. These amounts of steering don't change no matter how big the error is (they're fixed in value !).

Now look at the code fll-freak provided, especially this part ...

Code: Select all

  // Defining OrientationDiff as the target minus the current orientaion
  OrientationDiff = NormalizeHalfCircle(GOAL - headingInt);
  // Compute correction. 
  #define GAIN 50.0*30.0  // 50 ms servo change for 30 degree error
  #define OFFSET 105.0    // Servo neutral position
  correction = OrientationDiff * GAIN + OFFSET;

Note that the correction is now proportional to the error. That's the "P" he's talking about. A small error makes a small steering correction. A large error makes a large correction. It's like what you do when you drive a car. You don't twist the wheel a fixed amount as you drift slightly offcenter in your lane. (At least I hope you don't). You turn the wheel as hard as needed, a small amount if you're slightly offcenter and more if you weren't paying attention and are now crossing into another lane !

[sora62896]

but then why am i still getting an error message when compiling this?

**EDIT**

Aren't the parenthesis suppose to be before the semicolon?

[Mee_n_Mac]

but then why am i still getting an error message when compiling this?

**EDIT**

Aren't the parenthesis suppose to be before the semicolon?

If by "this" you meant ...

Code: Select all

if (ang >  180.0) (ang -= 360.0;)
  if (ang < -180.0) (ang += 360.0;) 

... then yes, the ;'s should be outside of the (). I'd have written that snippet like ...

Code: Select all

if (ang >  180.0){
   ang -= 360.0;
}
if (ang < -180.0){
   ang += 360.0;
} 

[sora62896]

well, it compiles now, but uploaded, it does absolutely nothing.....all connections are made......the wheel turns to the right and doesn't do anything else.......even with a full 180 degree spin..........now what??

[Mee_n_Mac]

First post the code that compiled so we can see what you're actually using. Then look at the different path the execution can take and put some debug print statements in those paths so you can "see" what's happening. I'd certainly print what the compass reading is, and see what it's doing when you spin 360 deg. Also print the ang above and any other variable you think wouldbe useful to know.

[sora62896]

give me a minute--just had some issues with my power supply on my laptop

[sora62896]

Code: Select all

     /* Code to make the Arduino turn the servo in the "target" direction
       and drive forward based on the current orientation

        Created on August 11th, 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

    // Prototypes
    float NormalizeHalfCircle(float ang);

    float correction;

    int HMC6352SlaveAddress = 0x42;
    int HMC6352ReadAddress = 0x41; //"A" in hex, A command is:


    float GOAL = 90.0; //Taget of the compass (where to orient to)


    Servo Steering; //defines the servo to be used to steer the car
    Servo Motor;    //defines the DC motor as a servo so it can be used with PWM


    int headingValue;
    int spd = 95;   //speed of the DC motor


    void setup(void) {
     
      Steering.attach(10);  //attaches the servo to pin 10
      Motor.attach(9);      //attach the DC motor to pin 9
      Motor.write(spd);     //drive the motor at speed--95
     
     
      // "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(void){
      float OrientationDiff;

      //"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");

      // Defining OrientationDiff as the target minus the current orientaion
      OrientationDiff = NormalizeHalfCircle(GOAL - headingInt);

      // Compute correction.
      #define GAIN 50.0*30.0  // 50 ms servo change for 30 degree error
      #define OFFSET 105.0    // Servo neutral position
      correction = OrientationDiff * GAIN + OFFSET;

      //Serial.print(OrientationDiff);
      //Serial.println(" degrees");
     
      Steering.write(correction);
    }


    float NormalizeHalfCircle(float ang) {

      if (ang >  180.0)
        (ang -= 360.0);
      if (ang < -180.0)
        (ang += 360.0);

      return (ang);
    }

that is the code of now-- other than the heading, what should i print?

[Mee_n_Mac]

that is the code of now-- other than the heading, what should i print?

I think you should print the heading, differece and correction. I'd also like to see how long it takes to go through the loop when it's running. You could print the time each pass too.

serial.print(millis());

I've had a quick look at the code and I see 2 big problem. First I believe the GAIN is waaaaay too high. What'll happen is that you'll compute a servo command that slams the steering hard over in one direction or the other, depending on what the heading error is when the loop starts running. Secondly since we don't know how quickly the loop runs (or the cars kinematics), I can't derive a GAIN term that I can guarantee is stable (ie - won't do the above). Also servos will only be updated every 20 msec. Calculating a new command every 1 msec won't change a thing. 19 of the 20 commands will get ignored. So I'd' like to add a delay() at the bottom of your loop and adjust it's value until the time printout above is showing a 20 msec difference between prints. I have no idea how long the loop takes to execute at present, so set the delay to 20 msec and we'll reduce it according to the printed values. Then I'd set the GAIN = 0.9 to start. Aim the car about in the correct direction and see what happens.

I assume the 105 deg offset is the command you've determined by experiment that points the wheel straight ahead ?

Lastly the gain term (ideally) should be a function of the cars speed. Think about how far and fast you can turn the steering wheel at parking lot speeds and have your car stay in control. Now imagine you turned the wheel 90 deg in 0.2 sec at highway speeds. You'd be off the road in no time. At "high" speeds you reduce your "gain" to stay in control. Same thing with this project. For now we'll try to come up with a GAIN value that works at all speeds, albeit perhaps sluggishly at low speeds. I know at present you have the car set to go just 1 speed ... but I'm thinking ahead.

[Mee_n_Mac]

It occured to me that if you want to do some homework / research you could measure the car's "steering gain" and thus allow me to model the car to tune the controller gain. What's "steering gain" you ask ? Well imagine that you command the steering servo to 105 deg. I believe that makes the car run in a straight line, no turning right or left. Now imagine you command 106 deg of servo angle. That will command the wheels 1 deg* offcenter and cause the car to turn (right or left, I don't know). Ideally on a flat level surface the car, running at your preset speed, will drive in a "big" circle over and over and over again. It'll take some amount of time to complete each circle, ideally the same time for each go-around. Now a circle is 360 deg around and if you divide that by the number of seconds it took to go around, you'd have a turning rate, a number of deg/sec that the car will turn (change direction) for 1 deg of steering angle commanded (different from straight ahead). That's "steering gain", the deg/sec per degree of steering angle commanded. Now 1 deg of steering angle may make an awfully big circle (or not, I can't say) but you could do the test with a steering angle of 5 deg or 10 deg or, better yet do 5 and 10 and 15 and do right and left turns too ! We can divide the measured turning rate by the command (5 or 10 or 15 or whatever you use) to get the steering gain. The GAIN in your code would then be a scale factor times that steering gain.

Now if you don't want to time the circles, you could always print out the compass angle and time [millis()] and record them into a file via the serial monitor. The turning rate can then be computed from the heading changes vs the times.

Then repeat some of that testing at 50% of the present speed and then 2x the present speed. You can see how the steering gain varies with car speed.

If you wanted to you could then model your car's response to varying GAIN values using a simple spreadsheet and "tune" the software to be what you want. And then compare your prediction to the car's real behavior. Your now an engineer !

*Actually given the linkages involved, a command of 1 deg might change the wheels by more, or less, than 1 deg. Doesn't matter, what "we" want it the relationship btw commanded angle and turning rate.