wrist

src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt

@@ -3,6 +3,7 @@ package com.frcteam3636.frc2024.subsystems.wrist
 import com.frcteam3636.frc2024.Robot
 import com.frcteam3636.frc2024.subsystems.intake.IntakeIO
 import com.frcteam3636.frc2024.subsystems.wrist.WristIO
+import edu.wpi.first.units.Units.Radians
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Subsystem
 import org.littletonrobotics.junction.Logger
@@ -15,6 +16,6 @@ object Wrist: Subsystem {
     override fun periodic() {
         Logger.processInputs("Intake", inputs)
         //the wrist should always be parallel to the ground
-        io.pivotToAndStop() //use position variable from arm
+        io.pivotToAndStop(Radians.zero()) //use position variable from arm
     }
 }

src/main/java/com/frcteam3636/frc2024/subsystems/wrist/WristIO.kt

@@ -48,7 +48,7 @@ interface WristIO {
     }
     fun updateInputs(inputs: Inputs)
 
-    fun pivotToAndStop(position: Rotation2d)
+    fun pivotToAndStop(position: Measure<Angle>)
 
     class WristIOKraken: WristIO {
         private val wristMotor = TalonFX(CTREMotorControllerId.WristMotor)
@@ -101,12 +101,12 @@ interface WristIO {
             inputs.voltage = Volts.zero()
         }
 
-        override fun pivotToAndStop(position: Rotation2d) {
+        override fun pivotToAndStop(position: Measure<Angle>) {
             Logger.recordOutput("Shooter/Pivot/Position Setpoint", position)
 
             val wristControl = MotionMagicTorqueCurrentFOC(0.0).apply {
                 Slot = 0
-                Position = position.rotations
+                Position = position.`in`(Rotations)
             }
             wristMotor.setControl(wristControl)
         }