From 6a97221d7d42a8b0b17af79a831dc75ee3d9e173 Mon Sep 17 00:00:00 2001
From: taimorioka <thetaimorioka@gmail.com>
Date: Wed, 20 Nov 2024 16:28:24 -0800
Subject: [PATCH] Added intake simulation

---
 .../frc2024/subsystems/intake/IntakeIO.kt     | 34 ++++++++++++++++---
 1 file changed, 30 insertions(+), 4 deletions(-)

diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/intake/IntakeIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/intake/IntakeIO.kt
index 55af93f..6b9b122 100644
--- a/src/main/java/com/frcteam3636/frc2024/subsystems/intake/IntakeIO.kt
+++ b/src/main/java/com/frcteam3636/frc2024/subsystems/intake/IntakeIO.kt
@@ -1,16 +1,20 @@
 package com.frcteam3636.frc2024.subsystems.intake
 import com.frcteam3636.frc2024.CANSparkFlex
 import com.frcteam3636.frc2024.REVMotorControllerId
+import com.frcteam3636.frc2024.Robot
 import com.revrobotics.CANSparkLowLevel
 import edu.wpi.first.math.geometry.Rotation2d
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.units.Units.*
+import edu.wpi.first.wpilibj.simulation.FlywheelSim
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
 
 
 interface IntakeIO {
     class Inputs : LoggableInputs {
-        var rollerVelocity = Rotation2d()
-        var current: Double = 0.0
+        var rollerVelocity = RotationsPerSecond.zero()
+        var current = Amps.zero()
 
         override fun toLog(table: LogTable) {
             table.put("Intake Velocity", rollerVelocity)
@@ -18,7 +22,7 @@ interface IntakeIO {
         }
 
         override fun fromLog(table: LogTable) {
-            rollerVelocity = table.get("Intake Velocity", rollerVelocity)!![0]
+            rollerVelocity = table.get("Intake Velocity", rollerVelocity)
             current = table.get("Intake Current", current)
         }
 
@@ -26,6 +30,8 @@ interface IntakeIO {
 
     fun setSpeed(percent: Double)
 
+    fun updateInputs(inputs: IntakeIO.Inputs)
+
     class IntakeIOReal: IntakeIO {
         private var motor =
             CANSparkFlex(
@@ -37,11 +43,31 @@ interface IntakeIO {
             assert(percent in -1.0..1.0)
             motor.set(percent)
         }
+
+        override fun updateInputs(inputs: Inputs) {
+            inputs.rollerVelocity = RotationsPerSecond.of(motor.encoder.velocity)
+            inputs.current = Amps.of(motor.outputCurrent)
+        }
     }
 
     class IntakeIOSim: IntakeIO {
+
+        var motor = FlywheelSim(
+            DCMotor.getNeoVortex(1),
+            1.0,
+            1.0
+        )
+
+        override fun updateInputs(inputs: Inputs) {
+            motor.update(Robot.period)
+            inputs.rollerVelocity = RadiansPerSecond.of(motor.angularVelocityRadPerSec)
+            inputs.current = Amps.of(motor.currentDrawAmps)
+
+        }
+
         override fun setSpeed(percent: Double) {
-            TODO("Not yet implemented")
+            assert(percent in -1.0..1.0)
+            motor.setInputVoltage(percent * 12)
         }
     }
 }
\ No newline at end of file