diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt index bb60687..800ebf2 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/Wrist.kt +++ b/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 } } \ No newline at end of file diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/WristIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/WristIO.kt index ae956e7..8cc78d4 100644 --- a/src/main/java/com/frcteam3636/frc2024/subsystems/wrist/WristIO.kt +++ b/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) 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) { 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) }