diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterConstants.kt index 4bf1bd4..811b2d7 100644 --- a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterConstants.kt +++ b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterConstants.kt @@ -15,12 +15,12 @@ object SpinShooterConstants { const val SECONDARY_CURRENT_LIMIT = 200.0 const val BRAKE_MODE = false - val SUBWOOFER_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(2800.0) - val SUBWOOFER_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(3450.0) - val ANYWHERE_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(3850.0) - val ANYWHERE_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(4850.0) - val PASS_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(3500.0) - val PASS_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(4500.0) + val SUBWOOFER_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(3450.0) + val SUBWOOFER_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(2800.0) + val ANYWHERE_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(4850.0) + val ANYWHERE_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(3850.0) + val PASS_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(4500.0) + val PASS_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(3500.0) val AMP_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(1800.0) val OUTTAKE_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(-200.0) diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooter.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooter.kt index 3afeef3..3d0eb5e 100644 --- a/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooter.kt +++ b/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooter.kt @@ -185,8 +185,8 @@ open class SpinShooter( fun shootSubwoofer(): Command { val cmd = this.run { shootPiece( - SpinShooterConstants.SUBWOOFER_RIGHT_SPEED, - SpinShooterConstants.SUBWOOFER_LEFT_SPEED + SpinShooterConstants.SUBWOOFER_LEFT_SPEED, + SpinShooterConstants.SUBWOOFER_RIGHT_SPEED ) } cmd.name = "shooting subwoofer" @@ -461,7 +461,7 @@ open class SpinShooter( return cmd } - fun shootPiece(rightSpeed: Double, leftSpeed: Double) { + fun shootPiece(leftSpeed: Double, rightSpeed: Double) { if (DriverStation.isDisabled()) { correct() } else { @@ -525,8 +525,8 @@ open class SpinShooter( }, this.run { shootPiece( - rightRateLimiter.calculate(0.0), - leftRateLimiter.calculate(0.0) + leftRateLimiter.calculate(0.0), + rightRateLimiter.calculate(0.0) ) }.until { abs(leftVelocity.get()) < SpinShooterConstants.MIN_COAST_VEL &&