Home        Store        Docs        Blog

Using PID with heading control

(Richard) #1

With respect to PID, how do you setup your code to handle the numerical discontinuity when passing across North. If you use headingError = abs(setHeading - currentHeading) then Ki can never decrease?



(Rusty) #2


You can just wrap the angle until it is within the range of -180 to 180. For example, if the vehicle heading is 10 degrees and the goal is 350 degrees, then you get an error 350-10 = 340. You can subtract 360 until you are in the proper range. 340-360 = -20 so the error is -20 and the vehicle will try to turn left to correct.

Does that make sense?


(Richard) #3

@Rusty- Not quite. Works fine for your example, but reverse the conditions:

vehicle is heading 350 and the goal is 10, then you get an error 10-350 = -340. In this case you need to add 360 to get a right turn of 20 degrees.

(Jacob) #4

error = target - heading;

if(error < -180)
error += 360;
else if(error > 180)
error -= 360;

now error is correct value

Does this work for you?

(Richard) #5

Yes that works, thanks.

(Rusty) #6


Yes, that’s what I meant by wrap the angle until it is within -180 to 180. Sorry that wasn’t more clear!


(Richard) #7

To account for the 1475 to 1525 usec deadband I thought I would do this:

drive = p + I + d;

if (drive > 0){ drive = 1525 + drive;}

else {drive = 1475 + drive;}

It may turn out I need to leave in some of the deadband.

(Rusty) #8

Hi Richard,

That would work although I would think that you’d want to leave the deadband in. Otherwise, the thrusters will be running all the time and you’ll end up with constant oscillation.

If something causes the vehicle to turn constantly, like a twisted tether, the I-term will take care of that regardless of the deadband.


(Richard) #9

I now, after many hours of testing, have a decent heading hold program. The problem is I don’t understand how it can possibly work with the K values I ended up with. Some background:

I rigged up 3 rheostats on my control box to send down 0-5 volts and give me a way to have variable K’s in the water. I started with the full 50 usec ESC deadband and went through every imaginable combination using integer values of K. I could never stop the oscillations to my satisfaction. I decided to divide all of my K values by 10 and work with decimals. That did the trick! In fact look at my .1 Kp and .1 Kd. I don’t understand how they can be so low and work. Because of the large deadband there was a slow drift back and forth within the deadband. I kept reducing it until I reached 4 usec and it works fine right there.

The compass noise is about +/- 1 degree.

By the math, the motor should not even turn until I am 20 degrees off setpoint. It does though, and holds within 2 degrees. The motor is not running continuously. Just “bumping” on every second or so. Once I start working in currents, and with the vertical stabilizer, a little Ki may come into play.

Somehow the ESC is seeing a drive 10 times my K values. Please see if you can pick it up somewhere in my program.



void loop(){
float Kp = .1;
float Ki = 0;
float Kd = .1;
int drive;
int errorPrevious = 0;
int errorAccumulator = 0;
int errorDelta = 0;
int newHdg = 0;
setHdg = hdg(); // lock in heading to use for auto Hold if ON
while (digitalRead(autoHdgPin) == HIGH){ // auto hold ON
newHdg = hdg();
int errorCurrent = newHdg - setHdg;
if(errorCurrent < -180){errorCurrent += 360;}
else if(errorCurrent > 180 ){errorCurrent -= 360;}
errorAccumulator = errorAccumulator + errorCurrent;
errorDelta = errorCurrent - errorPrevious;
int p = KperrorCurrent; //proportional
int i = Ki
errorAccumulator; //integral
int d = KderrorDelta; //differential
drive = p + i + d;
if(drive < 0){rightMotor.writeMicroseconds(drive + 1478);}
else {rightMotor.writeMicroseconds(drive + 1523);}
errorPrevious = errorCurrent;
while (digitalRead(autoHdgPin) == LOW){ //auto hdg hold OFF
//manual control
xVal = analogRead(xPin);
xVal = map(xVal, 0,992,1100,1900);
int hdg(){
Get a new sensor event */ }

(Richard) #10


(Jacob) #11

You are correct, this should work, even when using the float and int data types in the calculation. I suggest you check that the hdg() function is actually returning a range between 0 - 360. As in make sure it is not returning 0 - 3600 or something like that. Just immediately after the newHdg = hdg(); line, put
Serial.print("hdg: ");
You can do this similarly with the other values to confirm your calculations and find that stray value that causes the discrepancy.

(Richard) #12

Did you mean “this shouldn’t work”?

(Jacob) #13

Nope :), It should. I tried it out myself by just hard coding the values of setHdg and newHdg. Try printing out the values of each variable like I suggested, and the issue will become apparent.

(Richard) #14

Yeah, I did that. I see my p, i, and d are integers only. I thought the integer would converted to a float as a result of using a float in the expression.

I am still surprised that they are 1, not some larger number. It obviously takes very little thrust to turn the WTE. Interesting to see what happens with 2 motors and when I get out in changing currents. Thanks for the help.


(Richard) #15

Here is a picture from yesterday’s dockside heading lock test: