# Drive to point PID doesn’t work properly

This is my first post here. I am trying to get a robot I’m working on to drive to a point via a PID controller. My code works fine if the point is in front of the robot, but completely falls apart if the point is behind or to the side of the robot. I have Odometry running in the background, and I am able to access updated position information easily.

Here’s the code I’m using now to determine the error which I feed to the PID:

current = odom->getPos();

// calculate our error
// our distance error is the distance to the target
double distError = util::distance(current, target);
double angError = (util::getAngDiff(current, target) - current.theta);

if (angError > 180)
{
angError -= 360;
}

else if (angError < -180)
{
angError += 360;
}


And my getAngDiff:

double util::getAngDiff(Point a, Point b)
{
// current = a, target = b

double tAngle = atan2(b.y - a.y, b.x - a.x);

double aDiff = tAngle - a.theta;

// convert difference to degrees

// round to 100th place
return round(aDiff * 100) / 100;
}


It’s worth noting that my current angle (current.theta) is in degrees and is in the range [0-360), and I use the Euclidean distance formula to calculate distance error. From there, I simply add/subtract my angular error from my distance error and multiply the final error per side by kP. Is there anything obvious wrong with my math? Here’s a video (in a simulator) of what happens when I tell the robot to go to the point {0, 3}:

https://i.stack.imgur.com/V74X0.jpg

It should turn 90 degrees and then drive straight, but it obviously doesn’t. Thanks for any help!

• getAngDiff already gives you how much to turn in order to point to the target. Why do you subtract current.theta when calculating the error? Feb 7 at 15:27
• I subtracted it because my robot also has its own angle relative to the point, so I figured I need to take that into account. Feb 7 at 16:47
• Can you include the code for the PID controller, or at least a brief description of the control law? Feb 9 at 16:31
• Somehow missed the lower part of this question on first reading. First question is, what do you mean I simply add/subtract my angular error from my distance error and multiply the final error per side by kP. Is there anything obvious wrong with my math? - Yes, angular error and distance error are unrelated, and you shouldn't be mixing the two. Echoing @domo_arigato - please show how you're building and applying the PID signals. Which wheels are driven, how are you numbering/mapping those wheels, and how are your PID outputs applied to those wheels?
– Chuck
Feb 9 at 16:55
• Second question is how are you moving the vehicle in your simulator? Are you providing an x/y/rotation and letting the simulator render the result, or are you providing wheel speeds and letting the simulator determine position? I ask because it looks like you've got six wheels, with one being in the center of the vehicle aligned with the others and one being on one end perpendicular to the others. If you're relying on physics simulation to do your work then that perpendicular wheel might be preventing it from moving forward AND preventing proper rotation, depending on how you're driving it.
– Chuck
Feb 9 at 16:56

I don't understand

lib727::util::deg2rad(a.theta);


You're not assigning the output anywhere, so are you expecting that this is going to modify a.theta? If so, you're not modifying it back! This would seem to me then that you're constantly applying a degrees-to-radians conversion over top of the existing angle, which I believe should quickly drive it to zero, because the conversion of degrees to radians is approximately divide-by-sixty. First call it'd convert 120 to about 2, then the second call it'd convert a.theta from 2 to about 0.03, then on the third call it's basically zero.

I think it'd be easier to keep everything in degrees instead:

double tAngle = lib727::util::rad2deg(atan2(b.y - a.y, b.x - a.x));
double aDiff = tAngle - a.theta;


It's not clear what kind of robot you've got, or what you're doing with the PID control, but if you don't tune your angular controller to be significantly more aggressive than your linear controller then you'll probably wind up with some form of a limit cycle where your robot is just driving circles around the target position.

If you're trying to do this with an Ackermann-steered vehicle (as opposed to a differentially-steered vehicle) then you're probably not going to succeed with brute PID control alone; you'd need to do some trajectory planning and PID to that trajectory.

• The degToRad is able to modify values because it takes in a reference to a value and directly overwrites it. It essentially works out to (degrees * pi / 180). I will probably just eventually keep everything in degrees anyways like you suggested. As for the robot, it is not Ackermann-steered, and I limit my linear speed to 80% of the max to give angle correction some leeway. I can always decrease this number if that's way too high, or send more code if needed! Feb 7 at 23:46
• @SprintKeyz are you restoring a.theta somewhere else? I understand the function overwrites the value, but I can't see when or where the value is originally generated.
– Chuck
Feb 8 at 12:35
• I have an odometry task running in the background, and I can access the current position from it via Point current = odom->getPos(); (I have the variable initialized above this section of code, so I just do current = here). It returns me a point, which can have an X, Y, and angular variable attached to it. I have checked this part of code and it seems to work fine, but I can always send how I get that if needed. Feb 9 at 14:23