Trouble With Robot Drive Code

Our robot does run with the code that we have down below, in one direction the wheels are turning against each other. We are not using mecanum drive, we are using arcade drive on our robot. We are also having trouble understanding how the driving works.

package frc.robot.utilities;

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

import frc.robot.Robot;

import com.ctre.phoenix.motorcontrol.ControlMode;

public final class Drive
{
// TODO - Add mecanum drive, holonomic drive

TalonSRX talon1, talon2, talon3, talon4, talon5, talon6;
DriveMode driveMode;

public static enum DriveMode
{
    TWO, FOUR, SIX;

}

//This constructor is for 4 motors without follower

public Drive(TalonSRX leftFront, TalonSRX leftBack, TalonSRX rightFront, TalonSRX rightBack)
{
    driveMode = DriveMode.FOUR;
    talon1 = rightFront;
    talon2 = rightBack;
    talon3 = leftFront;
    talon4 = leftBack;
}

public void driveArcade(double moveValue, double rotateValue)
{
    double leftMotorSpeed;
    double rightMotorSpeed;

    //grabs axis location to use as percentage
    //double axis = Robot.oi.getYSpeedController_Drive();

    moveValue = limitValue(moveValue);
    rotateValue = limitValue(rotateValue);
    leftMotorSpeed = 0;
    rightMotorSpeed = 0;
    
    //System.out.println("Move Value: "+moveValue + " | " + "Rotate Value: "+ rotateValue );


    double maxInput = Math.copySign(Math.max(Math.abs(moveValue), Math.abs(rotateValue)), moveValue);

/*
// if(moveValue > 0.0 && rotateValue > 0.0)
if(moveValue > 0.1 && rotateValue > 0.1)
{
//DRIVE FORWARD
//System.out.println(“Moving Forward::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::”);
leftMotorSpeed = moveValue + rotateValue;
rightMotorSpeed = moveValue + rotateValue;
}

// else if(moveValue < 0.0) {
else if(moveValue < -0.1) {
//DRIVE BACKWARDS
//System.out.println(“Moving Backward:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::”);

        leftMotorSpeed = moveValue - rotateValue;
        rightMotorSpeed = moveValue - rotateValue;

    } 
    
    else {
        //Do nothing
        leftMotorSpeed = 0;
        rightMotorSpeed = 0;
    }

*/
//This section here says that if both the move and rotate value are positive turn the robot
double joystickDeadzone;
joystickDeadzone = 0.1;

    if (moveValue >= joystickDeadzone)
    {
        //forwards
        if (rotateValue >= joystickDeadzone)
        {
            leftMotorSpeed =   -0.5 * moveValue;            //moveValue - rotateValue;          //maxInput;
            rightMotorSpeed =  -0.5 * moveValue;                            //moveValue - rotateValue;          //-Math.max(moveValue, rotateValue);//originally not inversed
            //System.out.println("Case 1: rms=" + rightMotorSpeed + "|| lms=" + rightMotorSpeed);
            //System.out.println("Moving right:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::");;
        }
        //backwards
        else if (rotateValue <= -joystickDeadzone) 
        {
            leftMotorSpeed = 0.5 * moveValue;           //moveValue + rotateValue; //-Math.max(moveValue, rotateValue);
            rightMotorSpeed = -0.5 * moveValue;         //maxInput; //moveValue - rotateValue;
            //System.out.println("Case 2: rms=" + rightMotorSpeed + "|| lms=" + rightMotorSpeed);
            //System.out.println("Moving left:::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::");

        }
    }
    //
    else if (moveValue <= -joystickDeadzone)
    {
        // SHOULD turn right<--- it doesn't (Quadrant 3)
        if (rotateValue >= joystickDeadzone)
        {
            leftMotorSpeed = moveValue + rotateValue;// maxInput;    //-Math.max(-moveValue, rotateValue);
            rightMotorSpeed = -maxInput; //moveValue - rotateValue; //-maxInput;
            //System.out.println("Case 3: rms=" + rightMotorSpeed + "|| lms=" + rightMotorSpeed);
            //System.out.println("Moving ????? :::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::");

        }
        //SHOULD turn left <-- it doesn't (Quadrant 4)
        else if(rotateValue <= -joystickDeadzone)
        {
            leftMotorSpeed = moveValue - rotateValue; //moveValue - rotateValue; 
            rightMotorSpeed = moveValue - rotateValue; //maxInput;  //-Math.max(-moveValue, -rotateValue);
            //System.out.println("Case 4: rms=" + rightMotorSpeed + "|| lms=" + rightMotorSpeed);
            //System.out.println("Moving ?2?@?@?@?@?@::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::");

        }
    }
    moveValue = limitValue(moveValue);
    rotateValue = limitValue(rotateValue);

    
    setMotorOutputs(rightMotorSpeed,leftMotorSpeed);

}

//This method will write the motor percent output to both the right and the left
//this sets how much percentage of battery the motor is using
public void setMotorOutputs(double left, double right)
{

    if (driveMode == DriveMode.FOUR)
    {
        talon1.set(ControlMode.PercentOutput, right);
        talon2.set(ControlMode.PercentOutput, right);
        talon3.set(ControlMode.PercentOutput, left);
        talon4.set(ControlMode.PercentOutput, left);
    }
    
}

//This method is to limit the stick imputs to make sure that they dont go above or below 1 or -1
private double limitValue(double value)
{
if (value > 1.0)
value = 1.0;
if (value < -1.0)
value = -1.0;
return value;
}
}

Trying to wrap my brain around what you’re saying here. My best understanding is that you have four mechanically independent gearboxes, and that when you drive forward (for example), three wheels are rotating forward and one wheel is rotating backwards? If so, which one?

Are you planning to make this a mecanum drive train at some point? If not, dropping back to a simpler example code set (differential drive) would make sense.

I can’t immediately see the bug, but I can suggest simplifying your code down so that the bug is a little easier to find.

I recommend you do your drive calculations in the following order:

  • Do deadzone calculations
  • Calculate left/right outputs
  • Normalize values if necessary (optional)
  • Send to drive

something like

public void driveArcade(double moveValue, double rotateValue) {
        // enforce deadzones up front
        moveValue = enforceDeadzone(moveValue);
        rotateValue = enforceDeadzone(rotateValue);

        // calculate left/right powers
        double left = moveValue - rotateValue;
        double right = moveValue + rotateValue;

        // (Optional) normalize values if they are too large.
        double maxValue = Math.maxValue(Math.abs(left), Math.abs(right));
        if (maxValue > 1) {
            left /= maxValue;
            right /= maxValue;
        }

        // use your drive command
        setMotorOutputs(left, right);
    }

    private double enforceDeadzone(double value) {
        if (Math.abs(value) < 0.1) {
            return 0;
        }
        return value;
    }