Changes from Battlecry (#109)

* Move CBA closer to stage * Add HasData for each camera * Push back top ferry * Adjust D to Ferry * Adjust E to Ferry * Limit acceleration on NTF Start To D * Add withMaxSpeed to SwerveDriveToPose * Add max speed, increase timeout in AmpScoreRoutine * Increase ferrying shooter speeds --------- Co-authored-by: Prog694 <[email protected]>
StuyPulse · Jun 7, 2024 · a42e5bb · a42e5bb
1 parent 1eae6ec
commit a42e5bb
Show file tree

Hide file tree

Showing 14 changed files with 84 additions and 55 deletions.
diff --git a/src/main/deploy/pathplanner/paths/A to D.path b/src/main/deploy/pathplanner/paths/A to D.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.9000000000000004,
-        "y": 6.887856966597124
+        "x": 2.883271729453586,
+        "y": 6.722020177278586
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.890268068741571,
-        "y": 7.027030067557189
+        "x": 3.8735397981951567,
+        "y": 6.861193278238651
       },
       "isLocked": false,
       "linkedName": "A"

diff --git a/src/main/deploy/pathplanner/paths/A to E.path b/src/main/deploy/pathplanner/paths/A to E.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.9000000000000004,
-        "y": 6.887856966597124
+        "x": 2.883271729453586,
+        "y": 6.722020177278586
       },
       "prevControl": null,
       "nextControl": {
-        "x": 5.01984991243836,
-        "y": 6.971014628418452
+        "x": 5.003121641891946,
+        "y": 6.805177839099914
       },
       "isLocked": false,
       "linkedName": "A"

diff --git a/src/main/deploy/pathplanner/paths/B to A.path b/src/main/deploy/pathplanner/paths/B to A.path
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.9,
-        "y": 5.27
+        "x": 2.883271729453586,
+        "y": 5.059081387777612
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.7306942929129034,
-        "y": 5.892387378210238
+        "x": 2.7139660223664896,
+        "y": 5.68146876598785
       },
       "isLocked": false,
       "linkedName": "B"
     },
     {
       "anchor": {
-        "x": 2.9000000000000004,
-        "y": 6.887856966597124
+        "x": 2.883271729453586,
+        "y": 6.722020177278586
       },
       "prevControl": {
-        "x": 2.7469122475281997,
-        "y": 6.452168507627614
+        "x": 2.7301839769817855,
+        "y": 6.286331718309076
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/B to F.path b/src/main/deploy/pathplanner/paths/B to F.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.9,
-        "y": 5.27
+        "x": 2.883271729453586,
+        "y": 5.059081387777612
       },
       "prevControl": null,
       "nextControl": {
-        "x": 4.511078009317217,
-        "y": 5.211322262872771
+        "x": 4.494349738770803,
+        "y": 5.000403650650383
       },
       "isLocked": false,
       "linkedName": "B"

diff --git a/src/main/deploy/pathplanner/paths/C to B Close.path b/src/main/deploy/pathplanner/paths/C to B Close.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.9,
-        "y": 5.27
+        "x": 2.883271729453586,
+        "y": 5.059081387777612
       },
       "prevControl": {
-        "x": 2.5981160273115327,
-        "y": 5.094712531987342
+        "x": 2.581387756765119,
+        "y": 4.883793919764954
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/C to B.path b/src/main/deploy/pathplanner/paths/C to B.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.9,
-        "y": 5.27
+        "x": 2.883271729453586,
+        "y": 5.059081387777612
       },
       "prevControl": {
-        "x": 2.576402701523351,
-        "y": 4.817991630285256
+        "x": 2.559674430976937,
+        "y": 4.607073018062868
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/D to Ferry Shot.path b/src/main/deploy/pathplanner/paths/D to Ferry Shot.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.7,
-        "y": 7.529999999999999
+        "x": 7.930642280685416,
+        "y": 7.483224552754384
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.782397412017047,
-        "y": 7.253301738161635
+        "x": 7.0130396927024625,
+        "y": 7.20652629091602
       },
       "isLocked": false,
       "linkedName": "D Top Ferry"
@@ -29,7 +29,19 @@
     }
   ],
   "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [
+    {
+      "name": "Decrease Acceleration",
+      "minWaypointRelativePos": 0.65,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 4.5,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0
+      }
+    }
+  ],
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 4.5,

diff --git a/src/main/deploy/pathplanner/paths/E to Ferry Shot.path b/src/main/deploy/pathplanner/paths/E to Ferry Shot.path
@@ -29,7 +29,19 @@
     }
   ],
   "rotationTargets": [],
-  "constraintZones": [],
+  "constraintZones": [
+    {
+      "name": "Decrease Acceleration",
+      "minWaypointRelativePos": 0.6,
+      "maxWaypointRelativePos": 1.0,
+      "constraints": {
+        "maxVelocity": 5.0,
+        "maxAcceleration": 3.0,
+        "maxAngularVelocity": 540.0,
+        "maxAngularAcceleration": 720.0
+      }
+    }
+  ],
   "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 5.0,

diff --git a/src/main/deploy/pathplanner/paths/NTF Start To D.path b/src/main/deploy/pathplanner/paths/NTF Start To D.path
@@ -32,12 +32,12 @@
     },
     {
       "anchor": {
-        "x": 7.7,
-        "y": 7.529999999999999
+        "x": 7.930642280685416,
+        "y": 7.483224552754384
       },
       "prevControl": {
-        "x": 5.922230165687515,
-        "y": 7.685534706860702
+        "x": 6.152872446372931,
+        "y": 7.638759259615087
       },
       "nextControl": null,
       "isLocked": false,
@@ -54,11 +54,11 @@
   "constraintZones": [
     {
       "name": "Speed Up",
-      "minWaypointRelativePos": 1.0,
+      "minWaypointRelativePos": 1.75,
       "maxWaypointRelativePos": 2.0,
       "constraints": {
         "maxVelocity": 4.5,
-        "maxAcceleration": 9.0,
+        "maxAcceleration": 3.0,
         "maxAngularVelocity": 540.0,
         "maxAngularAcceleration": 720.0
       }

diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -322,8 +322,8 @@ public void configureAutons() {
         AutonConfig ReroutableHGF_RED = new AutonConfig("4 HGF", ReroutableFourPieceHGF::new, 
         "Start to H (HGF) Red", "H to HShoot (HGF) Red", "HShoot to G (HGF) Red", "G to Shoot (HGF) Red", "GShoot to F (HGF)", "F to Shoot (HGF)", "Rerouted H To G", "Rerouted G To F");
 
-            AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new,
-            "Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)", "F to Shoot (HGF)");
+        AutonConfig HGF = new AutonConfig("4 HGF", FourPieceHGF::new,
+        "Start to H (HGF)", "H to HShoot (HGF)", "HShoot to G (HGF)", "G to Shoot (HGF)", "GShoot to F (HGF)", "F to Shoot (HGF)");
         AutonConfig HGF_RED = new AutonConfig("4 HGF", FourPieceHGF::new,
             "Start to H (HGF) Red", "H to HShoot (HGF) Red", "HShoot to G (HGF) Red", "G to Shoot (HGF) Red", "GShoot to F (HGF)", "F to Shoot (HGF)");
 
@@ -403,8 +403,8 @@ public void configureAutons() {
         CBF.registerBlue(autonChooser)
             .registerRed(autonChooser);
 
-        ReroutableCBAEF.registerBlue(autonChooser)
-            .registerRed(autonChooser);
+        // ReroutableCBAEF.registerBlue(autonChooser)
+        //     .registerRed(autonChooser);
 
         GC.registerBlue(autonChooser);
         GC_RED.registerRed(autonChooser);

diff --git a/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java b/src/main/java/com/stuypulse/robot/commands/AmpScoreRoutine.java
@@ -37,7 +37,7 @@
 
 public class AmpScoreRoutine extends SequentialCommandGroup {
 
-    private static final double SCORE_ALIGN_TIMEOUT = 1.25;
+    private static final double SCORE_ALIGN_TIMEOUT = 1.75; // 1.25
 
     private static final double AMP_WALL_SETUP_X_TOLERANCE = Units.inchesToMeters(1.0);
     private static final double AMP_WALL_SETUP_Y_TOLERANCE = Units.inchesToMeters(4.0);
@@ -62,6 +62,7 @@ public AmpScoreRoutine() {
             new ParallelCommandGroup(
                 new WaitUntilCommand(() -> Amper.getInstance().hasNote()),
                 new SwerveDriveToPose(() -> getTargetPose(Alignment.AMP_WALL_SETUP_DISTANCE.get()))
+                    .withMaxSpeed(3.0)
                     .withTolerance(AMP_WALL_SETUP_X_TOLERANCE, AMP_WALL_SETUP_Y_TOLERANCE, AMP_WALL_SETUP_ANGLE_TOLERANCE)
                     .deadlineWith(new LEDSet(LEDInstructions.AMP_ALIGN))
             ),

diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java b/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java
@@ -72,6 +72,7 @@ public static SwerveDriveToPose speakerRelative(double angleToSpeaker) {
     private double yTolerance;
     private double thetaTolerance;
     private double velocityTolerance;
+    private double maxSpeed;
 
     private Pose2d targetPose;
 
@@ -86,6 +87,7 @@ public SwerveDriveToPose(Supplier<Pose2d> poseSupplier) {
         this.poseSupplier = poseSupplier;
 
         targetPose2d = odometry.getField().getObject("Target Pose");
+        maxSpeed = Swerve.MAX_MODULE_SPEED;
 
         controller = new HolonomicController(
             new PIDController(Translation.kP, Translation.kI, Translation.kD),
@@ -138,6 +140,11 @@ public SwerveDriveToPose withTolerance(Number x, Number y, Number theta) {
         return this;
     }
 
+    public SwerveDriveToPose withMaxSpeed(double speed) {
+        maxSpeed = speed;
+        return this;
+    }
+
     @Override
     public void initialize() {
         targetPose = poseSupplier.get();
@@ -155,7 +162,7 @@ public void execute() {
         controller.update(targetPose, odometry.getPose());
 
         Vector2D speed = new Vector2D(controller.getOutput().vxMetersPerSecond, controller.getOutput().vyMetersPerSecond)
-            .clamp(Swerve.MAX_MODULE_SPEED);
+            .clamp(maxSpeed);
         double rotation = SLMath.clamp(controller.getOutput().omegaRadiansPerSecond, Motion.MAX_ANGULAR_VELOCITY.get());
 
         SmartDashboard.putNumber("Alignment/Translation Target Speed", speed.distance());

diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -326,9 +326,9 @@ public interface Shooter {
         ShooterSpeeds REVERSE = new ShooterSpeeds(-3000, -3000);
 
         ShooterSpeeds FERRY = new ShooterSpeeds(
-            new SmartNumber("Shooter/Ferry Shooter RPM", 5500),
+            new SmartNumber("Shooter/Ferry Shooter RPM", 6000), // 5500
             500,
-            new SmartNumber("Shooter/Ferry Feeder RPM", 3000));
+            new SmartNumber("Shooter/Ferry Feeder RPM", 3500)); // 3000
 
         ShooterSpeeds WING_FERRY = new ShooterSpeeds(2000, 2500);
 
@@ -405,7 +405,7 @@ public interface Alignment {
         double PODIUM_SHOT_MAX_ANGLE = 80;
 
         SmartNumber AMP_WALL_SETUP_DISTANCE = new SmartNumber("Alignment/Amp/Setup Pose Distance to Wall", Units.inchesToMeters(25.5));
-        SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5 - 1.5)); // NOTE: remove "- 1.5" for battlecry
+        SmartNumber AMP_WALL_SCORE_DISTANCE = new SmartNumber("Alignment/Amp/Score Pose Distance to Wall", Units.inchesToMeters(22.5 - 1.75)); // NOTE: remove "- 1.5" for battlecry
 
         SmartNumber TRAP_SETUP_DISTANCE = new SmartNumber("Alignment/Trap/Setup Pose Distance", Units.inchesToMeters(21.0));
         SmartNumber TRAP_CLIMB_DISTANCE = new SmartNumber("Alignment/Trap/Climb Distance", Units.inchesToMeters(18.0));

diff --git a/src/main/java/com/stuypulse/robot/subsystems/vision/TheiaTagVision.java b/src/main/java/com/stuypulse/robot/subsystems/vision/TheiaTagVision.java
@@ -70,12 +70,9 @@ public void periodic() {
         super.periodic();
 
         outputs.clear();
-
-        boolean hasAnyData = false;
-
+
         for (TheiaCamera camera : cameras) {
-            if (camera.getVisionData().isPresent())
-                hasAnyData = true;
+            SmartDashboard.putBoolean(camera.getName() + "/Has Data", camera.getVisionData().isPresent());
 
             camera.getVisionData().ifPresent(
                 (VisionData data) -> {
@@ -84,7 +81,7 @@ public void periodic() {
                 });
         }
 
-        SmartDashboard.putBoolean("Vision/Has Any Data", hasAnyData);
+        SmartDashboard.putBoolean("Vision/Has Any Data", outputs.size() > 0);
     }
 
     private void updateTelemetry(String prefix, VisionData data) {