WPILIB mecanum code not working properly

Technical Programming
1

I have been working on getting our mecanum drive working for the past few days and have run into trouble while trying to program using the WPILIB mecanum class. When we try to drive, the forward and strafe commands work fine, but the turn command doesnt. instead of running the left side one direction and the right side another, it spins the wheels on the right towards each other, and the same on the left. If I invert 2 wheels of any pairing, the axis that doesnt work correctly changes. Below is the code that I am trying to use.

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    
    Joystick MainStick;
    Joystick SecStick;
    Gyro gyro;
    RobotDrive Drive;
    
    //Drive_Base Drive;
    public void robotInit() {
        MainStick = new Joystick(1);
        SecStick = new Joystick(2);
        gyro = new Gyro(1);
        Drive = new RobotDrive(10,1,2,3);
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        double theta = gyro.getAngle();

        double strafe = 1 * MainStick.getRawAxis(1);
        double forward = -1 * MainStick.getRawAxis(2);
        double rotation = 1* MainStick.getRawAxis(3);
        
        Drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, false);
        Drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, false);
        Drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false);
        Drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, false);
        
        Drive.mecanumDrive_Cartesian(strafe, forward, rotation, theta);

        SmartDashboard.putNumber("Gyro Angle", theta);
        
        if(MainStick.getRawButton(8)){
            gyro.reset();
        }
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        gyro.reset();
    }
    
}

However when I change the equations that calculate the wheel outputs from the ones in the WPILIB:

wheelSpeeds[MotorType.kFrontLeft_val] = xIn + yIn + rotation;
wheelSpeeds[MotorType.kFrontRight_val] = -xIn + yIn - rotation;
wheelSpeeds[MotorType.kRearLeft_val] = -xIn + yIn + rotation;
wheelSpeeds[MotorType.kRearRight_val] = xIn + yIn - rotation;

to the following:

double mFL = strafe + forward + rotation;
double mFR = strafe + forward - rotation;
double mRL = strafe - forward + rotation;
double mRR = strafe - forward - rotation;

It drives normally. My issue with changing the equations is that 1) I am using netbeans (old hardware on our practice bot) and cant change the libraries because they are all pre compiled and 2) We are trying to use field centric control and am not getting the results I’d like by trying to write the code using internet resources such as whitepapers by Ether and such.

My thought is that if I can get the WPILIB mecanum working, I can see how their field centric control operates in comparison.

I am really stumped at this point, any help is greatly appreciated.

2

Your code and your wiring probably disagree on which motors are in which corner of the robot. Try making some code that lets you control each motor independently so you can verify which physical motor responds to each PWM channel, and in which direction. Note carefully the order that the RobotDrive constructor wants them to be in.

3

I double checked the port numbers again, all the ports corresponded with the proper wheels. I’ve taken notes as to the direction each wheel spins individually however at the moment the problem is still there.

4

Leave the code as is and change the PWM wires. Flip your back left with front right. Or back right with front left

5

If I change the pwm cables (front left and rear right or front right and rear left) it just changes the axis that is not operating properly (the wheels spin towards each other). just like when I invert the motor pairs in the code.

6

Do you have the wheels mechanically mounted correctly? When looking from the top, the rollers should look like an X across the robot.

Also, unless gearing is different for each side, one side of the drive should be reversed, and one side should be normal. This should allow rotation correctly.

One way I like to check things is look at the lights on the motor controllers.

When going straight, 2 should be red, and 2 should be green.
When rotating, all 4 should be either red or green, depending on which direction you are rotating.
When strafing, 2 of the corners should be red, and the other 2 corners should be green.

1 More thing to check. When driving at a 45 degree angle, only 2 of the motors should be spinning, and they should be the 2 perpendicular to the direction you want to move. If thats not working correctly, then its most likely that either motor reversing isnt right, or the PWM ports are wrong.

7

What I would recommend doing is printing out your X and Y input and then your wheel outputs. Confirm X and Y are in-fact what you intend and then check that the correct wheels are moving. If that doesn’t work, you could always subclass RobotDrive and then override the following methods:
public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle);
public void mecanumDrive_Polar(double magnitude, double direction, double rotation);

If none of this works, could you make a codebase with only drive code and posting it here?

Good luck!

8

Can you describe exactly what each wheel does when given each command?

9

At the moment I’ve been able to get field centric control working using my own code but am still trying to work out what is wrong with my code that utilizes the robot drive class. Unfortunately I’ve had to go home for the night and won’t be able to test any more code until at least tomorrow. Might just leave the wpilib for now and continue with my own mecanum code.

10

If it works then go for it. I’m glad its working now

11

You say this works:

double mFL = strafe + forward + rotation;
double mFR = strafe + forward - rotation;
double mRL = strafe - forward + rotation;
double mRR = strafe - forward - rotation;

You also show that none of the motors are inverted in the initialization.

That’s weird. Your rear motors are reversed, and strafing definitely shouldn’t run all the motors in the same direction. I suggest that you triple-check your wiring. And have someone else check it again afterwards. In fact, take some good photos of how you have it wired up and post them here so we can all go over them.

Something is definitely wrong, and I do not believe it is the WPI library.

12

Believe me, I too am almost certain that its nothing wrong with the WPI. I will try and get some photos tomorrow however the wiring is pretty messy as it sits. hopefully we can clean it up some time soon.

13

Is there a reason your robot drive is 10,1,2,3? did you mean 0? And make sure the order you set the drive motors is frontLeft, rearLeft, frontRight, rearRight.

Check for inverted motors*** . If the transmission causes the motor to spin the same way as the wheel, then the left side should be inverted

14

^^^This.

15

We had a similar issue a few weeks ago. My student had swapped the x and y joystick inputs into the mechanum code call. It is not asking for field x and y but joystick x and y. Also the mechanum internally inverts the y so you don’t have to.

16

…unless you are using mecanum_Cartesian with a gyro signal connected to the gyro input. Then it is expecting field-centric inputs.

17

We were using all of those. The input parameter called x expects the x position of the joystick. The y parameter expects the y joystick position with no inversion. Our robot immediately started driving properly with field centric controls once this was straightened out.

18

Maybe I missed something here, but may I ask why you changed the signs in the equations?

19

Looking at the code for RobotDrive, I can see that the gyroAngle is used to rotate the values of xIn and yIn. This is what creates the field-centric driving.

Your code that works appears to put the values of “strafe”, “forwards”, and “rotation” directly into the motor controllers.

Are you discarding the rotation from the gyroAngle to get driving to work normally? In other words, could you get good normal driving behavior by keeping the gyro angle at a constant zero?

20

Unfortunately I couldn’t get my hands on the bot today as I was busy working on mechanical and also it was being used for our driver try out. Hopefully Monday I will be able to map out all off the wheel rotations. I also had two of our mentors go over and double check the wiring and neither could find anything wrong.

No I did mean to say 10. The way our electronics are layed out, it was easier to run a short pwm cable to port 10 on the side car rather than 4. Also note that this is our practice bot and it is running the Crio/associated hardware, not the new RoboRIO.

I changed the signs in the equation because earlier on while troubleshooting we realized that, based on the movements of the wheels we were seeing at the time, there would be a chance that if we changed some of the signs around, we might see the movements we were expecting. Sure enough the robot started driving normally when we changed up the signs.

Originally I didn’t even incorporate any gyro code as I just wanted to be sure we had basic functions before going any more complex. After I got the drive functioning properly (using the new equations as seen in my first post), I started working on gyro code. I now have standard mecanum drive working along with proper field centric control (again all using the new equations).

My next step is to try and get a PID loop running that keeps our heading to the desired angle as we are currently experiencing drift in our rotation. I have also been trying to get a velocity PID loop running on our wheels using encoders so that we can ensure that we are getting the desired RPM’s out of our wheels.