locked shooting

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index db87084b75436c3957a50944e37c27adc796cc12..ac8942bae0ffcdfe1b1f9fd75ef5fea72a28916c 100644 (file)
--- 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 1e5d23a512120d9aa0f3e1e55e1186592306f40b..304889a7790e2c114c8ab75275df18ca7bb77f02 100644 (file)
--- 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 1ffd4763c1f3b202b2753480509cc240d0fd3ce7..4b4496fae660d214865b146760fd2362fcf8e707 100644 (file)
--- 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 425d86d1792aff9de8647057f0258d67e6df4199..e9a4f897203bf50227d207e58411a1ff808a7149 100644 (file)
--- 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 8bf72954087624d62f07ea8ac6a55fb8c1451655..23ed33aeb0d4279280b67e0d616ab47889c7e1d8 100644 (file)
--- 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);