diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 599add33..eb147c17 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -8,7 +8,8 @@ public static final class CoralShooterConstant { public static final int kShooterRightMotorChannel = 2; public static final double kShooterMotorFastSpeed = 4; public static final double kShooterMotorSlowSpeed = 4; - public static final Boolean kCoralShooterMotorInverted = false; + public static final Boolean kCoralShooterRightMotorInverted = false; + public static final Boolean kCoralShooterLeftMotorInverted = false; } public static final class PowerDistributionConstant { diff --git a/src/main/java/frc/robot/subsystems/CoralShooterSubsystem.java b/src/main/java/frc/robot/subsystems/CoralShooterSubsystem.java index 69bf66b9..54f133e1 100644 --- a/src/main/java/frc/robot/subsystems/CoralShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/CoralShooterSubsystem.java @@ -29,8 +29,8 @@ public CoralShooterSubsystem(PowerDistribution powerDistribution) { coralShooterRightMotor = new VictorSPX(CoralShooterConstant.kShooterRightMotorChannel); distanceSensor = new DistanceSensor(Port.kOnboard); - coralShooterRightMotor.setInverted(CoralShooterConstant.kCoralShooterMotorInverted); - coralShooterLeftMotor.setInverted(CoralShooterConstant.kCoralShooterMotorInverted); + coralShooterRightMotor.setInverted(CoralShooterConstant.kCoralShooterRightMotorInverted); + coralShooterLeftMotor.setInverted(CoralShooterConstant.kCoralShooterLeftMotorInverted); } private void setMotorSpeed(double speed) { @@ -48,7 +48,12 @@ public void coralShooterFastOn() { // Motor on Fast } public void coralShooterSlowOn() { // Motor on Slow + if (powerDistribution.isCoralShooterOverCurrent()) { + setMotorSpeed(0); + return; + } setMotorSpeed(CoralShooterConstant.kShooterMotorSlowSpeed); + } } public void coralShooterStop() { // Motor stop