From c66a5f812182837302731ef26338e8d590ea3160 Mon Sep 17 00:00:00 2001
From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Wed, 18 Feb 2026 11:28:36 -0800
Subject: [PATCH] intake

---
 .../robot/commands/gpm/SimpleAutoShoot.java   |  2 --
 .../controls/PS5ControllerDriverConfig.java   | 11 ++++----
 .../frc/robot/subsystems/turret/Turret.java   | 25 +++++++++----------
 3 files changed, 17 insertions(+), 21 deletions(-)

diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
index 60dcbfe..ff801b9 100644
--- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
+++ b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java
@@ -22,12 +22,10 @@ import frc.robot.subsystems.turret.ShotInterpolation;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.util.FieldZone;
 import frc.robot.util.ShootingTarget;
-import frc.robot.util.Vision.TurretVision;
 
 public class SimpleAutoShoot extends Command {
     private Turret turret;
     private Drivetrain drivetrain;
-    private TurretVision turretVision;
     private Shooter shooter;
 
     private double fieldAngleRad;
diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
index e0631aa..2f47c82 100644
--- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
+++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java
@@ -18,8 +18,7 @@ import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.subsystems.spindexer.Spindexer;
 import frc.robot.subsystems.turret.Turret;
 import frc.robot.subsystems.hood.Hood;
-import frc.robot.subsystems.intake.Intake;
-import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.Intake.Intake;
 import lib.controllers.PS5Controller;
 import lib.controllers.PS5Controller.PS5Axis;
 import lib.controllers.PS5Controller.PS5Button;
@@ -75,13 +74,13 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig {
         if(intake != null){
             driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{
                 if(intakeBoolean){
-                    intake.setSetpoint(IntakeConstants.INTAKE_ANGLE);
-                    intake.setFlyWheel();
+                    intake.extend();
+                    intake.spinStart();
                     intakeBoolean = false;
                 }
                 else{
-                    intake.setSetpoint(IntakeConstants.STOW_ANGLE);
-                    intake.stopFlyWheel();
+                    intake.retract();
+                    intake.spinStop();
                 }
             }));
         }
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 1db640e..7e374bd 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -24,7 +24,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.Constants;
 import frc.robot.constants.IdConstants;
-import frc.robot.util.ChineseRemainderTheorem;
 
 public class Turret extends SubsystemBase implements TurretIO{
 
@@ -108,24 +107,24 @@ public class Turret extends SubsystemBase implements TurretIO{
 
 		//TODO: replace this stuff with Chinese Remainder Theorem calculator -- ignore this for now
 
-		double leftAbs = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET);
-		double rightAbs = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET);
+		// double leftAbs = wrapUnit(encoderLeft.getAbsolutePosition().getValueAsDouble() - TurretConstants.LEFT_ENCODER_OFFSET);
+		// double rightAbs = wrapUnit(encoderRight.getAbsolutePosition().getValueAsDouble() - TurretConstants.RIGHT_ENCODER_OFFSET);
 
-		int leftTooth = (int) Math.round(leftAbs * TurretConstants.LEFT_ENCODER_TEETH)
-				% TurretConstants.LEFT_ENCODER_TEETH;
+		// int leftTooth = (int) Math.round(leftAbs * TurretConstants.LEFT_ENCODER_TEETH)
+		// 		% TurretConstants.LEFT_ENCODER_TEETH;
 
-		int rightTooth = (int) Math.round(rightAbs * TurretConstants.RIGHT_ENCODER_TEETH)
-				% TurretConstants.RIGHT_ENCODER_TEETH;
+		// int rightTooth = (int) Math.round(rightAbs * TurretConstants.RIGHT_ENCODER_TEETH)
+		// 		% TurretConstants.RIGHT_ENCODER_TEETH;
 
-		int turretIndex = ChineseRemainderTheorem.solve(leftTooth, TurretConstants.LEFT_ENCODER_TEETH, rightTooth, TurretConstants.RIGHT_ENCODER_TEETH);
+		// int turretIndex = ChineseRemainderTheorem.solve(leftTooth, TurretConstants.LEFT_ENCODER_TEETH, rightTooth, TurretConstants.RIGHT_ENCODER_TEETH);
 
-		double totalTeeth = TurretConstants.LEFT_ENCODER_TEETH
-        * TurretConstants.RIGHT_ENCODER_TEETH;
+		// double totalTeeth = TurretConstants.LEFT_ENCODER_TEETH
+        // * TurretConstants.RIGHT_ENCODER_TEETH;
 
-		double turretRotations = turretIndex / (double) totalTeeth;
+		// double turretRotations = turretIndex / (double) totalTeeth;
 
-		double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO;
-		motor.setPosition(motorRotations);
+		// double motorRotations = turretRotations * TurretConstants.TURRET_GEAR_RATIO;
+		// motor.setPosition(motorRotations);
 
 		motor.setPosition(0.0); //TODO: remove after chinese remainder theorem works
 
-- 
2.39.5