From bcf53eda57738b3fcce45ad562d12e2f2c198270 Mon Sep 17 00:00:00 2001
From: taimorioka <thetaimorioka@gmail.com>
Date: Tue, 3 Dec 2024 16:51:52 -0800
Subject: [PATCH] Changed arm to update the relative encoders to the absolute
 encoders periodically

---
 .../frcteam3636/frc2024/subsystems/arm/Arm.kt  | 15 +++++++++++----
 .../frc2024/subsystems/arm/ArmIO.kt            | 18 ++++++++++++++++--
 2 files changed, 27 insertions(+), 6 deletions(-)

diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
index 558330c..1c5f517 100644
--- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
+++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/Arm.kt
@@ -2,13 +2,11 @@ package com.frcteam3636.frc2024.subsystems.arm
 
 import com.ctre.phoenix6.SignalLogger
 import com.frcteam3636.frc2024.Robot
-import com.frcteam3636.frc2024.subsystems.indexer.IndexerIOPrototype
-import com.frcteam3636.frc2024.subsystems.indexer.IndexerIOReal
-import com.frcteam3636.frc2024.subsystems.indexer.IndexerIOSim
 import edu.wpi.first.units.Angle
 import edu.wpi.first.units.Measure
 import edu.wpi.first.units.Units.Degrees
 import edu.wpi.first.units.Units.Volts
+import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj2.command.Subsystem
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction
@@ -16,6 +14,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism
 import org.littletonrobotics.junction.Logger
 
 
+private const val SECONDS_BETWEEN_ARM_UPDATES = 0.5
+
 object Arm : Subsystem {
     private var io: ArmIO = when (Robot.model) {
         Robot.Model.SIMULATION -> ArmIOSim()
@@ -34,10 +34,17 @@ object Arm : Subsystem {
         Mechanism(io::setVoltage, null, this)
     )
 
+    private var timer = Timer().apply {
+        start()
+    }
+
     override fun periodic() {
         io.updateInputs(inputs)
-
         Logger.processInputs("/Arm", inputs)
+        if (timer.advanceIfElapsed(SECONDS_BETWEEN_ARM_UPDATES) && inputs.absoluteEncoderConnected){
+                io.updatePosition(inputs.absoluteEncoderPosition)
+        }
+
     }
 
     fun moveToPosition(position: Position) =
diff --git a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
index 9ff9b48..3c5910d 100644
--- a/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
+++ b/src/main/kotlin/com/frcteam3636/frc2024/subsystems/arm/ArmIO.kt
@@ -16,7 +16,6 @@ import edu.wpi.first.units.Units.*
 import edu.wpi.first.units.Voltage
 import edu.wpi.first.wpilibj.DigitalInput
 import edu.wpi.first.wpilibj.DutyCycleEncoder
-import edu.wpi.first.wpilibj.simulation.FlywheelSim
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.Logger
@@ -72,9 +71,12 @@ interface ArmIO{
 
         fun pivotToPosition(position: Measure<Angle>)
 
-    fun setVoltage(volts: Measure<Voltage>)
+        fun setVoltage(volts: Measure<Voltage>)
+
+        fun updatePosition(position: Measure<Angle>)
     }
 
+
     class ArmIOReal: ArmIO{
 
         private val leftMotor = TalonFX(CTREMotorControllerId.LeftArmMotor)
@@ -102,6 +104,11 @@ interface ArmIO{
             rightMotor.setPosition(inputs.absoluteEncoderPosition.`in`(Rotations))
         }
 
+        override fun updatePosition(position: Measure<Angle>) {
+            leftMotor.setPosition(position.`in`(Rotations))
+            rightMotor.setPosition(position.`in`(Rotations))
+        }
+
         override fun pivotToPosition(position: Measure<Angle>) {
             Logger.recordOutput("Shooter/Pivot/Position Setpoint", position)
 
@@ -198,6 +205,10 @@ interface ArmIO{
         override fun setVoltage(volts: Measure<Voltage>) {
             armSim.setInputVoltage(volts.`in`(Volts))
         }
+
+        override fun updatePosition(position: Measure<Angle>) {
+            // no drifting in sim so no need to update
+        }
     }
 
     class ArmIOPrototype: ArmIO {
@@ -210,4 +221,7 @@ interface ArmIO{
 
         override fun setVoltage(volts: Measure<Voltage>) {
         }
+
+        override fun updatePosition(position: Measure<Angle>) {
+        }
     }
\ No newline at end of file