Merge branch 'Pre-MOStateChamps' of https://github.com/FRCTeam1987/Ro…

…bot2024 into Pre-MOStateChamps
FRCTeam1987 · May 31, 2024 · e8dbfe1 · e8dbfe1
2 parents 1d36b14 + 671f901
commit e8dbfe1
Show file tree

Hide file tree

Showing 6 changed files with 74 additions and 45 deletions.
diff --git a/src/main/deploy/pathplanner/autos/Middle Split 2.auto b/src/main/deploy/pathplanner/autos/Middle Split 2.auto
@@ -76,7 +76,7 @@
                     {
                       "type": "named",
                       "data": {
-                        "name": "InstantShoot"
+                        "name": "AutoAimAndShoot"
                       }
                     }
                   ]

diff --git a/src/main/deploy/pathplanner/paths/Middle Split 2 A.path b/src/main/deploy/pathplanner/paths/Middle Split 2 A.path
@@ -17,47 +17,47 @@
     {
       "anchor": {
         "x": 2.9,
-        "y": 6.23
+        "y": 6.25
       },
       "prevControl": {
         "x": 2.05,
-        "y": 6.23
+        "y": 6.25
       },
       "nextControl": {
-        "x": 5.9,
-        "y": 6.23
+        "x": 3.9,
+        "y": 6.25
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 8.05,
-        "y": 7.33
+        "x": 5.836521079212586,
+        "y": 6.25
       },
       "prevControl": {
-        "x": 8.050493794188853,
-        "y": 8.179999856568987
+        "x": 4.868260539606293,
+        "y": 6.340625
       },
       "nextControl": {
-        "x": 8.049709532830086,
-        "y": 6.830000084371184
+        "x": 6.804781618818879,
+        "y": 6.159375
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 8.0,
-        "y": 5.95
+        "x": 8.023042158425174,
+        "y": 5.6
       },
       "prevControl": {
-        "x": 8.0,
-        "y": 6.45
+        "x": 8.023042158425174,
+        "y": 5.85
       },
       "nextControl": {
-        "x": 8.0,
-        "y": 5.05
+        "x": 8.023042158425174,
+        "y": 5.35
       },
       "isLocked": false,
       "linkedName": null
@@ -114,15 +114,10 @@
       "rotationDegrees": 0,
       "rotateFast": false
     },
-    {
-      "waypointRelativePos": 2.0,
-      "rotationDegrees": -8.841524793534786,
-      "rotateFast": false
-    },
     {
       "waypointRelativePos": 3.0,
-      "rotationDegrees": -60.0,
-      "rotateFast": true
+      "rotationDegrees": 0.0,
+      "rotateFast": false
     },
     {
       "waypointRelativePos": 5.0,
@@ -133,6 +128,11 @@
       "waypointRelativePos": 4.0,
       "rotationDegrees": 0.0,
       "rotateFast": false
+    },
+    {
+      "waypointRelativePos": 2,
+      "rotationDegrees": 0,
+      "rotateFast": false
     }
   ],
   "constraintZones": [
@@ -149,8 +149,8 @@
     },
     {
       "name": "Slow Poop",
-      "minWaypointRelativePos": 1.9,
-      "maxWaypointRelativePos": 2.8499999999999996,
+      "minWaypointRelativePos": 2.8,
+      "maxWaypointRelativePos": 3.45,
       "constraints": {
         "maxVelocity": 1.5,
         "maxAcceleration": 4.25,
@@ -196,7 +196,7 @@
     },
     {
       "name": "Poop Stop",
-      "waypointRelativePos": 2.55,
+      "waypointRelativePos": 1.4,
       "command": {
         "type": "parallel",
         "data": {
@@ -213,7 +213,7 @@
     },
     {
       "name": "Collect",
-      "waypointRelativePos": 2.65,
+      "waypointRelativePos": 2.45,
       "command": {
         "type": "parallel",
         "data": {
@@ -247,7 +247,7 @@
     },
     {
       "name": "Shoot Prep",
-      "waypointRelativePos": 3.5,
+      "waypointRelativePos": 3.75,
       "command": {
         "type": "parallel",
         "data": {
@@ -264,7 +264,7 @@
     },
     {
       "name": "Enable LL Update",
-      "waypointRelativePos": 1.65,
+      "waypointRelativePos": 1.85,
       "command": {
         "type": "parallel",
         "data": {

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -357,6 +358,20 @@ private void configureCoDriverController() {
             new ParallelDeadlineGroup(
                 new IntakeNoteSequence(SHOOTER, INTAKE, WRIST, ELEVATOR),
                 new DriveToNoteAuto(DRIVETRAIN, INTAKE_PHOTON, SHOOTER, INTAKE, WRIST, ELEVATOR)));
+
+    CO_DRIVER_CONTROLLER
+        .leftStick()
+        .onTrue(
+            new InstantCommand(
+                () -> {
+                  if (CommandSwerveDrivetrain.getAlliance() == Alliance.Blue) {
+                    DRIVETRAIN.seedFieldRelative(
+                        new Pose2d(1.37, 5.52, Rotation2d.fromDegrees(0.0)));
+                  } else {
+                    DRIVETRAIN.seedFieldRelative(
+                        new Pose2d(15.2, 5.5, Rotation2d.fromDegrees(-180)));
+                  }
+                }));
   }
 
   private void configureDrivetrain() {
@@ -497,6 +512,13 @@ public void configureShuffleboard() {
                     DRIVETRAIN.seedFieldRelative(
                         new Pose2d(15.2, 5.5, Rotation2d.fromDegrees(-180))))
             .ignoringDisable(true));
+    COMMANDS_TAB.add(
+        "SetPoseBlueSubStart",
+        new InstantCommand(
+                () ->
+                    DRIVETRAIN.seedFieldRelative(
+                        new Pose2d(1.37, 5.52, Rotation2d.fromDegrees(0.0))))
+            .ignoringDisable(true));
 
     AUTO_CHOOSER.addOption(
         "SYSID-QUAS-F", DRIVETRAIN.sysIdQuasistatic(SysIdRoutine.Direction.kForward));

diff --git a/src/main/java/frc/robot/commands/control/auto/Madtown.java b/src/main/java/frc/robot/commands/control/auto/Madtown.java
@@ -31,13 +31,18 @@ public Madtown() {
             new ParallelRaceGroup(
                 AutoBuilder.buildAuto("Madtown Initial"),
                 new WaitUntilCommand(
-                    () -> { 
-                      final boolean shouldInterrupt = SHOULD_WATCH_FOR_NOTE && RobotContainer.INTAKE_PHOTON.hasTargets() && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 10.0;
+                    () -> {
+                      final boolean shouldInterrupt =
+                          SHOULD_WATCH_FOR_NOTE
+                              && RobotContainer.INTAKE_PHOTON.hasTargets()
+                              && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 20.0;
                       INITIAL_WAS_INTERRUPTED = shouldInterrupt;
                       return shouldInterrupt;
                     })),
             new ConditionalCommand(
-                new AutoCollectNote(2.75)  // TODO if this fails near note 4, then try for note 5, probably won't have time in 15 seconds
+                new AutoCollectNote(
+                        2.75) // TODO if this fails near note 4, then try for note 5, probably won't
+                    // have time in 15 seconds
                     .withTimeout(1.0)
                     .andThen(Util.PathFindToAutoSourceCloseShot())
                     .andThen(
@@ -48,10 +53,13 @@ public Madtown() {
             new AutoCollectNote(2.5),
             Util.PathFindToAutoMadtownShot(),
             new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER),
+            // new InstantShoot(RobotContainer.SHOOTER),
             new RotateUntilNote(false),
             new AutoCollectNote(2.75),
             Util.PathFindToAutoMadtownShot(),
-            new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER)),
+            new AutoAimAndShoot(RobotContainer.DRIVETRAIN, RobotContainer.SHOOTER)
+            // new InstantShoot(RobotContainer.SHOOTER)
+            ),
         new AutoAimLockWrist(RobotContainer.WRIST, RobotContainer::getAutoState),
         new AutoIdleShooter(RobotContainer.SHOOTER, RobotContainer::getAutoState));
   }

diff --git a/src/main/java/frc/robot/commands/control/auto/RotateUntilNote.java b/src/main/java/frc/robot/commands/control/auto/RotateUntilNote.java
@@ -5,9 +5,6 @@
 package frc.robot.commands.control.auto;
 
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
-
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotContainer;
@@ -33,7 +30,7 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    final double velocityScale = RobotContainer.INTAKE_PHOTON.hasTargets() ? 0.5 : 1.0;
+    final double velocityScale = RobotContainer.INTAKE_PHOTON.hasTargets() ? 0.5 : 1.5;
     RobotContainer.DRIVETRAIN.setControl(
         swerveRequest.withSpeeds(
             new ChassisSpeeds(0.0, 0.0, (Math.PI * velocityScale) * shouldTurnClockwise)));
@@ -48,6 +45,7 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return RobotContainer.INTAKE_PHOTON.hasTargets() && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 7.5;
+    return RobotContainer.INTAKE_PHOTON.hasTargets()
+        && Math.abs(RobotContainer.INTAKE_PHOTON.getYawVal()) < 7.5;
   }
 }
diff --git a/src/main/java/frc/robot/util/Util.java b/src/main/java/frc/robot/util/Util.java
@@ -121,12 +121,13 @@ public static double getDistanceToAllianceLob(Pose2d opose) {
   //     BLUE_AUTO_SOURCE_SHOOTING_POSE.getRotation().plus(Rotation2d.fromDegrees(180.0)));
   //     // new Pose2d(13.04, 3.22, Rotation2d.fromDegrees(-150));
 
-
-  public static final Pose2d BLUE_AUTO_SOURCE_CLOSE_SHOT = new Pose2d(3.7, 3.15, Rotation2d.fromDegrees(-30));
-  public static final Pose2d RED_AUTO_SOURCE_CLOSE_SHOT = new Pose2d(
-      16.56 - BLUE_AUTO_SOURCE_CLOSE_SHOT.getX(),
-      BLUE_AUTO_SOURCE_CLOSE_SHOT.getY(),
-      Rotation2d.fromDegrees(-150));
+  public static final Pose2d BLUE_AUTO_SOURCE_CLOSE_SHOT =
+      new Pose2d(3.7, 3.15, Rotation2d.fromDegrees(-30));
+  public static final Pose2d RED_AUTO_SOURCE_CLOSE_SHOT =
+      new Pose2d(
+          16.56 - BLUE_AUTO_SOURCE_CLOSE_SHOT.getX(),
+          BLUE_AUTO_SOURCE_CLOSE_SHOT.getY(),
+          Rotation2d.fromDegrees(-150));
 
   public static Command PathFindToAutoSourceCloseShot() {
     return new InstantCommand(() -> RobotContainer.setAutoState(AutoState.SHOOT_PREP))