Adds stateful command for Automated Teleop

doc/AutomatedTeleop.csv

@@ -0,0 +1,6 @@
+↓ Prev | Next → ,START,DRIVE_TO_SOURCE,ACQUIRE_NOTE,DRIVE_TO_SPEAKER,SHOOT_NOTE
+START,default,!hasNote(),Not allowed,hasNote(),Not allowed
+DRIVE_TO_SOURCE,Not allowed,default,Near Source? || Note Detected?,Not allowed,Not allowed
+ACQUIRE_NOTE,Not allowed,Not allowed,default,hasNote(),Not allowed
+DRIVE_TO_SPEAKER,Not allowed,!hasNote(),Not allowed,default,In range?
+SHOOT_NOTE,Not allowed,!hasNote(),Not allowed,Not allowed,default

doc/AutomatedTeleop.md

@@ -0,0 +1,43 @@
+# AutomatedTeleop Subsystem
+```mermaid
+flowchart TD
+
+classDef state font-size:40px,padding:10px
+
+node0:::state
+node0([<font size=11>START])
+node1:::state
+node1([<font size=11>DRIVE_TO_SOURCE])
+node2:::state
+node2([<font size=11>ACQUIRE_NOTE])
+node3:::state
+node3([<font size=11>DRIVE_TO_SPEAKER])
+node4:::state
+node4([<font size=11>SHOOT_NOTE])
+node0 --> node5
+node5 -.->|false| node1
+node5{"hasNote()"}
+node5 -.->|true| node3
+node1 --> node6
+node6{"Near Source?"}
+node6 -.->|true| node2
+node6 -.->|false| node7 
+node7{"Note Detected?"}
+node7 -.->|true| node2
+node7 -.->|false| node1 
+node2 --> node8
+node8{"hasNote()"}
+node8 -.->|true| node3
+node8 -.->|false| node2
+node3 --> node9
+node9{"!hasNote()"}
+node9 -.->|true| node1
+node9 -.->|false| node10
+node10{"In range?"}
+node10 -.->|true| node4
+node10 -.->|false| node3
+node4 --> node11
+node11{"!hasNote()"}
+node11 -.->|true| node1
+node11 -.->|false| node4
+```

src/main/deploy/pathplanner/paths/shopSource.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 6.965564788272434,
-        "y": 6.4355020138611705
+        "x": 6.38824683204922,
+        "y": 1.4115072813478964
       },
       "prevControl": null,
       "nextControl": {
-        "x": 7.595921477557241,
-        "y": 6.391523640190138
+        "x": 7.038208849565849,
+        "y": 1.2787831964552825
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 8.29957545629377,
-        "y": 6.801988461119779
+        "x": 8.090678181356362,
+        "y": 1.0574576195448695
       },
       "prevControl": {
-        "x": 7.874451177473783,
-        "y": 6.582096592764614
+        "x": 7.82867430549676,
+        "y": 1.2619774174733536
       },
       "nextControl": null,
       "isLocked": false,
@@ -39,7 +39,7 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": -150.461217740442,
+    "rotation": -60.0,
     "rotateFast": false
   },
   "reversed": false,

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

@@ -52,8 +52,8 @@ public static final class FeatureFlags {
         public static final boolean runVision = true;
         public static final boolean runLocalizer = true;
 
-        public static final boolean runIntake = false;
-        public static final boolean runScoring = false;
+        public static final boolean runIntake = true;
+        public static final boolean runScoring = true;
         public static final boolean runEndgame = false;
         public static final boolean runDrive = true;
 
@@ -114,6 +114,17 @@ public static final class DriveConstants {
         public static final double vYkD = 0.0;
     }
 
+    public static final class AutomatedTeleopConstants {
+        // TODO: Find constants for automated teleop command
+        public static final double intakeTimeoutSeconds = 0.5;
+        public static final double retryIntakeWaitSeconds = 0.5;
+
+        public static final double shootRangeMeters = 3.0;
+
+        // The distance from the source at which to stop driving the robot
+        public static final double sourceRangeMeters = 1.5;
+    }
+
     public static final class FieldConstants {
         public static final double lengthM = 16.451;
         public static final double widthM = 8.211;
@@ -173,6 +184,16 @@ public static final class FieldConstants {
 
         public static final Pose2d robotAgainstRedAmpZone =
                 new Pose2d(13.74, 7.68, Rotation2d.fromDegrees(-90));
+
+        public static final Pose2d robotAgainstBlueSource =
+                new Pose2d(14.82, 0.69, Rotation2d.fromDegrees(-60));
+
+        public static final Pose2d robotAgainstRedSource =
+                new Pose2d(1.63, 0.69, Rotation2d.fromDegrees(60));
+
+        // TODO: Find actual coordinates of shop source
+        public static final Pose2d robotAgainstShopSource =
+                new Pose2d(8.30, 6.80, Rotation2d.fromDegrees(60));
     }
 
     public static final class VisionConstants {

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

@@ -24,7 +24,7 @@
 import frc.robot.Constants.ScoringConstants;
 import frc.robot.Constants.TunerConstants;
 import frc.robot.Constants.VisionConstants;
-import frc.robot.commands.DriveWithJoysticks;
+import frc.robot.commands.AutomatedTeleop;
 import frc.robot.commands.EndgameSequence;
 import frc.robot.commands.ShootWithGamepad;
 import frc.robot.commands.WheelRadiusCharacterization;
@@ -878,17 +878,23 @@ public void testPeriodic() {}
     private void setUpDriveWithJoysticks() {
         if (FeatureFlags.runDrive) {
             drivetrain.setDefaultCommand(
-                    new DriveWithJoysticks(
+                    new AutomatedTeleop(
+                            scoringSubsystem,
+                            intakeSubsystem,
                             drivetrain,
-                            () -> leftJoystick.getY(),
-                            () -> leftJoystick.getX(),
-                            () -> rightJoystick.getX(),
-                            () -> !rightJoystick.trigger().getAsBoolean(),
-                            () -> rightJoystick.top().getAsBoolean(),
-                            () ->
-                                    VecBuilder.fill(
-                                            driveTelemetry.getVelocityX(),
-                                            driveTelemetry.getVelocityY())));
+                            driveTelemetry,
+                            () -> driveTelemetry.getFieldToRobot()));
+            // new DriveWithJoysticks(
+            //         drivetrain,
+            //         () -> leftJoystick.getY(),
+            //         () -> leftJoystick.getX(),
+            //         () -> rightJoystick.getX(),
+            //         () -> !rightJoystick.trigger().getAsBoolean(),
+            //         () -> rightJoystick.top().getAsBoolean(),
+            //         () ->
+            //                 VecBuilder.fill(
+            //                         driveTelemetry.getVelocityX(),
+            //                         driveTelemetry.getVelocityY())));
         }
     }
 

src/main/java/frc/robot/commands/AutomatedTeleop.java

@@ -0,0 +1,159 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
+import frc.robot.Constants.AutomatedTeleopConstants;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
+import frc.robot.subsystems.drive.CommandSwerveDrivetrain.AlignState;
+import frc.robot.subsystems.drive.CommandSwerveDrivetrain.AlignTarget;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.intake.IntakeSubsystem.IntakeAction;
+import frc.robot.subsystems.scoring.ScoringSubsystem;
+import frc.robot.subsystems.scoring.ScoringSubsystem.ScoringAction;
+import frc.robot.telemetry.Telemetry;
+import frc.robot.utils.notesimulator.NoteManager;
+import java.util.function.Supplier;
+import org.littletonrobotics.junction.Logger;
+
+public class AutomatedTeleop extends Command {
+    ScoringSubsystem scoringSubsystem;
+    IntakeSubsystem intakeSubsystem;
+    CommandSwerveDrivetrain drivetrain;
+
+    private Supplier<Pose2d> poseSupplier = () -> new Pose2d();
+
+    Telemetry telemetry;
+
+    private enum State {
+        START,
+        DRIVE_TO_SOURCE,
+        ACQUIRE_NOTE,
+        DRIVE_TO_SPEAKER,
+        SHOOT_NOTE,
+    }
+
+    private State state;
+
+    public AutomatedTeleop(
+            ScoringSubsystem scoringSubsystem,
+            IntakeSubsystem intakeSubsystem,
+            CommandSwerveDrivetrain drivetrain,
+            Telemetry telemetry,
+            Supplier<Pose2d> poseSupplier) {
+        this.scoringSubsystem = scoringSubsystem;
+        this.intakeSubsystem = intakeSubsystem;
+        this.drivetrain = drivetrain;
+        this.telemetry = telemetry;
+
+        this.poseSupplier = poseSupplier;
+
+        if (scoringSubsystem != null) {
+            addRequirements(scoringSubsystem);
+        }
+        if (intakeSubsystem != null) {
+            addRequirements(intakeSubsystem);
+        }
+        if (drivetrain != null) {
+            addRequirements(drivetrain);
+        }
+    }
+
+    @Override
+    public void initialize() {
+        this.state = State.START;
+
+        scoringSubsystem.setAction(ScoringAction.WAIT);
+        scoringSubsystem.forceHood(false);
+    }
+
+    private double findDistanceToSource() {
+        // NOTE: Leave one of the following lines commented out depending on the scenario:
+
+        // Uncomment this line for actual production code:
+        // Pose2d sourcePose = AllianceUtil.getPoseAgainstSource();
+
+        // Uncomment this line for testing with shop source
+        Pose2d sourcePose = FieldConstants.robotAgainstShopSource;
+
+        Pose2d robotPose = poseSupplier.get();
+        double distancetoGoal =
+                Math.sqrt(
+                        Math.pow(Math.abs(robotPose.getX() - sourcePose.getX()), 2)
+                                + Math.pow(Math.abs(robotPose.getY() - sourcePose.getY()), 2));
+        return distancetoGoal;
+    }
+
+    @Override
+    public void execute() {
+        Logger.recordOutput("automatedTeleop/State", state.toString());
+        switch (state) {
+            case START:
+                if (intakeSubsystem.hasNote()) {
+                    state = State.DRIVE_TO_SPEAKER;
+                } else {
+                    state = State.DRIVE_TO_SOURCE;
+                }
+                break;
+            case DRIVE_TO_SOURCE:
+                drivetrain.driveToSource();
+
+                scoringSubsystem.setAction(ScoringAction.WAIT);
+                // Once robot reaches the source, stop driving and try to acquire a note
+                if (findDistanceToSource()
+                        < AutomatedTeleopConstants.sourceRangeMeters /* || note detected */) {
+                    state = State.ACQUIRE_NOTE;
+                }
+                break;
+            case ACQUIRE_NOTE:
+                // TODO: Fully implement automation of intake
+
+                drivetrain.stopDriveToPose();
+                intakeSubsystem.run(IntakeAction.INTAKE);
+
+                NoteManager.intake();
+
+                if (intakeSubsystem.hasNote()
+                        || (Constants.currentMode == Constants.Mode.SIM
+                                && NoteManager.noteInRobot())) {
+                    state = State.DRIVE_TO_SPEAKER;
+                }
+                break;
+            case DRIVE_TO_SPEAKER:
+                // If we don't have a note, go get one
+                // This will work for if we drop our note or for after we've shot
+                if (!intakeSubsystem.hasNote()) {
+                    // TODO: Use note vision to pick up a nearby note rather than going to source?
+                    state = State.DRIVE_TO_SOURCE;
+                }
+
+                drivetrain.driveToSpeaker();
+
+                // Begin aligning to the speaker so robot is already aimed when it's ready to shoot
+                drivetrain.setAlignTarget(AlignTarget.SPEAKER);
+                drivetrain.setAlignState(AlignState.ALIGNING);
+
+                scoringSubsystem.setAction(ScoringAction.AIM);
+
+                // Once robot is in range, shoot
+                if (scoringSubsystem.findDistanceToGoal()
+                        < AutomatedTeleopConstants.shootRangeMeters) {
+                    state = State.SHOOT_NOTE;
+                }
+                break;
+            case SHOOT_NOTE:
+                if (!intakeSubsystem.hasNote()) {
+                    state = State.DRIVE_TO_SOURCE;
+                }
+
+                drivetrain.setAlignTarget(AlignTarget.SPEAKER);
+                drivetrain.setAlignState(AlignState.ALIGNING);
+                drivetrain.driveToSpeaker();
+
+                scoringSubsystem.setAction(ScoringAction.SHOOT);
+
+                break;
+        }
+    }
+}