convert to single flywheel

team4099 · Jan 21, 2024 · 7d7dead · 7d7dead
1 parent 2df927c
commit 7d7dead
Show file tree

Hide file tree

Showing 6 changed files with 121 additions and 230 deletions.
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt
@@ -40,30 +40,26 @@ class Flywheel (val io: FlywheelIO) {
         LoggedTunableValue(
             "Flywheel/kA",  FlywheelConstants.PID.FLYWHEEL_KA, Pair({ it.inVoltsPerRotationsPerMinutePerSecond}, { it.volts/ 1.0.rotations.perMinute.perSecond })
         )
-    var leftFlyWheelFeedForward: SimpleMotorFeedforward<Radian, Volt>
-    var rightFlyWheelFeedForward: SimpleMotorFeedforward<Radian, Volt>
+    var flywheelFeedForward: SimpleMotorFeedforward<Radian, Volt>
 
 
     var lastFlywheelRunTime = 0.0.seconds
     private var lastFlywheelVoltage = 0.0.volts
-    var leftTargetVoltage = 0.volts
-    var rightTargetVoltage = 0.volts
+    var flywheelTargetVoltage = 0.volts
+
+    var flywheelTargetVelocity: AngularVelocity = 0.rotations.perMinute
 
-    var leftTargetVelocity: AngularVelocity = 0.rotations.perMinute
-    var rightTargetVelocity: AngularVelocity = 0.rotations.perMinute
     var currentState = Companion.FlywheelStates.UNINITIALIZED
 
     var currentRequest: Request.FlywheelRequest = Request.FlywheelRequest.OpenLoop(0.0.volts, 0.0.volts)
     set(value) {
         when (value) {
             is Request.FlywheelRequest.OpenLoop -> {
-                leftTargetVoltage = value.leftFlywheelVoltage
-                rightTargetVoltage = value.rightFlywheelVoltage
+                flywheelTargetVoltage = value.leftFlywheelVoltage
             }
 
             is Request.FlywheelRequest.TargetingVelocity -> {
-                leftTargetVelocity = value.leftFlywheelVelocity
-                rightTargetVelocity = value.rightFlywheelVelocity
+                flywheelTargetVelocity = value.leftFlywheelVelocity
             }
             else -> {}
         }
@@ -82,14 +78,7 @@ class Flywheel (val io: FlywheelIO) {
 
             }
 
-            leftFlyWheelFeedForward =
-                SimpleMotorFeedforward(
-                    FlywheelConstants.PID.FLYWHEEL_KS,
-                    FlywheelConstants.PID.FLYWHEEL_KV,
-                    FlywheelConstants.PID.FLYWHEEL_KA
-                )
-
-            rightFlyWheelFeedForward =
+            flywheelFeedForward =
                 SimpleMotorFeedforward(
                     FlywheelConstants.PID.FLYWHEEL_KS,
                     FlywheelConstants.PID.FLYWHEEL_KV,
@@ -103,7 +92,7 @@ class Flywheel (val io: FlywheelIO) {
         }
 
         if(flywheelkA.hasChanged()||flywheelkV.hasChanged()||flywheelkS.hasChanged()){
-            leftFlyWheelFeedForward = SimpleMotorFeedforward(
+            flywheelFeedForward = SimpleMotorFeedforward(
                 flywheelkS.get(),
                 flywheelkV.get(),
                 flywheelkA.get()
@@ -116,14 +105,14 @@ class Flywheel (val io: FlywheelIO) {
                 nextState = Companion.FlywheelStates.OPEN_LOOP
             }
             Companion.FlywheelStates.OPEN_LOOP -> {
-                setFlywheelVoltage(leftTargetVoltage, rightTargetVoltage)
+                setFlywheelVoltage(flywheelTargetVoltage)
                 lastFlywheelRunTime = Clock.fpgaTime
 
                 nextState = fromRequestToState(currentRequest)
             }
 
            Companion.FlywheelStates.TARGETING_VELOCITY ->{
-                    setFlywheelVelocity(leftTargetVelocity, leftTargetVelocity)
+                    setFlywheelVelocity(flywheelTargetVelocity)
                     lastFlywheelRunTime = Clock.fpgaTime
                     nextState = fromRequestToState(currentRequest)
             }
@@ -132,16 +121,13 @@ class Flywheel (val io: FlywheelIO) {
 
     }
 
-    fun setFlywheelVoltage(leftAppliedVoltage: ElectricalPotential, rightAppliedVoltage: ElectricalPotential) {
-        io.setLeftFlywheelVoltage(leftAppliedVoltage)
-        io.setRightFlywheelVoltage(rightAppliedVoltage)
+    fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) {
+        io.setFlywheelVoltage(appliedVoltage)
     }
 
-    fun setFlywheelVelocity(leftVelocity: AngularVelocity, rightVelocity: AngularVelocity) {
-        val leftFeedForward = leftFlyWheelFeedForward.calculate(leftVelocity)
-        val rightFeedForward = rightFlyWheelFeedForward.calculate(rightVelocity)
-        io.setLeftFlywheelVelocity(leftVelocity, leftFeedForward)
-        io.setRightFlywheelVelocity(rightVelocity, rightFeedForward)
+    fun setFlywheelVelocity(flywheelVelocity: AngularVelocity) {
+        val feedForward = flywheelFeedForward.calculate(flywheelVelocity)
+        io.setFlywheelVelocity(flywheelVelocity, feedForward)
     }
 
 

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt
@@ -78,45 +78,27 @@ interface FlywheelIO {
             }
         }
     }
-        fun setLeftFlywheelVoltage(leftVoltage: ElectricalPotential) {
 
-        }
-
-        fun setRightFlywheelVoltage(leftVoltage: ElectricalPotential) {
-
-        }
-
-        fun setLeftFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {
+        fun setFlywheelVoltage(leftVoltage: ElectricalPotential) {
 
         }
 
-        fun setRightFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {
+        fun setFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {
 
         }
 
-        fun setLeftFlywheelBrakeMode(brake: Boolean) {
-
-        }
-
-        fun setRightFlywheelBrakeMode(brake: Boolean) {
+        fun setFlywheelBrakeMode(brake: Boolean) {
 
         }
 
         fun updateInputs(inputs: FlywheelIOInputs) {
 
         }
 
-        fun configLeftPID(
-            leftkP: ProportionalGain<Velocity<Radian>, Volt>,
-            leftkI: IntegralGain<Velocity<Radian>, Volt>,
-            leftkD: DerivativeGain<Velocity<Radian>, Volt>
+        fun configPID(
+            kP: ProportionalGain<Velocity<Radian>, Volt>,
+            kI: IntegralGain<Velocity<Radian>, Volt>,
+            kD: DerivativeGain<Velocity<Radian>, Volt>
         ) {}
 
-        fun configRightPID (
-            rightkP: ProportionalGain<Velocity<Radian>, Volt>,
-            rightkI: IntegralGain<Velocity<Radian>, Volt>,
-            rightkD: DerivativeGain<Velocity<Radian>, Volt>
-        ) {}
-
-
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOSim.kt
@@ -14,21 +14,13 @@ import org.team4099.lib.units.derived.*
 import org.team4099.lib.units.perMinute
 
 object FlywheelIOSim: FlywheelIO {
-    private val leftFlywheelSim: FlywheelSim = FlywheelSim(
+    private val flywheelSim: FlywheelSim = FlywheelSim(
         DCMotor.getNEO(1),
         FlywheelConstants.LEFT_GEAR_RATIO,
         FlywheelConstants.LEFT_INERTIA.inKilogramsMeterSquared
     )
 
-
-    private val rightFlywheelSim: FlywheelSim = FlywheelSim(
-        DCMotor.getNEO(1),
-        FlywheelConstants.RIGHT_GEAR_RATIO,
-        FlywheelConstants.RIGHT_INERTIA.inKilogramsMeterSquared
-    )
-
-    private var leftAppliedVoltage = 0.volts
-    private var rightAppliedVoltage = 0.volts
+    private var appliedVoltage = 0.volts
 
     private val flywheelController =
         PIDController(
@@ -39,29 +31,28 @@ object FlywheelIOSim: FlywheelIO {
 
     override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) {
 
-        leftFlywheelSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
-        rightFlywheelSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
+        flywheelSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
 
-        inputs.leftFlywheelVelocity = leftFlywheelSim.getAngularVelocityRPM().rotations.perMinute
-        inputs.leftFlywheelVelocity = leftFlywheelSim.getAngularVelocityRPM().rotations.perMinute
-        inputs.leftFlywheelAppliedVoltage = leftAppliedVoltage
+        inputs.leftFlywheelVelocity = flywheelSim.getAngularVelocityRPM().rotations.perMinute
+        inputs.leftFlywheelVelocity = flywheelSim.getAngularVelocityRPM().rotations.perMinute
+        inputs.leftFlywheelAppliedVoltage = appliedVoltage
         inputs.leftFlywheelSupplyCurrent = 0.amps
-        inputs.leftFlywheelStatorCurrent = leftFlywheelSim.currentDrawAmps.amps
+        inputs.leftFlywheelStatorCurrent = flywheelSim.currentDrawAmps.amps
         inputs.leftFlywheelTemperature = 0.0.celsius
 
-        inputs.rightFlywheelVelocity = rightFlywheelSim.getAngularVelocityRPM().rotations.perMinute
-        inputs.rightFlywheelVelocity = rightFlywheelSim.getAngularVelocityRPM().rotations.perMinute
-        inputs.rightFlywheelAppliedVoltage = rightAppliedVoltage
+        inputs.rightFlywheelVelocity = flywheelSim.getAngularVelocityRPM().rotations.perMinute
+        inputs.rightFlywheelVelocity = flywheelSim.getAngularVelocityRPM().rotations.perMinute
+        inputs.rightFlywheelAppliedVoltage = appliedVoltage
         inputs.rightFlywheelSupplyCurrent = 0.amps
-        inputs.rightFlywheelStatorCurrent = rightFlywheelSim.currentDrawAmps.amps
+        inputs.rightFlywheelStatorCurrent = flywheelSim.currentDrawAmps.amps
         inputs.rightFlywheelTemperature = 0.0.celsius
 
         inputs.isSimulated = true
     }
 
-    override fun setLeftFlywheelVoltage(leftVoltage: ElectricalPotential) {
-        leftAppliedVoltage = leftVoltage
-        leftFlywheelSim.setInputVoltage(
+    override fun setFlywheelVoltage(leftVoltage: ElectricalPotential) {
+        appliedVoltage = leftVoltage
+        flywheelSim.setInputVoltage(
             clamp(
                 leftVoltage,
                 -FlywheelConstants.VOLTAGE_COMPENSATION,
@@ -71,29 +62,10 @@ object FlywheelIOSim: FlywheelIO {
         )
     }
 
-    override fun setRightFlywheelVoltage(rightVoltage: ElectricalPotential) {
-        leftAppliedVoltage = rightVoltage
-        leftFlywheelSim.setInputVoltage(
-            clamp(
-                rightVoltage,
-                -FlywheelConstants.VOLTAGE_COMPENSATION,
-                FlywheelConstants.VOLTAGE_COMPENSATION
-            )
-                .inVolts
-        )
-    }
-
-    override fun setLeftFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {
-        val feedback = flywheelController.calculate(leftFlywheelSim.getAngularVelocityRPM().rotations.perMinute, velocity)
-        setLeftFlywheelVoltage(feedback + feedforward)
+    override fun setFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {
+        val feedback = flywheelController.calculate(flywheelSim.getAngularVelocityRPM().rotations.perMinute, velocity)
+        setFlywheelVoltage(feedback + feedforward)
     }
 
-    override fun setRightFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) {
-        val feedback = flywheelController.calculate(rightFlywheelSim.getAngularVelocityRPM().rotations.perMinute, velocity)
-        setRightFlywheelVoltage(feedback + feedforward)
-    }
-
-    override fun setLeftFlywheelBrakeMode(brake: Boolean) {}
-
-    override fun setRightFlywheelBrakeMode(brake: Boolean) {}
+    override fun setFlywheelBrakeMode(brake: Boolean) {}
 }