Shuffleboard abstraction

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

@@ -52,7 +52,9 @@ public static final class InternalConstants {
     public static final class IntakeConstants {
         public static final int intakePort = 12;
         public static final int deploymentPort = 11;
-        public static final int limitSwitchPort = 3;
+
+        public static final int tLimitSwitchPort = 3;
+        public static final int bLimitSwitchPort = 2;
     }
 
     public static final class ClimbConstants {
@@ -73,7 +75,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,7 @@ public class RobotContainer {
         mechRBumper = new JoystickButton(mechController, XboxController.Button.kRightBumper.value);
 
     // Commands
-    private Command autonCommand;
-
-    // Debug flags
-    // Whether to run an auton path or skip auton and set starting position manually.
-    private static final boolean SKIP_AUTON = false;
+    private final SendableChooser<Command> autonChooser;
 
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -111,26 +102,31 @@ 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) {
-            double hubDist = 70;
-            Pose2d initialPose = new Pose2d(Units.inchesToMeters(hubDist), 0, new Rotation2d());
-            setInitialPosition(initialPose);
+        // 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);
+
+        double hubDist = 70;
+        Pose2d initialPose = new Pose2d(Units.inchesToMeters(hubDist), 0, new Rotation2d());
+
+        // Setting initial position assuming we are facing the hub
+        // at a distance `hubDist` inches and 0 on the y axis.
+        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);
-        }
+        // Shuffleboard manual reset button
+        new GRTShuffleboardTab("Turret").addWidget("Zero position", new InstantCommand(() -> {
+            setInitialPosition(initialPose);
+            turretSubsystem.zeroEncoders();
+        }, turretSubsystem, tankSubsystem));
     }
 
     /**
@@ -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);