-
Notifications
You must be signed in to change notification settings - Fork 0
Shooter test #203
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Open
linglejack06
wants to merge
12
commits into
main
Choose a base branch
from
shooter-test
base: main
Could not load branches
Branch not found: {{ refName }}
Loading
Could not load tags
Nothing to show
Loading
Are you sure you want to change the base?
Some commits from the old base branch may be removed from the timeline,
and old review comments may become outdated.
Open
Shooter test #203
Changes from all commits
Commits
Show all changes
12 commits
Select commit
Hold shift + click to select a range
1070c23
Add skeleton of AutomatedTeleop command
aidnem c6914ef
Make AutomatedTeleop command drive automatically
aidnem b0f2ee2
Update secretary and add diagram for AutomatedTeleop command
aidnem 092ac53
add rotation override when align state is set to aligning
linglejack06 2a13c41
forgot format
linglejack06 b4fd04a
remove align state manual in driveToPose / driveToPath
linglejack06 ad95bd3
format
linglejack06 465aa7f
add demo mode to drivetrain
linglejack06 88c899e
remove automated teleop drive to path with demo
linglejack06 e755e95
add demo shooting
linglejack06 35cb5c0
add set output methods to scoring to remove activation of motors
linglejack06 1b75915
fix aim encoder position and fix demo mode errors (drive + shooting)
gColliver File filter
Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
There are no files selected for viewing
This file contains hidden or 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 hidden or 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 hidden or 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 hidden or 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 hidden or 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 hidden or 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; | ||
| } | ||
| } | ||
| } |
This file contains hidden or 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
Oops, something went wrong.
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Uh oh!
There was an error while loading. Please reload this page.