WPILIB mecanum code not working properly

[cad321]

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.

[Alan Anderson]

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.

[cad321]

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.

[mschwab013]

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

[cad321]

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.

[Thad House]

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.

[baumgartensam]

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!

[mschwab013]

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

[cad321]

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.

[mschwab013]

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

[Alan Anderson]

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.

[cad321]

Originally Posted by Alan Anderson

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.

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.

[TimTheGreat]

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

[Ether]

Originally Posted by mschwab013

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

^^^This.

[engunneer]

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.