From 376763e3f3233649e50f7c884ef725d6e4657fea Mon Sep 17 00:00:00 2001 From: Alex Sirota Date: Mon, 9 Dec 2024 13:55:47 -0800 Subject: [PATCH 1/2] Refactor to use command factories instead of public methods --- src/main/java/frc/robot/RobotContainer.java | 18 +++------- .../frc/robot/subsystems/DriveSubsystem.java | 34 ++++++++++++++----- 2 files changed, 30 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7ad0ae3..c2ffca3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,11 +4,8 @@ package frc.robot; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.subsystems.DriveSubsystem; @@ -17,17 +14,12 @@ public class RobotContainer { private final DriveSubsystem driveSubsystem = new DriveSubsystem(); public RobotContainer() { - driveSubsystem.setDefaultCommand( - new RunCommand( - () -> driveSubsystem.driveArcade(driverXbox.getLeftY(),driverXbox.getLeftX()), - driveSubsystem - ) - ); - driverXbox.leftBumper().onTrue(new InstantCommand(() -> driveSubsystem.shiftGearHighSpeed())); - driverXbox.rightBumper().onTrue(new InstantCommand(() -> driveSubsystem.shiftGearLowSpeed())); - + driveSubsystem.setDefaultCommand(driveSubsystem.driveArcadeCommand(driverXbox::getLeftX, () -> {return driverXbox.getLeftY() * -1.0;})); + driverXbox.leftBumper().onTrue(driveSubsystem.shiftHighCommand()); + driverXbox.rightBumper().onTrue(driveSubsystem.shiftLowCommand()); + } - + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 1be17b8..84d41db 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -1,5 +1,7 @@ package frc.robot.subsystems; +import java.util.function.DoubleSupplier; + import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -8,6 +10,7 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PneumaticHub; import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; public class DriveSubsystem implements Subsystem{ @@ -36,7 +39,7 @@ public DriveSubsystem() { m_leftMotorLeader.setNeutralMode(NeutralMode.Brake); m_rightMotorFollower.setNeutralMode(NeutralMode.Brake); m_leftMotorFollower.setNeutralMode(NeutralMode.Brake); - + m_rightMotorLeader.setInverted(true); m_leftMotorFollower.setInverted(InvertType.FollowMaster); m_rightMotorFollower.setInverted(InvertType.FollowMaster); @@ -51,29 +54,42 @@ public DriveSubsystem() { m_leftMotorLeader.config_kP(0, 1.0); m_leftMotorLeader.config_kI(0, 0.000001); - shiftGearLowSpeed(); + this.shiftLowCommand().execute(); m_robotDrive = new DifferentialDrive(m_leftMotorLeader, m_rightMotorLeader); - } - public void shiftGearHighSpeed() { + } + + public Command shiftHighCommand() { + return runOnce(() -> { m_isGearedFast = true; m_solnoidLeft.set(DoubleSolenoid.Value.kReverse); m_solnoidRight.set(DoubleSolenoid.Value.kReverse); + }); } - public void shiftGearLowSpeed() { + + public Command shiftLowCommand() { + return runOnce(() -> { m_isGearedFast = false; m_solnoidLeft.set(DoubleSolenoid.Value.kForward); - m_solnoidRight.set(DoubleSolenoid.Value.kForward); + m_solnoidRight.set(DoubleSolenoid.Value.kForward); + }); } - public void driveArcade(double forward, double turn) { - m_robotDrive.arcadeDrive(forward, turn); + public Command driveArcadeCommand(DoubleSupplier forward, DoubleSupplier turn) { + return run(() -> m_robotDrive.arcadeDrive(forward.getAsDouble(), turn.getAsDouble())) + .withName("arcadeDrive"); } + public Command driveTankCommand(DoubleSupplier left, DoubleSupplier right) { + return run(() -> m_robotDrive.tankDrive(left.getAsDouble(), right.getAsDouble())) + .withName("tankDrive"); + } + + public void driveTank(double left, double right) { m_robotDrive.tankDrive(left, right); } - + @Override public void periodic() { } From 755e5fa96bace213b308d0032e1df688d9c82f60 Mon Sep 17 00:00:00 2001 From: Alex Sirota Date: Mon, 9 Dec 2024 13:56:24 -0800 Subject: [PATCH 2/2] fmt --- src/main/java/frc/robot/RobotContainer.java | 6 +- .../frc/robot/subsystems/DriveSubsystem.java | 159 +++++++++--------- 2 files changed, 81 insertions(+), 84 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c2ffca3..7a422c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -14,13 +14,13 @@ public class RobotContainer { private final DriveSubsystem driveSubsystem = new DriveSubsystem(); public RobotContainer() { - driveSubsystem.setDefaultCommand(driveSubsystem.driveArcadeCommand(driverXbox::getLeftX, () -> {return driverXbox.getLeftY() * -1.0;})); + driveSubsystem.setDefaultCommand(driveSubsystem.driveArcadeCommand(driverXbox::getLeftX, () -> { + return driverXbox.getLeftY() * -1.0; + })); driverXbox.leftBumper().onTrue(driveSubsystem.shiftHighCommand()); driverXbox.rightBumper().onTrue(driveSubsystem.shiftLowCommand()); - } - public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 84d41db..80bf104 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -13,85 +13,82 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; -public class DriveSubsystem implements Subsystem{ - private final WPI_TalonSRX m_leftMotorLeader = new WPI_TalonSRX(10); - private final WPI_TalonSRX m_leftMotorFollower = new WPI_TalonSRX(11); - private final WPI_TalonSRX m_rightMotorLeader = new WPI_TalonSRX(12); - private final WPI_TalonSRX m_rightMotorFollower = new WPI_TalonSRX(13); - - private final PneumaticHub m_pH = new PneumaticHub(1); - private final DoubleSolenoid m_solnoidLeft = m_pH.makeDoubleSolenoid(0,1); - private final DoubleSolenoid m_solnoidRight = m_pH.makeDoubleSolenoid(2,3); - private boolean m_isGearedFast = false; - - private DifferentialDrive m_robotDrive; - - public DriveSubsystem() { - m_leftMotorFollower.follow(m_leftMotorLeader); - m_rightMotorFollower.follow(m_rightMotorLeader); - - m_rightMotorLeader.configPeakCurrentLimit(25); - m_leftMotorLeader.configPeakCurrentLimit(25); - m_rightMotorFollower.configPeakCurrentLimit(25); - m_leftMotorFollower.configPeakCurrentLimit(25); - - m_rightMotorLeader.setNeutralMode(NeutralMode.Brake); - m_leftMotorLeader.setNeutralMode(NeutralMode.Brake); - m_rightMotorFollower.setNeutralMode(NeutralMode.Brake); - m_leftMotorFollower.setNeutralMode(NeutralMode.Brake); - - m_rightMotorLeader.setInverted(true); - m_leftMotorFollower.setInverted(InvertType.FollowMaster); - m_rightMotorFollower.setInverted(InvertType.FollowMaster); - - m_rightMotorLeader.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); - m_leftMotorLeader.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); - - - m_rightMotorLeader.config_kP(0, 1.0); - m_rightMotorLeader.config_kI(0, 0.000001); - - m_leftMotorLeader.config_kP(0, 1.0); - m_leftMotorLeader.config_kI(0, 0.000001); - - this.shiftLowCommand().execute(); - - m_robotDrive = new DifferentialDrive(m_leftMotorLeader, m_rightMotorLeader); - } - - public Command shiftHighCommand() { - return runOnce(() -> { - m_isGearedFast = true; - m_solnoidLeft.set(DoubleSolenoid.Value.kReverse); - m_solnoidRight.set(DoubleSolenoid.Value.kReverse); - }); - } - - public Command shiftLowCommand() { - return runOnce(() -> { - m_isGearedFast = false; - m_solnoidLeft.set(DoubleSolenoid.Value.kForward); - m_solnoidRight.set(DoubleSolenoid.Value.kForward); - }); - } - - public Command driveArcadeCommand(DoubleSupplier forward, DoubleSupplier turn) { - return run(() -> m_robotDrive.arcadeDrive(forward.getAsDouble(), turn.getAsDouble())) - .withName("arcadeDrive"); - } - - public Command driveTankCommand(DoubleSupplier left, DoubleSupplier right) { - return run(() -> m_robotDrive.tankDrive(left.getAsDouble(), right.getAsDouble())) - .withName("tankDrive"); - } - - - public void driveTank(double left, double right) { - m_robotDrive.tankDrive(left, right); - } - - @Override - public void periodic() { - } +public class DriveSubsystem implements Subsystem { + private final WPI_TalonSRX m_leftMotorLeader = new WPI_TalonSRX(10); + private final WPI_TalonSRX m_leftMotorFollower = new WPI_TalonSRX(11); + private final WPI_TalonSRX m_rightMotorLeader = new WPI_TalonSRX(12); + private final WPI_TalonSRX m_rightMotorFollower = new WPI_TalonSRX(13); + + private final PneumaticHub m_pH = new PneumaticHub(1); + private final DoubleSolenoid m_solnoidLeft = m_pH.makeDoubleSolenoid(0, 1); + private final DoubleSolenoid m_solnoidRight = m_pH.makeDoubleSolenoid(2, 3); + private boolean m_isGearedFast = false; + + private DifferentialDrive m_robotDrive; + + public DriveSubsystem() { + m_leftMotorFollower.follow(m_leftMotorLeader); + m_rightMotorFollower.follow(m_rightMotorLeader); + + m_rightMotorLeader.configPeakCurrentLimit(25); + m_leftMotorLeader.configPeakCurrentLimit(25); + m_rightMotorFollower.configPeakCurrentLimit(25); + m_leftMotorFollower.configPeakCurrentLimit(25); + + m_rightMotorLeader.setNeutralMode(NeutralMode.Brake); + m_leftMotorLeader.setNeutralMode(NeutralMode.Brake); + m_rightMotorFollower.setNeutralMode(NeutralMode.Brake); + m_leftMotorFollower.setNeutralMode(NeutralMode.Brake); + + m_rightMotorLeader.setInverted(true); + m_leftMotorFollower.setInverted(InvertType.FollowMaster); + m_rightMotorFollower.setInverted(InvertType.FollowMaster); + + m_rightMotorLeader.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); + m_leftMotorLeader.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); + + m_rightMotorLeader.config_kP(0, 1.0); + m_rightMotorLeader.config_kI(0, 0.000001); + + m_leftMotorLeader.config_kP(0, 1.0); + m_leftMotorLeader.config_kI(0, 0.000001); + + this.shiftLowCommand().execute(); + + m_robotDrive = new DifferentialDrive(m_leftMotorLeader, m_rightMotorLeader); + } + + public Command shiftHighCommand() { + return runOnce(() -> { + m_isGearedFast = true; + m_solnoidLeft.set(DoubleSolenoid.Value.kReverse); + m_solnoidRight.set(DoubleSolenoid.Value.kReverse); + }); + } + + public Command shiftLowCommand() { + return runOnce(() -> { + m_isGearedFast = false; + m_solnoidLeft.set(DoubleSolenoid.Value.kForward); + m_solnoidRight.set(DoubleSolenoid.Value.kForward); + }); + } + + public Command driveArcadeCommand(DoubleSupplier forward, DoubleSupplier turn) { + return run(() -> m_robotDrive.arcadeDrive(forward.getAsDouble(), turn.getAsDouble())) + .withName("arcadeDrive"); + } + + public Command driveTankCommand(DoubleSupplier left, DoubleSupplier right) { + return run(() -> m_robotDrive.tankDrive(left.getAsDouble(), right.getAsDouble())) + .withName("tankDrive"); + } + + public void driveTank(double left, double right) { + m_robotDrive.tankDrive(left, right); + } + + @Override + public void periodic() { + } } -