MECANUM WHEELS PROGRAMING

[dona klarson]

hi
i have a problime understanding the concept of mecanums wheels' programing .. i mean there the magnitute .. direction ... which i don't get ....
so please help me ...
dona

[jhersh]

Think of a vector. It has a magnitude and a direction. The magnitude is between 0 and 1 how fast to move, and the direction is in degrees. These two parameters are for translational movement. For rotation, you can spin in place or spin and translate at the same time... the range is -1 to 1 for counter-clockwise and clockwise rotation.

[oddjob]

These are the important parts to get started. We don't have a chassis yet, but we wired up 4 Jaguars and 4 motors and the motors move as they should:

RobotDrive *mecanumDrive;
Joystick *Attack3Joystick;
...

mecanumDrive = new RobotDrive(1, 2, 3, 4);	// frontLeft, rearLeft, frontRight, rearRight
Attack3Joystick = new Joystick(1);
...

mecanumDrive->HolonomicDrive(Attack3Joystick->GetMagnitude(), Attack3Joystick->GetDirectionDegrees(), Attack3Joystick->GetTwist());

The kit of parts joysticks don't have a twist handle so I didn't test the twist (rotation).

[slavik262]

I'd assume that GetTwist() gets the z-axis value, which for the KOP joysticks is the throttle at the bottom of it.

Also, this thread is from a year ago, but I created a custom joystick class for use in omni-directional drive trains:

http://www.chiefdelphi.com/forums/sh...ad.php?t=70468

Correct me if I'm wrong, but I believe GetMagnitude() just takes the square root of the squares of the x and y factors of the joystick to get its value (the Pythagorean Theorem approach). This is flawed in that due to special right triangles, sticking the joystick in the corner of its field of motion will generate a magnitude sqrt(2) larger than sticking the joystick all the way to a side. I created my own algorithm which "normalizes" the magnitude of the joystick vector so that it has a max value of 1 all the way around its bounds. Details are in the linked thread.

I wrote the OmniJoystick class before I knew about function overloading and inheritence, so to make life simpler you can change GetAngle() and GetR() in the my code to GetMagnitude() and GetDirectionDegrees() if you'd rather use the "official" function names.

[oddjob]

Originally Posted by slavik262

Correct me if I'm wrong, but I believe GetMagnitude() just takes the square root of the squares of the x and y factors of the joystick to get its value (the Pythagorean Theorem approach). This is flawed...

I could be wrong, but I don't think your code is optimum for a mecanum drive. Looking at the WPI code, nor is that.

If I push the joystick all the way to the right and forwards, the magnitude is 1.414 and the angle is 45 degrees. (maybe it's -45 degrees with the joystick defaults, but the idea is the same). If those magnitude and direction values are entered into the HolonomicDrive method, the inference is that I want to move forwards and to the right at 100% speed for both, so all wheels get 100% power with appropriate direction for each to make the chassis move diagonally as fast as it can. Looking at the WPI HolonomicDrive code, that's not what I get:

void RobotDrive::HolonomicDrive(float magnitude, float direction, float rotation)
{
...
	magnitude = Limit(magnitude);
...
}

float RobotDrive::Limit(float num)
{
	if (num > 1.0)
	{
		return 1.0;
	}
	if (num < -1.0)
	{
		return -1.0;
	}
	return num;
}

If the joystick is moved far right and forwards, the HolonomicDrive method gives only 71% power (100%/sqrt(2)) to each wheel because it limits the magnitude to 1.0. That's not what I expect or want. Each wheel should be getting 100% power. It's not a big deal because I don't expect to be running the robot at full speed during this game and the difference between 71% and 100% power when moving diagonally is not something to go crazy over.

[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; 
};

[Ether]

Originally Posted by pafwl

We do not use the code from WPI....

See post number 9 in the following thread:

http://www.chiefdelphi.com/forums/sh...ad.php?t=81025


~

[oddjob]

I took a closer look at the WPI HolonomicDrive code. If the joystick is pushed full forwards (magnitude 1.0, direction 90 degrees) the drive wheels get only 71% power due to the sin and cos of (direction+/-45degrees) functions. The WPI code trades off maximum power for directional control, which is certainly a valid solution but I have a different strategy

Originally Posted by Ether

See post number 9 in the following thread:
http://www.chiefdelphi.com/forums/sh...ad.php?t=81025

[Ether]

Originally Posted by oddjob

The WPI code trades off maximum power for directional control, which is certainly a valid solution but I have a different strategy

I think you can have your cake (max power) and eat it too (correct direction), by using the scaling technique suggested in post number 9 in the following thread:

http://www.chiefdelphi.com/forums/sh...ad.php?t=81025


~

[jhersh]

This has been updated in WPILib and will be in the next update.

[Ether]

Originally Posted by jhersh

This has been updated in WPILib and will be in the next update.

What does the pronoun "this" refer to?

Which post are you replying to?

There's no context, so it's not clear.

Thanks.

~

[jhersh]

Originally Posted by Ether

What does the pronoun "this" refer to?

Which post are you replying to?

There's no context, so it's not clear.

Here is the change list description:

+ Add a Cartesian implementation of the mecanum drive method.
* Clean up the expressions in the polar mecanum drive method.
* Replace the saturation approach with a normalization to prevent inconsistent wheel speed ratios.
* Scale the magnitude by sqrt(2) to get full power along the axes.

[oddjob]

Great work! Except now our robot won't have a power and pushing advantage over the other mecanum's using HolonomicDrive, LOL.

Originally Posted by jhersh

Here is the change list description:

+ Add a Cartesian implementation of the mecanum drive method.
* Clean up the expressions in the polar mecanum drive method.
* Replace the saturation approach with a normalization to prevent inconsistent wheel speed ratios.
* Scale the magnitude by sqrt(2) to get full power along the axes.

[Ether]

Originally Posted by jhersh

* Scale the magnitude by sqrt(2) to get full power along the axes.

The approach suggested in post number 9 in the following thread:

http://www.chiefdelphi.com/forums/sh...ad.php?t=81025

suggests scaling by the largest magnitude when any magnitude exceeds 1, rather than scaling by a fixed scalar (ie sqrt(2)).

the reason for doing so is explained in the post.

your thoughts?

~

[Abrakadabra]

Originally Posted by jhersh

This has been updated in WPILib and will be in the next update.

<baited_breath> So when do we get this next update you speak of? </baited_breath>

Can you just commit the RobotDrive changes to FIRSTForge? We would be happy to help test.