void RobotDrive::MecanumDrive_Polar(float magnitude, float direction, float rotation)
{
	// Normalized for full power along the Cartesian axes.
	magnitude = Limit(magnitude) * sqrt(2.0);
	// The rollers are at 45 degree angles.
	double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
	double cosD = cos(dirInRad);
	double sinD = sin(dirInRad);

	double wheelSpeeds[kMaxNumberOfMotors];
	wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
	wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
	wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
	wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;

	Normalize(wheelSpeeds);

	m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor]);
	m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor]);
	m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor]);
	m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor]);
}