diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index a9b95e3..0b32521 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -50,9 +50,9 @@ public class Climber extends SubsystemBase { } public Command maintainPosition(ClimberPositions position) { - targetPosition = position; - return run(() -> { + targetPosition = position; + controller.setSetpoint( position.getPositionMeters(), ControlType.kPosition @@ -61,9 +61,9 @@ public class Climber extends SubsystemBase { } public Command manualSpeed(DoubleSupplier speed) { - targetPosition = null; - return run(() -> { + targetPosition = null; + motor.set(speed.getAsDouble()); }); } diff --git a/src/main/java/frc/robot/subsystems/IntakePivot.java b/src/main/java/frc/robot/subsystems/IntakePivot.java index 2924d02..d4626f6 100644 --- a/src/main/java/frc/robot/subsystems/IntakePivot.java +++ b/src/main/java/frc/robot/subsystems/IntakePivot.java @@ -60,9 +60,9 @@ public class IntakePivot extends SubsystemBase { } public Command maintainPosition(IntakePivotPosition position) { - currentTargetPosition = position; - return run(() -> { + currentTargetPosition = position; + if(currentTargetPosition == null) { leftMotor.disable(); } else { @@ -72,9 +72,9 @@ public class IntakePivot extends SubsystemBase { } public Command manualSpeed(DoubleSupplier speed) { - currentTargetPosition = null; - return run(() -> { + currentTargetPosition = null; + leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier); }); } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 0118ded..ac9f1ee 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -80,9 +80,9 @@ public class Shooter extends SubsystemBase { } public Command maintainSpeed(ShooterSpeeds speeds) { - targetSpeeds = speeds; - return run(() -> { + targetSpeeds = speeds; + if(targetSpeeds == null) { leftMotor.disable(); rightMotor.disable(); @@ -101,9 +101,9 @@ public class Shooter extends SubsystemBase { } public Command manualSpeed(DoubleSupplier speed) { - targetSpeeds = null; - return run(() -> { + targetSpeeds = null; + leftMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier); rightMotor.set(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier); });