From: WesleyWong-972 <wesleycwong@gmail.com>
Date: Fri, 27 Feb 2026 18:47:01 +0000 (-0800)
Subject: I think this will work now
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=ce91a7a00147344b8aeb805dd63cbd3abb35480b;p=FRC2026.git

I think this will work now
---

diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index d48f879..10d26ba 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -64,6 +64,8 @@ public class Turret extends SubsystemBase implements TurretIO{
 
 	private final MotionMagicVoltage mmVoltageRequest = new MotionMagicVoltage(0);
 
+	ModifiedCRT crt;
+
 	/* ---------------- Constructor ---------------- */
 
 	public Turret() {
@@ -110,9 +112,9 @@ public class Turret extends SubsystemBase implements TurretIO{
 		double rightPosition = encoderRight.getAbsolutePosition().getValueAsDouble();
 		double rightAbs = wrapUnit(rightPosition - TurretConstants.RIGHT_ENCODER_OFFSET);
 
-		ModifiedCRT crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT);
+		crt = new ModifiedCRT(TurretConstants.LEFT_ENCODER_TEETH, TurretConstants.RIGHT_ENCODER_TEETH, TurretConstants.TURRET_TEETH_COUNT);
 
-		double turretRot = crt.solve(leftAbs, rightAbs);
+		double turretRot = crt.solve(leftAbs, rightAbs); 
 		//SmartDashboard.putNumber("Turret Index", turretIndex);
 
 		SmartDashboard.putNumber("CRT Position", Units.rotationsToDegrees(turretRot));
@@ -120,10 +122,9 @@ public class Turret extends SubsystemBase implements TurretIO{
 		double motorRotations = turretRot * TurretConstants.GEAR_RATIO;
 
 		//Sets the initial motor position
+		inputs.positionDeg = turretRot;
 		motor.setPosition(motorRotations);
 
-		motor.setPosition(0.0);
-
 		SmartDashboard.putData("Turn to 0", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(0), 0.0);}));
 		SmartDashboard.putData("Turn to -90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(-90), 0.0);}));
 		SmartDashboard.putData("Turn to 90", new InstantCommand(()->{setFieldRelativeTarget(Rotation2d.fromDegrees(90), 0.0);}));
@@ -256,12 +257,12 @@ public class Turret extends SubsystemBase implements TurretIO{
 
 	@Override
 	public void updateInputs() {
-		// inputs.positionDeg = Units.rotationsToDegrees(motor.getPosition().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
 		inputs.velocityRadPerSec = Units.rotationsToRadians(motor.getVelocity().getValueAsDouble()) / TurretConstants.GEAR_RATIO;
 		inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
         inputs.encoderLeftRot = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble());
         inputs.encoderRightRot = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble());
 		inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
+		inputs.positionDeg = crt.solve(inputs.encoderLeftRot, inputs.encoderRightRot);
 	}
 
 	/**