diff --git a/src/main/java/frc/robot/climber/ClimberSide.java b/src/main/java/frc/robot/climber/Actuator.java similarity index 98% rename from src/main/java/frc/robot/climber/ClimberSide.java rename to src/main/java/frc/robot/climber/Actuator.java index 80a9dc3..4e396f0 100644 --- a/src/main/java/frc/robot/climber/ClimberSide.java +++ b/src/main/java/frc/robot/climber/Actuator.java @@ -17,7 +17,7 @@ import frc.robot.Constants.ClimberConstants.CalibrationState; import frc.robot.Constants.RobotConstants; -public class ClimberSide { +public class Actuator { public final String climberName; @@ -37,7 +37,7 @@ public class ClimberSide { private CalibrationState m_calibrationState = CalibrationState.UNCALIBRATED; - public ClimberSide(String climberName, int climberMotorChannel) { + public Actuator(String climberName, int climberMotorChannel) { this.climberName = climberName; this.motorChannel = climberMotorChannel; diff --git a/src/main/java/frc/robot/climber/Climber.java b/src/main/java/frc/robot/climber/Climber.java index 33cb79b..07e74b4 100644 --- a/src/main/java/frc/robot/climber/Climber.java +++ b/src/main/java/frc/robot/climber/Climber.java @@ -8,29 +8,26 @@ public class Climber extends SubsystemBase { - private ClimberSide[] m_actuators = new ClimberSide[2]; - - DifferentialDrive m_differentialDrive; + private final Actuator[] m_actuators = new Actuator[2]; + private final DifferentialDrive m_differentialDrive; public Climber() { - - m_actuators[0] = new ClimberSide("Left", ClimberConstants.kLeftMotorPort); - m_actuators[1] = new ClimberSide("Right", ClimberConstants.kRightMotorPort); - + m_actuators[0] = new Actuator("Left", ClimberConstants.kLeftMotorPort); + m_actuators[1] = new Actuator("Right", ClimberConstants.kRightMotorPort); m_differentialDrive = new DifferentialDrive(m_actuators[0]::setPower, m_actuators[1]::setPower); } /** * @return Vector of climber actuators */ - public ClimberSide[] getClimberSides() { + public Actuator[] getClimberSides() { return m_actuators; } public Command createStopCommand() { return this.runOnce( () -> { - for (ClimberSide actuator : m_actuators) actuator.stop(); + for (Actuator actuator : m_actuators) actuator.stop(); }); } @@ -43,6 +40,7 @@ public Command createArcadeDriveCommand(XboxController xboxController) { @Override public void periodic() { - for (ClimberSide actuator : m_actuators) actuator.periodic(); + m_differentialDrive.feed(); + for (Actuator actuator : m_actuators) actuator.periodic(); } } diff --git a/src/main/java/frc/robot/climber/commands/CalibrateCommand.java b/src/main/java/frc/robot/climber/commands/CalibrateCommand.java index 75ff5da..c49ebb6 100644 --- a/src/main/java/frc/robot/climber/commands/CalibrateCommand.java +++ b/src/main/java/frc/robot/climber/commands/CalibrateCommand.java @@ -5,13 +5,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.ClimberConstants.CalibrationState; +import frc.robot.climber.Actuator; import frc.robot.climber.Climber; -import frc.robot.climber.ClimberSide; public class CalibrateCommand extends Command { private final Climber m_climber; - private final ClimberSide[] m_actuators; + private final Actuator[] m_actuators; public CalibrateCommand(Climber subsystem) { m_climber = subsystem; @@ -21,7 +21,7 @@ public CalibrateCommand(Climber subsystem) { @Override public void initialize() { - for (ClimberSide actuator : m_actuators) { + for (Actuator actuator : m_actuators) { actuator.configureUpperLimit(false); actuator.configurePositionController( ClimberConstants.slowConstraints, ClimberConstants.kSeekPosition); @@ -31,12 +31,12 @@ public void initialize() { @Override public void execute() { - for (ClimberSide actuator : m_actuators) { + for (Actuator actuator : m_actuators) { if (actuator.getCalibrationState() != CalibrationState.CALIBRATED) calibrate(actuator); } } - private void calibrate(ClimberSide actuator) { + private void calibrate(Actuator actuator) { if (actuator.getCurrentSenseState()) { actuator.stop(); actuator.resetEncoder(); @@ -49,13 +49,13 @@ private void calibrate(ClimberSide actuator) { @Override public boolean isFinished() { - for (ClimberSide actuator : m_actuators) + for (Actuator actuator : m_actuators) if (actuator.getCalibrationState() != CalibrationState.CALIBRATED) return false; return true; } @Override public void end(boolean interrupted) { - for (ClimberSide actuator : m_actuators) actuator.configureUpperLimit(true); + for (Actuator actuator : m_actuators) actuator.configureUpperLimit(true); } } diff --git a/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java b/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java index 81d0681..d13e374 100644 --- a/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java +++ b/src/main/java/frc/robot/climber/commands/DriveToPositionCommand.java @@ -4,13 +4,13 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ClimberConstants; +import frc.robot.climber.Actuator; import frc.robot.climber.Climber; -import frc.robot.climber.ClimberSide; public class DriveToPositionCommand extends Command { private final Climber m_climber; - private final ClimberSide[] m_actuators; + private final Actuator[] m_actuators; private final double m_targetPosition; public DriveToPositionCommand(Climber subsystem, double targetPosition) { @@ -22,14 +22,14 @@ public DriveToPositionCommand(Climber subsystem, double targetPosition) { @Override public void initialize() { - for (ClimberSide actuator : m_actuators) { + for (Actuator actuator : m_actuators) { actuator.configurePositionController(ClimberConstants.rapidConstraints, m_targetPosition); } } @Override public void execute() { - for (ClimberSide actuator : m_actuators) { + for (Actuator actuator : m_actuators) { actuator.driveToTargetPosition(); } } @@ -40,7 +40,7 @@ public void execute() { */ @Override public boolean isFinished() { - for (ClimberSide actuator : m_actuators) if (!actuator.atGoal()) return false; + for (Actuator actuator : m_actuators) if (!actuator.atGoal()) return false; return true; } }