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!