diff --git a/doc/AutomatedTeleop.csv b/doc/AutomatedTeleop.csv new file mode 100644 index 00000000..eec270ad --- /dev/null +++ b/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 \ No newline at end of file diff --git a/doc/AutomatedTeleop.md b/doc/AutomatedTeleop.md new file mode 100644 index 00000000..790eb9c4 --- /dev/null +++ b/doc/AutomatedTeleop.md @@ -0,0 +1,43 @@ +# AutomatedTeleop Subsystem +```mermaid +flowchart TD + +classDef state font-size:40px,padding:10px + +node0:::state +node0([START]) +node1:::state +node1([DRIVE_TO_SOURCE]) +node2:::state +node2([ACQUIRE_NOTE]) +node3:::state +node3([DRIVE_TO_SPEAKER]) +node4:::state +node4([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 +``` diff --git a/src/main/deploy/pathplanner/paths/shopSource.path b/src/main/deploy/pathplanner/paths/shopSource.path index bc433205..d4b42095 100644 --- a/src/main/deploy/pathplanner/paths/shopSource.path +++ b/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, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 4ad21a28..6da75682 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 14a0eea5..f6d45cc4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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()))); } } diff --git a/src/main/java/frc/robot/commands/AutomatedTeleop.java b/src/main/java/frc/robot/commands/AutomatedTeleop.java new file mode 100644 index 00000000..e24a620c --- /dev/null +++ b/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 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 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; + } + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java index 6423aafc..7f9fdbde 100644 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.pathfinding.LocalADStar; @@ -42,6 +43,7 @@ import frc.robot.utils.AllianceUtil; import frc.robot.utils.GeomUtil; import frc.robot.utils.InterpolateDouble; +import java.util.Optional; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -120,12 +122,15 @@ public enum AlignState { private Notifier simNotifier = null; private double lastSimTime; + private String lastCommandedPath = ""; private Command pathfindCommand = null; private Pose2d pathfindPose = new Pose2d(); private ChassisSpeeds stopSpeeds = new ChassisSpeeds(0, 0, 0); + private Rotation2d desiredHeading = new Rotation2d(); + public CommandSwerveDrivetrain( SwerveDrivetrainConstants driveTrainConstants, double OdometryUpdateFrequency, @@ -235,7 +240,7 @@ public void configurePathPlanner() { }, // Change this if the path needs to be flipped on red vs blue this); // Subsystem for requirements - // PPHolonomicDriveController.setRotationTargetOverride(this::getRotationTargetOverride); + PPHolonomicDriveController.setRotationTargetOverride(this::getOverrideRotation); // autoChooser = AutoBuilder.buildAutoChooser(); autoChooser.setDefaultOption("Default (nothing)", Commands.none()); // S1-W1-W2-W3 @@ -330,7 +335,7 @@ public void setTuningVolts(double tuningVolts) { private void controlDrivetrain() { Pose2d pose = getFieldToRobot.get(); - Rotation2d desiredHeading = pose.getRotation(); + desiredHeading = pose.getRotation(); if (alignState == AlignState.ALIGNING) { switch (alignTarget) { case AMP: @@ -545,21 +550,20 @@ public boolean atPathfindPose() { } public void driveToPose(Pose2d targetPose) { - this.setAlignState(AlignState.MANUAL); - pathfindCommand = getPathfindCommand(targetPose); pathfindCommand.schedule(); } public void driveToPath(String pathName) { - this.setAlignState(AlignState.MANUAL); + if (pathName == lastCommandedPath) { + // TODO: Determine if we need/want this check + // return; + } else { + lastCommandedPath = pathName; + } PathPlannerPath path = PathPlannerPath.fromPathFile(pathName); - // if(DriverStation.getAlliance() === Alliance.Blue) { - // path.flipPath(); - // } - PathConstraints constraints = new PathConstraints( 3.0, @@ -572,15 +576,31 @@ public void driveToPath(String pathName) { Logger.recordOutput("targetPose", target); }); + // Logging callback for the active path, this is sent as a list of poses + PathPlannerLogging.setLogActivePathCallback( + (poses) -> { + // TODO: Log the actual trajectory + }); + pathfindCommand = AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0); pathfindCommand.schedule(); } + public Optional getOverrideRotation() { + if (alignState == AlignState.ALIGNING) { + return Optional.of(desiredHeading); + } else { + return Optional.empty(); + } + } + public void stopDriveToPose() { if (pathfindCommand != null) { pathfindCommand.cancel(); } + lastCommandedPath = ""; + setGoalChassisSpeeds(stopSpeeds, true); } diff --git a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java index 7ab08f44..9413cfa3 100644 --- a/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java +++ b/src/main/java/frc/robot/subsystems/scoring/ScoringSubsystem.java @@ -407,7 +407,7 @@ private void temporarySetpoint() { } } - private double findDistanceToGoal() { + public double findDistanceToGoal() { Translation2d speakerPose = AllianceUtil.getFieldToSpeaker(); Pose2d robotPose = poseSupplier.get(); double distancetoGoal = diff --git a/src/main/java/frc/robot/utils/AllianceUtil.java b/src/main/java/frc/robot/utils/AllianceUtil.java index 278a1ec3..17998b28 100644 --- a/src/main/java/frc/robot/utils/AllianceUtil.java +++ b/src/main/java/frc/robot/utils/AllianceUtil.java @@ -88,6 +88,18 @@ public static Pose2d getPoseAgainstAmpZone() { return FieldConstants.robotAgainstRedAmpZone; } + public static Pose2d getPoseAgainstSource() { + if (!DriverStation.getAlliance().isEmpty()) { + switch (DriverStation.getAlliance().get()) { + case Blue: + return FieldConstants.robotAgainstRedSource; + case Red: + return FieldConstants.robotAgainstBlueSource; + } + } + return FieldConstants.robotAgainstRedSource; + } + public static Rotation2d getSourceHeading() { if (!DriverStation.getAlliance().isEmpty()) { switch (DriverStation.getAlliance().get()) { diff --git a/util/secretary/driver.py b/util/secretary/driver.py index f89f15cd..5f12f984 100644 --- a/util/secretary/driver.py +++ b/util/secretary/driver.py @@ -4,6 +4,9 @@ import subprocess def main(): + print("WARNING: Secretary does not work consistently at the moment.") + print("\tBe sure to verify that generated diagrams look correct before committing.") + secretary = os.path.join(os.path.dirname(__file__), "secretary.py"); if not os.path.isfile(secretary): print(f"[ERROR] secretary not found at {secretary}") diff --git a/util/secretary/secretary.py b/util/secretary/secretary.py index b94987b9..93c1eb35 100644 --- a/util/secretary/secretary.py +++ b/util/secretary/secretary.py @@ -73,7 +73,62 @@ def render_single(condition: str, in_node: str, out_true: str, out_false: str): print(f"{in_node} -.->|true| {out_true}") print(f"{in_node} -.->|false| {out_false}") -NON_TRANSITIONS = ['default', 'Not allowed', 'TBD', 'N/A', 'not possible'] +def render_one_check(row: list[str], states_row: list[str], in_node: str): + for i, condition in enumerate(row[1:]): + if condition in NON_TRANSITIONS: + continue + next_state = states_row[i] + next_state_node = get_node_id(next_state) + + if condition.startswith('!'): + print(f"{in_node} -.->|false| {next_state_node}") + else: + print(in_node + '{"' + condition + '"}') + print(f"{in_node} -.->|true| {next_state_node}") + +NON_TRANSITIONS = ['default', 'Not allowed', 'TBD'] + +def is_one_check(row: list[str]) -> bool: + condition = "" + found_true = False + found_false = False + + for transition in row[1:]: + if transition in NON_TRANSITIONS: + continue + if transition.startswith("!"): + if found_false: + # We've already found one condition starting with `!` + # This means this row can't be one check + return False + elif found_true: + if transition[1:] == condition: + found_false = True + else: + # The `!...` condition doesn't match the `...` condition + # Can't be one check + return False + else: + found_false = True + condition = transition[1:] + else: + if found_true: + # We've already found one condition not starting with `!` + # This means this row can't be one check + return False + elif found_false: + if transition == condition: + found_true = True + else: + # The `!...` condition doesn't match the `...` condition + # Can't be one check + return False + else: + found_true = True + condition = transition + + # If we found both a true and false without duplicates, it must be a one check + return found_true and found_false def render_table(rows: list[list[str]], combine: bool=False): states_row = rows[0][1:] # Chop off first, empty square @@ -102,6 +157,9 @@ def render_table(rows: list[list[str]], combine: bool=False): if num_transitions > 0: in_node = get_node_id() print(f"{current_state_node} --> {in_node}") + if is_one_check(row): + render_one_check(row, states_row, in_node) + continue for i, condition in enumerate(row[1:]): if condition in NON_TRANSITIONS: continue