Mecanum Wheels & Encoders -- Java code

Hello all,

I am from team 4687, and I am having some trouble coding the encoders for a mecanum wheel robot. I currently have some code developed to read the encoder values, but I am unsure of how to change the speeds of the wheels based off of these values. Here is what I would like to do:
I would like to use 1 wheel and call it the master wheel. This wheel will provide me with an encoder value. This encoder value will then be used to “enslave” the other wheels, which will then match the master wheel’s rpm. The only differences in the wheels will happen when I change the direction of the wheels to move in the directions I need them to move in.

If you guys have any other way to implement some encoder code into a mecanum wheel drive to get all the wheels to move the same speed all the time, it would be great if you could advise me on that too. I will leave you all with my current code, with encoders implemented but doing nothing.

Thank you all in advance for your help in this project.

//I do my brackets differently than most
//import statements
public class RobotTemplate extends SimpleRobot
RobotDrive drive =- new RobotDrive(1,2,3,4);
Joystick joystick = new Joystick(1);
Encoder wheelOne = new Encoder(8,7);
Encoder wheelTwo = new Encoder(4,3);
Encoder wheelThree = new Encoder(6,5);
Encoder wheelFour = new Encoder (2,1);

     public void OperatorControl()
          while(true && isOperatorControl() && isEnabled())
          drive.mecanumDrive_Cartesian(joystick.getX(), joystick.getY(), -1*joystick.getTwist(), 0);
          int one = wheelOne.get();
          int two = wheelTwo.get();
          int three = wheelThree.get();
          int four = wheelFour.get();

Your post is confusing.

What is the point of having mecanum wheels if you are going to spin them all at the same speed?

The only differences in the wheels will happen when I change the direction of the wheels to move in the directions I need them to move in.

? You don’t change the direction of a mecanum wheel to achieve rotation and strafe. You change its speed.

Feel free to send my team an email (2177 (at)… we’ve been doing work with encoders for a while now, and are actually right now working on using them for feedback in a swerve drive robot (at our last meeting, we had just gotten everything set up to find our PID constants for the speed encoders, and we should be working on finding them this week). You’re a little outside the cities, but if you wanted to come over to our space we could spend some time going over what we know and how it all works.

Feedback loops are a fairly complicated subject, and in my experience don’t transfer well over a forum. Look at the PIDController object and google a white paper “PID Without a PhD” to get started, and we’re certainly willing to help!

If I am driving a mecanum wheel drive with all wheels at the same speed, I can change the direction each wheel is spinning relative to the front to achieve each individual result.
If I spin all 4 wheels forward relative to the front, the robot will move forward.
If I spin the front-left and the back-right motors forward relative to the front, and the front-right and back-left motors backwards relative to the front, the chassis will slide to the left.
If I spin the front-left and back-left wheels backwards relative to the front, and the front-right and back-right wheels forwards relative to the front, I will get a Counter Clockwise turn.

That’s true… but trust me, you’re going to want control in between those extremes in motion. For example, what about turning while strafing? It’s the whole idea of being able to move around a stationary object while always facing it. Trust me, it’s easier to go for complete control from the start than it is to implement the extremes then try to jury-rig in everything in between.

No. It will slide to the right.

If all 4 wheel speeds are the same, you will get rotate-in-place (spin)

Now, what do you do when you want to go forward and turn at the same time?

Even if I did want to be able to modify the speeds of the wheels, one of the problems we are having is that the wheels are spinning at different speeds even if we are only trying to go in one direction. Example: Each time we move forward, the wheels are misaligned so that the robot curves significantly. Do you guys have some code to fix this?

Yes, the encoders help fix that. Look at the PIDController class and the white paper I mentioned - they will help you to understand feedback loops and how to get started with them, although be forewarned - they can be difficult to figure out! Spend some time going through all that and then please contact my team (address already given) - we’re happy to help out! We’ve been giving the Java presentation at MN Splash for years now, are an Alpha/Beta team for the new control system, have been playing around with PID loops since 2008, and we really aren’t that far away from you guys. Trust me - if you want the robot to go straight, my students know how to make that happen.

First order of business is to fix all mechanical design and craftsmanship problems.

Put the bot up on blocks and power each wheel one at a time in the forward direction at various command levels. Are some wheels noticeably slower or faster than others? Take notes.

Then repeat the test except spin the wheels in the reverse direction.

Tell us what you find.

With all four wheels being commanded to have the same speed (albeit in different directions), you’ll have a robot whose steering cannot be controlled while moving. Is that your aspiration?

At this point in time, that is our aspiration. I am the only one who knows how to program the robot on our team, and I simply do not have the time right now to put into learning about PID. I will learn about that for competition, but right now we are doing some out of competition practice. I have been having trouble with setting up the encoders and the master/slave wheels. Do you guys have some kind of set-up for that?

*I highly recommend you forego the master/slave all-wheel-speeds-the-same approach you are describing and do the following instead:

1) fix the mechanical issues first. see my earlier post.

2) forget the PID and encoder for now. instead, program the wheel speeds properly using either the WPILib RobotDrive class or home-brew code described here. This will give the driver full control of steering and strafing.

3) Once you’ve got that working, there’s a good chance you won’t need PID at all.

No one here has been very helpful yet with OP’s original project goals, so I’ll chime in here. I see exactly what what you want to do, so here’s what I suggest:

First, learn how the mecanum drive classes actually map joystick inputs to individual motor powers and generate an equation describing them. Stick with linear control and it should be relatively simple to interpolate.

For example, look at the constraints first:

When JoyY1 = 1 and JoyX = 0, all four motors output “1” (for purposes of this exercise, reverse polarity on the opposing wheels due to gearbox orientation)

When JoyY1 = -1 and JoyX = 0, all four motors output “-1”
When JoyY1 = 0 and JoyX = 1, Front outputs “1” and rear outputs “-1”
When JoyY1 = 0 and JoyX = -1, Rear outputs “-1” and front output “1”
When JoyY1 = 1 and JoyX = 1, Front outputs “1” and rear outputs “0”

etc etc

Anyway, map the entire region on an XY plane and you can see how the X input and Y input varies each motor power. Note that they are dependent.

Now, in your code you can record the X and Y input values, and calculate the “expected” motor output powers. Use your reference encoder to relate power vs rpm (say, 100 rpm = 0.7 power output) and calculate a “control” constant (although this could be a function and not a constant – determine this by testing at different powers). C = 100/0.7 (or whatever units you want).

For the rest of the wheels, read in encoder values and calculate new power ratios based on how different each wheel’s power correlates to their rpm.
100 rpm / C = output power.
If “output power” is exactly 0.7 here as well, the wheel speeds are equal. But since they’re not due to friction or other reasons, you will need to generate new constants for each wheel.
100 rpm / C = 0.7 * constant

that constant will be close to 1 – maybe 0.95 if that particular wheel has less friction, 1.05 if it has more.
Anyway, take this constant and bias the mecanum drive output: motor1.set(motor1_output*constant);

That way, in the case where JoyY = 1 and JoyX = 0 (for example), Motor1 = 1 but Motor2 = 0.95, thus keeping the robot driving straight.

Last thing – you’d have to determine the “worst-case” motor, the one that performs the worst for a given output power, and use that as your limiting output power. Otherwise the less powerful wheel will saturate (above calc will be >1) and the power will truncate to 1. So all the constants to 3 motors should be <1, and the least effective motor is exactly 1.