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); } }