Additional prework: split out driver controls, clean things up, pathfinding during tele, auto aim

.github/workflows/build.yml

@@ -0,0 +1,52 @@
+name: Build
+
+on:
+  push:
+    branches:
+      - master
+      - main
+  pull_request:
+    branches:
+      - master
+      - main
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+
+    steps:
+      - name: Checkout code
+        uses: actions/checkout@v4
+
+      - name: Set up JDK 17
+        uses: actions/setup-java@v4
+        with:
+          java-version: '17'
+          distribution: 'temurin'
+
+      - name: Grant execute permission for gradlew
+        run: chmod +x gradlew
+
+      - name: Cache Gradle packages
+        uses: actions/cache@v4
+        with:
+          path: |
+            ~/.gradle/caches
+            ~/.gradle/wrapper
+          key: ${{ runner.os }}-gradle-${{ hashFiles('**/*.gradle*', '**/gradle-wrapper.properties') }}
+          restore-keys: |
+            ${{ runner.os }}-gradle-
+
+      - name: Build with Gradle
+        run: ./gradlew build
+
+      - name: Run tests
+        run: ./gradlew test
+
+      - name: Upload build artifacts
+        uses: actions/upload-artifact@v4
+        if: always()
+        with:
+          name: build-reports
+          path: build/reports/
+          retention-days: 7

simgui-ds.json

@@ -90,6 +90,14 @@
     }
   ],
   "robotJoysticks": [
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    },
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    },
     {
       "guid": "78696e70757401000000000000000000",
       "useGamepad": true

src/main/java/frc/robot/Constants.java

@@ -11,14 +11,13 @@ public final class Constants {
   public static final double LOOP_TIME = 0.13; // s, 20ms + 110ms spark max velocity lag
   public static final double MAX_SPEED = Units.feetToMeters(14.5);
 
-  public static class OperatorConstants {
+  public static class ControllerConstants {
     public static final int kDriverControllerPort = 0;
+    public static final int kOperatorControllerPort = 1;
+    public static final int kPoseControllerPort = 2;
 
     // Joystick Deadband
     public static final double DEADBAND = 0.1;
-    public static final double LEFT_Y_DEADBAND = 0.1;
-    public static final double RIGHT_X_DEADBAND = 0.1;
-    public static final double TURN_CONSTANT = 6;
   }
 
   public static class DriveConstants {

src/main/java/frc/robot/Robot.java

@@ -5,7 +5,6 @@
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.NT4Publisher;
-import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Translation2d;

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

@@ -3,106 +3,39 @@
 
 import java.io.File;
 
-import org.ironmaple.simulation.SimulatedArena;
-import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
-import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeAlgaeOnFly;
-import org.littletonrobotics.junction.Logger;
-
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
-import edu.wpi.first.math.util.Units;
-import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.Feet;
-import static edu.wpi.first.units.Units.FeetPerSecond;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Filesystem;
-import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.Constants.OperatorConstants;
+import frc.robot.Constants.ControllerConstants;
+import frc.robot.controls.DriverControls;
+import frc.robot.controls.OperatorControls;
+import frc.robot.controls.PoseControls;
 import frc.robot.subsystems.SwerveSubsystem;
 import swervelib.SwerveDrive;
-import swervelib.SwerveInputStream;
 
 public class RobotContainer {
-  // The robot's subsystems and commands are defined here...
-  // private final AlgaeSubsystem m_algaeSubsystem = new AlgaeSubsystem();
-  // private final CoralSubsystem m_coralSubsystem = new CoralSubsystem();
-
   private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve"));
 
-  // Replace with CommandPS4Controller or CommandJoystick if needed
-  private final CommandXboxController driverXbox = new CommandXboxController(
-      OperatorConstants.kDriverControllerPort);
-
   private final SendableChooser<Command> autoChooser;
 
-  /**
-   * Converts driver input into a field-relative ChassisSpeeds that is controlled
-   * by angular velocity.
-   */
-  SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(),
-      () -> driverXbox.getLeftY() * -1,
-      () -> driverXbox.getLeftX() * -1)
-      .withControllerRotationAxis(() -> driverXbox.getRightX() * -1)
-      .deadband(OperatorConstants.DEADBAND)
-      // .scaleTranslation(0.8) // TODO: Tune speed scaling
-      .allianceRelativeControl(true);
-
-  /**
-   * Clone's the angular velocity input stream and converts it to a fieldRelative
-   * input stream.
-   */
-  SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX,
-      driverXbox::getRightY)
-      .headingWhile(true);
-
-  /**
-   * Clone's the angular velocity input stream and converts it to a robotRelative
-   * input stream.
-   */
-  SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true)
-      .allianceRelativeControl(false);
-
-  SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(),
-      () -> -driverXbox.getLeftY(),
-      () -> -driverXbox.getLeftX())
-      .withControllerRotationAxis(() -> driverXbox.getRawAxis(4))
-      .deadband(OperatorConstants.DEADBAND)
-      .scaleTranslation(0.8)
-      .allianceRelativeControl(true);
-
-  // Derive the heading axis with math!
-  SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy()
-      .withControllerHeadingAxis(
-          () -> driverXbox.getRawAxis(4),
-          () -> driverXbox.getRawAxis(5))
-      .headingWhile(true)
-      .translationHeadingOffset(true)
-      .translationHeadingOffset(Rotation2d.fromDegrees(0));
-
   /**
    * The container for the robot. Contains subsystems, I/O devices, and commands.
    */
   public RobotContainer() {
     // Configure the trigger bindings
     configureBindings();
     buildNamedAutoCommands();
-    DriverStation.silenceJoystickConnectionWarning(true);
+
+    if (!Robot.isReal()) {
+      DriverStation.silenceJoystickConnectionWarning(true);
+    }
 
     // Have the autoChooser pull in all PathPlanner autos as options
     autoChooser = AutoBuilder.buildAutoChooser();
@@ -119,63 +52,10 @@ public RobotContainer() {
   }
 
   private void configureBindings() {
-    Command driveFieldOrientedAngularVelocity = drivebase.driveFieldOriented(driveAngularVelocity);
-    // Command driveSetpointGen =
-    // drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngle);
-
-    Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard)
-        .withName("Drive.FieldOrientedKeyboard");
-    // Command driveSetpointGenKeyboard =
-    // drivebase.driveWithSetpointGeneratorFieldRelative(driveDirectAngleKeyboard);
-
-    if (RobotBase.isSimulation()) {
-      drivebase.setDefaultCommand(driveFieldOrientedDirectAngleKeyboard);
-    } else {
-      // FIELD RELATIVE DRIVE
-      drivebase.setDefaultCommand(driveFieldOrientedAngularVelocity.withName("Drive.FieldOriented"));
-
-      // ROBOT RELATIVE DRIVE
-      // Command driveRobotOrientedAngularVelocity =
-      // drivebase.driveFieldOriented(driveRobotOriented);
-      // drivebase.setDefaultCommand(driveRobotOrientedAngularVelocity.withName("Drive.RobotOriented"));
-    }
-
-    if (Robot.isSimulation()) {
-      Pose2d target = new Pose2d(new Translation2d(1, 4), Rotation2d.fromDegrees(90));
-      drivebase.getSwerveDrive().field.getObject("targetPose").setPose(target);
-
-      driveDirectAngleKeyboard.driveToPose(() -> target,
-          new ProfiledPIDController(5, 0, 0,
-              new Constraints(5, 2)),
-          new ProfiledPIDController(5, 0, 0,
-              new Constraints(
-                  Units.degreesToRadians(360),
-                  Units.degreesToRadians(180))));
-      driverXbox.start().onTrue(Commands.runOnce(() -> drivebase.resetOdometry(new Pose2d(3, 3, new Rotation2d()))));
-      driverXbox.button(1).whileTrue(drivebase.sysIdDriveMotorCommand());
-      driverXbox.button(2).whileTrue(Commands.runEnd(
-          () -> driveDirectAngleKeyboard.driveToPoseEnabled(true),
-          () -> driveDirectAngleKeyboard.driveToPoseEnabled(false)));
-    }
-
-    if (DriverStation.isTest()) {
-      drivebase.setDefaultCommand(driveFieldOrientedAngularVelocity); // Overrides drive command above!
-
-      driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
-      driverXbox.start().onTrue((Commands.runOnce(drivebase::zeroGyro)));
-      driverXbox.back().whileTrue(drivebase.centerModulesCommand());
-      driverXbox.leftBumper().onTrue(Commands.none());
-      driverXbox.rightBumper().onTrue(Commands.none());
-    } else {
-      driverXbox.a().onTrue((Commands.runOnce(drivebase::zeroGyro)));
-
-      driverXbox.start().whileTrue(drivebase.sysIdAngleMotorCommand());
-      driverXbox.back().whileTrue(drivebase.sysIdDriveMotorCommand());
-      // driverXbox.back().whileTrue(fireAlgae());
-
-      driverXbox.leftBumper().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly());
-      driverXbox.rightBumper().onTrue(Commands.none());
-    }
+    // Set up controllers
+    DriverControls.configure(ControllerConstants.kDriverControllerPort, drivebase, null);
+    OperatorControls.configure(ControllerConstants.kOperatorControllerPort, drivebase, null);
+    PoseControls.configure(ControllerConstants.kPoseControllerPort, drivebase);
   }
 
   private void buildNamedAutoCommands() {
@@ -195,56 +75,11 @@ public Command getAutonomousCommand() {
     return autoChooser.getSelected();
   }
 
-  public void setMotorBrake(boolean brake) {
-    drivebase.setMotorBrake(brake);
-  }
-
   public SwerveDrive getSwerveDrive() {
     return drivebase.getSwerveDrive();
   }
 
   public Pose2d getRobotPose() {
     return drivebase.getPose();
   }
-
-  public SwerveDriveSimulation getSwerveDriveSimulation() {
-    return drivebase.getSwerveDrive().getMapleSimDrive().orElseThrow();
-  }
-
-  public Command fireAlgae() {
-    return Commands.runOnce(() -> {
-      System.err.println("FIRE!");
-
-      SimulatedArena arena = SimulatedArena.getInstance();
-
-      // Translation2d robotPosition,
-      // Translation2d shooterPositionOnRobot,
-      // ChassisSpeeds chassisSpeeds,
-      // Rotation2d shooterFacing,
-      // Distance initialHeight,
-      // LinearVelocity launchingSpeed,
-      // Angle shooterAngle
-
-      ReefscapeAlgaeOnFly algae = new ReefscapeAlgaeOnFly(
-          drivebase.getPose().getTranslation(),
-          new Translation2d(),
-          drivebase.getSwerveDrive().getRobotVelocity().times(-1),
-          drivebase.getSwerveDrive().getOdometryHeading(),
-          Distance.ofBaseUnits(1, Feet),
-          LinearVelocity.ofBaseUnits(5, FeetPerSecond),
-          Angle.ofBaseUnits(45, Degrees));
-
-      // Configure callbacks to visualize the flight trajectory of the projectile
-      algae.withProjectileTrajectoryDisplayCallBack(
-          // Callback for when the note will eventually hit the target (if configured)
-          (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileSuccessfulShot",
-              pose3ds.toArray(Pose3d[]::new)),
-          // Callback for when the note will eventually miss the target, or if no target
-          // is configured
-          (pose3ds) -> Logger.recordOutput("FieldSimulation/Shooter/ProjectileUnsuccessfulShot",
-              pose3ds.toArray(Pose3d[]::new)));
-
-      arena.addGamePieceProjectile(algae);
-    }).withName("Fire.Algae");
-  }
 }