Hunt

Author: ds12a

src/main/deploy/paths/MiddleInitial1_MiddleNote3.toml

@@ -1,5 +1,5 @@
 max_velocity = 5.0 #m/s
-max_acceleration = 4.7 #m/s
+max_acceleration = 4.5 #m/s
 start_velocity = 0.0 #m/s
 end_velocity = 0.0 #m/s 
 is_reversed = false
@@ -10,13 +10,13 @@ target_yaw = 0.0 #degrees
 
 [end_pose]
     dataPoint = "M3"
-    dX = -0.1
+    dX = -0.2
     dY = 0.0
 
 # W2
 [[internal_points]]
-    x = 2.94 
-    y = 5.55
+    x = 3.02 
+    y = 5.60 # 5.55
 
 [[internal_points]]
     x = 5.221

src/main/deploy/paths/MiddleInitial1_WingNote1.toml

@@ -19,4 +19,4 @@ target_yaw = 26.6 #degrees - old 22.0
 
 [[internal_points]]
     x = 1.9
-    y = 6.46
+    y = 6.3

src/main/deploy/paths/MiddleNote3_MiddleShoot3.toml

@@ -7,15 +7,15 @@ target_yaw = 0.0 #degrees
 
 [start_pose]
     dataPoint = "M3"
-    dX = 0.1
+    dX = 0.0
     dY = 0.0
     angle = 180.0
 
 [end_pose]
     dataPoint = "MS3"
-    dX = 0.9
+    dX = 0.45
     dY = 0.0
-    angle = 180.0 # 200.0
+    angle = 180.0
 
 [[internal_points]]
     x = 5.364 # 5.221

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.05
+    dY = 0.0
     angle = -30.0
 
 [[internal_points]]

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

@@ -17,6 +17,8 @@ public class DeadeyeHuntCommand extends Command {
   private ProfiledPIDController deadeyeYDrive;
   private PIDController deadeyeXDrive;
   private LedSubsystem ledSubsystem;
+  private HuntState huntState;
+  private int seenNote = 0;
 
   public DeadeyeHuntCommand(
       DeadEyeSubsystem deadeye,
@@ -34,12 +36,13 @@ public DeadeyeHuntCommand(
             AutonConstants.kIDeadEyeYDrive,
             AutonConstants.kDDeadEyeYDrive,
             new Constraints(
-                AutonConstants.kMaxVelDeadeyeDrive, AutonConstants.kMaxAccelDeadeyeDrive));
+                AutonConstants.kMaxAutonVelDeadeyeDrive, AutonConstants.kMaxAccelDeadeyeDrive));
     deadeyeXDrive =
         new PIDController(
             AutonConstants.kPDeadEyeXDrive,
             AutonConstants.kIDeadEyeXDrive,
             AutonConstants.kDDeadEyeXDrive);
+    huntState = HuntState.SEARCHING;
   }
 
   @Override
@@ -51,30 +54,49 @@ public void initialize() {
       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, -4.0, false);
+      driveSubsystem.move(0.0, 0.0, AutonConstants.kDeadeyeHuntOmegaRadps, 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 {
-      driveSubsystem.move(0.0, 0.0, -4.0, false);
+    switch (huntState) {
+      case SEARCHING:
+        if (deadeye.getNumTargets() > 0) {
+          seenNote++;
+          if (seenNote >= AutonConstants.kFoundNoteLoopCounts) {
+            huntState = HuntState.DRIVING;
+          }
+        } else {
+          driveSubsystem.move(0.0, 0.0, AutonConstants.kDeadeyeHuntOmegaRadps, false);
+          seenNote = 0;
+        }
+        break;
+      case DRIVING:
+        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);
+        }
+        break;
     }
   }
 
   @Override
   public void end(boolean interrupted) {
-    driveSubsystem.move(0, 0, 0, true);
+    driveSubsystem.move(0, 0, 0, false);
   }
 
   @Override
   public boolean isFinished() {
     return robotStateSubsystem.hasNote();
   }
+
+  private enum HuntState {
+    SEARCHING,
+    DRIVING
+  }
 }

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

@@ -94,7 +94,7 @@ public MiddleNote3AndWingNotesCommand(
             new SequentialCommandGroup(
                 new IgnoreNotesCommand(
                     robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem),
-                new WaitCommand(1.5),
+                new WaitCommand(2.0),
                 new IntakeCommand(
                     robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem))),
         new ParallelCommandGroup(
@@ -118,6 +118,12 @@ public MiddleNote3AndWingNotesCommand(
             robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem),
         new DeadeyeHuntCommand(deadeye, driveSubsystem, robotStateSubsystem, ledSubsystem),
         new AutoWaitNoteStagedCommand(robotStateSubsystem),
+        new VisionShootCommand(
+            robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem),
+
+        // Backup
+        new DeadeyeHuntCommand(deadeye, driveSubsystem, robotStateSubsystem, ledSubsystem),
+        new AutoWaitNoteStagedCommand(robotStateSubsystem),
         new VisionShootCommand(
             robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
   }

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

@@ -133,6 +133,7 @@ public boolean isFinished() {
   public void end(boolean interrupted) {
     driveSubsystem.setEnableHolo(false);
     driveSubsystem.setDeadEyeDrive(false);
+    driveSubsystem.recordAutoTrajectory(null);
 
     if (!lastPath) {
       driveSubsystem.calculateController(

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

@@ -126,14 +126,14 @@ public void execute() {
 
   @Override
   public boolean isFinished() {
-    return timer.hasElapsed(trajectory.getTotalTimeSeconds())
-        || robotStateSubsystem.intakeHasNote();
+    return timer.hasElapsed(trajectory.getTotalTimeSeconds());
   }
 
   @Override
   public void end(boolean interrupted) {
     driveSubsystem.setEnableHolo(false);
     driveSubsystem.setDeadEyeDrive(false);
+    driveSubsystem.recordAutoTrajectory(null);
 
     if (!lastPath) {
       driveSubsystem.calculateController(

src/main/java/frc/robot/commands/drive/DriveAutonCommand.java

@@ -84,6 +84,7 @@ public boolean isFinished() {
   @Override
   public void end(boolean interrupted) {
     driveSubsystem.setEnableHolo(false);
+    driveSubsystem.recordAutoTrajectory(null);
 
     if (!lastPath) {
       driveSubsystem.calculateController(

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

@@ -13,12 +13,15 @@ public final class AutonConstants {
   public static final double kIDeadEyeXDrive = 0.0;
   public static final double kDDeadEyeXDrive = 0.0;
   public static final double kMaxVelDeadeyeDrive = 4.0;
+  public static final double kMaxAutonVelDeadeyeDrive = 2.0;
   public static final double kMaxAccelDeadeyeDrive = 3.5;
   public static final double kXSpeed = 2.0;
   public static final double kMaxXOff = 1.0;
   public static final double kSwitchXLine = 6.5;
-  public static final double kWingSwitchXLine = 2.0;
+  public static final double kWingSwitchXLine = 1.4;
   public static final double kPercentLeft = 0.65;
+  public static final double kDeadeyeHuntOmegaRadps = -1.5;
+  public static final double kFoundNoteLoopCounts = 3;
 
   public static final int kSwitchStableCounts = 100;