From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com>
Date: Mon, 6 Apr 2026 19:31:42 +0000 (-0700)
Subject: locked shooting
X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=1692fa855666f715d5f5cf5458fe85309fdea209;p=FRC2026.git

locked shooting
---

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index db87084..ac8942b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -26,6 +26,7 @@ import frc.robot.commands.gpm.AutoShootCommand;
 import frc.robot.commands.gpm.BrownOutControl;
 import frc.robot.commands.gpm.ClimbDriveCommand;
 import frc.robot.commands.gpm.IntakeMovementCommand;
+import frc.robot.commands.gpm.LockedShoot;
 import frc.robot.commands.gpm.RunSpindexer;
 import frc.robot.commands.gpm.Superstructure;
 import frc.robot.commands.vision.ShutdownAllPis;
@@ -167,6 +168,8 @@ public class RobotContainer {
         // put the Chooser on the SmartDashboard
         SmartDashboard.putData("Auto chooser", autoChooser);
 
+        SmartDashboard.putData("Lock Shooting", new LockedShoot(turret, drive, hood, shooter));
+
         if (turret != null) {
           turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer));
         }
diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
index 1e5d23a..304889a 100644
--- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
+++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java
@@ -70,34 +70,34 @@ public class DefaultDriveCommand extends Command {
             Logger.recordOutput("TrenchAlign", swerve.getTrenchAlign());
             Logger.recordOutput("AlignZones", TrenchAssistConstants.ALIGN_ZONES);
         }
-        if (swerve.getTrenchAlign()) {
-            boolean inZone = false;
-            for (Rectangle2d rectangle : TrenchAssistConstants.ALIGN_ZONES) {
-                if (rectangle.contains(swerve.getPose().getTranslation())) {
-                    inZone = true;
-                }
-            }
-
-            if (inZone) {
-
-                double yawDegrees = swerve.getYaw().getDegrees();
-                // double snappedDeg = Math.round(yawDegrees / 90.0) * 90.0;
-                if (Math.abs(yawDegrees) <= 90) {
-                    swerve.setAlignAngle(Units.degreesToRadians(0.0));
-                } else {
-                    swerve.setAlignAngle(Units.degreesToRadians(180.0));
-                }
-                // swerve.setAlignAngle(snappedDeg);
-                // Logger.recordOutput("snappy", snappedDeg);
-            } else {
-                swerve.setIsAlign(false);
-            }
-        } else {
-            swerve.setIsAlign(false);
-        }
-        if (!Constants.DISABLE_LOGGING) {
-            Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist());
-        }
+        // if (swerve.getTrenchAlign()) {
+        //     boolean inZone = false;
+        //     for (Rectangle2d rectangle : TrenchAssistConstants.ALIGN_ZONES) {
+        //         if (rectangle.contains(swerve.getPose().getTranslation())) {
+        //             inZone = true;
+        //         }
+        //     }
+
+        //     if (inZone) {
+
+        //         double yawDegrees = swerve.getYaw().getDegrees();
+        //         // double snappedDeg = Math.round(yawDegrees / 90.0) * 90.0;
+        //         if (Math.abs(yawDegrees) <= 90) {
+        //             swerve.setAlignAngle(Units.degreesToRadians(0.0));
+        //         } else {
+        //             swerve.setAlignAngle(Units.degreesToRadians(180.0));
+        //         }
+        //         // swerve.setAlignAngle(snappedDeg);
+        //         // Logger.recordOutput("snappy", snappedDeg);
+        //     } else {
+        //         swerve.setIsAlign(false);
+        //     }
+        // } else {
+        //     swerve.setIsAlign(false);
+        // }
+        // if (!Constants.DISABLE_LOGGING) {
+        //     Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist());
+        // }
 
         if (swerve.getTrenchAssist()) {
             drive(TrenchAssist.calculate(swerve, corrected, trenchAssistPid));
diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java
index 1ffd476..4b4496f 100644
--- a/src/main/java/frc/robot/commands/gpm/Superstructure.java
+++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java
@@ -182,6 +182,7 @@ public class Superstructure extends Command {
         lastHoodAngle = hoodAngle;
 
         distanceFromTarget = target.getDistance(lookaheadPose.getTranslation());
+        Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget);
     }
 
     public void updateDrivePose(){
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
index 425d86d..e9a4f89 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java
@@ -272,7 +272,7 @@ public class Drivetrain extends SubsystemBase {
      */
     public void driveHeading(double xSpeed, double ySpeed, double heading, boolean fieldRelative) {
         double rot = rotationController.calculate(getYaw().getRadians(), heading);
-        ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, -rot);
+        ChassisSpeeds speeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
         if (fieldRelative) {
             speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getYaw());
         }
diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java
index 8bf7295..23ed33a 100644
--- a/src/main/java/frc/robot/subsystems/turret/Turret.java
+++ b/src/main/java/frc/robot/subsystems/turret/Turret.java
@@ -119,6 +119,7 @@ public class Turret extends SubsystemBase implements TurretIO{
 
 			SmartDashboard.putData("Turret Test Positions", turretTestChooser);
 		}
+		SmartDashboard.putData("Set Locked", new InstantCommand(() -> {locked = !locked;}));
 		//motor.setPosition(Units.degreesToRotations(238.86) * TurretConstants.GEAR_RATIO);
 
 		motor.setPosition(0.0);