Hello! We’re on our third day of of four day “build season”, and are almost ready to test drive. I’ve been reading about mecanum wheels here for a while, and am starting to worry that our motors may not be capable of spinning synchronously (well, we haven’t tested yet, but it seems to happen to a lot of other people). We’re using C++, and I don’t currently have one of the developer laptops with the programming environment and WPI source to use as a reference. What I was hoping we could do was to drag the entire Mecanum_Cartesian method out of WPI and insert it into a method below the periodic portion of our code so we could easily manipulate the individual motor values to get the correct speeds (and so that we can see the math involved instead of putting all our faith in the library). Could someone give me some direction? Thanks!
The source for wpilib is included as a project in Java, but I don’t know if it’s included in the C++ release.
If you want to right your own mecanum code, it’s pretty easy. Here’s the important part from the Java library.
frontLeft = xIn + yIn + rotation;
frontRight = -xIn + yIn - rotation;
rearLeft = -xIn + yIn + rotation;
rearRight = xIn + yIn - rotation;
Just make sure that you invert your y axis on the joystick. Also, you should have you code check to see if any of the wheels speeds are > 1, and if so, divide them all by the largest value present to make sure that you still get the right response.
As for the wheel speed issue, it can definitely help to use closed loop control for the velocity of the wheels, but it can be fairly difficult to implement well, and it will take a long time to tune it well. I would recommend just sticking with the simpler method unless your robot really isn’t controllable. If it is, make sure there’s no excess friction in any gearboxes/drive components/rollers on the wheels.
How might one go about doing this? Unfortunately my programming skills are lacking at best but I get the feeling it is fairly simple to do.
double max2(double a,b){ return (a>b)?a:b; }
double max5(double a,double b,double c,double d,double e){
return max2(max2(max2(a,b),max2(c,d)),e);
}
...
divisor=max5(leftFront,rightFront,leftRear,rightRear,1);
leftFront/=divisor;
rightFront/=divisor;
...
*You need to find the max absolute value, since speeds can be +/-.
Let fr, fl, rl, and rr be the 4 wheel speed commands in the range +/- 1. Then here’s the pseudo-code:
max=fabs(fr);
if (max<fabs(fl)) max=fabs(fl);
if (max<fabs(rl)) max=fabs(rl);
if (max<fabs(rr)) max=fabs(rr);
if (max>1){fr/=max; fl/=max; rl/=max; rr/=max;}
Indeed - I implemented exactly what was asked for rather than what was needed.
Now you’ve taken all the fun out of it! 
Thank you!! Since we just finished the chassis, we’re trying to get it working using the library code… and there have been some complications. We can rotate flawlessly, and we can drive in a more-or-less straight line (probably due to our ancient 2 axis joystick with no deadzone). The problem we’re having is that we can’t strafe (and the x axis, y axis, and rotation are all mixed up). When we try to strafe, the robot simply spins the wheels towards or away from the central point of the robot (i.e the front two wheels spin inwards and the back two wheels spin inwards xor vice versa) and it stays in place or runs in circles. The rollers are arranged in an “X” pattern when viewed from the top. Here’s the code we’re using, in case it’s related to that…
#include <WPILib.h>
class DefaultRobot : public SimpleRobot
{
Joystick *Saitek;
Talon *frontLeft;
Talon *frontRight;
Talon *backLeft;
Talon *backRight;
RobotDrive *myRobot;
public:
DefaultRobot(void)
{ //these must be called in the same order as initialized
Saitek = new Joystick(1);
frontLeft = new Talon(2,1);
backLeft = new Talon(2,2);
frontRight = new Talon(2,3);
backRight = new Talon(2,4);
myRobot = new RobotDrive(frontLeft, backLeft,
frontRight, backRight);
}
void Autonomous(void)
{
while(IsAutonomous())
{
}
}
void OperatorControl(void)
{
float tval, sval;
while (IsOperatorControl())
{
float xVal = Saitek->GetRawAxis(1);
float yVal = Saitek->GetRawAxis(2);
bool trigButton = Saitek->GetRawButton(1);
bool leftButton = Saitek->GetRawButton(2);
bool midlButton = Saitek->GetRawButton(3);
bool rightButton = Saitek->GetRawButton(4);
myRobot->MecanumDrive_Cartesian(sval*0.5, tval*0.25, -yVal*0.25, 0.0);
if (leftButton && !rightButton){ tval = -1; }
else if (rightButton && !leftButton) { tval = 1; }
else if (leftButton && rightButton) { tval = 0; }
else tval = 0;
if (midlButton && !trigButton) { sval = -1; }
else if (trigButton && !midlButton) { sval = 1; }
else if (trigButton && midlButton) { sval = 0; }
else sval = 0;
//backRight->Set(0.25);
//backLeft->Set(0.25);
//frontRight->Set(0.25);
//frontLeft->Set(0.25);
}
}
};
START_ROBOT_CLASS(DefaultRobot);
Thanks!
[edit] Also, if we tell an individual motor to do something (i.e just sending a value to a single motor controller), the other motors move and twitch… I have no idea how to fix this
I had a similar problem this past year while programming our mecanum drive. I fixed it by putting the axis’ in this order: y axis, rotation, x axis.
The code looks like this:
stick_LeftY = stick->GetLeftY();
stick_LeftX = stick->GetLeftX();
stick_RightX = stick->GetRightX();
myRobot->MecanumDrive_Cartesian(stick_LeftY, stick_RightX, stick_LeftX);
I was using a logitech gamepad but I believe the principles would be the same for sending commands. I hope this helps! If you have any questions you can PM me. I’ve been working with mecanum for several years now and may be able to help
Thanks 
How on earth are we supposed to get which motor goes where and which need to be inverted and which need to be switched and…? Is there some standard way of doing this? Also… the twitching and creeping while no commands are being sent really weirds me out…
You just need to know which way the wheels need to be spinning. I’ll try to explain. Here goes.
Forward- all wheels forward
Backward- all wheels backward
Rotate right- 2 left wheels forward, 2 right wheels backward
Rotate left- 2 left wheels backward, 2 right wheels forward
Strafe right- front left and back right wheels forward, front right and back left wheels backward
Strafe left - front left and back right wheels backward, front right and back left wheels forward
This can be due simply to the joysticks being old and not having a proper deadzone. One can easily be programmed in though
Thanks! We ended up telling identifying each motor as “frontleft” etc, and then calling them one by one and replacing the PWM cables to get the right wheel to correspond with the right motor controller name (regardless of the direction). Then we put the motor values into MacanumDrive_Cartesian and set it to drive in a straight line (while on blocks) and reversed all the motors that needed to be reversed. We don’t know how to use “SetInvertedMotor()”… Now we only have one problem… Talon SRs spin signifigantly (and consistently) faster in reverse than while going forwards (so we can’t simply multiply individual motor values to regulate them). Thanks, by the way! We printed out your response and cable-tied it to the robot (just the directional stuff). edit also, it creeps without the joysticks plugged in. It only creeps and jitters when on blocks or low friction surfaces (although the motor controller lights do change colours while on high-friction surfaces, but don’t give the motors enough power to move). edit2 I just realized that the speed controllers are not initialized in the right order. Fixing that shouldn’t mess anything up, right?
Glad I could help! The fact that the wheels twitch when there’s no joysticks plugged in is rather bizarre. Unfortunately I’m not really sure what to tell you about that.
If you change the order that the motors are initialized this would change the order they would need to be put into the Mecanum_Cartesian function. This would change things but can be easily fixed on the robot by moving the PWM cables around
Creeping and twitching. Sounds like me after too much sugar.
Have you changed the calibration on your talons at all? I have no experience with talons at all, but i have plenty with victors. Someone was calibrating a drivetrain to stop creeping, but did it wrong. The victor now only knew full front and full reverse. But your problem is not that extreme.
Also, make sure your joysticks are centered when they are first plugged in, and the driver station loads. Otherwise whatever position they were in when they were plugged in will be the new origin.
You could also program in a deadzone function to set values between -.01 and .01 to = 0. for both axes, and a function to set a new origin (rezero).
The twitching is most likely that the Talons are not calibrated. Calibration basically tells the Talons what input values correspond to full forward, full reverse, and neutral.
To calibrate the Talons (from the Talon user manual):
- Press and hold the button labeled “CAL” with a paper clip. The
LED should begin to blink red/green.- Continue to keep the button pressed while moving the joystick
full forward and full reverse. You may do this as many times as
you like.- Center the joystick and then release the CAL button.
- If calibration was successful, the LED will blink green several
times. If the LED blinks red several times, the calibration was
not valid. If this happens, the Talon will use the last valid
calibration values.
All calibration values are retained after power cycle or reset.
We just tried calibrating the motor controllers. It fixed the twitching issue, but the motors still spin too fast in reverse. The input voltage for all three motor controllers is the same, but the output for the forwards ones is consistently more than the reversed ones (by around 50%). Would fixing the arrangement of the declarations in the code possibly fix this? Probably not… I’m running out of ideas
If your robot drives OK, you may not have to do anything. You’ll need to check with something close to the expected weight and weight distribution. You probably want to hook things up so that going forward has all four motors running faster than going in reverse – this may mean you need to reverse the two motor wires that attach to the output of two of the speed controllers. This way, forward and reverse should work well. If rotation and going to one side or the other don’t work as you like, you can reduce the power in the forward direction so that it matches that in reverse, via software. Of course, you’ll only want to do this when you need to, not for forward and reverse. Hopefully, this makes sense…
I’m going to guess that you meant to do all the other things recommended here in this thread (i.e. write my own Mecanum code and place it into it’s own method below the main method) and then modify the motor controller speeds as needed. I’m not on our development laptop right now, so I threw together the following code:
#include <WPILib.h>
class DefaultRobot : public SimpleRobot
{
Joystick *xBox;
Talon *frontLeft;
Talon *frontRight;
Talon *backLeft;
Talon *backRight;
public:
DefaultRobot(void)
{ //these must be called in the same order as initialized
xBox = new Joystick(1);
frontLeft = new Talon(2,2);
backLeft = new Talon(2,1);
frontRight = new Talon(2,3);
backRight = new Talon(2,4);
}
void Autonomous(void)
{
while(IsAutonomous())
{
}
}
void OperatorControl(void)
{
float lXaxis, lYaxis, triggers, rXaxis, rYaxis;
while (IsOperatorControl())
{
//initialize buttons and joystick axes
float lXaxis = xBox -> GetRawAxis(1);
float lYaxis = xBox -> GetRawAxis(2);
float triggers = xBox -> GetRawAxis(3);
float rXaxis = xBox -> GetRawAxis(4);
float rYaxis = xBox -> GetRawAxis(5);
//call MecanumDrive method
MecanumDrive(deadZ(lXaxis), deadZ(lYaxis), deadZ(rXaxis));
//order: strafe, forward, rotate
}
}
void MecanumDrive (float xVal, float yVal, float tVal){
float fL, bL, fR, bR, max;
//step 1: generate speeds for each motor
fL = xVal + yVal + tVal;
fR = -xVal + yVal - tVal;
bL = -xVal + yVal + tVal;
bR = xVal + yVal - tVal;
//step 2: divide by greatest value if greater than 1
max=fabs(fr);
if (max < fabs(fL)) max = fabs(fL);
if (max < fabs(bL)) max = fabs(bL);
if (max < fabs(bR)) max = fabs(bR);
if (max > 1){ fR/=max; fL/=max; bL/=max; bR/=max; }
//final step
frontLeft -> Set(fL * fix(fL));
backLeft -> Set(bL * fix(bL));
frontRight-> Set(fR * fix(fR));
backRight -> Set(bR * fix(bR));
}
//deadzone method
float deadZ (float val) {
return val > 0.06 || val < -0.6? val: 0;
}
//method to modify motor values based on direction
float fix (float fixme) {
return fixme > 0? 1 : fixme < 0? 0.95 : 0;
//change '0.95' to the best possible alternative
}
};
I apparently have a few more days before the big event at which I intend to show off the new robot (new member meeting this Wednesday), so I’ll have a little extra time for trial and error I’m a decent programmer, but I haven’t done much over the summer and don’t currently have access to an environment that can do debugging (just good old Notepad++ running off of my flashdrive 
), so I expect some trivial mistakes. As soon as I can get this to work, I’ll go back to using IterativeRobot instead of SimpleRobot (SimpleRobot is the only one I could remember the structure of without looking at already-made code). Logically, does this make sense? I’m thinking about changing ‘float fix(float fixme){}’ to something like ‘float fix(String motor, float fixme){}’ so that I can modify each and every motor value more personally (‘String motor’ would be an identifying string associated with a particular motor).
*[strike]max=fabs(fr);[/strike] max=fabs(fR);
[strike]return fixme > 0? 1 : fixme < 0? 0.95 : 0;[/strike] return fixme>0? 1:0.95
You have the right idea. I imagine a little trial and error will get you there!
It might be good to only apply the ‘fix’ logic if not all wheels are going in the same direction – this way, you only cut the power if it is needed.
Good luck, and if you have further trouble, just post…
Hey! I just wanted to let you all know that the robot worked flawlessly 
I’ll put up some pictures of the robot we built in four days when we can get a hold of a decent resolution camera. Thank you all so much! This is hands down the most helpful and informative thread I’ve ever seen. Much appreciated, CD.