Demo mode

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/java/frc/robot/Constants.java

@@ -56,6 +56,7 @@ public static final class FeatureFlags {
         public static final boolean runScoring = true;
         public static final boolean runEndgame = true;
         public static final boolean runDrive = true;
+        public static final boolean demoMode = true;
 
         public static final boolean enableLEDS = true;
     }
@@ -114,6 +115,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 +185,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 {
@@ -553,6 +575,9 @@ public static final class ScoringConstants {
         public static final double hoodMaxVelocity = 0.5;
         public static final double hoodMaxAcceleration = 0.5;
 
+        public static final double demoShooterRPM = 1000;
+        public static final double demoAimAngle = 0.4;
+
         // NOTE - This should be monotonically increasing
         // Key - Distance in meters
         // Value - Aimer angle in radians

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

@@ -135,6 +135,9 @@ public void configureSubsystems() {
                 if (FeatureFlags.runDrive) {
                     driveTelemetry =
                             new Telemetry(DriveConstants.MaxSpeedMetPerSec, new TelemetryIOLive());
+                    if (FeatureFlags.demoMode) {
+                        drivetrain.setDemo(true);
+                    }
                 }
 
                 if (FeatureFlags.runScoring) {
@@ -143,6 +146,9 @@ public void configureSubsystems() {
                                     new ShooterIOTalon(),
                                     new AimerIORoboRio(),
                                     new HoodIOSparkFlex());
+                    if (FeatureFlags.demoMode) {
+                        scoringSubsystem.setDemo(true);
+                    }
                 }
 
                 if (FeatureFlags.runEndgame) {
@@ -393,6 +399,7 @@ private void configureBindings() {
         }
 
         if (FeatureFlags.runScoring) {
+            if(!FeatureFlags.demoMode) {
             scoringSubsystem.setDefaultCommand(new ShootWithGamepad(
                 () -> rightJoystick.getHID().getRawButton(4),
                 controller.getHID()::getRightBumper,
@@ -401,6 +408,9 @@ private void configureBindings() {
                 controller.getHID()::getAButton,
                 controller.getHID()::getBButton, scoringSubsystem,
                 FeatureFlags.runDrive ? drivetrain::getAlignTarget : () -> AlignTarget.NONE));
+            } else {
+                controller.y().onTrue(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.SHOOT))).onFalse(new InstantCommand(() -> scoringSubsystem.setAction(ScoringAction.WAIT)));
+            }
 
             rightJoystick.button(11).onTrue(new InstantCommand(() -> scoringSubsystem.setArmDisabled(true)));
             rightJoystick.button(16).onTrue(new InstantCommand(() -> scoringSubsystem.setArmDisabled(false)));

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

@@ -0,0 +1,133 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.AutomatedTeleopConstants;
+import frc.robot.Constants.FieldConstants;
+import frc.robot.subsystems.drive.CommandSwerveDrivetrain;
+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 java.util.function.Supplier;
+
+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;
+
+        addRequirements(scoringSubsystem, intakeSubsystem, 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() {
+        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);
+
+                if (intakeSubsystem.hasNote()) {
+                    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();
+                scoringSubsystem.setAction(ScoringAction.AIM);
+
+                // Once we are 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.driveToSpeaker();
+                scoringSubsystem.setAction(ScoringAction.SHOOT);
+
+                break;
+        }
+    }
+}