[mikets]

Tomy, if you only care about going straight forward, the algorithm may be a lot simpler without considering all the corner cases. When going straight forward, all four wheels will turn in the same direction with the same strength. So you could do the following:

double distanceTraveled = (leftfrontEncoder->GetDistance() +
                                     rightFrontEncoder->GetDistance() +
                                     leftRearEncoder->GetDistance() +
                                     rightRearEncoder->GetDistance())/4.0;
if (distanceTraveled < targetDistance)
{
    MecaumDrive_Polar(forwardPower, 0.0, 0.0);
}
else
{
   MecaumDrive_Polar(0.0, 0.0, 0.0);
}