I am working on an autonomous car. I want the PID control to work such that the car steers at the angle between its current location and a location that I have given it.
The problem I'm having is that when the angle the car is actually going (the heading) is greater than the angle the car is supposed to go (the bearing), the car drives straight. As you can see from the debug data below, when the heading is less than the bearing (0 degrees < heading < 271.7 degrees), it steers as I would expect it to. However, when the heading is greater than the bearing (271.7 degrees < heading < 360 degrees), it steers straight. (The steering servo range is 1100 us (0 degrees) to 1900 us (180 degrees)--1500 us is straight ahead)
At first glance, you may think that the problem is due to my decision to offset the steering servo angle + 400 us. However, this is necessary, because without the offset, the car steers about 120 degrees counter-clockwise of where it is supposed to go. Even with the offset removed, the PID output is 0 from 271.7 to 360 degrees.
Any thoughts on why this might be and what I can do to fix it?
Below is the relevant debug data (for all of the debug data see http://jeffsinventions.com/wp-content/uploads/2012/07/PID-steering-debug-data.txt):
Bearing to way point: 271.712036 degrees
Heading: 270 degrees
PID output: 1.71
Raw steering servo angle: 1103 us
Steering servo angle: 1503 us
Bearing to way point: 271.712036 degrees
Heading: 276 degrees
PID output: 0.00
Raw steering servo angle: 1100 us
Steering servo angle: 1500 us
...
Bearing to way point: 271.712036 degrees
Heading: 338 degrees
PID output: 0.00
Raw steering servo angle: 1100 us
Steering servo angle: 1500 us
Bearing to way point: 271.712036 degrees
Heading: -15 degrees
PID output: 255.00
Raw steering servo angle: 1900 us
Steering servo angle: 1900 us
Here is a selection of the code (for all of the code see http://jeffsinventions.com/wp-content/uploads/2012/07/PID-steering-code.txt):
// setup PID
#include <PID_v1.h>
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint,1,0,0, DIRECT);
int outputOffset = 0;
...
void setup() {
Serial.begin(115200);
// turn PID on
if(debug) { Serial.println("Starting PID"); }
myPID.SetMode(AUTOMATIC);
...
}
void loop() {
...
// calculate bearing
if(debug) { Serial.println("Calculating bearing..."); }
bearing = calculateBearing();
if (debug) { Serial.print("Bearing to way point: "); Serial.print(bearing,6); Serial.println(" degrees"); }
// read compass
if(debug) { Serial.println("Reading compass..."); }
compass.read();
heading = compass.heading((LSM303::vector){0,-1,0});
if(debug) { Serial.print("Raw heading: "); Serial.print(heading); Serial.println(" degrees"); }
Input = heading;
Setpoint = bearing;
myPID.Compute();
if(debug) { Serial.print("Raw PID output: "); Serial.println(Output); }
steeringServoAngle = map(Output,0,255,1100,1900);
if(debug) { Serial.print("Raw steering servo angle: "); Serial.print(steeringServoAngle); Serial.println(" us"); }
steeringServoAngle = steeringServoAngle + steeringServoAngleOffset;
if(steeringServoAngle <= 1100) { steeringServoAngle = 1100; }
if(steeringServoAngle >= 1900) { steeringServoAngle = 1900; }
if(debug) { Serial.print("Steering servo angle: "); Serial.print(steeringServoAngle); Serial.println(" us"); }
}