added kraken spinshooter

blair-robot-project · May 14, 2024 · 3fcaa2c · 3fcaa2c
1 parent 3433d9c
commit 3fcaa2c
Show file tree

Hide file tree

Showing 3 changed files with 621 additions and 20 deletions.
diff --git a/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterConstants.kt b/src/main/kotlin/frc/team449/robot2024/constants/subsystem/SpinShooterConstants.kt
@@ -1,36 +1,49 @@
 package frc.team449.robot2024.constants.subsystem
 
+import com.ctre.phoenix6.signals.InvertedValue
+import com.ctre.phoenix6.signals.NeutralModeValue
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap
-import edu.wpi.first.math.util.Units
 import kotlin.math.PI
 import kotlin.math.pow
 
 object SpinShooterConstants {
+  val BRAKE_MODE = false
   const val EFFICIENCY = 0.8
+
+  const val DUTY_CYCLE_DEADBAND = 0.001
   const val RIGHT_MOTOR_ID = 45
   const val RIGHT_MOTOR_INVERTED = false
+  val RIGHT_MOTOR_ORIENTATION = InvertedValue.CounterClockwise_Positive
+  val RIGHT_NEUTRAL_MODE = NeutralModeValue.Coast
   const val LEFT_MOTOR_ID = 46
   const val LEFT_MOTOR_INVERTED = true
-  const val CURRENT_LIMIT = 85
+  val LEFT_MOTOR_ORIENTATION = InvertedValue.CounterClockwise_Positive
+  val LEFT_NEUTRAL_MODE = NeutralModeValue.Coast
+
+  const val UPDATE_FREQUENCY = 5.0
+
+  const val STATOR_CURRENT_LIMIT = 150.0
+  const val SUPPLY_CURRENT_LIMIT = 40.0
+  const val BURST_CURRENT_LIMIT = 60.0
+  const val BURST_TIME_LIMIT = 0.25
   const val SECONDARY_CURRENT_LIMIT = 200.0
-  const val BRAKE_MODE = false
-
-  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(3900.0)
-  val PASS_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(2900.0)
-  val PASS2_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(3550.0)
-  val PASS2_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(2550.0)
-  val PASS3_LEFT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(3550.0)
-  val PASS3_RIGHT_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(2550.0)
-  val AMP_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(1800.0)
-  val OUTTAKE_SPEED = Units.rotationsPerMinuteToRadiansPerSecond(-200.0)
+
+  val SUBWOOFER_LEFT_SPEED = 3450.0 / 60
+  val SUBWOOFER_RIGHT_SPEED = 2800.0 / 60
+  val ANYWHERE_LEFT_SPEED = 4850.0 / 60
+  val ANYWHERE_RIGHT_SPEED = 3850.0 / 60
+  val PASS_LEFT_SPEED = 3900.0 / 60
+  val PASS_RIGHT_SPEED = 2900.0 / 60
+  val PASS2_LEFT_SPEED = 3550.0 / 60
+  val PASS2_RIGHT_SPEED = 2550.0 / 60
+  val PASS3_LEFT_SPEED = 3550.0 / 60
+  val PASS3_RIGHT_SPEED = 2550.0 / 60
+  val AMP_SPEED = 1800.0 / 60
+  val OUTTAKE_SPEED = -200.0 / 60
 
   const val AUTO_SHOOT_TOL = 25.0
 
-  val BRAKE_RATE_LIMIT = Units.rotationsPerMinuteToRadiansPerSecond(5250.0)
+  val BRAKE_RATE_LIMIT = 5250.0 / 60
 
   val SHOOTING_MAP = InterpolatingDoubleTreeMap()
   val TIME_MAP = InterpolatingDoubleTreeMap()
@@ -41,6 +54,12 @@ object SpinShooterConstants {
   const val RIGHT_KV = 0.011597 // 0.010836
   const val LEFT_KA = 0.0050 // 0.0061217
   const val RIGHT_KA = 0.0060 // 0.00815
+  const val LEFT_KP = 0.2377 // 0.19599
+  const val RIGHT_KP = 0.32957 // 0.28982
+  const val LEFT_KI = 0.010607 // 0.010993
+  const val RIGHT_KI = 0.011597 // 0.010836
+  const val LEFT_KD = 0.0050 // 0.0061217
+  const val RIGHT_KD = 0.0060 // 0.00815
 
   const val IN_TOLERANCE = 10.0
   const val AIM_TOLERANCE = 15.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
@@ -30,7 +30,6 @@ import kotlin.Pair
 import kotlin.jvm.optionals.getOrNull
 import kotlin.math.*
 
-
 open class SpinShooter(
   val rightMotor: WrappedMotor,
   val leftMotor: WrappedMotor,
@@ -673,7 +672,7 @@ open class SpinShooter(
           depth = SpinShooterConstants.INTERNAL_ENC_DEPTH
         ),
         inverted = SpinShooterConstants.RIGHT_MOTOR_INVERTED,
-        currentLimit = SpinShooterConstants.CURRENT_LIMIT,
+        currentLimit = SpinShooterConstants.STATOR_CURRENT_LIMIT.toInt(),
         secondaryCurrentLimit = SpinShooterConstants.SECONDARY_CURRENT_LIMIT,
         enableBrakeMode = SpinShooterConstants.BRAKE_MODE
       )
@@ -688,7 +687,7 @@ open class SpinShooter(
           depth = SpinShooterConstants.INTERNAL_ENC_DEPTH
         ),
         inverted = SpinShooterConstants.LEFT_MOTOR_INVERTED,
-        currentLimit = SpinShooterConstants.CURRENT_LIMIT,
+        currentLimit = SpinShooterConstants.STATOR_CURRENT_LIMIT.toInt(),
         secondaryCurrentLimit = SpinShooterConstants.SECONDARY_CURRENT_LIMIT,
         enableBrakeMode = SpinShooterConstants.BRAKE_MODE
       )