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);
lib727::util::deg2rad(a.theta);
// robot angle is in radians, target angle is in radians
double aDiff = tAngle - a.theta;
// convert difference to degrees
lib727::util::rad2deg(aDiff);
// 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 subtractcurrent.theta
when calculating the error? $\endgroup$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? $\endgroup$