-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Add skeleton of AutomatedTeleop command * Make AutomatedTeleop command drive automatically * Update secretary and add diagram for AutomatedTeleop command * add rotation override when align state is set to aligning * forgot format * remove align state manual in driveToPose / driveToPath * format * add demo mode to drivetrain * remove automated teleop drive to path with demo * add demo shooting * adjust demo constants and logic for shooter ready --------- Co-authored-by: aidnem <[email protected]> Co-authored-by: minhnguyenbhs <[email protected]>
- Loading branch information
3 people
authored
Nov 3, 2024
1 parent
857687c
commit cc1b40b
Showing
10 changed files
with
392 additions
and
53 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 | ||
| ``` |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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; | ||
| } | ||
| } | ||
| } |
Oops, something went wrong.