final flywheel tuning:

src/main/kotlin/com/team4099/robot2023/BuildConstants.kt

@@ -4,12 +4,12 @@ package com.team4099.robot2023
  * Automatically generated file containing build version information.
  */
 const val MAVEN_GROUP = ""
-const val MAVEN_NAME = "Crescendo-2024v2"
+const val MAVEN_NAME = "Crescendo-2024"
 const val VERSION = "unspecified"
-const val GIT_REVISION = 103
-const val GIT_SHA = "af060f29e117bcb356a8028ace24474cd3e9c484"
-const val GIT_DATE = "2024-01-28T19:36:14Z"
+const val GIT_REVISION = 105
+const val GIT_SHA = "84e9b2f55aa0a00ffff0ceae0c0cdb639f003072"
+const val GIT_DATE = "2024-01-28T21:35:02Z"
 const val GIT_BRANCH = "shooter"
-const val BUILD_DATE = "2024-01-28T21:31:48Z"
-const val BUILD_UNIX_TIME = 1706495508778L
+const val BUILD_DATE = "2024-01-29T11:11:27Z"
+const val BUILD_UNIX_TIME = 1706544687987L
 const val DIRTY = 1

src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt

@@ -20,7 +20,7 @@ import org.team4099.lib.units.perSecond
 object FlywheelConstants {
   val LEFT_GEAR_RATIO = 1.0
 
-  val RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS = 48.0/24.0
+  val RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS = 24.0/48.0
 
   val VOLTAGE_COMPENSATION = 12.volts
 
@@ -46,32 +46,40 @@ object FlywheelConstants {
       0.0.volts / (1.0.rotations.perMinute.perSecond)
 
     val RIGHT_SIM_KP: ProportionalGain<Velocity<Radian>, Volt> =
-      0.001.volts / 1.0.rotations.perMinute
+      0.015.volts / 1.0.rotations.perMinute
     val RIGHT_SIM_KI: IntegralGain<Velocity<Radian>, Volt> =
-      0.0.volts / (1.0.radians.perSecond * 1.0.seconds)
+      0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
     val RIGHT_SIM_KD: DerivativeGain<Velocity<Radian>, Volt> =
       0.0.volts / (1.0.rotations.perMinute.perSecond)
 
     val LEFT_REAL_KP: ProportionalGain<Velocity<Radian>, Volt> =
-      0.01.volts / 1.0.rotations.perMinute
+      0.015.volts / 1.0.rotations.perMinute
     val LEFT_REAL_KI: IntegralGain<Velocity<Radian>, Volt> =
       0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
     val LEFT_REAL_KD: DerivativeGain<Velocity<Radian>, Volt> =
       0.0.volts / (1.0.rotations.perMinute.perSecond)
 
     val LEFT_SIM_KP: ProportionalGain<Velocity<Radian>, Volt> =
-      0.01.volts / 1.0.rotations.perMinute
+      0.0015.volts / 1.0.rotations.perMinute
     val LEFT_SIM_KI: IntegralGain<Velocity<Radian>, Volt> =
-      0.0.volts / (1.0.rotations.perSecond * 1.0.seconds)
+      0.0.volts / (1.0.rotations.perMinute * 1.0.seconds)
     val LEFT_SIM_KD: DerivativeGain<Velocity<Radian>, Volt> =
       0.0.volts / (1.0.rotations.perMinute.perSecond)
 
-    val RIGHT_FLYWHEEL_KS = 0.001.volts
-    val RIGHT_FLYWHEEL_KV = 0.0083.volts / 1.radians.perSecond
-    val RIGHT_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond
+    val RIGHT_REAL_FLYWHEEL_KS = 0.0.volts
+    val RIGHT_REAL_FLYWHEEL_KV = 0.0099.volts / 1.radians.perSecond
+    val RIGHT_REAL_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond
+
+    val LEFT_REAL_FLYWHEEL_KS = 0.0.volts
+    val LEFT_REAL_FLYWHEEL_KV = 0.0197.volts / 1.radians.perSecond
+    val LEFT_REAL_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond
+
+    val RIGHT_SIM_FLYWHEEL_KS = 0.0.volts
+    val RIGHT_SIM_FLYWHEEL_KV = 0.0099.volts / 1.radians.perSecond
+    val RIGHT_SIM_FLYWHEEL_KA = 0.09.volts / 1.radians.perSecond.perSecond
 
-    val LEFT_FLYWHEEL_KS = 0.001.volts
-    val LEFT_FLYWHEEL_KV = 0.02.volts / 1.radians.perSecond
-    val LEFT_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond
+    val LEFT_SIM_FLYWHEEL_KS = 0.0.volts
+    val LEFT_SIM_FLYWHEEL_KV = 0.0197.volts / 1.radians.perSecond
+    val LEFT_SIM_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond
   }
 }

src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt

@@ -56,22 +56,19 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
     )
 
   val inputs = FlywheelIO.FlywheelIOInputs()
-  /*private val flywheelRightkS =
+  private val flywheelRightkS =
     LoggedTunableValue(
       "Flywheel/Right kS",
-      FlywheelConstants.PID.RIGHT_FLYWHEEL_KS,
       Pair({ it.inVolts }, { it.volts })
     )
   private val flywheelRightkV =
     LoggedTunableValue(
       "Flywheel/Right kV",
-      FlywheelConstants.PID.RIGHT_FLYWHEEL_KV,
       Pair({ it.inVoltsPerRadianPerSecond }, { it.volts.perRadianPerSecond })
     )
   private val flywheelRightkA =
     LoggedTunableValue(
       "Flywheel/Right kA",
-      FlywheelConstants.PID.RIGHT_FLYWHEEL_KA,
       Pair(
         { it.inVoltsPerRadianPerSecondPerSecond },
         { it.volts/1.0.radians.perSecond.perSecond }
@@ -81,24 +78,21 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
   private val flywheelLeftkS =
     LoggedTunableValue(
       "Flywheel/Left kS",
-      FlywheelConstants.PID.LEFT_FLYWHEEL_KS,
       Pair({ it.inVolts }, { it.volts })
     )
   private val flywheelLeftkV =
     LoggedTunableValue(
       "Flywheel/Left kV",
-      FlywheelConstants.PID.LEFT_FLYWHEEL_KV,
       Pair({ it.inVoltsPerRadianPerSecond }, { it.volts.perRadianPerSecond })
     )
   private val flywheelLeftkA =
     LoggedTunableValue(
       "Flywheel/Left kA",
-      FlywheelConstants.PID.LEFT_FLYWHEEL_KA,
       Pair(
         { it.inVoltsPerRadianPerSecondPerSecond },
         { it.volts / 1.0.radians.perSecond.perSecond }
       )
-    )*/
+    )
 
   var flywheelRightFeedForward: SimpleMotorFeedforward<Radian, Volt>
   var flywheelLeftFeedForward: SimpleMotorFeedforward<Radian, Volt>
@@ -150,6 +144,28 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
       leftkP.initDefault(FlywheelConstants.PID.LEFT_REAL_KP)
       leftkI.initDefault(FlywheelConstants.PID.LEFT_REAL_KI)
       leftkD.initDefault(FlywheelConstants.PID.LEFT_REAL_KD)
+
+      flywheelRightFeedForward =
+        SimpleMotorFeedforward(
+          FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KS,
+          FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KV,
+          FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KA
+        )
+
+      flywheelLeftFeedForward =
+        SimpleMotorFeedforward(
+          FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KS,
+          FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KV,
+          FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KA
+        )
+
+      flywheelLeftkS.initDefault(FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KS)
+      flywheelLeftkV.initDefault(FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KV)
+      flywheelLeftkA.initDefault(FlywheelConstants.PID.LEFT_REAL_FLYWHEEL_KA)
+
+      flywheelRightkS.initDefault(FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KS)
+      flywheelRightkV.initDefault(FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KV)
+      flywheelRightkA.initDefault(FlywheelConstants.PID.RIGHT_REAL_FLYWHEEL_KA)
     } else {
       rightkP.initDefault(FlywheelConstants.PID.RIGHT_SIM_KP)
       rightkI.initDefault(FlywheelConstants.PID.RIGHT_SIM_KI)
@@ -158,21 +174,31 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
       leftkP.initDefault(FlywheelConstants.PID.LEFT_SIM_KP)
       leftkI.initDefault(FlywheelConstants.PID.LEFT_SIM_KI)
       leftkD.initDefault(FlywheelConstants.PID.LEFT_SIM_KD)
+
+      flywheelLeftkS.initDefault(FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KS)
+      flywheelLeftkV.initDefault(FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KV)
+      flywheelLeftkA.initDefault(FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KA)
+
+      flywheelRightkS.initDefault(FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KS)
+      flywheelRightkV.initDefault(FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KV)
+      flywheelRightkA.initDefault(FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KA)
+
+      flywheelRightFeedForward =
+        SimpleMotorFeedforward(
+          FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KS,
+          FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KV,
+          FlywheelConstants.PID.RIGHT_SIM_FLYWHEEL_KA
+        )
+
+      flywheelLeftFeedForward =
+        SimpleMotorFeedforward(
+          FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KS,
+          FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KV,
+          FlywheelConstants.PID.LEFT_SIM_FLYWHEEL_KA
+        )
     }
 
-    flywheelRightFeedForward =
-      SimpleMotorFeedforward(
-        FlywheelConstants.PID.RIGHT_FLYWHEEL_KS,
-        FlywheelConstants.PID.RIGHT_FLYWHEEL_KV,
-        FlywheelConstants.PID.RIGHT_FLYWHEEL_KA
-      )
 
-    flywheelLeftFeedForward =
-      SimpleMotorFeedforward(
-        FlywheelConstants.PID.LEFT_FLYWHEEL_KS,
-        FlywheelConstants.PID.LEFT_FLYWHEEL_KV,
-        FlywheelConstants.PID.LEFT_FLYWHEEL_KA
-      )
     io.configPID(
       rightkP.get(), rightkI.get(), rightkD.get(), leftkP.get(), leftkI.get(), leftkD.get()
     )
@@ -206,7 +232,7 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
       )
     }
 
-    /*if (flywheelRightkA.hasChanged() ||
+    if (flywheelRightkA.hasChanged() ||
       flywheelRightkV.hasChanged() ||
       flywheelRightkS.hasChanged()
     ) {
@@ -219,7 +245,7 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
     if (flywheelLeftkA.hasChanged() || flywheelLeftkV.hasChanged() || flywheelLeftkS.hasChanged()) {
       flywheelLeftFeedForward =
         SimpleMotorFeedforward(flywheelLeftkS.get(), flywheelLeftkV.get(), flywheelLeftkA.get())
-    }*/
+    }
 
     var nextState = currentState
     when (currentState) {
@@ -265,7 +291,7 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
 
   fun flywheelSpinUpCommand(): Command {
     return runOnce({
-      currentRequest = Request.FlywheelRequest.TargetingVelocity(10000.rotations.perMinute)
+      currentRequest = Request.FlywheelRequest.TargetingVelocity(6000.rotations.perMinute)
     })
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt

@@ -7,10 +7,16 @@ import edu.wpi.first.math.system.plant.DCMotor
 import edu.wpi.first.wpilibj.simulation.FlywheelSim
 import org.team4099.lib.controller.PIDController
 import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.celsius
 import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.derived.DerivativeGain
 import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.IntegralGain
+import org.team4099.lib.units.derived.ProportionalGain
+import org.team4099.lib.units.derived.Radian
+import org.team4099.lib.units.derived.Volt
 import org.team4099.lib.units.derived.inKilogramsMeterSquared
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.rotations
@@ -21,14 +27,15 @@ object FlywheelIOSim : FlywheelIO {
   private val flywheelRightSim: FlywheelSim =
     FlywheelSim(
       DCMotor.getKrakenX60Foc(2),
-      FlywheelConstants.LEFT_GEAR_RATIO,
+      FlywheelConstants.RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS
+      ,
       FlywheelConstants.INERTIA.inKilogramsMeterSquared
     )
 
   private val flywheelLeftSim: FlywheelSim =
     FlywheelSim(
       DCMotor.getKrakenX60Foc(2),
-      FlywheelConstants.RIGHT_MOTOR_REVOLUTIONS_PER_FLYWHEEL_REVOLUTIONS,
+      FlywheelConstants.LEFT_GEAR_RATIO,
       FlywheelConstants.INERTIA.inKilogramsMeterSquared
     )
 
@@ -115,4 +122,17 @@ object FlywheelIOSim : FlywheelIO {
   }
 
   override fun setFlywheelBrakeMode(brake: Boolean) {}
+
+  override fun configPID(
+    rightkP: ProportionalGain<Velocity<Radian>, Volt>,
+    rightkI: IntegralGain<Velocity<Radian>, Volt>,
+    rightkD: DerivativeGain<Velocity<Radian>, Volt>,
+    leftkP: ProportionalGain<Velocity<Radian>, Volt>,
+    leftkI: IntegralGain<Velocity<Radian>, Volt>,
+    leftkD: DerivativeGain<Velocity<Radian>, Volt>
+  ) {
+    rightFlywheelController.setPID(rightkP, rightkI, rightkD)
+
+    leftFlywheelController.setPID(leftkP, leftkI, leftkD)
+  }
 }