Merge pull request #111 from strykeforce/4or5ThenWing3ThenSearch

4or5 then wing3 then search

src/main/deploy/paths/MiddleInitial1_WingNote1.toml

@@ -3,19 +3,19 @@ max_acceleration = 3.0 #m/s
 start_velocity = 0.0 #m/s
 end_velocity = 0.0 #m/s 
 is_reversed = false
-target_yaw = 26.6 #degrees - old 22.0
+target_yaw = 13.0 #degrees - old 26.6
 
 [start_pose]
     dataPoint = "MI1"
     dX = 0.0
-    dY = 0.0
-    angle = 0.0
+    dY = 0.25
+    angle = 30.0
 
 [end_pose]
     dataPoint "W1"
     dX = -0.1
-    dY = -0.35
-    angle = 26.6
+    dY = 0.05
+    angle = 13.0
 
 [[internal_points]]
     x = 1.9

src/main/deploy/paths/MiddleNote4_NonAmpShoot4.toml

@@ -0,0 +1,24 @@
+max_velocity = 5.0 #m/s
+max_acceleration = 3.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -34.439 #degrees
+
+[start_pose]
+    dataPoint = "M4"
+    dX = -0.3
+    dY = 0.55
+    angle = 180.0
+
+[end_pose]
+    dataPoint = "NAS4"
+    angle = 143.27
+
+[[internal_points]]
+    x = 5.56
+    y = 1.8
+
+[[internal_points]]
+    x = 2.483
+    y = 3.154

src/main/deploy/paths/MiddleNote5_NonAmpShoot4.toml

@@ -0,0 +1,22 @@
+max_velocity = 5.0 #m/s
+max_acceleration = 3.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = -34.439 #degrees
+
+[start_pose]
+    dataPoint = "M5"
+    dX = -0.5
+    dY = 0.35
+
+[end_pose]
+    dataPoint = "NAS4"
+
+[[internal_points]]
+    x = 5.021
+    y = 1.395
+
+[[internal_points]]
+    x = 2.483
+    y = 3.154

src/main/deploy/paths/MiddleShoot3_WingNote3.toml

@@ -11,7 +11,7 @@ target_yaw = -30.0 #degrees
 [end_pose]
     dataPoint = "W3"
     dX = 0.1
-    dY = 0.15
+    dY = 0.2
     angle = -30.0
 
 [[internal_points]]

src/main/deploy/paths/NonAmpInitial1_MiddleNote4.toml

@@ -10,8 +10,8 @@ target_yaw = 0.0 #degrees
 
 [end_pose]
     dataPoint = "M4"
-    dX = -0.5
-    dY = 0.75
+    dX = -0.3
+    dY = 0.55
     angle = 0.0
 
 

src/main/deploy/paths/NonAmpShoot4_MiddleNote4.toml

@@ -0,0 +1,22 @@
+max_velocity = 5.0 #m/s
+max_acceleration = 3.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "NAS4"
+    angle = -36.72
+
+[end_pose]
+    dataPoint = "M4"
+    angle = 0.0
+
+[[internal_points]]
+    x = 2.71
+    y = 2.8
+
+[[internal_points]]
+    x = 5.56
+    y = 2.0

src/main/deploy/paths/NonAmpShoot4_MiddleNote5.toml

@@ -0,0 +1,16 @@
+max_velocity = 5.0 #m/s
+max_acceleration = 3.0 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "NAS4"
+
+[end_pose]
+    dataPoint = "M5"
+
+[[internal_points]]
+    x = 5.021
+    y = 1.395

src/main/deploy/paths/NonampShoot4_WingNote3.toml

@@ -0,0 +1,19 @@
+max_velocity = 3.0 #m/s
+max_acceleration = 2.5 #m/s
+start_velocity = 0.0 #m/s
+end_velocity = 0.0 #m/s 
+is_reversed = false
+target_yaw = 0.0 #degrees
+
+[start_pose]
+    dataPoint = "NAS4"
+    angle = 0.0
+[end_pose]
+    dataPoint = "W3"
+    dX = -0.1
+    dY = 0.0
+    angle = 0.0
+
+[[internal_points]]
+    x = 2.2
+    y = 4.11

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

@@ -5,6 +5,7 @@
 package frc.robot;
 
 import ch.qos.logback.classic.util.ContextInitializer;
+import com.ctre.phoenix6.SignalLogger;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -147,6 +148,7 @@ public void autonomousExit() {
 
   @Override
   public void teleopInit() {
+    SignalLogger.start();
     m_robotContainer.enableDeadeye();
     logger.info("Teleop Init | Match #{}", DriverStation.getMatchNumber());
     m_robotContainer.setIsAuto(false);

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

@@ -113,6 +113,7 @@
 import frc.robot.subsystems.magazine.MagazineSubsystem;
 import frc.robot.subsystems.pathHandler.PathHandler;
 import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.FeedMode;
 import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
 import frc.robot.subsystems.shooter.ShooterIOFX;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
@@ -327,7 +328,7 @@ public RobotContainer() {
     configureTuningDashboard();
     robotStateSubsystem.setAllianceColor(Alliance.Blue);
 
-    RobotController.setBrownoutVoltage(6.3);
+    RobotController.setBrownoutVoltage(5.75); // 6.3, want test 5.75
 
     // configureTelemetry();
     // configurePitDashboard();
@@ -641,6 +642,13 @@ private void configureMatchDashboard() {
         .withPosition(5, 2)
         .withSize(1, 1);
 
+    allianceColor =
+        Shuffleboard.getTab("Match")
+            .addBoolean(
+                "Middle Pass Mode?", () -> robotStateSubsystem.getFeedMode() == FeedMode.MIDDLE)
+            .withSize(1, 1)
+            .withPosition(5, 3);
+
     allianceColor =
         Shuffleboard.getTab("Match")
             .addBoolean("AllianceColor", () -> alliance != Alliance.Blue)
@@ -848,7 +856,9 @@ public void setAllianceColor(Alliance alliance) {
     allianceColor.withProperties(Map.of("colorWhenTrue", "red", "colorWhenFalse", "blue"));
     robotStateSubsystem.setAllianceColor(alliance);
 
+    driveSubsystem.setRecordTrajectoryAllowed(true);
     autoSwitch.getAutoCommand().generateTrajectory();
+    driveSubsystem.setRecordTrajectoryAllowed(false);
 
     // Flips gyro angle if alliance is red team
     if (robotStateSubsystem.getAllianceColor() == Alliance.Red) {
@@ -907,7 +917,9 @@ private void configureOperatorBindings() {
 
     // UnJam Note
     new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
-        .onTrue(new UnJamUpperNoteCommand(magazineSubsystem));
+        .onTrue(
+            new UnJamUpperNoteCommand(magazineSubsystem, intakeSubsystem, robotStateSubsystem)
+                .withTimeout(0.5));
 
     // Open Loop Elbow
     // new Trigger((() -> xboxController.getRightY() > RobotConstants.kJoystickDeadband))

src/main/java/frc/robot/commands/auton/DeadeyeHuntCommand.java

@@ -48,22 +48,21 @@ public DeadeyeHuntCommand(
 
   @Override
   public void initialize() {
+    driveSubsystem.setAutoDebugMsg("Initialize DeadEYE Hunt - Rotating");
     ledSubsystem.setColor(120, 38, 109);
 
     driveSubsystem.setIsAligningShot(false);
+
+    driveSubsystem.move(
+        0.0,
+        0.0,
+        robotStateSubsystem.getAllianceColor() == Alliance.Blue
+            ? AutonConstants.kDeadeyeHuntOmegaRadps
+            : -AutonConstants.kDeadeyeHuntOmegaRadps,
+        false);
+
     if (deadeye.getNumTargets() > 0) {
-      double ySpeed = deadeyeYDrive.calculate(deadeye.getDistanceToCamCenter(), 0.0);
-      double xSpeed = deadeyeXDrive.calculate(deadeye.getDistanceToCamCenter(), 0.0);
-      driveSubsystem.move(AutonConstants.kXSpeed / (xSpeed * xSpeed + 1), ySpeed, 0.0, false);
       seenNote++;
-    } else {
-      driveSubsystem.move(
-          0.0,
-          0.0,
-          robotStateSubsystem.getAllianceColor() == Alliance.Blue
-              ? AutonConstants.kDeadeyeHuntOmegaRadps
-              : -AutonConstants.kDeadeyeHuntOmegaRadps,
-          false);
     }
   }
 
@@ -75,6 +74,7 @@ public void execute() {
           seenNote++;
           if (seenNote >= AutonConstants.kFoundNoteLoopCounts) {
             huntState = HuntState.DRIVING;
+            driveSubsystem.setAutoDebugMsg("Stop rotating, Note Found!");
           }
         } else {
           driveSubsystem.move(
@@ -101,6 +101,7 @@ public void execute() {
   @Override
   public void end(boolean interrupted) {
     driveSubsystem.move(0, 0, 0, false);
+    driveSubsystem.setAutoDebugMsg("End DeadEYE Hunt");
   }
 
   @Override

src/main/java/frc/robot/commands/auton/DeadeyeHuntRotateCommand.java

@@ -0,0 +1,122 @@
+package frc.robot.commands.auton;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.AutonConstants;
+import frc.robot.subsystems.auto.AutoCommandInterface;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.led.LedSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.vision.DeadEyeSubsystem;
+import net.jafama.FastMath;
+
+public class DeadeyeHuntRotateCommand extends Command implements AutoCommandInterface {
+  private DeadEyeSubsystem deadeye;
+  private DriveSubsystem driveSubsystem;
+  private RobotStateSubsystem robotStateSubsystem;
+  private ProfiledPIDController deadeyeYDrive;
+  private PIDController deadeyeXDrive;
+  private LedSubsystem ledSubsystem;
+  private double turnTo;
+  private double turnToInverted;
+  private double deltaTurn;
+  private boolean reachedEnd = false;
+  private boolean generated = false;
+
+  public DeadeyeHuntRotateCommand(
+      DeadEyeSubsystem deadeye,
+      DriveSubsystem driveSubsystem,
+      RobotStateSubsystem robotStateSubsystem,
+      LedSubsystem ledSubsystem,
+      double turnTo) {
+    addRequirements(driveSubsystem);
+    this.deadeye = deadeye;
+    this.driveSubsystem = driveSubsystem;
+    this.robotStateSubsystem = robotStateSubsystem;
+    this.ledSubsystem = ledSubsystem;
+    this.turnTo = turnTo;
+    deadeyeYDrive =
+        new ProfiledPIDController(
+            AutonConstants.kPDeadEyeYDrive,
+            AutonConstants.kIDeadEyeYDrive,
+            AutonConstants.kDDeadEyeYDrive,
+            new Constraints(
+                AutonConstants.kMaxVelDeadeyeDrive, AutonConstants.kMaxAccelDeadeyeDrive));
+    deadeyeXDrive =
+        new PIDController(
+            AutonConstants.kPDeadEyeXDrive,
+            AutonConstants.kIDeadEyeXDrive,
+            AutonConstants.kDDeadEyeXDrive);
+  }
+
+  @Override
+  public void initialize() {
+    driveSubsystem.setAutoDebugMsg("DeadEYE Hunt Initialized");
+    deltaTurn =
+        turnToInverted - getCorrectRange() >= 0
+            ? AutonConstants.kHuntTurnSpeed
+            : -AutonConstants.kHuntTurnSpeed;
+    ledSubsystem.setColor(120, 38, 109);
+
+    driveSubsystem.setIsAligningShot(false);
+    if (deadeye.getNumTargets() > 0) {
+      double ySpeed = deadeyeYDrive.calculate(deadeye.getDistanceToCamCenter(), 0.0);
+      double xSpeed = deadeyeXDrive.calculate(deadeye.getDistanceToCamCenter(), 0.0);
+      driveSubsystem.move(AutonConstants.kXSpeed / (xSpeed * xSpeed + 1), ySpeed, 0.0, false);
+    } else if (deltaTurn > 0 && turnToInverted < getCorrectRange()
+        || deltaTurn < 0 && turnToInverted > getCorrectRange()) {
+      reachedEnd = true;
+    } else {
+      driveSubsystem.move(0.0, 0.0, deltaTurn, false);
+    }
+  }
+
+  @Override
+  public void execute() {
+    if (deadeye.getNumTargets() > 0) {
+      double ySpeed = deadeyeYDrive.calculate(deadeye.getDistanceToCamCenter(), 0.0);
+      double xSpeed = deadeyeXDrive.calculate(deadeye.getDistanceToCamCenter(), 0.0);
+      driveSubsystem.recordYVel(ySpeed);
+      driveSubsystem.move(AutonConstants.kXSpeed / (xSpeed * xSpeed + 1), ySpeed, 0.0, false);
+    } else if (deltaTurn > 0 && turnToInverted < getCorrectRange()
+        || deltaTurn < 0 && turnToInverted > getCorrectRange()) {
+      reachedEnd = true;
+    } else {
+      driveSubsystem.move(0.0, 0.0, deltaTurn, false);
+    }
+  }
+
+  @Override
+  public void end(boolean interrupted) {
+    driveSubsystem.setAutoDebugMsg("Finished Deadeye Hunt");
+  }
+
+  @Override
+  public boolean isFinished() {
+    return robotStateSubsystem.hasNote();
+  }
+
+  @Override
+  public void generateTrajectory() {
+    turnToInverted =
+        robotStateSubsystem.getAllianceColor() == Alliance.Blue
+            ? FastMath.normalizeZeroTwoPi(turnTo)
+            : Math.PI - FastMath.normalizeZeroTwoPi(turnTo);
+
+    generated = true;
+  }
+
+  @Override
+  public boolean hasGenerated() {
+    return generated;
+  }
+
+  public double getCorrectRange() {
+    return robotStateSubsystem.getAllianceColor() == Alliance.Blue
+        ? FastMath.normalizeZeroTwoPi(driveSubsystem.getGyroRotation2d().getRadians())
+        : driveSubsystem.getGyroRotation2d().getRadians();
+  }
+}