Working

Author: ds12a

src/main/deploy/paths/AmpInitial3_MiddleNote1_Part1.toml

@@ -1,5 +1,5 @@
 max_velocity = 2.0 #m/s
-max_acceleration = 2.5 #m/s
+max_acceleration = 1.0 #m/s
 start_velocity = 0.0 #m/s
 end_velocity = 2.0 #m/s 
 is_reversed = false
@@ -14,4 +14,4 @@ target_yaw = 90.0 #degrees
 
 [[internal_points]]
     x = 1.93 #1.93
-    y = 7.66
+    y = 7.60 # 7.66

src/main/deploy/paths/MiddleInitial1_WingNote2.toml

@@ -11,6 +11,7 @@ target_yaw = 0.0 #degrees
     angle = 0.0
 [end_pose]
     dataPoint "W2"
+    dX = 0.15
 
 [[internal_points]]
     x = 2.0

src/main/deploy/paths/MiddleInitial1_WingNote3.toml

@@ -10,8 +10,8 @@ target_yaw = -30.0 #degrees
     angle = 0.0
 [end_pose]
     dataPoint "W3"
-    dX = -0.1
-    dY = 0.25
+    dX = 0.0
+    dY = 0.1
     angle = -30.0
 
 [[internal_points]]

src/main/deploy/paths/MiddleNote3_MiddleShoot3.toml

@@ -23,8 +23,8 @@ target_yaw = 0.0 #degrees
 
 [[internal_points]]
     x = 4.0
-    y = 5.0
+    y = 5.05
 
 [[internal_points]]
     x = 3.007
-    y = 5.113
+    y = 5.163

src/main/deploy/paths/MiddleShoot3_WingNote3.toml

@@ -10,8 +10,8 @@ target_yaw = -30.0 #degrees
     angle = -30.0
 [end_pose]
     dataPoint = "W3"
-    dX = -0.1
-    dY = 0.0
+    dX = 0.1
+    dY = 0.15
     angle = -30.0
 
 [[internal_points]]

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

@@ -103,6 +103,7 @@ public MiddleNote3AndWingNotesCommand(
                 new AutoWaitNoteStagedCommand(robotStateSubsystem),
                 new PrepShooterCommand(
                     superStructure, robotStateSubsystem, AutonConstants.Setpoints.MS3))),
+        new WaitCommand(0.02),
         new SubWooferCommand(robotStateSubsystem, superStructure, magazineSubsystem),
         middleShoot3WingNote3,
         new ParallelCommandGroup(
@@ -118,14 +119,14 @@ 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));
+
+    // Backup
+    // new DeadeyeHuntCommand(deadeye, driveSubsystem, robotStateSubsystem, ledSubsystem),
+    // new AutoWaitNoteStagedCommand(robotStateSubsystem),
+    // new VisionShootCommand(
+    //     robotStateSubsystem, superStructure, magazineSubsystem, intakeSubsystem));
   }
 
   public void generateTrajectory() {

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

@@ -103,7 +103,8 @@ public void execute() {
         case NORM_DRIVE:
           driveSubsystem.calculateController(desiredState, robotHeading);
           if (deadeye.getNumTargets() > 0
-              && timer.hasElapsed(trajectory.getTotalTimeSeconds() * AutonConstants.kPercentLeft)
+              && timer.hasElapsed(
+                  trajectory.getTotalTimeSeconds() * AutonConstants.kWingPercentLeft)
               && ((robotStateSubsystem.getAllianceColor() == Alliance.Blue
                       && curX >= AutonConstants.kWingSwitchXLine)
                   || (robotStateSubsystem.getAllianceColor() == Alliance.Red

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

@@ -20,6 +20,7 @@ public final class AutonConstants {
   public static final double kSwitchXLine = 6.5;
   public static final double kWingSwitchXLine = 1.4;
   public static final double kPercentLeft = 0.65;
+  public static final double kWingPercentLeft = 0.5;
   public static final double kDeadeyeHuntOmegaRadps = -1.5;
   public static final double kFoundNoteLoopCounts = 3;
 
@@ -163,7 +164,7 @@ public final class Setpoints {
         new Pose2d(3.052, 3.063, Rotation2d.fromDegrees(-47.39)); // Disrupt auto
 
     // Path Midpoints
-    public static final Pose2d MP1 = new Pose2d(2.5, 7.67, Rotation2d.fromDegrees(0.0)); // 5,7.61
+    public static final Pose2d MP1 = new Pose2d(2.7, 7.67, Rotation2d.fromDegrees(0.0)); // 5,7.61
     public static final Pose2d MP2 = new Pose2d(3.89, 7.2, Rotation2d.fromDegrees(-90)); // AS4
   }
 

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

@@ -27,7 +27,7 @@ public final class MagazineConstants {
   public static final double kTrapReleaseTime = 0.75;
   public static final double kAmpReleaseSpeed = 40.0; // 20.0
   public static final double kPodiumRumbleSpeed = 80;
-  public static final double kFastEmptyingSpeed = -75;
+  public static final double kFastEmptyingSpeed = -55; // -75
 
   public static final TalonFXConfiguration getMagazineConfig() {
     TalonFXConfiguration config = new TalonFXConfiguration();

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

@@ -107,6 +107,6 @@ public final class SuperStructureConstants {
   // Auto Ignore Note
   public static final double kElbowIgnoreNoteSetPoint = kElbowSubwooferSetPoint;
   public static final double kBlueLeftShooterIgnoreNoteSetPoint = 10;
-  public static final double kBlueRightShooterIgnoreNoteSetPoint = 60; // 50
+  public static final double kBlueRightShooterIgnoreNoteSetPoint = 50;
   public static final double kWristIgnoreNoteSetPoint = kWristSubwooferSetPoint;
 }