States Fixes

src/main/java/frc/robot/Robot.java

@@ -81,13 +81,11 @@ public void robotInit() {
       m_robotContainer.configureTelemetry();
       // m_robotContainer.configurePitDashboard();
     }
-    m_robotContainer.enableDeadeye();
     Shuffleboard.getTab("Match")
         .add(new ToggleAllianceColorCommand(m_robotContainer))
         .withSize(1, 1)
         .withPosition(2, 0);
     logger.info("robotinit");
-    m_robotContainer.enableDeadeye();
   }
 
   @Override
@@ -127,6 +125,7 @@ public void disabledExit() {}
 
   @Override
   public void autonomousInit() {
+    m_robotContainer.enableDeadeye();
     logger.info("Auto Init");
     m_robotContainer.setIsAuto(true);
     // if (!m_robotContainer.hasElbowZeroed()) m_robotContainer.zeroElbow();
@@ -148,6 +147,7 @@ public void autonomousExit() {
 
   @Override
   public void teleopInit() {
+    m_robotContainer.enableDeadeye();
     logger.info("Teleop Init | Match #{}", DriverStation.getMatchNumber());
     m_robotContainer.setIsAuto(false);
     if (m_autonomousCommand != null) {

src/main/java/frc/robot/RobotContainer.java

@@ -34,7 +34,7 @@
 import frc.robot.commands.climb.JogClimbClosedLoopCommand;
 import frc.robot.commands.climb.ToggleRatchetCommand;
 import frc.robot.commands.climb.ToggleTrapBarPosCommand;
-import frc.robot.commands.climb.TrapClimbAdjustCommand;
+import frc.robot.commands.climb.TrapClimbCommand;
 import frc.robot.commands.climb.ZeroClimbCommand;
 import frc.robot.commands.drive.DriveAutonCommand;
 import frc.robot.commands.drive.DriveTeleopCommand;
@@ -62,7 +62,6 @@
 import frc.robot.commands.robotState.FeedCommand;
 import frc.robot.commands.robotState.FullTrapClimbCommand;
 import frc.robot.commands.robotState.IntakeCommand;
-import frc.robot.commands.robotState.MovingVisionShootCommand;
 import frc.robot.commands.robotState.OperatorRumbleCommand;
 import frc.robot.commands.robotState.PodiumCommand;
 import frc.robot.commands.robotState.PositionShootCommand;
@@ -76,6 +75,7 @@
 import frc.robot.commands.robotState.TuningOffCommand;
 import frc.robot.commands.robotState.TuningShootCommand;
 import frc.robot.commands.robotState.UpdateElbowOffsetCommand;
+import frc.robot.commands.robotState.VisionShootCommand;
 import frc.robot.commands.wrist.ClosedLoopWristCommand;
 import frc.robot.commands.wrist.OpenLoopWristCommand;
 import frc.robot.commands.wrist.WriteWristToStowCommand;
@@ -392,7 +392,7 @@ public void configurePitDashboard() {
         .withPosition(0, 0);
 
     Shuffleboard.getTab("Pit")
-        .add("Elbow Pos Zero ", new ClosedLoopElbowCommand(elbowSubsystem, 0.0))
+        .add("Elbow Pos Zero ", new ClosedLoopElbowOffsetCommand(elbowSubsystem, 0.0, () -> 0.0))
         .withSize(1, 1)
         .withPosition(1, 0);
     Shuffleboard.getTab("Pit")
@@ -450,7 +450,7 @@ public void configurePitDashboard() {
         .withSize(1, 1);
 
     Shuffleboard.getTab("Pit")
-        .add("Adjusted Climb Pos", new TrapClimbAdjustCommand(climbSubsystem))
+        .add("Adjusted Climb Pos", new TrapClimbCommand(climbSubsystem))
         .withPosition(8, 0)
         .withSize(1, 1);
     //     Shuffleboard.getTab("Pit")
@@ -972,7 +972,7 @@ private void configureDriverBindings() {
     // Vision Shoot
     new JoystickButton(driveJoystick, Button.M_SWH.id)
         .onTrue(
-            new MovingVisionShootCommand(
+            new VisionShootCommand(
                 robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
 
     // Release Game Piece Command

src/main/java/frc/robot/commands/climb/TrapClimbAdjustCommand.java → src/main/java/frc/robot/commands/climb/TrapClimbCommand.java

@@ -4,16 +4,16 @@
 import frc.robot.subsystems.climb.ClimbSubsystem;
 import frc.robot.subsystems.climb.ClimbSubsystem.ClimbStates;
 
-public class TrapClimbAdjustCommand extends Command {
+public class TrapClimbCommand extends Command {
   private ClimbSubsystem climbSubsystem;
 
-  public TrapClimbAdjustCommand(ClimbSubsystem climbSubsystem) {
+  public TrapClimbCommand(ClimbSubsystem climbSubsystem) {
     this.climbSubsystem = climbSubsystem;
   }
 
   @Override
   public void initialize() {
-    climbSubsystem.trapClimbAdjust();
+    climbSubsystem.trapClimb();
   }
 
   @Override

src/main/java/frc/robot/commands/robotState/ClimbCommand.java

@@ -22,7 +22,8 @@ public ClimbCommand(
 
   @Override
   public void initialize() {
-    if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED) {
+    if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED
+        || robotStateSubsystem.getState() == RobotStates.POST_CLIMB) {
       robotStateSubsystem.climb(false, false);
     } else {
       climbAllowed = false;

src/main/java/frc/robot/commands/robotState/ClimbTrapDecendCommand.java

@@ -21,7 +21,8 @@ public ClimbTrapDecendCommand(
 
   @Override
   public void initialize() {
-    if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED) {
+    if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED
+        || robotStateSubsystem.getState() == RobotStates.POST_CLIMB) {
       robotStateSubsystem.climb(true, true);
     } else {
       climbAllowed = false;

src/main/java/frc/robot/commands/robotState/FullTrapClimbCommand.java

@@ -21,7 +21,8 @@ public FullTrapClimbCommand(
 
   @Override
   public void initialize() {
-    if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED) {
+    if (robotStateSubsystem.getState() == RobotStates.CLIMB_PREPPED
+        || robotStateSubsystem.getState() == RobotStates.POST_CLIMB) {
       robotStateSubsystem.climb(true, false);
     } else {
       climbAllowed = false;

src/main/java/frc/robot/constants/AutonConstants.java

@@ -141,7 +141,7 @@ public final class Setpoints {
     public static final Pose2d MS1 = new Pose2d(4.3, 5.55, Rotation2d.fromDegrees(0.0));
     public static final Pose2d NAS1 = new Pose2d(4.2, 2.8, Rotation2d.fromDegrees(-33.2));
     public static final Pose2d NAS2 =
-        new Pose2d(4.4, 4.6, Rotation2d.fromDegrees(-12.2)); // 4,5.1, -6.7
+        new Pose2d(4.55, 4.6, Rotation2d.fromDegrees(-12.2)); // 4,5.1, -6.7
 
     // Path Midpoints
     public static final Pose2d MP1 = new Pose2d(5, 7.61, Rotation2d.fromDegrees(0.0));

src/main/java/frc/robot/constants/ClimbConstants.java

@@ -58,8 +58,8 @@ public final class ClimbConstants {
   public static final double kRightClimbTrapPos = 12.056; // 5.5
 
   // TRAP ADJUST
-  public static final double kLeftClimbTrapAdjustPos = kLeftClimbTrapPos + kRevsPerInch;
-  public static final double kRightClimbTrapAdjustPos = kLeftClimbTrapPos + kRevsPerInch;
+  public static final double kLeftClimbTrapAdjustPos = kLeftClimbTrapPos + kRevsPerInch * 2;
+  public static final double kRightClimbTrapAdjustPos = kLeftClimbTrapPos + kRevsPerInch * 2;
 
   public static final double kLeftStowPos = 2.5;
   public static final double kRightStowPos = 2.5;

src/main/java/frc/robot/constants/DriveConstants.java

@@ -119,8 +119,8 @@ public static TalonFXConfiguration getDriveTalonConfig() {
     TalonFXConfiguration driveConfig = new TalonFXConfiguration();
 
     CurrentLimitsConfigs currentConfig = new CurrentLimitsConfigs();
-    currentConfig.SupplyCurrentLimit = 50;
-    currentConfig.SupplyCurrentThreshold = 55;
+    currentConfig.SupplyCurrentLimit = 40;
+    currentConfig.SupplyCurrentThreshold = 45;
     currentConfig.SupplyTimeThreshold = 0.0;
     currentConfig.SupplyCurrentLimitEnable = true;
     currentConfig.StatorCurrentLimitEnable = false;

src/main/java/frc/robot/constants/LedConstants.java

@@ -11,8 +11,8 @@ public class LedConstants {
   public static final Color kGreen = new Color(0, 255, 0);
   public static final Color kYellowish = new Color(166, 255, 0);
 
-  public static final int kBlinkOffCount = 15;
-  public static final int kBlinkOnCount = 30; // > kBlinkOffCount
+  public static final int kBlinkOffCount = 7;
+  public static final int kBlinkOnCount = 14; // > kBlinkOffCount
   public static final int kLoopCounterCandy = 5;
 
   public static final Color[] candy = {

src/main/java/frc/robot/constants/MagazineConstants.java

@@ -16,7 +16,7 @@ public final class MagazineConstants {
   public static final int kMinBeamBreaks = 0; // 3
   public static final double kIntakingSpeed = -25; // -46
   public static final double kFastIntakingSpeed = -25; // -90
-  public static final double kEmptyingSpeed = -90; // -72
+  public static final double kEmptyingSpeed = -70; // -90
   public static final double kReversingSpeed = 4.8; // TODO do testing to determine correct speed
 
   public static final double kReleaseTime = 0.75;

src/main/java/frc/robot/constants/RobotConstants.java

@@ -37,7 +37,7 @@ public class RobotConstants {
   public static final Translation2d kBlueFeedTargetPos =
       new Translation2d(kBlueSpeakerXPos + 1, kBlueSpeakerYPos + 1);
 
-  public static final double kDegreeShootOffset = Units.degreesToRadians(-3.5);
+  public static final double kDegreeShootOffset = Units.degreesToRadians(-3.5); // -3.5
 
   // Robot Sizes
   public static final double kShooterOffset = 0.2; // meters

src/main/java/frc/robot/constants/RobotStateConstants.java

@@ -8,7 +8,7 @@ public final class RobotStateConstants {
   public static final double kLookupMaxDistance = 5.4; // 540cm
   public static final double kDistanceIncrement = 0.01; // 1cm
   public static final double kDistanceOffset = 0.00;
-  public static final double kElbowShootOffset = 0.0; // .5 .7
+  public static final double kElbowShootOffset = 0.00321; // .5 .7
   public static final String kElbowPreferencesKey = "Elbow/Offset";
   public static final double kLeftFeedLinearCoeff = 2.5;
   public static final double kRightFeedLinearCoeff = 0;

src/main/java/frc/robot/constants/SuperStructureConstants.java

@@ -56,7 +56,7 @@ public final class SuperStructureConstants {
   // SUBWOOFER
   public static final double kWristSubwooferSetPoint = kWristIntakeSetPoint;
   public static final double kElbowSubwooferSetPoint = kElbowIntakeSetPoint;
-  public static final double kShooterSubwooferSetPoint = 40; // 60
+  public static final double kShooterSubwooferSetPoint = 50; // 60
 
   // FEEDING
   public static final double kWristFeedingSetPoint = kWristIntakeSetPoint;

src/main/java/frc/robot/constants/VisionConstants.java

@@ -34,9 +34,9 @@ public final class VisionConstants {
   public static final double multiTagCoeff = 18.0 / 100.0;
   public static final double baseNumber = Math.E;
   public static final double powerNumber = 4.0;
-  public static final double FOV45MultiTagCoeff = 15.0 / 100.0;
-  public static final double FOV45powerNumber = 4.0;
-  public static final double FOV45SinlgeTagCoeff = 19.0 / 100.0;
+  public static final double FOV45MultiTagCoeff = 16.0 / 100.0;
+  public static final double FOV45powerNumber = 4.5;
+  public static final double FOV45SinlgeTagCoeff = 21.0 / 100.0;
   public static final double FOV58MJPGMultiTagCoeff = 16.0 / 100.0;
   public static final double FOV58MJPGPowerNumber = 3.5;
   public static final double FOV58MJPGSingleTagCoeff = 21.0 / 100.0;

src/main/java/frc/robot/subsystems/auto/AutoSwitch.java

@@ -289,6 +289,22 @@ private AutoCommandInterface getAutoCommand(int switchPos) {
             List.of(5, 4, 3),
             4.0,
             AutonConstants.Setpoints.NAS2);
+      case 0x25:
+        return new SmartNonAmpAutoCommand(
+            driveSubsystem,
+            robotStateSubsystem,
+            superStructure,
+            magazineSubsystem,
+            intakeSubsystem,
+            elbowSubsystem,
+            pathHandler,
+            deadeye,
+            ledSubsystem,
+            "NonAmpInitial1_MiddleNote3",
+            AutonConstants.kNonAmpPathMatrix,
+            List.of(3, 5, 4),
+            4.0,
+            AutonConstants.Setpoints.NAS2);
       case 0x30:
         return new DoNothingCommand(
             robotStateSubsystem, driveSubsystem, superStructure, magazineSubsystem, elbowSubsystem);

src/main/java/frc/robot/subsystems/robotState/RobotStateSubsystem.java

@@ -4,7 +4,6 @@
 import com.ctre.phoenix6.CANBus.CANBusStatus;
 import com.opencsv.CSVReader;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -306,6 +305,7 @@ public void startShootKnownPos(Pose2d pos) {
 
     getShootSolution(driveSubsystem.getDistanceToSpeaker(pos));
 
+    intakeSubsystem.toEjecting();
     magazineSubsystem.setSpeed(0.0);
     superStructure.shoot(shootSolution[0], shootSolution[1], shootSolution[2]);
     ledSubsystem.setOff();
@@ -340,6 +340,7 @@ public void startShoot() {
     shootKnownPos = false;
     movingShoot = false;
 
+    intakeSubsystem.toEjecting();
     driveSubsystem.setIsAligningShot(true);
 
     getShootSolution(driveSubsystem.getDistanceToSpeaker());
@@ -358,6 +359,7 @@ public void startShootDistance(double distance) {
 
     getShootSolution(distance);
 
+    intakeSubsystem.toEjecting();
     magazineSubsystem.setSpeed(0.0);
     superStructure.shoot(shootSolution[0], shootSolution[1], shootSolution[2]);
     ledSubsystem.setOff();
@@ -373,17 +375,22 @@ public void startMovingShoot() {
     driveSubsystem.setIsAligningShot(true);
     driveSubsystem.setIsMoveAndShoot(true);
 
+    intakeSubsystem.toEjecting();
     Translation2d virtualT = driveSubsystem.getPoseMeters().getTranslation();
     ChassisSpeeds speeds = driveSubsystem.getFieldRelSpeed();
-    getShootSolution(driveSubsystem.getDistanceToSpeaker(new Pose2d(virtualT, new Rotation2d())));
+    getShootSolution(
+        driveSubsystem.getDistanceToSpeaker(
+            new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation())));
 
     for (int i = 0; i < RobotStateConstants.kMoveWhileShootIterations; i++) {
       virtualT =
           virtualT.plus(
               new Translation2d(
                   speeds.vxMetersPerSecond * shootSolution[3],
                   speeds.vyMetersPerSecond * shootSolution[3]));
-      getShootSolution(driveSubsystem.getDistanceToSpeaker(new Pose2d(virtualT, new Rotation2d())));
+      getShootSolution(
+          driveSubsystem.getDistanceToSpeaker(
+              new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation())));
     }
     driveSubsystem.setMoveAndShootVirtualPose(
         new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation()));
@@ -751,7 +758,8 @@ public void periodic() {
         Translation2d virtualT = driveSubsystem.getPoseMeters().getTranslation();
         ChassisSpeeds speeds = driveSubsystem.getFieldRelSpeed();
         getShootSolution(
-            driveSubsystem.getDistanceToSpeaker(new Pose2d(virtualT, new Rotation2d())));
+            driveSubsystem.getDistanceToSpeaker(
+                new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation())));
 
         for (int i = 0; i < RobotStateConstants.kMoveWhileShootIterations; i++) {
           virtualT =
@@ -760,7 +768,8 @@ public void periodic() {
                       speeds.vxMetersPerSecond * shootSolution[3],
                       speeds.vyMetersPerSecond * shootSolution[3]));
           getShootSolution(
-              driveSubsystem.getDistanceToSpeaker(new Pose2d(virtualT, new Rotation2d())));
+              driveSubsystem.getDistanceToSpeaker(
+                  new Pose2d(virtualT, driveSubsystem.getPoseMeters().getRotation())));
         }
 
         superStructure.shoot(shootSolution[0], shootSolution[1], shootSolution[2]);

src/main/java/frc/robot/subsystems/shooter/ShooterIOFX.java

@@ -23,7 +23,7 @@ public class ShooterIOFX implements ShooterIO, Checkable {
 
   @HealthCheck
   @Timed(
-      percentOutput = {0.2, -0.2, 0.8, -0.8},
+      percentOutput = {0.2, -0.2, 0.8, 0.0, -0.8},
       duration = 3)
   private TalonFX shooterLeft;