Mecanum Drive Motors Programmed Incorrectly?

[star]

Hi there! Team 4663 here.

We’re trying to switch off from LabVIEW to Java for our main programming language, but we’re having some troubles. I used the Mecanum drive example code and edited it to work with our motor controllers, but the motors aren’t moving correctly. Can anyone help us out?

Pure Forward
R F

F R
Reverse
R F

R F
Strafe Right
F R

F R
Strafe Left
F F

F F
Rotate Right
F R

F R
Rotate Left
R F

R F

Here’s the Robot.java code:
// Copyright (c) FIRST and other WPILib contributors.

// Open Source Software; you can modify and/or share it under the terms of

// the WPILib BSD license file in the root directory of this project.

package frc.robot;

import com.ctre.phoenix.motorcontrol.can.*;

import edu.wpi.first.wpilibj.Joystick;

import edu.wpi.first.wpilibj.TimedRobot;

import edu.wpi.first.wpilibj.drive.MecanumDrive;

/** This is a demo program showing how to use Mecanum control with the MecanumDrive class. */

public class Robot extends TimedRobot {

// private static final int kFrontLeftChannel = 2;

// private static final int kRearLeftChannel = 3;

// private static final int kFrontRightChannel = 1;

// private static final int kRearRightChannel = 0;

WPI_TalonSRX _talonFL = new WPI_TalonSRX(2);

WPI_TalonSRX _talonRL = new WPI_TalonSRX(3);

WPI_TalonSRX _talonFR = new WPI_TalonSRX(1);

WPI_TalonSRX _talonRR = new WPI_TalonSRX(4);

private static final int kLJoystickChannel = 0;

private static final int kRJoystickChannel = 1;

private MecanumDrive m_robotDrive;

private Joystick l_stick;

private Joystick r_stick;

@Override

public void robotInit() {

// TalonSRX frontLeft = new TalonSRX(_talonFL);

// TalonSRX rearLeft = new TalonSRX(_talonRL);

// TalonSRX frontRight = new TalonSRX(kFrontRightChannel);

// TalonSRX rearRight = new TalonSRX(kRearRightChannel);

_talonFL.configFactoryDefault();

_talonRL.configFactoryDefault();

_talonFR.configFactoryDefault();

_talonRR.configFactoryDefault();

// Invert the right side motors.

// You may need to change or remove this to match your robot.

_talonFL.setInverted(false);

_talonRL.setInverted(false);

_talonFR.setInverted(true);

_talonRR.setInverted(true);

 

m_robotDrive = new MecanumDrive(_talonFL, _talonRL, _talonFR, _talonRR);

//m_robotDrive.setRightSideInverted(false);

r_stick = new Joystick(kRJoystickChannel);

l_stick = new Joystick(kLJoystickChannel);

}

@Override

public void teleopPeriodic() {

// Use the joystick X axis for lateral movement, Y axis for forward

// movement, and Z axis for rotation.

m_robotDrive.driveCartesian(l_stick.getX(), l_stick.getY(), l_stick.getZ(), 0.0);

}

}

Thank you!

[Fletch1373]

One thing that jumps out at me is that setInverted(false) is the default behavior, so setting it here is unnecessary. Also, WPILib no longer auto-inverts the right side motors, so you’ll need to add setInverted(true) for two of them (which side that would need to be will depend on your mechanism design, but most likely right side).

You should test this end-to-end.

Try outputting the values of your joystick inputs to confirm they’re reading as you’d expect (I assume you want X+ to the right, Y+ forward, Z+ clockwise). If not, adjust the inputs to driveCartesian() as needed.

Once those are right, ensure your mecanum wheels are mounted correctly (X vs O pattern) and fix as necessary.

Then drive forward and reverse and compare outputs. When viewed from the center of the robot, left wheels should spin clockwise and right wheels should spin CCW when driving forward (and reversed when driving reverse).

Then test strafing left and right and compare outputs. Wheels should spin inward on the “near” side and outward on the far side. (“near” being the side being strafed toward when viewed from a position on that side)

Then you can test rotation just to confirm it’s correct (though it most likely will be)

[Joe_Ross]

@star

m_robotDrive.driveCartesian(l_stick.getX(), l_stick.getY(), l_stick.getZ(), 0.0);

What happens if you change it to the following? m_robotDrive.driveCartesian(-l_stick.getY(), l_stick.getX(), l_stick.getZ(), 0.0);
`