Add a left motor for flywheel and make it completely independent of t…

…he right motor.

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

@@ -51,9 +51,13 @@ object FlywheelConstants {
     val SIM_KD: DerivativeGain<Velocity<Radian>, Volt> =
       0.0.volts / (1.0.rotations.perMinute.perSecond)
 
-    val FLYWHEEL_KS = 0.001.volts
-    val FLYWHEEL_KV = 0.01.volts / 1.radians.perSecond
-    val FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond
+    val RIGHT_FLYWHEEL_KS = 0.001.volts
+    val RIGHT_FLYWHEEL_KV = 0.01.volts / 1.radians.perSecond
+    val RIGHT_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond
+
+    val LEFT_FLYWHEEL_KS = 0.001.volts
+    val LEFT_FLYWHEEL_KV = 0.01.volts / 1.radians.perSecond
+    val LEFT_FLYWHEEL_KA = 0.03.volts / 1.radians.perSecond.perSecond
 
     val FLYWHEEL_INIT_VOLTAGE = 0.0.volts
 

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

@@ -5,10 +5,8 @@ import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.FlywheelConstants
 import com.team4099.robot2023.subsystems.superstructure.Request
-import com.team4099.robot2023.subsystems.wrist.Wrist.Companion.fromRequestToState
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.Command
-import edu.wpi.first.wpilibj2.command.Commands.runOnce
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.SimpleMotorFeedforward
@@ -28,63 +26,110 @@ import org.team4099.lib.units.perMinute
 import org.team4099.lib.units.perSecond
 
 class Flywheel(val io: FlywheelIO) : SubsystemBase() {
-  private val kP =
+  private val rightkP =
     LoggedTunableValue(
-      "Flywheel/kP",
+      "Flywheel/Right kP",
       Pair({ it.inVoltsPerRotationsPerMinute }, { it.volts / 1.0.rotations.perMinute })
     )
-  private val kI =
+  private val rightkI =
     LoggedTunableValue(
-      "Flywheel/kI",
+      "Flywheel/Right kI",
       Pair({ it.inVoltsPerRotations }, { it.volts / (1.0.rotations.perMinute * 1.0.seconds) })
     )
-  private val kD =
+  private val rightkD =
     LoggedTunableValue(
-      "Flywheel/kD",
+      "Flywheel/Right kD",
+      Pair(
+        { it.inVoltsPerRotationsPerMinutePerSecond },
+        { it.volts / 1.0.rotations.perMinute.perSecond }
+      )
+    )
+
+  private val leftkP =
+    LoggedTunableValue(
+      "Flywheel/Left kP",
+      Pair({ it.inVoltsPerRotationsPerMinute }, { it.volts / 1.0.rotations.perMinute })
+    )
+  private val leftkI =
+    LoggedTunableValue(
+      "Flywheel/Left kI",
+      Pair({ it.inVoltsPerRotations }, { it.volts / (1.0.rotations.perMinute * 1.0.seconds) })
+    )
+  private val leftkD =
+    LoggedTunableValue(
+      "Flywheel/Left kD",
       Pair(
         { it.inVoltsPerRotationsPerMinutePerSecond },
         { it.volts / 1.0.rotations.perMinute.perSecond }
       )
     )
 
   val inputs = FlywheelIO.FlywheelIOInputs()
-  private val flywheelkS =
+  private val flywheelRightkS =
     LoggedTunableValue(
-      "Flywheel/kS", FlywheelConstants.PID.FLYWHEEL_KS, Pair({ it.inVolts }, { it.volts })
+      "Flywheel/Right kS", FlywheelConstants.PID.RIGHT_FLYWHEEL_KS, Pair({ it.inVolts }, { it.volts })
     )
-  private val flywheelkV =
+  private val flywheelRightkV =
     LoggedTunableValue(
-      "Flywheel/kV",
-      FlywheelConstants.PID.FLYWHEEL_KV,
+      "Flywheel/Right kV",
+      FlywheelConstants.PID.RIGHT_FLYWHEEL_KV,
       Pair({ it.inVoltsPerRotationsPerMinute }, { it.volts / 1.0.rotations.perMinute })
     )
-  private val flywheelkA =
+  private val flywheelRightkA =
     LoggedTunableValue(
-      "Flywheel/kA",
-      FlywheelConstants.PID.FLYWHEEL_KA,
+      "Flywheel/Right kA",
+      FlywheelConstants.PID.RIGHT_FLYWHEEL_KA,
       Pair(
         { it.inVoltsPerRotationsPerMinutePerSecond },
         { it.volts / 1.0.rotations.perMinute.perSecond }
       )
     )
-  var flywheelFeedForward: SimpleMotorFeedforward<Radian, Volt>
+
+  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.inVoltsPerRotationsPerMinute }, { it.volts / 1.0.rotations.perMinute })
+    )
+  private val flywheelLeftkA =
+    LoggedTunableValue(
+      "Flywheel/Left kA",
+      FlywheelConstants.PID.LEFT_FLYWHEEL_KA,
+      Pair(
+        { it.inVoltsPerRotationsPerMinutePerSecond },
+        { it.volts / 1.0.rotations.perMinute.perSecond }
+      )
+    )
+
+  var flywheelRightFeedForward: SimpleMotorFeedforward<Radian, Volt>
+  var flywheelLeftFeedForward: SimpleMotorFeedforward<Radian, Volt>
 
   var lastFlywheelRunTime = 0.0.seconds
-  private var lastFlywheelVoltage = 0.0.volts
-  var flywheelTargetVoltage = 0.volts
+  private var lastRightFlywheelVoltage = 0.0.volts
+
+  var flywheelRightTargetVoltage = 0.0.volts
+  var flywheelRightTargetVelocity: AngularVelocity = 0.0.rotations.perMinute
 
-  var flywheelTargetVelocity: AngularVelocity = 0.rotations.perMinute
+  var flywheelLeftTargetVoltage = 0.0.volts
+  var flywheelLeftTargetVelocity: AngularVelocity = 0.0.rotations.perMinute
 
   var currentState = Companion.FlywheelStates.UNINITIALIZED
 
   var currentRequest: Request.FlywheelRequest = Request.FlywheelRequest.OpenLoop(0.0.volts)
     set(value) {
       when (value) {
         is Request.FlywheelRequest.OpenLoop -> {
-          flywheelTargetVoltage = value.flywheelVoltage
+          flywheelRightTargetVoltage = value.flywheelVoltage
+          flywheelLeftTargetVoltage = value.flywheelVoltage
         }
         is Request.FlywheelRequest.TargetingVelocity -> {
-          flywheelTargetVelocity = value.flywheelVelocity
+          flywheelRightTargetVelocity = value.flywheelVelocity
+          //left needs to be half of the right one
+          flywheelRightTargetVelocity = value.flywheelVelocity/2
         }
         else -> {}
       }
@@ -93,39 +138,52 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
 
   init {
     if (RobotBase.isReal()) {
-      kP.initDefault(FlywheelConstants.PID.REAL_KP)
-      kI.initDefault(FlywheelConstants.PID.REAL_KI)
-      kD.initDefault(FlywheelConstants.PID.REAL_KD)
+      rightkP.initDefault(FlywheelConstants.PID.REAL_KP)
+      rightkI.initDefault(FlywheelConstants.PID.REAL_KI)
+      rightkD.initDefault(FlywheelConstants.PID.REAL_KD)
     } else {
-      kP.initDefault(FlywheelConstants.PID.SIM_KP)
-      kI.initDefault(FlywheelConstants.PID.SIM_KI)
-      kD.initDefault(FlywheelConstants.PID.SIM_KD)
+      rightkP.initDefault(FlywheelConstants.PID.SIM_KP)
+      rightkI.initDefault(FlywheelConstants.PID.SIM_KI)
+      rightkD.initDefault(FlywheelConstants.PID.SIM_KD)
     }
 
-    flywheelFeedForward =
+    flywheelRightFeedForward =
       SimpleMotorFeedforward(
-        FlywheelConstants.PID.FLYWHEEL_KS,
-        FlywheelConstants.PID.FLYWHEEL_KV,
-        FlywheelConstants.PID.FLYWHEEL_KA
+        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
       )
   }
   override fun periodic() {
     io.updateInputs(inputs)
     Logger.processInputs("Flywheel", inputs)
 
     if (Constants.Tuning.DEBUGING_MODE) {
-      Logger.recordOutput("Flywheel/FlywheelTargetVoltage", flywheelTargetVoltage.inVolts)
-      Logger.recordOutput("Flywheel/FlywheelTargetVelocity", flywheelTargetVelocity.inRadiansPerSecond)
-      Logger.recordOutput("Flywheel/FlywheelLastVoltage", lastFlywheelVoltage.inVolts)
+      Logger.recordOutput("Flywheel/FlywheelTargetVoltage", flywheelRightTargetVoltage.inVolts)
+      Logger.recordOutput("Flywheel/FlywheelTargetVelocity", flywheelRightTargetVelocity.inRadiansPerSecond)
+      Logger.recordOutput("Flywheel/FlywheelLastVoltage", lastRightFlywheelVoltage.inVolts)
+    }
+
+    if (rightkP.hasChanged() || rightkI.hasChanged() || rightkD.hasChanged()
+      ||leftkP.hasChanged() || leftkI.hasChanged() || leftkD.hasChanged()) {
+      io.configPID(rightkP.get(), rightkI.get(), rightkD.get(), leftkP.get(), leftkI.get(), leftkD.get())
     }
 
-    if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) {
-      io.configPID(kP.get(), kI.get(), kD.get())
+    if (flywheelRightkA.hasChanged() || flywheelRightkV.hasChanged() || flywheelRightkS.hasChanged()) {
+      flywheelRightFeedForward =
+        SimpleMotorFeedforward(flywheelRightkS.get(), flywheelRightkV.get(), flywheelRightkA.get())
     }
 
-    if (flywheelkA.hasChanged() || flywheelkV.hasChanged() || flywheelkS.hasChanged()) {
-      flywheelFeedForward =
-        SimpleMotorFeedforward(flywheelkS.get(), flywheelkV.get(), flywheelkA.get())
+    if (flywheelLeftkA.hasChanged() || flywheelLeftkV.hasChanged() || flywheelLeftkS.hasChanged()) {
+      flywheelLeftFeedForward =
+        SimpleMotorFeedforward(flywheelLeftkS.get(), flywheelLeftkV.get(), flywheelLeftkA.get())
     }
 
     var nextState = currentState
@@ -134,27 +192,28 @@ class Flywheel(val io: FlywheelIO) : SubsystemBase() {
         nextState = Companion.FlywheelStates.OPEN_LOOP
       }
       Companion.FlywheelStates.OPEN_LOOP -> {
-        println("aryan is a monkey")
-        setFlywheelVoltage(flywheelTargetVoltage)
+        println("ryan lowkey the best")
+        setFlywheelVoltage(flywheelRightTargetVoltage, flywheelLeftTargetVoltage)
         lastFlywheelRunTime = Clock.fpgaTime
 
         nextState = fromRequestToState(currentRequest)
       }
       Companion.FlywheelStates.TARGETING_VELOCITY -> {
-        setFlywheelVelocity(flywheelTargetVelocity)
+        setFlywheelVelocity(flywheelRightTargetVelocity, flywheelLeftTargetVelocity)
         lastFlywheelRunTime = Clock.fpgaTime
         nextState = fromRequestToState(currentRequest)
       }
     }
   }
 
-  fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) {
-    io.setFlywheelVoltage(appliedVoltage)
+  fun setFlywheelVoltage(appliedVoltageRight: ElectricalPotential, appliedVoltageLeft: ElectricalPotential) {
+    io.setFlywheelVoltage(appliedVoltageRight, appliedVoltageLeft)
   }
 
-  fun setFlywheelVelocity(flywheelVelocity: AngularVelocity) {
-    val feedForward = flywheelFeedForward.calculate(flywheelVelocity)
-    io.setFlywheelVelocity(flywheelVelocity, feedForward)
+  fun setFlywheelVelocity(flywheelRightVelocity: AngularVelocity, flywheelLeftVelocity: AngularVelocity) {
+    val rightFeedForward = flywheelRightFeedForward.calculate(flywheelRightVelocity)
+    val leftFeedForward = flywheelLeftFeedForward.calculate(flywheelLeftVelocity)
+    io.setFlywheelVelocity(flywheelRightVelocity, flywheelLeftVelocity, leftFeedForward, rightFeedForward)
   }
 
   fun flywheelSpinUpCommand(): Command {

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

@@ -14,7 +14,9 @@ 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.inNewtons
 import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.newtons
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.derived.rotations
 import org.team4099.lib.units.derived.volts
@@ -30,13 +32,16 @@ interface FlywheelIO {
     var rightFlywheelSupplyCurrent = 0.amps
     var rightFlywheelTemperature = 0.celsius
     var rightFlywheelDutyCycle = 0.0.volts
-    var rightFlywheelTorque = 0.0
+    var rightFlywheelTorque = 0.0.newtons
+
 
     var leftFlywheelVelocity = 0.0.rotations.perMinute
     var leftFlywheelAppliedVoltage = 0.volts
     var leftFlywheelStatorCurrent = 0.amps
     var leftFlywheelSupplyCurrent = 0.amps
     var leftFlywheelTemperature = 0.celsius
+    var leftFlywheelDutyCycle = 0.0.volts
+    var leftFlywheelTorque = 0.0.newtons
 
     var isSimulated = false
 
@@ -46,12 +51,16 @@ interface FlywheelIO {
       table.put("flywheelRightStatorCurrent", rightFlywheelStatorCurrent.inAmperes)
       table.put("flywheelRightSupplyCurrent", rightFlywheelSupplyCurrent.inAmperes)
       table.put("flywheelRightTemperature", rightFlywheelTemperature.inCelsius)
+      table.put("rightFlywheelDutyCycle", rightFlywheelDutyCycle.inVolts)
+      table.put("rightFlywheelTorque", rightFlywheelTorque.inNewtons)
 
       table.put("flywheelLeftVelocityRPM", leftFlywheelVelocity.inRadiansPerSecond)
       table.put("flywheelLeftAppliedVoltage", leftFlywheelAppliedVoltage.inVolts)
       table.put("flywheelLeftStatorCurrent", leftFlywheelStatorCurrent.inAmperes)
       table.put("flywheelLeftSupplyCurrent", leftFlywheelSupplyCurrent.inAmperes)
       table.put("flywheelLeftTemperature", leftFlywheelTemperature.inCelsius)
+      table.put("leftFlywheelDutyCycle", leftFlywheelDutyCycle.inVolts)
+      table.put("leftFlywheelTorque", leftFlywheelTorque.inNewtons)
     }
 
     override fun fromLog(table: LogTable) {
@@ -71,6 +80,12 @@ interface FlywheelIO {
       table.get("rightFlywheelTemperature", rightFlywheelTemperature.inCelsius).let {
         rightFlywheelTemperature = it.celsius
       }
+      table.get("rightFlywheelDutyCycle", rightFlywheelDutyCycle.inVolts).let {
+        rightFlywheelDutyCycle = it.volts
+      }
+      table.get("rightFlywheelTorque", rightFlywheelTorque.inNewtons).let {
+        rightFlywheelTorque = it.newtons
+      }
 
       // Left motor
       table.get("leftFlywheelVelocityRPM", leftFlywheelVelocity.inRadiansPerSecond).let {
@@ -88,20 +103,29 @@ interface FlywheelIO {
       table.get("leftFlywheelTemperature", leftFlywheelTemperature.inCelsius).let {
         leftFlywheelTemperature = it.celsius
       }
+      table.get("leftFlywheelDutyCycle", leftFlywheelDutyCycle.inVolts).let {
+        leftFlywheelDutyCycle = it.volts
+      }
+      table.get("leftFlywheelTorque", leftFlywheelTorque.inNewtons).let {
+        leftFlywheelTorque = it.newtons
+      }
     }
   }
 
-  fun setFlywheelVoltage(voltage: ElectricalPotential) {}
+  fun setFlywheelVoltage(voltageRight: ElectricalPotential, voltageLeft: ElectricalPotential) {}
 
-  fun setFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {}
+  fun setFlywheelVelocity(rightVelocity: AngularVelocity, leftVelocity : AngularVelocity, feedforwardLeft: ElectricalPotential, feedforwardRight: ElectricalPotential) {}
 
   fun setFlywheelBrakeMode(brake: Boolean) {}
 
   fun updateInputs(inputs: FlywheelIOInputs) {}
 
   fun configPID(
-    kP: ProportionalGain<Velocity<Radian>, Volt>,
-    kI: IntegralGain<Velocity<Radian>, Volt>,
-    kD: DerivativeGain<Velocity<Radian>, Volt>
+    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>
   ) {}
 }