Shuffleboard abstraction

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

@@ -73,7 +73,7 @@ public static final class JetsonConstants {
         public static final int turretCameraPort = 1181;
         public static final int intakeCameraPort = 1182;
         public static final int jetsonPort = 5800;
-        public static final String jetsonIP = "10.1.92.94";
+        public static final String jetsonIP = "10.1.92.12";
     }
 
     public static final class ShuffleboardConstants {

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

@@ -126,7 +126,7 @@ public void testInit() {
         // Sweep intake from -1 to 1 power
         intakeSubsystem.setDriverOverride(true);
         for (double pow = -1; pow <= 1; pow += 0.1) {
-            intakeSubsystem.setIntakePower(pow);
+            intakeSubsystem.setDriverPower(pow);
             Timer.delay(0.1);
         }
         intakeSubsystem.setDriverOverride(false);

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

@@ -4,17 +4,12 @@
 
 package frc.robot;
 
-import java.util.List;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.networktables.EntryListenerFlags;
-import edu.wpi.first.networktables.EntryNotification;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
@@ -32,9 +27,9 @@
 import frc.robot.commands.tank.AutonRedBottomSequence;
 import frc.robot.commands.tank.AutonRedMiddleSequence;
 import frc.robot.commands.tank.AutonRedTopSequence;
-import frc.robot.commands.tank.FollowPathCommand;
 import frc.robot.commands.tank.PlebAutonSequence;
 import frc.robot.jetson.JetsonConnection;
+import frc.robot.shuffleboard.GRTShuffleboardTab;
 import frc.robot.subsystems.ClimbSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.TurretSubsystem;
@@ -77,11 +72,11 @@ public class RobotContainer {
         mechRBumper = new JoystickButton(mechController, XboxController.Button.kRightBumper.value);
 
     // Commands
-    private Command autonCommand;
+    private final SendableChooser<Command> autonChooser;
 
     // Debug flags
-    // Whether to run an auton path or skip auton and set starting position manually.
-    private static final boolean SKIP_AUTON = false;
+    // Whether to set starting position manually. This position will still be overriden if an auton sequence is run.
+    private static final boolean MANUAL_START_POS = true;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -111,25 +106,26 @@ public RobotContainer() {
         // Configure the button bindings
         configureButtonBindings();
 
-        // Instantiate auton command.
-        // If skipping autonomous, run an empty command in auton and set initial position
-        // from a manual hub distance. This assumes we are facing directly away from the hub
-        // at 0 degrees with a distance of `hubDist` between the robot and the hub.
-        if (SKIP_AUTON) {
+        // Add auton sequences to the chooser and add the chooser to shuffleboard
+        autonChooser = new SendableChooser<>();
+        autonChooser.addOption("Red top", new AutonRedTopSequence(this));
+        autonChooser.addOption("Red middle", new AutonRedMiddleSequence(this));
+        autonChooser.addOption("Red bottom", new AutonRedBottomSequence(this));
+        autonChooser.addOption("Blue top", new AutonBlueTopSequence(this));
+        autonChooser.addOption("Blue middle", new AutonBlueMiddleSequence(this));
+        autonChooser.addOption("Blue bottom", new AutonBlueBottomSequence(this));
+        autonChooser.setDefaultOption("Pleb auton", PlebAutonSequence.from(this));
+        autonChooser.addOption("Skip auton", new InstantCommand());
+
+        new GRTShuffleboardTab("Drivetrain").addWidget("Auton sequence", autonChooser);
+
+        // If setting initial position manually, start it at a position assuming we are facing the hub
+        // at a distance `hubDist` inches and 0 on the y axis.
+        // TODO: is this worth having be a flag at all?
+        if (MANUAL_START_POS) {
             double hubDist = 70;
             Pose2d initialPose = new Pose2d(Units.inchesToMeters(hubDist), 0, new Rotation2d());
             setInitialPosition(initialPose);
-
-            autonCommand = new InstantCommand();
-        } else {
-            // Set the auton command from the shuffleboard int.
-            // 1, 2, 3 -> red top, middle, bottom
-            // 4, 5, 6 -> blue top, middle, bottom
-            // 7 -> pleb auton sequence
-            // 8 -> InstantCommand (skip auton)
-            int autonSequence = 7;
-            Shuffleboard.getTab("Drivetrain").add("Auton sequence", autonSequence).getEntry()
-                .addListener(this::setAutonCommand, EntryListenerFlags.kImmediate | EntryListenerFlags.kUpdate | EntryListenerFlags.kNew);
         }
     }
 
@@ -150,7 +146,7 @@ public RobotContainer() {
      * X button -> start climb sequence (climb)
      */
     private void configureButtonBindings() {
-        // driveAButton.whenPressed(new RequestShotCommand(internalSubsystem));
+        driveAButton.whenPressed(new RequestShotCommand(internalSubsystem));
         // driveBButton.whenPressed(new DeployIntakeCommand(intakeSubsystem));
         // driveYButton.whenPressed(new RaiseIntakeCommand(intakeSubsystem));
         driveXButton.whenPressed(new InstantCommand(turretSubsystem::toggleClimb));
@@ -212,29 +208,12 @@ private void configureButtonBindings() {
         }, climbSubsystem));
     }
 
-    /**
-     * Sets the selected auton command from the shuffleboard integer value.
-     * @param change The EntryNotification representing a shuffleboard value change.
-     */
-    private void setAutonCommand(EntryNotification change) {
-        switch ((int) change.value.getDouble()) {
-            case 1: autonCommand = new AutonRedTopSequence(this); break;
-            case 2: autonCommand = new AutonRedMiddleSequence(this); break;
-            case 3: autonCommand = new AutonRedBottomSequence(this); break;
-            case 4: autonCommand = new AutonBlueTopSequence(this); break;
-            case 5: autonCommand = new AutonBlueMiddleSequence(this); break;
-            case 6: autonCommand = new AutonBlueBottomSequence(this); break;
-            case 7: autonCommand = new PlebAutonSequence(this); break;
-            case 8: autonCommand = new InstantCommand(); break;
-        }
-    }
-
     /**
      * Use this to pass the autonomous command to the main {@link Robot} class.
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        return autonCommand;
+        return autonChooser.getSelected();
     }
 
     /**

src/main/java/frc/robot/commands/intake/DeployIntakeCommand.java

@@ -5,16 +5,27 @@
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.IntakePosition;
 
+/**
+ * Deploys and runs the intake at a supplied power (defaults to 1.0).
+ */
 public class DeployIntakeCommand extends InstantCommand {
-    private final IntakeSubsystem intake;
+    private final IntakeSubsystem intakeSubsystem;
+    private final double intakePow;
 
-    public DeployIntakeCommand(IntakeSubsystem intake) {
-        this.intake = intake;
-        addRequirements(intake);
+    public DeployIntakeCommand(IntakeSubsystem intakeSubsystem, double intakePow) {
+        this.intakeSubsystem = intakeSubsystem;
+        this.intakePow = intakePow;
+        addRequirements(intakeSubsystem);
+    }
+
+    public DeployIntakeCommand(IntakeSubsystem intakeSubsystem) {
+        this(intakeSubsystem, 1.0);
     }
 
     @Override
     public void initialize() {
-        intake.setPosition(IntakePosition.DEPLOYED);
+        intakeSubsystem.setPosition(IntakePosition.DEPLOYED);
+        intakeSubsystem.setAutoOverride(true);
+        intakeSubsystem.setAutoPower(intakePow);
     }
 }

src/main/java/frc/robot/commands/intake/RaiseIntakeCommand.java

@@ -5,16 +5,20 @@
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.IntakePosition;
 
+/**
+ * Raises the intake and stops the intake rollers.
+ */
 public class RaiseIntakeCommand extends InstantCommand {
-    private final IntakeSubsystem intake;
+    private final IntakeSubsystem intakeSubsystem;
 
-    public RaiseIntakeCommand(IntakeSubsystem intake) {
-        this.intake = intake;
-        addRequirements(intake);
+    public RaiseIntakeCommand(IntakeSubsystem intakeSubsystem) {
+        this.intakeSubsystem = intakeSubsystem;
+        addRequirements(intakeSubsystem);
     }
 
     @Override
     public void initialize() {
-        intake.setPosition(IntakePosition.RAISED);
+        intakeSubsystem.setPosition(IntakePosition.RAISED);
+        intakeSubsystem.setAutoOverride(false);
     }
 }

src/main/java/frc/robot/commands/intake/RunIntakeCommand.java

@@ -20,8 +20,8 @@ public RunIntakeCommand(IntakeSubsystem intakeSubsystem, XboxController xboxCont
             double leftTrigger = xboxController.getLeftTriggerAxis();
             double rightTrigger = xboxController.getRightTriggerAxis();
 
-            intakeSubsystem.setIntakePower(rightTrigger - leftTrigger);
-            
+            intakeSubsystem.setDriverPower(rightTrigger - leftTrigger);
+
             // If the driver is supplying intake power with the triggers, override intake control
             if (leftTrigger > 0 || rightTrigger > 0) {
                 intakeSubsystem.setDriverOverride(true);

src/main/java/frc/robot/commands/tank/AutonBlueBottomSequence.java

@@ -19,7 +19,7 @@ public class AutonBlueBottomSequence extends GRTAutonSequence {
         Units.inchesToMeters(5.472), 
         Units.inchesToMeters(-60.540), 
         Rotation2d.fromDegrees(-66)); // TODO: are negative angles ok?
-    private static final Pose2d ballOnePose = localizeBallCoordinate(LEFT_BOTTOM_BLUE, 260.251594482);
+    private static final Pose2d ballOnePose = localizeToIntakeFront(LEFT_BOTTOM_BLUE, 260.251594482);
 
     public AutonBlueBottomSequence(RobotContainer robotContainer) {
         super(robotContainer, initialPose, ballOnePose);

src/main/java/frc/robot/commands/tank/AutonBlueMiddleSequence.java

@@ -21,8 +21,8 @@ public class AutonBlueMiddleSequence extends GRTAutonSequence {
         Units.inchesToMeters(-44.575), 
         Units.inchesToMeters(-41.328), 
         Rotation2d.fromDegrees(204));
-    private static Pose2d ballOnePose = localizeBallCoordinate(LEFT_MID_BLUE, 215.248230);
-    private static Pose2d ballTwoPose = localizeBallCoordinate(TERMINAL_BLUE, 225);
+    private static Pose2d ballOnePose = localizeToRobotFront(LEFT_MID_BLUE, 215.248230);
+    private static Pose2d ballTwoPose = localizeToIntakeFront(TERMINAL_BLUE, 225);
 
     public AutonBlueMiddleSequence(RobotContainer robotContainer) {
         super(robotContainer, initialPose, ballOnePose, ballTwoPose);

src/main/java/frc/robot/commands/tank/AutonBlueTopSequence.java

@@ -19,7 +19,7 @@ public class AutonBlueTopSequence extends GRTAutonSequence {
         Units.inchesToMeters(-41.328), 
         Units.inchesToMeters(44.575), 
         Rotation2d.fromDegrees(114));
-    private static final Pose2d ballOnePose = localizeBallCoordinate(LEFT_TOP_BLUE, 147.74823);
+    private static final Pose2d ballOnePose = localizeToRobotFront(LEFT_TOP_BLUE, 147.74823);
 
     public AutonBlueTopSequence(RobotContainer robotContainer) {
         super(robotContainer, initialPose, ballOnePose);

src/main/java/frc/robot/commands/tank/AutonRedBottomSequence.java

@@ -19,7 +19,7 @@ public class AutonRedBottomSequence extends GRTAutonSequence {
         Units.inchesToMeters(41.328), 
         Units.inchesToMeters(-44.575), 
         Rotation2d.fromDegrees(-66)); // TODO: are negative angles ok?
-    private static final Pose2d ballOnePose = localizeBallCoordinate(RIGHT_BOTTOM_RED, -32.2517702051);
+    private static final Pose2d ballOnePose = localizeToRobotFront(RIGHT_BOTTOM_RED, -32.2517702051);
 
     public AutonRedBottomSequence(RobotContainer robotContainer) {
         super(robotContainer, initialPose, ballOnePose);

src/main/java/frc/robot/commands/tank/AutonRedMiddleSequence.java

@@ -21,8 +21,8 @@ public class AutonRedMiddleSequence extends GRTAutonSequence {
         Units.inchesToMeters(44.575), 
         Units.inchesToMeters(41.328), 
         Rotation2d.fromDegrees(24));
-    private static Pose2d ballOnePose = localizeBallCoordinate(RIGHT_MID_RED, 35.2482302738);
-    private static Pose2d ballTwoPose = localizeBallCoordinate(TERMINAL_RED, 45);
+    private static Pose2d ballOnePose = localizeToRobotFront(RIGHT_MID_RED, 35.2482302738);
+    private static Pose2d ballTwoPose = localizeToIntakeFront(TERMINAL_RED, 45);
 
     public AutonRedMiddleSequence(RobotContainer robotContainer) {
         super(robotContainer, initialPose, ballOnePose, ballTwoPose);

src/main/java/frc/robot/commands/tank/AutonRedTopSequence.java

@@ -19,7 +19,7 @@ public class AutonRedTopSequence extends GRTAutonSequence {
         Units.inchesToMeters(-5.472), 
         Units.inchesToMeters(60.540), 
         Rotation2d.fromDegrees(114));
-    private static final Pose2d ballOnePose = localizeBallCoordinate(RIGHT_TOP_RED, 90);
+    private static final Pose2d ballOnePose = localizeToIntakeFront(RIGHT_TOP_RED, 90);
 
     public AutonRedTopSequence(RobotContainer robotContainer) {
         super(robotContainer, initialPose, ballOnePose);

src/main/java/frc/robot/commands/tank/GRTAutonSequence.java

@@ -20,10 +20,13 @@
  * The superclass for all auton sequences, containing helper methods and shared constructor logic.
  */
 public abstract class GRTAutonSequence extends SequentialCommandGroup {
+    private final RobotContainer robotContainer;
     private final TankSubsystem tankSubsystem;
     private final InternalSubsystem internalSubsystem;
     private final IntakeSubsystem intakeSubsystem;
 
+    private final Pose2d initialPose;
+
     /**
      * Creates a GRTAutonSequence from an initial and ball one pose. This is used for shared initialization logic
      * between all auton paths, as well as one-ball sequences like the top and bottom paths. Drives to the ball, 
@@ -34,31 +37,39 @@ public abstract class GRTAutonSequence extends SequentialCommandGroup {
      * @param ballOnePose The first ball pose of the sequence.
      */
     public GRTAutonSequence(RobotContainer robotContainer, Pose2d initialPose, Pose2d ballOnePose) {
+        this.robotContainer = robotContainer;
+        this.initialPose = initialPose;
+
         tankSubsystem = robotContainer.getTankSubsystem();
         internalSubsystem = robotContainer.getInternalSubsystem();
         intakeSubsystem = robotContainer.getIntakeSubsystem();
 
         addRequirements(tankSubsystem, internalSubsystem, intakeSubsystem);
-        robotContainer.setInitialPosition(initialPose);
 
         addCommands(
-            new DeployIntakeCommand(intakeSubsystem), // TODO: actually supply power to intake and rely on autodeploy
+            new DeployIntakeCommand(intakeSubsystem),
             new FollowPathCommand(tankSubsystem, initialPose, List.of(), ballOnePose)
                 .withTimeout(5),
             new ShootCommand(internalSubsystem),
             new ShootCommand(internalSubsystem)
         );
     }
 
+    @Override
+    public void initialize() {
+        robotContainer.setInitialPosition(initialPose);
+    }
+
     /**
      * Creates a GRTAutonSequence from an initial, ball one, back, and final pose. This is used for the red top and 
      * blue bottom paths, where we may not taxi if we just drive to the ball; the wall is too close. Drives to
-     * the ball, shoots twice, then drives in reverse through the back waypoint to the final (taxi) pose.
+     * the ball, shoots twice, then drives to the final pose.
      * 
      * @param robotContainer The RobotContainer instance, for calling `.setInitialPose()`.
      * @param initialPose The initial pose of the sequence.
      * @param ballOnePose The first ball pose of the sequence.
      * @param finalPose The ending pose of the sequence.
+     * @param finalReversed Whether the command should drive to the final pose in reverse.
      */
     public GRTAutonSequence(RobotContainer robotContainer, Pose2d initialPose, Pose2d ballOnePose, Pose2d finalPose, boolean finalReversed) {
         this(robotContainer, initialPose, ballOnePose);
@@ -99,24 +110,48 @@ public GRTAutonSequence(RobotContainer robotContainer, Pose2d initialPose, Pose2
     }
 
     /**
-     * Localizes a ball coordinate to a robot pose used in path following. The robot cannot path follow
-     * directly to the ball coordinate, as the robot's position is based on the center of the chassis;
-     * instead, the robot should path follow to the Pose2d localized from the intake and chassis width.
+     * Localizes a ball coordinate to a robot pose used in path following by using the coordinate of the ball
+     * touching the front of the robot at an approach of `angleOfApproach` degrees.
      * 
      * @param ball The ball coordinate, represented as a Translation2d.
      * @param angleOfApproach The angle the robot should approach the ball, in degrees.
      * @return The Pose2d to path follow to.
      */
-    protected static Pose2d localizeBallCoordinate(Translation2d ball, double angleOfApproach) {
-        // Robot length / 2
-        double h = 34 / 2;
+    protected static Pose2d localizeToRobotFront(Translation2d ball, double angleOfApproach) {
+        return localize(ball, angleOfApproach, 34 / 2); // Robot width / 2
+    }
+
+    /**
+     * Localizes a ball coordinate to a robot pose used in path following by using the coordinate of the ball
+     * touching the front of the intake at an approach of `angleOfApproach` degrees.
+     * 
+     * @param ball The ball coordinate, represented as a Translation2d.
+     * @param angleOfApproach The angle the robot should approach the ball, in degrees.
+     * @return The Pose2d to path follow to.
+     */
+    protected static Pose2d localizeToIntakeFront(Translation2d ball, double angleOfApproach) {
+        return localize(ball, angleOfApproach, 34 / 2 + 11.743); // Robot width / 2 + intake width
+    }
+
+    /**
+     * Localizes a ball coordinate to a robot pose used in path following by using the coordinate of the ball
+     * touching the center of the robot at an approach of `angleOfApproach` degrees.
+     * 
+     * @param ball The ball coordinate, represented as a Translation2d.
+     * @param angleOfApproach The angle the robot should approach the ball, in degrees.
+     * @return The Pose2d to path follow to.
+     */
+    protected static Pose2d localizeToRobotCenter(Translation2d ball, double angleOfApproach) {
+        return localize(ball, angleOfApproach, 0);
+    }
 
-        double rads = Math.toRadians(angleOfApproach);
+    private static Pose2d localize(Translation2d trans, double angle, double h) {
+        double rads = Math.toRadians(angle);
         double dx = Units.inchesToMeters(h * Math.cos(rads));
         double dy = Units.inchesToMeters(h * Math.sin(rads));
 
-        double x = ball.getX() - dx;
-        double y = ball.getY() - dy;
+        double x = trans.getX() - dx;
+        double y = trans.getY() - dy;
         return new Pose2d(x, y, new Rotation2d(rads));
     }
 }