fix: remove 360 degree encoder jumps

FRC3636 · May 8, 2024 · a5722b7 · a5722b7
1 parent 403358e
commit a5722b7
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 42 deletions.
diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/PivotIO.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/PivotIO.kt
@@ -111,10 +111,8 @@ class PivotIOKraken : PivotIO {
     private val absoluteEncoder = DutyCycleEncoder(DigitalInput(2)).apply {
         distancePerRotation = SENSOR_TO_PIVOT_RATIO
     }
-    private var absoluteEncoderOffset = ABSOLUTE_ENCODER_OFFSET
     private val rawAbsoluteEncoderPosition
-        get() = Rotation2d.fromRotations(-absoluteEncoder.get())
-    private var previousAbsoluteEncoderPosition: Rotation2d? = null
+        get() = Rotation2d.fromRotations(-absoluteEncoder.absolutePosition)
 
     init {
         val config = TalonFXConfiguration().apply {
@@ -159,33 +157,7 @@ class PivotIOKraken : PivotIO {
         inputs.leftLimitSwitchUnpressed = leftLimitSwitchUnpressed.get()
 
         inputs.uncorrectedEncoderPosition = this.rawAbsoluteEncoderPosition
-        inputs.absoluteEncoderPosition = Rotation2d(inputs.uncorrectedEncoderPosition.radians + absoluteEncoderOffset.radians)
-        if (previousAbsoluteEncoderPosition != null) {
-            // If a jump occurs, this might be 362 degrees
-            val deltaEncoderPosition = Rotation2d(inputs.absoluteEncoderPosition.radians - previousAbsoluteEncoderPosition!!.radians)
-            Logger.recordOutput("Shooter/Pivot/Delta Encoder Position", deltaEncoderPosition)
-            // Correction math would undo the 360 degree jump, making this 2 degrees
-            var correctedDeltaPositionRadians = deltaEncoderPosition.radians
-            while (abs(correctedDeltaPositionRadians) > Units.degreesToRadians(180.0)) {
-                correctedDeltaPositionRadians -= TAU * sign(correctedDeltaPositionRadians)
-            }
-            Logger.recordOutput("Shooter/Pivot/Corrected Delta Encoder Position", correctedDeltaPositionRadians)
-            // The offset required to move the uncorrected encoder position to the corrected encoder position.
-            // E.g. -360
-            val offset = Rotation2d(correctedDeltaPositionRadians - deltaEncoderPosition.radians)
-            Logger.recordOutput("Shooter/Pivot/Correction offset", offset)
-            absoluteEncoderOffset = Rotation2d(absoluteEncoderOffset.radians + offset.radians)
-            inputs.absoluteEncoderPosition = Rotation2d(inputs.absoluteEncoderPosition.radians + offset.radians)
-
-            if (deltaEncoderPosition.radians != correctedDeltaPositionRadians) {
-                DriverStation.reportWarning(
-                    "Pivot absolute encoder jumped by ${-(offset).degrees} degrees!",
-                    false
-                )
-            }
-        }
-        previousAbsoluteEncoderPosition = inputs.absoluteEncoderPosition
-        Logger.recordOutput("Shooter/Pivot/Absolute Encoder Offset", absoluteEncoderOffset)
+        inputs.absoluteEncoderPosition = Rotation2d(inputs.uncorrectedEncoderPosition.radians + ABSOLUTE_ENCODER_OFFSET.radians)
 
         inputs.leftPosition = Rotation2d.fromRotations(leftMotor.position.value)
         inputs.leftVelocity = Rotation2d.fromRotations(leftMotor.velocity.value)
@@ -283,7 +255,7 @@ class PivotIOKraken : PivotIO {
         const val RIGHT_ZERO_OFFSET = 0.38
 
         val LIMIT_SWITCH_OFFSET = Rotation2d.fromDegrees(-27.0)
-        val ABSOLUTE_ENCODER_OFFSET = Rotation2d.fromDegrees(10.18) + LIMIT_SWITCH_OFFSET
+        val ABSOLUTE_ENCODER_OFFSET = Rotation2d.fromDegrees(9.13) + LIMIT_SWITCH_OFFSET
     }
 
     override val talonCANStatuses = listOf(leftMotor.version, rightMotor.version)

diff --git a/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/Shooter.kt b/src/main/java/com/frcteam3636/frc2024/subsystems/shooter/Shooter.kt
@@ -24,10 +24,7 @@ import edu.wpi.first.wpilibj2.command.Commands
 import edu.wpi.first.wpilibj2.command.Subsystem
 import edu.wpi.first.wpilibj2.command.button.Trigger
 import org.littletonrobotics.junction.Logger
-import kotlin.math.abs
-import kotlin.math.absoluteValue
-import kotlin.math.atan
-import kotlin.math.pow
+import kotlin.math.*
 
 object Shooter {
     object Flywheels : Subsystem {
@@ -187,19 +184,21 @@ object Shooter {
         }
         private val inputs = PivotIO.Inputs()
 
+        var target: Target = Target.AIM
+
+
         private val processedAbsoluteEncoderPosition
             get() =
-                Rotation2d(inputs.absoluteEncoderPosition.radians)
-
-        var target: Target = Target.AIM
+                Rotation2d((inputs.absoluteEncoderPosition.radians + PIVOT_MOD_OFFSET.radians).mod(TAU) - PIVOT_MOD_OFFSET.radians)
 
         override fun periodic() {
             io.updateInputs(inputs)
             Logger.processInputs("Shooter/Pivot", inputs)
 
-            io.setPivotPosition(processedAbsoluteEncoderPosition)
-            Logger.recordOutput("Shooter/Pivot/Processed Absolute Encoder", processedAbsoluteEncoderPosition)
-            Logger.recordOutput("Shooter/Pivot/Required Offset", -inputs.absoluteEncoderPosition)
+            val encoderPos = processedAbsoluteEncoderPosition
+            io.setPivotPosition(encoderPos)
+            Logger.recordOutput("Shooter/Pivot/Processed Encoder Position", encoderPos)
+            Logger.recordOutput("Shooter/Pivot/Required Offset", -inputs.uncorrectedEncoderPosition)
 
             armLigament.angle = inputs.leftPosition.degrees
 
@@ -395,4 +394,6 @@ internal val FLYWHEEL_VELOCITY_TOLERANCE = RadiansPerSecond.of(13.0)
 
 internal val FLYWHEEL_SIDE_SEPERATION = Units.inchesToMeters(9.0)
 internal val FLYWHEEL_PID_GAINS = PIDGains(0.0029805, 0.0, 0.0)
-internal val FLYWHEEL_FF_GAINS = MotorFFGains(0.26294, 0.10896 / TAU, 0.010373 / TAU)
+internal val FLYWHEEL_FF_GAINS = MotorFFGains(0.26294, 0.10896 / TAU, 0.010373 / TAU)
+
+internal val PIVOT_MOD_OFFSET = Rotation2d(PI/2)