68 lines
2.2 KiB
Java
68 lines
2.2 KiB
Java
package frc.robot.subsystems;
|
|
|
|
import java.util.function.DoubleSupplier;
|
|
|
|
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
|
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
|
import frc.robot.constants.DrivetrainConstants;
|
|
|
|
public class Drivetrain extends SubsystemBase {
|
|
private VictorSP frontLeft;
|
|
private VictorSP rearLeft;
|
|
private VictorSP frontRight;
|
|
private VictorSP rearRight;
|
|
|
|
private DifferentialDrive drive;
|
|
|
|
private boolean slowMode;
|
|
|
|
public Drivetrain(boolean slowModeAtStartup) {
|
|
frontLeft = new VictorSP(DrivetrainConstants.kFrontLeftPWM);
|
|
rearLeft = new VictorSP(DrivetrainConstants.kRearLeftPWM);
|
|
frontRight = new VictorSP(DrivetrainConstants.kFrontRightPWM);
|
|
rearRight = new VictorSP(DrivetrainConstants.kRearRightPWM);
|
|
|
|
frontLeft.addFollower(rearLeft);
|
|
frontRight.addFollower(rearRight);
|
|
frontRight.setInverted(true);
|
|
|
|
drive = new DifferentialDrive(frontLeft, frontRight);
|
|
|
|
slowMode = slowModeAtStartup;
|
|
}
|
|
|
|
public boolean isSlowMode() {
|
|
return slowMode;
|
|
}
|
|
|
|
public Command toggleSlowMode() {
|
|
return runOnce(() -> {
|
|
slowMode = !slowMode;
|
|
});
|
|
}
|
|
|
|
public Command tankDrive(DoubleSupplier leftSpeed, DoubleSupplier rightSpeed) {
|
|
return run(() -> {
|
|
drive.tankDrive(
|
|
(DrivetrainConstants.kInvertForwardBackward ? -1 : 1) * leftSpeed.getAsDouble() * (slowMode ? .75 : 1),
|
|
(DrivetrainConstants.kInvertForwardBackward ? -1 : 1) * rightSpeed.getAsDouble() * (slowMode ? .75 : 1)
|
|
);
|
|
});
|
|
}
|
|
|
|
public Command arcadeDrive(DoubleSupplier speed, DoubleSupplier rotation) {
|
|
return run(() -> {
|
|
drive.arcadeDrive(
|
|
(DrivetrainConstants.kInvertForwardBackward ? -1 : 1) * speed.getAsDouble() * (slowMode ? DrivetrainConstants.kSlowModeMultiplier : 1),
|
|
(DrivetrainConstants.kInvertLeftRight ? -1 : 1) * rotation.getAsDouble() * (slowMode ? DrivetrainConstants.kSlowModeMultiplier : 1)
|
|
);
|
|
});
|
|
}
|
|
|
|
public Command stop() {
|
|
return tankDrive(() -> 0, () -> 0);
|
|
}
|
|
}
|