Mec_driver vector math:

[AhmedSamara]

So last meeting I tried to figure out a way to go backwards from knowing the speed of each individual motor to finding out what the overall speed was.

I thought this would be some fairly simple vector math, but nothing I do seems to be giving the correct answer.

The first method I tried was the one used in MecanumDrive.pdf.

The X/Y plan was turned 45 degrees and we treatedt the diagonal with the front_right wheel and back_left wheels as the x-axis.

This made the vector math really simple (Or so I thought) because then the speed is just:

Vx = front_right + Back_left
Vy = front_left + back_right

Speed = sqrt(Vx**2 + Vy**2)

The interesting thing about this one is that it consistently gave double the correct answer.

The other method I tried was just drawing the force diagram for a car moving straight forward myself and adding up the x/y components of each wheel, so the formulae were:

Vx = front_left*cos(pi/4) -front_right*cos(pi/4) - back_left * cos(pi/4) 
Vy = front_left*sin(pi/4) + front_right*sin(pi/4) + back_left*sin(pi/4) + back_right*sin(pi/4)

Speed = sqrt(Vx**2 + Vy**2)

Again, this gives incorrect numbers, but the same ones as the previous numbers and it's always double what it's expected to be, though that may just be a consequence of the fact that I used simple numbers.

I've been testing this by using python as a calculator and just assuming that all wheel speeds are '1', calculating the overall speed, and then trying to calculate a wheel_speed using the original formulas in MecanumDrive.pdf.

So now I'm out of ideas on vector math.

I didn't really have a clear idea of where I was going with this before, and I wasn't so all I have is a sloppy "calculator junk" file, but I'll work on a more formal test framework now.

If anyone can find the mistakes in my math, or figure out why it's giving the wrong values, please let me know.

[whiteskulleton]

Ok. The math behind this is pretty complex. A few months ago, I spent a few hours looking at the math behind the mecanum wheels. I sent an email out to the team telling them what I had found. Anyways, I just spent about 3 hours reworking the math so it can take different inputs. I created an excel file that compiled my results. I need to show you the formulas and the calculations that I did in person because it is too hard to explain online. However, my formula tells me that if you want to go 85 degrees north-west as fast as possible you need to tell the motors:

front-left: spin at 83.91% power counterclockwise back-left: spin at 100% power clockwise back-right: spin at 83.91% power clockwise front-right: spin at 100% power counterclockwise

The algorithm works for any angle if the robot has no rotation. The algorithm can also take into account the rotation of the robot. However, I need to know some additional information about the motors first before that part is accurate.

[dfarrell07]

I need to know some additional information about the motors first before that part is accurate.

What do you need to know about the motors?

Can you write your math out in a post, to document it here? Or just upload the Excel file?

Also, yay for Mark's first post on GitHub! :)

[whiteskulleton]

I just uploaded the excel file under mechanical folder. Mess around with the Velocity Limit percentage. The other 4 motor columns tell you the direction to spin the wheels and how much "power" you need to apply.

I need to know the maximum speed that the motors can turn at.

[AhmedSamara]

We already have a method for figuring out how to tell the motors where to go, my problem now is going backwards.

Using each (known) wheel_speed to calculate what the overall speed is.

[whiteskulleton]

You might want to take a look at the Mecanum Kinematic Analysis. I uploaded it to the drive. It is very complex and long so I'm going to quote the key parts.

So this is pretty much your problem: "given the four wheel rotational velocities [Ω], find the resulting vehicle motion [V]."

The equation to solve this problem is: Where [V] = velocity of robot (matrix) r = radius of the wheels [Ω] = angular velocity in radians per second of the wheels (matrix) [F] = matrix that solves for the velocity (matrix)

The units for the velocity of the robot depend on what units you use for the radius of the wheels and for [F]. Ex. if r=1.0665 inches and [F] is in inches then velocity is measured in inches per second if r= 27.09 millimeters and [F] is in millimeters then velocity is measure in millimeters per second

"This problem, in general, has no solution.... The physical meaning of this is: if four arbitrary rotational velocities are chosen for the four wheels, there is, in general, no vehicle motion which does not involve some wheel "scrubbing" (slipping) on the floor. However, a matrix [F] which generates a "best fit" least squares solution can be found:"

They then go into some detailed calculations and get matrix F. Basically, the solution that they get is an educated guess on where the robot is going to go.

Here is the diagram:

They don't do a good job of giving a straight answer so I plugged in the matrices and got this:

Consult the diagram to understand it better Vx is the velocity of the robot straight ahead Vy is the velocity of the robot sideways wv is the angular velocity of the robot - how fast it is spinning.

K is the X distance from the center of the robot to the wheel plus the Y distance from the center of the robot to the wheel.