EBR Code

build.gradle

@@ -34,6 +34,8 @@ deploy {
                     jvmArgs.add("-Dcom.sun.management.jmxremote.local.only=false")
                     jvmArgs.add("-Dcom.sun.management.jmxremote.ssl=false")
                     jvmArgs.add("-Dcom.sun.management.jmxremote.authenticate=false")
+                    jvmArgs.add("-XX:+HeapDumpOnOutOfMemoryError")
+                    jvmArgs.add("-XX:HeapDumpPath=/home/lvuser/frc-usercode.hprof")
                     jvmArgs.add("-Djava.rmi.server.hostname=10.9.72.2") // Replace TE.AM with team number
                 }
 

src/main/deploy/pathplanner/paths/Gronud P2 Mirrored.path

@@ -80,8 +80,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/Gronud P2.path

@@ -81,7 +81,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 3.5,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/GroundB #6 Mirrored.path

@@ -64,8 +64,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/GroundB #6.path

@@ -65,7 +65,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 3.5,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/P 1 Mirrored.path

@@ -67,8 +67,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/P 1.path

@@ -67,8 +67,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.5,
-    "maxAcceleration": 3.5,
+    "maxVelocity": 2.0,
+    "maxAcceleration": 2.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/P 3 Mirrored.path

@@ -45,8 +45,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 2.5,
-    "maxAcceleration": 2.5,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/P 3.path

@@ -46,7 +46,7 @@
   ],
   "globalConstraints": {
     "maxVelocity": 3.5,
-    "maxAcceleration": 3.5,
+    "maxAcceleration": 4.0,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/deploy/pathplanner/paths/P 5 Mirrored.path

@@ -45,8 +45,8 @@
     }
   ],
   "globalConstraints": {
-    "maxVelocity": 3.0,
-    "maxAcceleration": 3.0,
+    "maxVelocity": 3.5,
+    "maxAcceleration": 3.5,
     "maxAngularVelocity": 540.0,
     "maxAngularAcceleration": 720.0,
     "nominalVoltage": 12.0,

src/main/java/frc/robot/RobotContainer.java

@@ -22,6 +22,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc.robot.commands.DoNothing;
 import frc.robot.commands.drive_comm.DefaultDriveCommand;
 import frc.robot.commands.drive_comm.DriveToPose;
 import frc.robot.commands.gpm.IntakeCoral;
@@ -72,12 +73,10 @@ public class RobotContainer {
   private Elevator elevator = null;
   private Climb climb = null;
   private Arm arm = null;
-  Command auto;
+  private Command auto = new DoNothing();
 
-  public double armWaitTime = 0.5;
-
-    // Dashboard inputs
-  //private final LoggedDashboardChooser<Command> autoChooser;
+  // Dashboard inputs
+  // private final LoggedDashboardChooser<Command> autoChooser;
 
   // Controllers are defined here
   private BaseDriverConfig driver = null;
@@ -136,28 +135,28 @@ public RobotContainer(RobotId robotId) {
         DetectedObject.setDrive(drive);
 
         //SignalLogger.start();
+
 
         driver.configureControls();
         //operator.configureControls();
 
         initializeAutoBuilder();
         registerCommands();
-        drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
         PathGroupLoader.loadPathGroups();
-
-
+        // Load the auto command
+        try {
+          PathPlannerAuto.getPathGroupFromAutoFile("Left Side");
+          auto = new PathPlannerAuto("Left Side");
+        } catch (IOException | ParseException e) {
+            e.printStackTrace();
+        }
+        drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
         break;
       }
 
     // This is really annoying so it's disabled
     DriverStation.silenceJoystickConnectionWarning(true);
-    try {
-      PathPlannerAuto.getPathGroupFromAutoFile("Left Side");
-      auto = new PathPlannerAuto("Left Side");
-    } 
-    catch (IOException | ParseException e) {
-        e.printStackTrace();
-    }
+
     //addPaths(); 
     // TODO: verify this claim.
     // LiveWindow is causing periodic loop overruns
@@ -202,7 +201,7 @@ public void registerCommands() {
       NamedCommands.registerCommand("L4", 
         new ParallelCommandGroup(
           new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
-          new MoveArm(arm, ArmConstants.L4_SETPOINT)
+          new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT)
         )
       );
       NamedCommands.registerCommand("backdrive", new InstantCommand(() -> outtake.setMotor(0.02)));
@@ -215,7 +214,7 @@ public void registerCommands() {
       NamedCommands.registerCommand("Score L4", new SequentialCommandGroup(
         new ParallelCommandGroup(
           new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
-          new MoveArm(arm, ArmConstants.L4_SETPOINT)
+          new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT)
         ),
         new OuttakeCoral(outtake, elevator, arm)
       ));
@@ -269,13 +268,12 @@ public void registerCommands() {
       Pose2d redStationRight = new Pose2d(FieldConstants.FIELD_LENGTH-blueStationRight.getX(), blueStationLeft.getY(), blueStationRight.getRotation().plus(new Rotation2d(Math.PI)));
       Pose2d redStationLeft = new Pose2d(FieldConstants.FIELD_LENGTH-blueStationLeft.getX(), blueStationRight.getY(), blueStationLeft.getRotation().plus(new Rotation2d(Math.PI)));
       NamedCommands.registerCommand("Drive To Left Station", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationLeft : blueStationLeft));
-      // TODO: update the positions for opposite side field as well, so far it's just updated for our practice field station
       NamedCommands.registerCommand("Drive To Right Station Intake", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationRight : blueStationIntakeRight));
       NamedCommands.registerCommand("Drive To Left Station Intake", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationLeft : blueStationIntakeLeft));
 
       NamedCommands.registerCommand("Drive To Right Station", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? redStationRight : blueStationRight));
-      NamedCommands.registerCommand("Drive To 6/19 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_LEFT.l4Pose));
-      NamedCommands.registerCommand("Drive To 6/19 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_RIGHT.l4Pose));
+      NamedCommands.registerCommand("Drive To 6/19 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_LEFT.l4Pose).withTimeout(1));
+      NamedCommands.registerCommand("Drive To 6/19 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_6_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_19_RIGHT.l4Pose).withTimeout(1));
       NamedCommands.registerCommand("Drive To 7/18 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_7_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_18_LEFT.l4Pose));
       NamedCommands.registerCommand("Drive To 7/18 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_7_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_18_RIGHT.l4Pose));
       NamedCommands.registerCommand("Drive To 10/21 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_9_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_22_RIGHT.l4Pose));
@@ -285,8 +283,8 @@ public void registerCommands() {
       == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_9_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_22_LEFT.l4Pose)); 
 
 
-      NamedCommands.registerCommand("Drive To 8/17 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_LEFT.l4Pose));
-      NamedCommands.registerCommand("Drive To 8/17 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_RIGHT.l4Pose));
+      NamedCommands.registerCommand("Drive To 8/17 Left", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_LEFT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_LEFT.l4Pose).withTimeout(1));
+      NamedCommands.registerCommand("Drive To 8/17 Right", new DriveToPose(drive, () -> DriverStation.getAlliance().get() == DriverStation.Alliance.Red ? VisionConstants.REEF.RED_BRANCH_8_RIGHT.l4Pose : VisionConstants.REEF.BLUE_BRANCH_17_RIGHT.l4Pose).withTimeout(1));
     }
   }
 

src/main/java/frc/robot/commands/gpm/OuttakeCoralBasic.java

@@ -13,7 +13,7 @@
  * Wants coral to be present.
  */
 public class OuttakeCoralBasic extends Command {
-    public static final double L1_SPEED = -0.3;
+    public static final double L1_SPEED = -0.2;
     public static final double L4_SPEED = -0.8;
     public static final double OUTTAKE_SPEED = -0.45;
 

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

@@ -31,14 +31,16 @@ public class ArmConstants {
 
     //4 in offset
     // public static final double L4_SETPOINT = 11;
-    public static final double L4_SETPOINT = 7.5;
+    // Original L4: 7.5 degrees
+    public static final double L4_SETPOINT_RIGHT = 7.5;
+    public static final double L4_SETPOINT_LEFT = 6;
     public static final double L2_L3_SETPOINT = 21.25;
 
     //touching reef
     public static final double L4_SETPOINT_ALT = 4.5;
     public static final double L2_L3_SETPOINT_ALT = 12.23;
 
-    public static final double ALGAE_SETPOINT = -16.37;
+    public static final double ALGAE_SETPOINT = -19.37;
     public static final double ALGAE_NET_SETPOINT_1 = 85.0;
     public static final double ALGAE_NET_SETPOINT_2 = 25;
     public static final double ALGAE_STOW_SETPOINT  = 50;

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

@@ -43,8 +43,8 @@ public class ElevatorConstants {
     public static final double L4_SETPOINT_ALT = 1.675;
     //Dunk L4 = 1.5
 
-    public static final double BOTTOM_ALGAE_SETPOINT = 0.385;
-    public static final double TOP_ALGAE_SETPOINT = 0.779;
+    public static final double BOTTOM_ALGAE_SETPOINT = 0.405;
+    public static final double TOP_ALGAE_SETPOINT = 0.799;
 
     public static final double NET_SETPOINT = MAX_HEIGHT;
     public static final double STATION_INTAKE_SETPOINT = 0.233;

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

@@ -347,7 +347,7 @@ private Pose2d getL1Pose() {
                 double adjustedYOffset = DriveConstants.ROBOT_WIDTH_WITH_BUMPERS / 2.0;
 
                 // Apply both X and Y offsets to calculate the reef branch pose
-                Transform3d transform = new Transform3d(adjustedYOffset, -Math.signum(xOffset)*Units.inchesToMeters(37.04/2-2), 0, new Rotation3d(0, 0, 0));
+                Transform3d transform = new Transform3d(adjustedYOffset, -Math.signum(xOffset)*Units.inchesToMeters(37.04/2-1), 0, new Rotation3d(0, 0, 0));
 
                 Pose3d branchPose3d = basePose3d.plus(transform);
 

src/main/java/frc/robot/controls/GameControllerDriverConfig.java

@@ -86,7 +86,7 @@ public void configureControls() {
       driver.get(driver.LEFT_TRIGGER_BUTTON).onTrue(
           new SequentialCommandGroup(
               new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
-              new MoveArm(arm, ArmConstants.L4_SETPOINT)));
+              new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT)));
 
       Command l2Coral = new SequentialCommandGroup(
           new MoveElevator(elevator, ElevatorConstants.L2_SETPOINT),

src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java

@@ -119,16 +119,25 @@ public void configureControls() {
                     new ConditionalCommand(
                         new ParallelCommandGroup(
                             new MoveElevator(elevator, ElevatorConstants.L4_SETPOINT),
-                            new MoveArm(arm, ArmConstants.L4_SETPOINT),
+                            new ConditionalCommand(
+                                new MoveArm(arm, ArmConstants.L4_SETPOINT_RIGHT),
+                                new MoveArm(arm, ArmConstants.L4_SETPOINT_LEFT),
+                                () -> selectedDirection >= 0
+                            ),
                             new DriveToPose(getDrivetrain(), ()->alignmentPose)
                         ),
                         // This is instant so it doesn't requre the drivetrain for more than 1 frame
                         new InstantCommand(()->{
                             elevator.setSetpoint(ElevatorConstants.L4_SETPOINT);
-                            arm.setSetpoint(ArmConstants.L4_SETPOINT);
+                            arm.setSetpoint(ArmConstants.L4_SETPOINT_RIGHT);
                         }),
                         ()->selectedDirection != 0
                     ),
+                    new ConditionalCommand(
+                        new WaitCommand(0.5),
+                        new DoNothing(),
+                        () -> selectedDirection < 0
+                    ),
                     new ConditionalCommand(
                         new OuttakeCoral(outtake, elevator, arm)
                         .andThen(new InstantCommand(()->{

src/main/java/frc/robot/util/Vision/Vision.java

@@ -599,6 +599,7 @@ public ArrayList<EstimatedRobotPose> getEstimatedPose(DoubleUnaryOperator yawFun
 
       // The latest camera results
       for(PhotonPipelineResult result : inputs.results){
+        // TODO: This could be improved by averaging all targets instead of only using 1
         // Gets the best target to use for the calculations
         PhotonTrackedTarget target = result.getBestTarget();
         // Continue if the target doesn't exist or it should be ignored