Turning affects Odometry position

Programming Support
,
#1

Hey everyone!

I have been debugging my DriveToPoint PID for a while, but I have a feeling the problem lies in my odometry. Doing an on-the-spot turn affects the X and Y position of the robot. This may be due to the fact that I don’t take into account the offset of my tracking wheels, but I don’t know how to implement it.

Here’s my odometry config:
One tracking wheel (middle wheel) parallel to the direction of motion
One tracking wheel (back wheel) perpendicular to the direction of motion
An IMU

And here’s my code:

double currentAngle = m_imu->get_heading();

double middleEncoderPos = m_middleEncoder->get_position();
double backEncoderPos = m_backEncoder->get_position();

util::centiDegToDeg(middleEncoderPos);
util::centiDegToDeg(backEncoderPos);

m_prevBackEncoderPos = backEncoderPos;
m_prevMiddleEncoderPos = middleEncoderPos;

m_prevAngle = m_imu->get_heading();

while (true)
{
    middleEncoderPos = m_middleEncoder->get_position();
    backEncoderPos = m_backEncoder->get_position();
    util::centiDegToDeg(middleEncoderPos);
    util::centiDegToDeg(backEncoderPos);

    currentAngle = m_imu->get_heading();

    double dHeading = currentAngle - m_prevAngle;

    double dFwd = util::toInches(middleEncoderPos - m_prevMiddleEncoderPos, m_encoderWheelRadius);
    double dSide = util::toInches(backEncoderPos - m_prevBackEncoderPos, m_encoderWheelRadius);

    double headingRad = currentAngle;
    util::deg2rad(headingRad);

    double cosHeading = cos(headingRad);
    double sinHeading = sin(headingRad);

    double dX = cosHeading * dFwd - sinHeading * dSide;           
    double dY = sinHeading * dFwd + cosHeading * dSide;

    m_prevMiddleEncoderPos = middleEncoderPos;
    m_prevBackEncoderPos = backEncoderPos;
    m_prevAngle = currentAngle;

    m_position.x += dX;
    m_position.y += dY;
    m_position.theta = util::convert180(currentAngle);

    pros::delay(ODOM_TIMESTEP);
}

Thanks for any help!

Drive to Point PID