[theprgramerdude]

Originally Posted by SuperNerd256

The themselves haven't programmed it, but from reading the WPI code and looking at how to make it better, they said getting it perfect (not 100%, but a close 99%) would take some extra code.

I'll ask them at our next meeting, though. Now I'm interested, too.

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]);
}

This is straight from the RobotDrive source. What could possibly be improved?