[MichaelB]

Originally Posted by Ether

Can you articulate in a bit more detail what part you do not understand?

I read a post from you (that I can't find anymore) about estimating a maximum speed for the wheels and then scaling that to 1. What I don't know is how to control each wheel individually like that. I tried to get the mecanum code from the DriveTrain class (changed a bit because we're not using a gyro or CAN), but now I'm not sure how to set the proper speeds. Do I just change in mecanumDrive_Cartesian "leftFront.set(...)" to "leftFrontPID.setSetpoint(...)" and et cetera?

public class DriveTrain extends Subsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
    RobotDrive driveTrain;
    Encoder leftFrontEncoder;
    Encoder leftBackEncoder;
    Encoder rightFrontEncoder;
    Encoder rightBackEncoder;
    
    PIDController leftFrontPID;
    PIDController leftBackPID;
    PIDController rightFrontPID;
    PIDController rightBackPID;
    
    private static final double Kp = 0.3;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;
    
    public static final int kMaxNumberOfMotors = 4;
    public static Jaguar leftFront;
    public static Jaguar leftBack;
    public static Jaguar rightFront;
    public static Jaguar rightBack;
    
    protected final int m_invertedMotors[] = {1,1,1,1};
    
    public DriveTrain() {
        //super("DriveTrain", Kp, Ki, Kd);
        
        driveTrain = new RobotDrive(RobotMap.fLeftFrontMotorPort, RobotMap.rLeftBackMotorPort, RobotMap.fRightFrontMotorPort, RobotMap.rRightBackMotorPort);
        driveTrain.setSafetyEnabled(false);
        leftFrontEncoder = new Encoder(RobotMap.frontLeftEncoderPort1, RobotMap.frontLeftEncoderPort2, false, Encoder.EncodingType.k2X);
        leftBackEncoder = new Encoder(RobotMap.backLeftEncoderPort1, RobotMap.backLeftEncoderPort2, true, Encoder.EncodingType.k2X);
        rightFrontEncoder = new Encoder(RobotMap.frontRightEncoderPort1, RobotMap.frontRightEncoderPort2, true, Encoder.EncodingType.k2X);
        rightBackEncoder = new Encoder(RobotMap.backRightEncoderPort1, RobotMap.backRightEncoderPort2, true, Encoder.EncodingType.k2X);
        leftFrontEncoder.setDistancePerPulse(2*3*3.14/360/12); //should be in inches/rev? might be 250 instead of 360
        leftBackEncoder.setDistancePerPulse(2*3*3.14/360/12);
        rightFrontEncoder.setDistancePerPulse(2*3*3.14/360/12);
        rightBackEncoder.setDistancePerPulse(2*3*3.14/360/12);
        
        leftFrontEncoder.setSamplesToAverage(100);
        leftBackEncoder.setSamplesToAverage(100);
        rightFrontEncoder.setSamplesToAverage(100);
        rightBackEncoder.setSamplesToAverage(100);
        leftFrontEncoder.start();
        leftFrontEncoder.reset();
        rightFrontEncoder.start();
        rightFrontEncoder.reset();
        leftBackEncoder.start();
        leftBackEncoder.reset();
        rightBackEncoder.start();
        rightBackEncoder.reset();
        
        leftFrontPID =  new PIDController(Kp, Ki, Kd, leftFrontEncoder, leftFront);
        rightFrontPID =  new PIDController(Kp, Ki, Kd, rightFrontEncoder, rightFront);
        leftBackPID =  new PIDController(Kp, Ki, Kd, leftBackEncoder, leftBack);
        rightBackPID =  new PIDController(Kp, Ki, Kd, rightBackEncoder, rightBack);
        
        leftFrontPID.enable();
        leftBackPID.enable();
        rightFrontPID.enable();
        rightBackPID.enable();
    }

    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        setDefaultCommand(new DriveWithJoystickArcade());
        //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);
        //System.out.println("Left Front: " + leftFront.get()/360 + " Left Back: " + leftBack.get()/360 + " Right Front: " + rightFront.get()/360 + " Right Back: " + rightBack.get()/360);
        System.out.println("Left Front: " + leftFrontEncoder.getRate() + " Left Back: " + leftBackEncoder.getRate() + " Right Front: " + rightFrontEncoder.getRate() + " Right Back: " + rightBackEncoder.getRate());
    }
    
    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 void mecanumDrive_Cartesian(double x, double y, double rotation)
    {
        double xIn = x;
        double yIn = y;
        // Negate y for the joystick.
        yIn = -yIn;

        double wheelSpeeds[] = new double[kMaxNumberOfMotors];
        wheelSpeeds[RobotMap.kFrontLeft_val] = xIn + yIn + rotation;
        wheelSpeeds[RobotMap.kFrontRight_val] = -xIn + yIn - rotation;
        wheelSpeeds[RobotMap.kRearLeft_val] = -xIn + yIn + rotation;
        wheelSpeeds[RobotMap.kRearRight_val] = xIn + yIn - rotation;

        normalize(wheelSpeeds);

        leftFront.set(wheelSpeeds[RobotMap.kFrontLeft_val] * m_invertedMotors[RobotMap.kFrontLeft_val] * RobotMap.maxOutput);
        rightFront.set(wheelSpeeds[RobotMap.kFrontRight_val] * m_invertedMotors[RobotMap.kFrontRight_val] * RobotMap.maxOutput);
        leftBack.set(wheelSpeeds[RobotMap.kRearLeft_val] * m_invertedMotors[RobotMap.kRearLeft_val] * RobotMap.maxOutput);
        rightBack.set(wheelSpeeds[RobotMap.kRearRight_val] * m_invertedMotors[RobotMap.kRearRight_val] * RobotMap.maxOutput);
    }
        
    protected static void normalize(double wheelSpeeds[]) {
        double maxMagnitude = Math.abs(wheelSpeeds[0]);
        int i;
        for (i=1; i<kMaxNumberOfMotors; i++) {
            double temp = Math.abs(wheelSpeeds[i]);
            if (maxMagnitude < temp) maxMagnitude = temp;
        }
        if (maxMagnitude > 1.0) {
            for (i=0; i<kMaxNumberOfMotors; i++) {
                wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
            }
        }
    }
}