Drive to Point PID

Programming Support
,
#1

Hey everyone!

I’m trying to implement a Drive to Point PID, but I can’t get it to work for the life of me. Essentially, if I plug in the point {10, 10} it gets to around {10, 8} and then starts accelerating since the distance error isn’t 0. I know my error calculation itself is correct. Can someone please point out what I’m doing wrong?

Here’s my driveToPoint function:

void PID::driveToPoint(Point target, Odometry *odom, int maxSpeed)
{
    // This function will drive the robot to the target point

    double prevDistError = 0;
    double prevAngError = 0;

    // current point
    Point current = odom->getPos();
    Point initial = current;

    double distIntegral = 0;
    double angIntegral = 0;

    while (true)
    {
        current = odom->getPos();

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

        // check if we are done
        if (canExitDTP(m_currentLeftVoltage, m_currentRightVoltage, distError, angError))
        {
            break;
        }

        distIntegral += distError * (PID_TIMESTEP / 1000.0);
        angIntegral += angError * (PID_TIMESTEP / 1000.0);

        // prevent integral windup
        if (std::signbit(distError) != std::signbit(prevDistError))
        {
            distIntegral = 0;
        }

        if (std::signbit(angError) != std::signbit(prevAngError))
        {
            angIntegral = 0;
        }

        double distDerivative = distError - prevDistError;
        double angDerivative = angError - prevAngError;

        double outputDist = (m_driveConstants.kP * distError) + (m_driveConstants.kI * distIntegral) + (m_driveConstants.kD * distDerivative);
        double outputAng = (m_turnConstants.kP * angError) + (m_turnConstants.kI * angIntegral) + (m_turnConstants.kD * angDerivative);

        double lVoltage = outputDist * (MAX_LIN_DTP_SPEED / 100.0) + outputAng;
        double rVoltage = outputDist * (MAX_LIN_DTP_SPEED / 100.0) - outputAng;

        // move motors!
        m_left->move(lVoltage * (maxSpeed / 100.0));
        m_right->move(rVoltage * (maxSpeed / 100.0));

        prevDistError = distError;
        prevAngError = angError;
        m_currentLeftVoltage = lVoltage;
        m_currentRightVoltage = rVoltage;

        debug++;

        pros::delay(PID_TIMESTEP);
    }

    m_left->brake();
    m_right->brake();

    m_currentLeftVoltage = 0;
    m_currentRightVoltage = 0;
}

If anyone can help, I would really appreciate it. Thank you so much!

#2

So just from an initial look at your implementation, it seems like the problem is the fact that you don’t handle backwards movement.

Let’s say you have a point behind the robot (0, -10). Computing the distance error would give us a distance to travel of 10 and an angle error of 180 (assuming the robot was facing towards positive 90 at (0, 0)).

The problem is that your PID controllers, given these numbers, will attempt to move the robot forward due to that distance error being 10. Now let’s suppose, using your example of a point (10, 10). Your robot moves to (10, 10), but because it isn’t perfect, it slightly overshoots to (10.326, 10.177). Same problem, because you overshot the target the only thing the robot knows to do is to go forwards.

The fix to this is to perform a simple check. If the angError is above abs(90) degrees in either direction, then subtract 180 from the angle error and multiply the distance error by -1.

if (std::abs(angError) >= 90) {
	angError = util::convert180(angError - 180);
	distError *= -1;
}

Now, if the robot overshoots its target, it’ll know to drive backwards at the inverse angle rather than attempt a complete 180.

#3

I’m going to assume you are using a tank drive because this problem doesn’t really happen on x-drive.

There are a few ways to implement the solution, but the general idea is that you want the robot to drive to the point backwards if going backwards is faster than turning all the way around.

The way we did it was just to multiply our drive output by the heading scale factor, or the cosine of the heading error. That way once you pass the target point, the robot wants to start driving backwards, since cosine 180 is -1.

Just make sure to add 180 to the heading pid error (or subtract it, depending on which way you’re going) if you’re reversed; otherwise your robot will be trying to turn way too far when driving backwards.

You can see how we did it in the JAR-Template in line 262 of src/JAR-Template/drive.cpp

Hope this helps.

1 Like
#4

Thanks for the suggestions! I implemented those, but I believe my issue is caused by my odometry. I made a thread here: Turning affects Odometry position.