IRI Auto

src/main/deploy/choreo/2025-project.chor

@@ -216,6 +216,20 @@
      "val":0.0
     }
    },
+   "bargeDeep":{
+    "x":{
+     "exp":"barge.x",
+     "val":7.7
+    },
+    "y":{
+     "exp":"7.092 m",
+     "val":7.092
+    },
+    "heading":{
+     "exp":"barge.heading",
+     "val":0.0
+    }
+   },
    "fetch":{
     "x":{
      "exp":"1.69577419757843 m",

src/main/deploy/choreo/deepBargeToE.traj

@@ -0,0 +1,74 @@
+{
+ "name":"deepBargeToE",
+ "version":1,
+ "snapshot":{
+  "waypoints":[
+    {"x":7.7, "y":7.092, "heading":0.0, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":5.422460748154253, "y":5.032141990263579, "heading":4.1887902047863905, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":17.548, "h":8.052}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":4.0}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":10.0}}, "enabled":true}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"bargeDeep.x", "val":7.7}, "y":{"exp":"bargeDeep.y", "val":7.092}, "heading":{"exp":"bargeDeep.heading", "val":0.0}, "intervals":35, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"I.x", "val":5.422460748154253}, "y":{"exp":"I.y", "val":5.032141990263579}, "heading":{"exp":"I.heading", "val":4.1887902047863905}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"17.548 m", "val":17.548}, "h":{"exp":"8.052 m", "val":8.052}}}, "enabled":false},
+    {"from":0, "to":1, "data":{"type":"MaxVelocity", "props":{"max":{"exp":"4 m / s", "val":4.0}}}, "enabled":true},
+    {"from":0, "to":1, "data":{"type":"MaxAcceleration", "props":{"max":{"exp":"10 m / s ^ 2", "val":10.0}}}, "enabled":true}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.21908],
+  "samples":[
+    {"t":0.0, "x":7.7, "y":7.092, "heading":0.0, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-7.92358, "ay":-5.01833, "alpha":-25.51924, "fx":[-2.81779,-184.47719,-199.16664,-141.86725], "fy":[-199.14368,4.39322,0.01542,-139.87727]},
+    {"t":0.03483, "x":7.69519, "y":7.08896, "heading":0.0, "vx":-0.27599, "vy":-0.17479, "omega":-0.88886, "ax":-7.56482, "ay":-6.43547, "alpha":-21.87993, "fx":[-2.85819,-161.16128,-199.10488,-141.28324], "fy":[-199.08925,-87.98946,-1.61385,-140.41239]},
+    {"t":0.06966, "x":7.68099, "y":7.07896, "heading":-0.03096, "vx":-0.53947, "vy":-0.39895, "omega":-1.65096, "ax":-6.56358, "ay":-7.49306, "alpha":-21.77628, "fx":[-8.322,-86.08917,-199.01352,-144.222], "fy":[-198.84899,-164.88925,1.41569,-137.30003]},
+    {"t":0.10449, "x":7.65822, "y":7.06052, "heading":-0.08846, "vx":-0.76809, "vy":-0.65994, "omega":-2.40944, "ax":-6.3062, "ay":-7.72156, "alpha":-22.23992, "fx":[-20.51849,-52.72119,-198.83687,-148.40896], "fy":[-197.81139,-185.67973,1.22134,-132.58884]},
+    {"t":0.13932, "x":7.62764, "y":7.03285, "heading":-0.17239, "vx":-0.98774, "vy":-0.92888, "omega":-3.18408, "ax":-6.40238, "ay":-7.65209, "alpha":-22.53822, "fx":[-35.14395,-40.14054,-198.30655,-153.30762], "fy":[-195.30897,-188.92036,0.36255,-126.36003]},
+    {"t":0.17415, "x":7.58935, "y":6.99586, "heading":-0.28329, "vx":-1.21074, "vy":-1.19541, "omega":-3.9691, "ax":-6.62639, "ay":-7.4675, "alpha":-5.91184, "fx":[-88.6434,-97.2752,-134.02985,-121.88646], "fy":[-146.06861,-129.19469,-99.41897,-123.23657]},
+    {"t":0.20899, "x":7.54316, "y":6.94969, "heading":-0.42154, "vx":-1.44154, "vy":-1.45551, "omega":-4.17502, "ax":-6.78948, "ay":-7.31123, "alpha":18.46137, "fx":[-181.94481,-153.52507,-66.76909,-50.47065], "fy":[-31.49769,-112.7858,-177.0498,-166.1655]},
+    {"t":0.24382, "x":7.48883, "y":6.89456, "heading":-0.56696, "vx":-1.67803, "vy":-1.71017, "omega":-3.53199, "ax":-7.20535, "ay":-6.88787, "alpha":19.64709, "fx":[-185.01525,-164.13648,-82.41421,-48.87312], "fy":[-16.66583,-98.8397,-172.04903,-171.71537]},
+    {"t":0.27865, "x":7.42602, "y":6.83081, "heading":-0.68998, "vx":-1.929, "vy":-1.95008, "omega":-2.84767, "ax":-7.81554, "ay":-6.11908, "alpha":22.41507, "fx":[-190.83882,-178.64658,-99.97831,-51.66124], "fy":[17.18313,-78.44613,-167.02739,-179.7183]},
+    {"t":0.31348, "x":7.35409, "y":6.75918, "heading":-0.78917, "vx":-2.20122, "vy":-2.16321, "omega":-2.06693, "ax":-7.97752, "ay":-4.74944, "alpha":26.80128, "fx":[-179.88677,-189.48288,-115.63813,-46.91772], "fy":[69.1749,-48.89924,-157.22409,-179.73534]},
+    {"t":0.34831, "x":7.27258, "y":6.68095, "heading":-0.86116, "vx":-2.47908, "vy":-2.32864, "omega":-1.13342, "ax":-7.13851, "ay":-5.25482, "alpha":26.30491, "fx":[-175.59973,-180.76115,-103.42389,-16.1972], "fy":[36.89183,-58.88533,-159.05614,-169.33162]},
+    {"t":0.38314, "x":7.1819, "y":6.59666, "heading":-0.90064, "vx":-2.72772, "vy":-2.51167, "omega":-0.2172, "ax":-1.33857, "ay":-1.17954, "alpha":5.40644, "fx":[-25.1708,-36.78325,-24.21448,-3.08446], "fy":[1.9355,-16.33803,-38.34543,-25.90128]},
+    {"t":0.41797, "x":7.08608, "y":6.50846, "heading":-0.9082, "vx":-2.77435, "vy":-2.55276, "omega":-0.02888, "ax":-0.45103, "ay":0.49556, "alpha":-0.00698, "fx":[-8.19075,-7.25394,-7.13455,-7.49438], "fy":[8.37418,8.38353,8.12819,8.15684]},
+    {"t":0.4528, "x":6.98917, "y":6.41984, "heading":-0.90921, "vx":-2.79006, "vy":-2.53549, "omega":-0.02913, "ax":0.06495, "ay":-0.07213, "alpha":0.00224, "fx":[1.18034,-0.13353,1.81869,1.46493], "fy":[-1.89209,-1.57227,-0.49383,-0.85132]},
+    {"t":0.48763, "x":6.89203, "y":6.33148, "heading":-0.91022, "vx":-2.78779, "vy":-2.53801, "omega":-0.02905, "ax":-0.06315, "ay":0.07004, "alpha":-0.00126, "fx":[-1.24173,-0.79325,-0.86547,-1.31019], "fy":[1.83015,0.19711,1.11677,1.52637]},
+    {"t":0.52246, "x":6.79489, "y":6.24313, "heading":-0.91124, "vx":-2.78999, "vy":-2.53557, "omega":-0.02909, "ax":0.01232, "ay":-0.01454, "alpha":0.00247, "fx":[0.38289,-0.54984,0.48808,0.50049], "fy":[-0.72202,-0.41186,0.23781,-0.07324]},
+    {"t":0.55729, "x":6.69772, "y":6.1548, "heading":-0.91225, "vx":-2.78956, "vy":-2.53607, "omega":-0.02901, "ax":0.00635, "ay":-0.00605, "alpha":-0.00154, "fx":[0.05471,0.27122,0.15715,-0.05953], "fy":[0.12888,-0.24386,-0.21531,-0.07306]},
+    {"t":0.59213, "x":6.60056, "y":6.06646, "heading":-0.91326, "vx":-2.78934, "vy":-2.53628, "omega":-0.02906, "ax":0.00856, "ay":-0.01119, "alpha":0.00412, "fx":[0.19318,-0.01167,0.22876,0.1602], "fy":[-0.23951,-0.20284,-0.13363,-0.17032]},
+    {"t":0.62696, "x":6.50341, "y":5.97812, "heading":-0.91427, "vx":-2.78905, "vy":-2.53667, "omega":-0.02892, "ax":0.0569, "ay":-0.06334, "alpha":0.00241, "fx":[0.9351,0.86348,0.96172,1.03358], "fy":[-1.3027,-0.67042,-1.10846,-1.1421]},
+    {"t":0.66179, "x":6.4063, "y":5.88972, "heading":-0.91528, "vx":-2.78706, "vy":-2.53888, "omega":-0.02883, "ax":-0.00487, "ay":0.00889, "alpha":-0.007, "fx":[-0.35293,0.4989,-0.1344,-0.33603], "fy":[0.52792,0.17258,-0.23204,0.12431]},
+    {"t":0.69662, "x":6.30922, "y":5.8013, "heading":-0.91628, "vx":-2.78723, "vy":-2.53857, "omega":-0.02908, "ax":-0.04118, "ay":0.03579, "alpha":0.01969, "fx":[-0.92436,-0.90257,-0.44799,-0.47079], "fy":[0.10866,1.39175,0.31176,0.57455]},
+    {"t":0.73145, "x":6.21212, "y":5.7129, "heading":-0.9173, "vx":-2.78867, "vy":-2.53732, "omega":-0.02839, "ax":-0.54675, "ay":0.61851, "alpha":-0.02943, "fx":[-10.26835,-7.85707,-9.06193,-9.26874], "fy":[10.95211,9.78516,9.64716,10.85665]},
+    {"t":0.76628, "x":6.11465, "y":5.6249, "heading":-0.91828, "vx":-2.80771, "vy":-2.51578, "omega":-0.02942, "ax":-0.9813, "ay":1.29004, "alpha":-0.37269, "fx":[-18.90541,-15.1744,-13.70401,-17.64749], "fy":[20.64573,18.8577,21.88575,24.62803]},
+    {"t":0.80111, "x":6.01626, "y":5.53805, "heading":-0.91931, "vx":-2.84189, "vy":-2.47085, "omega":-0.0424, "ax":2.76878, "ay":6.71121, "alpha":-19.75716, "fx":[40.39127,121.13249,51.83289,-28.73985], "fy":[45.13422,97.75426,162.35888,142.24352]},
+    {"t":0.83594, "x":5.91896, "y":5.45606, "heading":-0.92079, "vx":-2.74545, "vy":-2.23709, "omega":-0.73056, "ax":5.98065, "ay":6.90801, "alpha":-25.69105, "fx":[170.95689,173.12117,81.84599,-27.14571], "fy":[19.1198,82.61132,174.28594,184.59562]},
+    {"t":0.87077, "x":5.82696, "y":5.38233, "heading":-0.94623, "vx":-2.53714, "vy":-1.99648, "omega":-1.6254, "ax":7.34682, "ay":6.3842, "alpha":-24.18211, "fx":[188.82757,181.29619,103.92894,15.81904], "fy":[-1.38349,71.68157,165.16834,190.21962]},
+    {"t":0.9056, "x":5.74304, "y":5.31666, "heading":-1.00285, "vx":-2.28124, "vy":-1.77411, "omega":-2.46768, "ax":7.96472, "ay":5.94874, "alpha":-21.38901, "fx":[185.28581,183.39924,118.23906,44.14802], "fy":[2.29909,59.90193,152.69946,181.75017]},
+    {"t":0.94043, "x":5.66842, "y":5.25848, "heading":-1.0888, "vx":-2.00383, "vy":-1.56691, "omega":-3.21268, "ax":8.31145, "ay":5.49899, "alpha":-19.26284, "fx":[179.5081,183.74968,129.03601,61.89769], "fy":[6.5644,48.41591,139.98071,171.70132]},
+    {"t":0.97526, "x":5.60366, "y":5.20724, "heading":-1.2007, "vx":-1.71433, "vy":-1.37538, "omega":-3.88362, "ax":8.53407, "ay":5.16378, "alpha":-18.67276, "fx":[177.06905,185.65669,137.51627,68.79319], "fy":[7.58833,36.75193,130.99285,168.97802]},
+    {"t":1.0101, "x":5.54913, "y":5.16247, "heading":-1.33597, "vx":-1.41708, "vy":-1.19552, "omega":-4.53401, "ax":8.64638, "ay":4.98307, "alpha":-18.81115, "fx":[174.51507,188.17085,143.10026,70.7376], "fy":[10.01707,24.97524,126.35234,170.91673]},
+    {"t":1.04493, "x":5.50501, "y":5.12385, "heading":-1.49389, "vx":-1.11592, "vy":-1.02195, "omega":-5.18922, "ax":8.59518, "ay":5.05184, "alpha":23.07942, "fx":[146.58574,33.77809,194.63796,198.10815], "fy":[134.04745,195.31007,-3.89776,11.38707]},
+    {"t":1.07976, "x":5.47136, "y":5.09132, "heading":-1.67464, "vx":-0.81654, "vy":-0.84599, "omega":-4.38534, "ax":8.43236, "ay":5.29077, "alpha":21.87331, "fx":[156.18842,28.31643,179.72016,198.02839], "fy":[123.33385,196.82727,50.8605,-18.24323]},
+    {"t":1.11459, "x":5.44803, "y":5.06506, "heading":-1.82738, "vx":-0.52284, "vy":-0.66171, "omega":-3.62348, "ax":5.92091, "ay":7.08136, "alpha":27.54934, "fx":[164.83572,44.53603,-11.4016,196.82478], "fy":[111.74537,194.01876,195.89283,-29.4857]},
+    {"t":1.14942, "x":5.43341, "y":5.04631, "heading":-1.95359, "vx":-0.31661, "vy":-0.41506, "omega":-2.66391, "ax":4.60548, "ay":6.19964, "alpha":36.90159, "fx":[171.10899,48.34177,-105.17446,192.80842], "fy":[101.97834,193.19056,167.75984,-49.54843]},
+    {"t":1.18425, "x":5.42518, "y":5.03561, "heading":-2.04638, "vx":-0.15619, "vy":-0.19912, "omega":-1.37859, "ax":4.48432, "ay":5.7168, "alpha":39.57963, "fx":[175.64958,53.2177,-118.32491,188.46338], "fy":[94.00814,191.95401,159.45264,-64.22969]},
+    {"t":1.21908, "x":5.42246, "y":5.03214, "heading":-2.0944, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}

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

@@ -0,0 +1,151 @@
+package frc.robot.commands.auton;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.commands.drive.DriveAutonCommand;
+import frc.robot.commands.drive.PrepOdomForAutoCommand;
+import frc.robot.commands.elevator.ZeroElevatorCommand;
+import frc.robot.commands.robotState.AutoScoreAlgaeCommand;
+import frc.robot.commands.robotState.ForceBargeCommand;
+import frc.robot.commands.robotState.ForceLowFloorAlgaeCommand;
+import frc.robot.commands.robotState.SetAutoPlacingCommand;
+import frc.robot.subsystems.algae.AlgaeSubsystem;
+import frc.robot.subsystems.biscuit.BiscuitSubsystem;
+import frc.robot.subsystems.coral.CoralSubsystem;
+import frc.robot.subsystems.drive.DriveSubsystem;
+import frc.robot.subsystems.elevator.ElevatorSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoreSide;
+import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
+import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
+import frc.robot.subsystems.vision.VisionSubsystem;
+import java.util.ArrayList;
+import java.util.List;
+
+public class IRIAutonCommand extends SequentialCommandGroup implements AutoCommandInterface {
+
+  private DriveSubsystem driveSubsystem;
+  private CoralSubsystem coralSubsystem;
+  private RobotStateSubsystem robotStateSubsystem;
+  private VisionSubsystem visionSubsystem;
+
+  private ArrayList<AutoCommandInterface> pathCommands = new ArrayList<>();
+
+  public IRIAutonCommand(
+      DriveSubsystem driveSubsystem,
+      RobotStateSubsystem robotStateSubsystem,
+      AlgaeSubsystem algaeSubsystem,
+      BiscuitSubsystem biscuitSubsystem,
+      CoralSubsystem coralSubsystem,
+      ElevatorSubsystem elevatorSubsystem,
+      TagAlignSubsystem tagAlignSubsystem,
+      VisionSubsystem visionSubsystem,
+      List<String> grabPaths,
+      List<Double> grabYOffsets,
+      List<Double> delays,
+      List<String> bargePaths,
+      List<RobotStateSubsystem.ScoringLevel> algaeLevels,
+      Pose2d startPose) {
+    addRequirements(
+        driveSubsystem, algaeSubsystem, biscuitSubsystem, coralSubsystem, elevatorSubsystem);
+    this.driveSubsystem = driveSubsystem;
+    this.coralSubsystem = coralSubsystem;
+    this.robotStateSubsystem = robotStateSubsystem;
+    this.visionSubsystem = visionSubsystem;
+
+    // BooleanSupplier awayCondition =
+    //     () -> DriverStation.getMatchTime() < AutonConstants.kBargeScoreMinTime;
+
+    addCommands(
+        new ParallelCommandGroup(
+            new PrepOdomForAutoCommand(
+                robotStateSubsystem, driveSubsystem, Rotation2d.fromDegrees(90.0), startPose),
+            new ZeroElevatorCommand(elevatorSubsystem)),
+        new ForceLowFloorAlgaeCommand(
+            robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem),
+        new SetAutoPlacingCommand(robotStateSubsystem, true),
+        new ForceBargeCommand(
+            robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem),
+        new SetAutoPlacingCommand(robotStateSubsystem, false));
+
+    for (int i = 0; i < grabPaths.size(); i++) {
+      boolean last = i == grabPaths.size() - 1;
+      var algaeDrive =
+          new DriveAlgaeAutonServoCommand(
+              driveSubsystem,
+              tagAlignSubsystem,
+              elevatorSubsystem,
+              biscuitSubsystem,
+              robotStateSubsystem,
+              visionSubsystem,
+              grabPaths.get(i),
+              i == 0,
+              true,
+              i == 0,
+              grabYOffsets.get(i),
+              algaeLevels.get(i));
+      var bargeDrive =
+          last
+              ? new DriveAutonCommand(driveSubsystem, bargePaths.get(i), true, false, false)
+              : new DriveBargeAutonCommand(
+                  driveSubsystem,
+                  tagAlignSubsystem,
+                  elevatorSubsystem,
+                  biscuitSubsystem,
+                  robotStateSubsystem,
+                  visionSubsystem,
+                  bargePaths.get(i),
+                  true,
+                  false);
+
+      pathCommands.add(algaeDrive);
+      pathCommands.add(bargeDrive);
+
+      addCommands(algaeDrive, new WaitCommand(delays.get(i)), bargeDrive);
+
+      if (!last) {
+        addCommands(
+            new AutoScoreAlgaeCommand(
+                robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
+      }
+    }
+    /*
+    addCommands(
+        new ConditionalCommand(
+            new DriveAutonCommand(driveSubsystem, "bargeLeave", true, false, false),
+            new SequentialCommandGroup(
+                algaeDrive,
+                new WaitCommand(delays.get(i))),
+            awayCondition),
+        new ConditionalCommand(
+            new DriveAutonCommand(driveSubsystem, "bargeLeave", true, false, false),
+            new SequentialCommandGroup(
+                    bargeDrive,
+                    new AutoScoreAlgaeCommand(
+                        robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
+                .until(awayCondition),
+            awayCondition));
+    }
+    */
+
+    // addCommands(new DriveAutonCommand(driveSubsystem, "bargeLeave", true, false, false));
+  }
+
+  @Override
+  public void reassignAlliance() {
+    driveSubsystem.teleResetGyro();
+    coralSubsystem.setAutoPreload();
+    robotStateSubsystem.setIsAutoPlacing(true);
+    robotStateSubsystem.setScoringLevel(ScoringLevel.L4);
+    robotStateSubsystem.setGetAlgaeOnCycle(true);
+    robotStateSubsystem.setScoreSide(ScoreSide.LEFT);
+    visionSubsystem.setVisionUpdating(true);
+
+    for (var command : pathCommands) {
+      command.reassignAlliance();
+    }
+  }
+}

src/main/java/frc/robot/commands/robotState/ForceBargeCommand.java

@@ -58,7 +58,7 @@ public boolean isFinished() {
       return robotStateSubsystem.getState() == RobotStates.PROCESSOR_ALGAE
           || (robotStateSubsystem.getState() == RobotStates.BARGE_ALGAE
               && !robotStateSubsystem.getIsAutoPlacing())
-          || !robotStateSubsystem.getIsBargeSafe();
+      /*|| !robotStateSubsystem.getIsBargeSafe() */ ;
     }
   }
 }

src/main/java/frc/robot/commands/robotState/SetAutoPlacingCommand.java

@@ -0,0 +1,19 @@
+package frc.robot.commands.robotState;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.robotState.RobotStateSubsystem;
+
+public class SetAutoPlacingCommand extends InstantCommand {
+  private RobotStateSubsystem robotStateSubsystem;
+  private boolean isAutoPlacing;
+
+  public SetAutoPlacingCommand(RobotStateSubsystem robotState, boolean isAutoPlacing) {
+    this.robotStateSubsystem = robotState;
+    this.isAutoPlacing = isAutoPlacing;
+  }
+
+  @Override
+  public void initialize() {
+    robotStateSubsystem.setIsAutoPlacing(isAutoPlacing);
+  }
+}

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

@@ -29,7 +29,7 @@ public class AlgaeConstants {
   public static final double kHoldSpeed = 1;
   public static final double kCoralHoldSpeed = -0.05;
   public static final double kBargeScoreSpeed = -1;
-  public static final double kProcessorScoreSpeed = -1;
+  public static final double kProcessorScoreSpeed = -0.5; // was -1.0
   public static final double kCoralScoreSpeed = 0.3; // 0.5; was 0.4
   public static final double kIntakingSpeed = 1; // 0.75
   public static final double kCoralIntakingSpeed = -0.3; // -0.75;

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

@@ -30,4 +30,6 @@ public class AutonConstants {
 
   public static final Pose2d kMiddleBargeStart =
       new Pose2d(7.1, 3.7209, Rotation2d.fromDegrees(180));
+
+  public static final Pose2d kDeepBarge = new Pose2d(7.7, 7.092, Rotation2d.fromDegrees(90));
 }

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

@@ -4,12 +4,16 @@
 
 public class BargeAlignConstants {
   public static final double kXSpeed = 0.45;
+  public static final double kXRevSpeed = 2;
 
   public static final double kBlueEjectAlgaeX = 7.67; // 7.72
   public static final double kRedEjectAlgaeX = 9.86; // 9.81
   public static final double kBlueRaiseElevatorX = 6.82; // 7.02
   public static final double kRedRaiseElevatorX = 10.71; // 10.51
 
+  public static final double kBlueRevDoneX = kBlueRaiseElevatorX + 0.25;
+  public static final double kRedRevDoneX = kRedRaiseElevatorX - 0.25;
+
   public static final double kBlueUnsafeX = 7.02;
   public static final double kRedUnsafeX = 10.51;