Merge pull request #5 from FRC3636/indexer_sim

FRC3636 · Nov 21, 2024 · 7c5d87b · 7c5d87b
2 parents e05c406 + b994c72
commit 7c5d87b
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 4 deletions.
diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/Indexer.kt
@@ -1,12 +1,17 @@
 package com.frcteam3636.frc2024.subsystems.indexer
 
+import com.frcteam3636.frc2024.Robot
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Subsystem
 import org.littletonrobotics.junction.Logger
 
 object Indexer: Subsystem {
-    private var io: IndexerIO = IndexerIOReal()
+    private var io: IndexerIO = when (Robot.model) {
+        Robot.Model.SIMULATION -> IndexerIOSim()
+        Robot.Model.COMPETITION -> IndexerIOReal()
+        Robot.Model.PROTOTYPE -> IndexerIOPrototype()
+    }
 
     var inputs = IndexerIO.Inputs()
 

diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt
@@ -5,8 +5,13 @@ import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
 import com.frcteam3636.frc2024.CANSparkFlex
 import com.frcteam3636.frc2024.REVMotorControllerId
+import com.frcteam3636.frc2024.Robot
 import com.frcteam3636.frc2024.utils.LimelightHelpers
 import com.revrobotics.CANSparkLowLevel
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.units.Units.RadiansPerSecond
+import edu.wpi.first.units.Units.RotationsPerSecond
+import edu.wpi.first.wpilibj.simulation.FlywheelSim
 
 public enum class BalloonState {
     Blue,
@@ -16,7 +21,7 @@ public enum class BalloonState {
 
 interface IndexerIO {
     class Inputs : LoggableInputs {
-        var indexerVelocity = Rotation2d()
+        var indexerVelocity = RotationsPerSecond.zero()
         var indexerCurrent: Double = 0.0
         var balloonState: BalloonState = BalloonState.None
 
@@ -27,7 +32,7 @@ interface IndexerIO {
         }
 
         override fun fromLog(table: LogTable) {
-            indexerVelocity = table.get("Indexer Velocity", indexerVelocity)!![0]
+            indexerVelocity = table.get("Indexer Velocity", indexerVelocity)!!
             indexerCurrent = table.get("Indexer Wheel Current", indexerCurrent)
             balloonState = table.get("Balloon Color", balloonState)
         }
@@ -52,7 +57,7 @@ class IndexerIOReal : IndexerIO{
         )
 
     override fun updateInputs(inputs: IndexerIO.Inputs) {
-        inputs.indexerVelocity = Rotation2d(indexerMotor.encoder.velocity)
+        inputs.indexerVelocity = RadiansPerSecond.of(indexerMotor.encoder.velocity)
         inputs.indexerCurrent = indexerMotor.outputCurrent
 
         when (val colorClass = LimelightHelpers.getClassifierClass("limelight-sensor")) {
@@ -67,4 +72,31 @@ class IndexerIOReal : IndexerIO{
         assert(speed in -1.0..1.0)
         indexerMotor.set(speed)
     }
+}
+
+class IndexerIOSim: IndexerIO {
+    val flywheelSim = FlywheelSim(
+        DCMotor.getNeoVortex(1),
+        1.0,
+        1.0
+    )
+
+    override fun updateInputs(inputs: IndexerIO.Inputs) {
+        flywheelSim.update(Robot.period)
+        inputs.indexerVelocity = RadiansPerSecond.of(flywheelSim.angularVelocityRadPerSec)
+    }
+
+    override fun setSpinSpeed(speed: Double) {
+        assert(speed in -1.0..1.0)
+        flywheelSim.setInputVoltage(speed*12.0)
+    }
+}
+
+class IndexerIOPrototype: IndexerIO {
+    override fun updateInputs(inputs: IndexerIO.Inputs) {
+    }
+
+    override fun setSpinSpeed(speed: Double) {
+    }
+
 }