From b994c72540cb6582c7da161dd730cc499682c603 Mon Sep 17 00:00:00 2001
From: njoy3663 <naojoy5@gmail.com>
Date: Wed, 20 Nov 2024 16:08:48 -0800
Subject: [PATCH] indexer sim

---
 .../frc2024/subsystems/indexer/Indexer.kt     |  7 ++++++-
 .../frc2024/subsystems/indexer/IndexerIO.kt   | 21 ++++++++++++++-----
 2 files changed, 22 insertions(+), 6 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
index 7b6a577..19f9a4d 100644
--- 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
index 00abf14..e3a1c30 100644
--- a/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt
+++ b/src/main/java/com/frcteam3636/frc2024/subsystems/indexer/IndexerIO.kt
@@ -9,6 +9,8 @@ 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 {
@@ -19,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
 
@@ -30,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)
         }
@@ -55,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")) {
@@ -81,11 +83,20 @@ class IndexerIOSim: IndexerIO {
 
     override fun updateInputs(inputs: IndexerIO.Inputs) {
         flywheelSim.update(Robot.period)
-        inputs.indexerVelocity = flywheelSim.angularVelocityRadPerSec
+        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) {
-        TODO("Not yet implemented")
     }
 
 }
\ No newline at end of file