[MichaelB]

My team's mecanum drive is not working properly. It used to do everything beautifully, but then it stopped. We've been having a lot of problems with the belts we're using loosening, but I tightened them today and tried to drive immediately afterwards with bad results. It goes forwards and backwards wonderfully, but strafing has a curve to it and rotating in place just kind of jitters without going anywhere, though if the robot is on blocks the wheels are moving. We did add a lot of weight recently in the form of an arm, but it was having these problems before that. I think that our wheels are in the correct orientation, but I've included a worm's eye view picture just in case.

I've also heard that encoders are very helpful when dealing with a mecanum drivetrain. I got my electrical team to put some on, only to find that nowhere is particularly helpful in showing how to use encoders to get a well-functioning drive system. We're using PWM, not CAN, and the encoders are the US Digital ones that come in the kit. They are mounted on the gearboxes of the motors.

I'm including the two relevant classes, DriveTrain and DriveWithJoystick, below.

public class DriveTrain extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    RobotDrive driveTrain;
    Encoder leftFront;
    Encoder leftBack;
    Encoder rightFront;
    Encoder rightBack;
    
    public DriveTrain() {
        driveTrain = new RobotDrive(RobotMap.fLeftFrontMotorPort, RobotMap.rLeftBackMotorPort, RobotMap.fRightFrontMotorPort, RobotMap.rRightBackMotorPort);
        driveTrain.setSafetyEnabled(false);
        leftFront = new Encoder(RobotMap.frontLeftEncoderPort1, RobotMap.frontLeftEncoderPort2, true, Encoder.EncodingType.k4X);
        leftBack = new Encoder(RobotMap.backLeftEncoderPort1, RobotMap.backLeftEncoderPort2, true, Encoder.EncodingType.k4X);
        rightFront = new Encoder(RobotMap.frontRightEncoderPort1, RobotMap.frontRightEncoderPort2, true, Encoder.EncodingType.k4X);
        rightBack = new Encoder(RobotMap.backRightEncoderPort1, RobotMap.backRightEncoderPort2, true, Encoder.EncodingType.k4X);
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new DriveWithJoystick());
        //setDefaultCommand(new DriveWithJoystickPolar());    Doesn't work, don't know why
    }
    
    public void driveWithJoystick(double x, double y, double twist) {
        driveTrain.mecanumDrive_Cartesian(-x,twist,-y, 0);
        System.out.println("X: " + x + "Y: " + y + "Twist: " + twist);
    }
    
    public void driveWithJoystickPolar(double magnitude, double direction, double twist) {
        driveTrain.mecanumDrive_Polar(magnitude, direction, twist);
        System.out.println("Mag: " + magnitude + " Dir: " + direction + " Twist: " + twist);
    }
    
    public void drive(double x, double twist){
        driveTrain.arcadeDrive(x,twist);
    }
}

public class DriveWithJoystick extends CommandBase {
    
    public DriveWithJoystick() {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(driveTrain);
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        double x = oi.getRightStick().getX();
        double y = oi.getRightStick().getY();
        double twist =oi.getLeftStick().getX();
        x = (x - Robot3573.initialX) / (1 - Robot3573.initialX);  //reset rest position to 0
        y = (y - Robot3573.initialY) / (1 - Robot3573.initialY);
        twist = (twist - Robot3573.initialTwist) / (1 - Robot3573.initialTwist);
        x = (x > 0) ? x*x :-(x*x);  //squaring for increased precision
        y = (y > 0) ? y*y :-(y*y);
        twist = (twist > 0) ? twist*twist :-(twist*twist);
        
        x = x < RobotMap.joystickTolerance && x > -RobotMap.joystickTolerance ? 0 : x;  //dead zone of 0.1 around 0
        y = y < RobotMap.joystickTolerance && y > -RobotMap.joystickTolerance ? 0 : y;
        twist = twist < RobotMap.joystickTolerance && twist > -RobotMap.joystickTolerance ? 0 : twist;
        driveTrain.driveWithJoystick(x, y, twist);
        lcd.println(DriverStationLCD.Line.kUser1, 1, "X: " + ((int)(x*10))/10.0);
        lcd.println(DriverStationLCD.Line.kUser2, 1, "Y: " + ((int)(y*10))/10.0);
        lcd.println(DriverStationLCD.Line.kUser3, 1, "Twist: " + ((int)(twist*10))/10.0);
        lcd.updateLCD();
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}