[pafwl]

We do not use the code from WPI. I and my students have written our own. We did this so they understand how it works. When carbbing the opposite corners move in the same way. When turning the sides move together.


You the function below once for each wheel to get the wheels power. It works very well and is easly understood. It assumes that forward is 1.0, back is -1.0, crab left is 1.0, right is -1.0, turn to left is 1.0 and right is -1.0.

The joystick values are assigned when we read in the DS values. They may need to be reversed via negation. We also may need to negate again when we assign the values to motors. (left vs right side)

Right now the rotation seems backwards and may be the result of us switching front to back but for now it works great.

We do it this way so we can use the same code in autonomous. We just pass in the values we want in the auto code and it all works great.

float LC2010::GetMecanumPower( int i_wheel, float f_direction, float f_power, float f_turn ) 
{
	float retValue = 0.0;

	switch( i_wheel )						// Apply Forward and side motion
	{
		case LC2010::kMecanumLeftFront:
		case LC2010::kMecanumRightRear:
			retValue =  f_power - f_direction;
			break;

		case LC2010::kMecanumLeftRear:
		case LC2010::kMecanumRightFront:
			retValue =  f_power + f_direction;
			break;
		
		default:
			printf("GetGetMacannumMotorValue: Bad Wheel passed....\r");
			break;
	}

	switch( i_wheel )						// Apply the rotation
	{
		case LC2010::kMecanumLeftFront:
		case LC2010::kMecanumLeftRear:
			retValue += f_turn;				// retValue - f_turn;
			break;

		case LC2010::kMecanumRightFront:
		case LC2010::kMecanumRightRear:
			retValue -= f_turn;				//retValue + f_turn;
			break;
	
		default:
			printf("GetMacannumPower: Bad Wheel passed....\r");
			break;
	}


	if( retValue < -1.0) 								// condition before recasting to unsigned char
		retValue = -1.0;

	if( retValue > 1.0 ) 
		retValue = 1.0; 

	return retValue; 
};