diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 39a435e..2ff85d4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -185,10 +185,22 @@ private void configureButtonBindings() { // .withTimeout(3))))); controller - .povDown() + .povLeft() .whileTrue( Commands.startEnd(() -> shooter.setVoltage(-9), () -> shooter.setVoltage(0), pivot)); + controller + .povUp() + .onTrue( + Commands.startEnd(() -> shooter.changeVoltage(1), () -> shooter.setVoltage(1)) + ); + + controller + .povUp() + .onTrue( + Commands.startEnd(() -> shooter.changeVoltage(-1), () -> shooter.setVoltage(-1)) + ); + controller .povRight() .whileTrue( diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index b0f741a..4df7e79 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -28,6 +28,13 @@ public void setVoltage(double voltage) { io.setVoltage(voltage); } + public void changeVoltage(double dv) { + setLeftRightVoltage( + inputs.leftVoltage >= 1 + dv ? inputs.leftVoltage + dv : 0, + inputs.rightVoltage >= 1 + dv ? inputs.rightVoltage + dv : 0 + ); + } + public void setLeftRightVoltage(double leftVoltage, double rightVoltage) { io.setRightVoltage(rightVoltage); io.setLeftVoltage(leftVoltage); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java index b348493..3721d6e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java @@ -7,6 +7,8 @@ public interface ShooterIO { public static class ShooterIOInputs { public double leftMotorVelocity = 0.0; public double rightMotorVelocity = 0.0; + public double leftVoltage = 0.0; + public double rightVoltage = 0.0; } public default void spinForwards() {} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java index 0a32b81..efa5e31 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOReal.java @@ -62,5 +62,7 @@ public double getRightvelocity() { public void updateInputs(ShooterIOInputs inputs) { inputs.leftMotorVelocity = getLeftvelocity(); inputs.rightMotorVelocity = getRightvelocity(); + inputs.leftVoltage = leftShooterMotor.getBusVoltage(); + inputs.leftVoltage = rightShooterMotor.getBusVoltage(); } }