-
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 choreo; change approach to organizing and selecting auto modes (#5)
* add choreo, move auton and alliance switches to util subsystems * alliance, auto selectors publish booleanEvents when changed * add javadoc
- Loading branch information
1 parent
3a03f52
commit 6644b9a
Showing
12 changed files
with
464 additions
and
309 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,72 @@ | ||
| package frc.lib; | ||
|
|
||
| import edu.wpi.first.wpilibj.DigitalInput; | ||
| import edu.wpi.first.wpilibj.DriverStation; | ||
| import edu.wpi.first.wpilibj.DriverStation.Alliance; | ||
| import edu.wpi.first.wpilibj.event.BooleanEvent; | ||
| import edu.wpi.first.wpilibj.event.EventLoop; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import java.util.Optional; | ||
|
|
||
| public class AllianceSelector { | ||
|
|
||
| private Alliance m_currentColor; | ||
| private DigitalInput m_allianceSelectionSwitch; | ||
| private EventLoop m_loop; | ||
|
|
||
| /** | ||
| * Constructs an alliance color selector switch | ||
| * | ||
| * @param port DIO port for reading the alliance color input | ||
| */ | ||
| public AllianceSelector(int port) { | ||
| this.m_allianceSelectionSwitch = new DigitalInput(port); | ||
| } | ||
|
|
||
| private Alliance getAllianceFromSwitch() { | ||
| return m_allianceSelectionSwitch.get() ? Alliance.Red : Alliance.Blue; | ||
| } | ||
|
|
||
| private boolean updateAlliance() { | ||
| Alliance m_newColor = getAllianceFromSwitch(); | ||
|
|
||
| if (m_newColor.equals(m_currentColor)) return false; | ||
| else { | ||
| m_currentColor = m_newColor; | ||
| return true; | ||
| } | ||
| } | ||
|
|
||
| public BooleanEvent changedAlliance = new BooleanEvent(m_loop, () -> updateAlliance()); | ||
|
|
||
| private boolean agreementInAllianceInputs() { | ||
| Optional<Alliance> allianceFromFMS = DriverStation.getAlliance(); | ||
| Alliance allianceFromSwitch = getAllianceFromSwitch(); | ||
|
|
||
| if (allianceFromFMS.isPresent()) { | ||
| return allianceFromSwitch.equals(allianceFromFMS.get()); | ||
| } else return false; | ||
| } | ||
|
|
||
| public BooleanEvent agreementInAllianceInputs = | ||
| new BooleanEvent(m_loop, () -> agreementInAllianceInputs()); | ||
|
|
||
| /** | ||
| * @return Whether the field is rotated from the driver's perspective | ||
| */ | ||
| public boolean fieldRotated() { | ||
| return m_currentColor.equals(Alliance.Red); | ||
| } | ||
|
|
||
| /** | ||
| * @return The current alliance | ||
| */ | ||
| public Alliance getAllianceColor() { | ||
| return m_currentColor; | ||
| } | ||
|
|
||
| public void disabledPeriodic() { | ||
| m_loop.poll(); | ||
| SmartDashboard.putString("Alliance Color", getAllianceColor().name()); | ||
| } | ||
| } |
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,56 @@ | ||
| package frc.lib; | ||
|
|
||
| import edu.wpi.first.wpilibj.DriverStation.Alliance; | ||
| import frc.robot.autos.ChoreoAuto; | ||
|
|
||
| public class AutoOption { | ||
| private Alliance m_color; | ||
| private int m_option; | ||
| private ChoreoAuto m_auto; | ||
|
|
||
| /** | ||
| * Constructs a selectable autonomous mode option | ||
| * | ||
| * @param color Alliance for which the option is valid | ||
| * @param option Selector switch index for which the option is valid | ||
| * @param auto Command which runs the autonomous mode | ||
| */ | ||
| public AutoOption(Alliance color, int option, ChoreoAuto auto) { | ||
| this.m_color = color; | ||
| this.m_option = option; | ||
| this.m_auto = auto; | ||
| } | ||
|
|
||
| /** | ||
| * Constructs a null autonomous mode option | ||
| * | ||
| * @param color Alliance for which the option is valid | ||
| * @param option Selector switch index for which the option is valid | ||
| */ | ||
| public AutoOption(Alliance color, int option) { | ||
| this.m_color = color; | ||
| this.m_option = option; | ||
| this.m_auto = null; | ||
| } | ||
|
|
||
| /** | ||
| * @return Alliance for which the option is valid | ||
| */ | ||
| public Alliance getColor() { | ||
| return this.m_color; | ||
| } | ||
|
|
||
| /** | ||
| * @return Selector switch index for which the option is valid | ||
| */ | ||
| public int getOption() { | ||
| return this.m_option; | ||
| } | ||
|
|
||
| /** | ||
| * @return The command which runs the selected autonomous mode | ||
| */ | ||
| public ChoreoAuto getChoreoAuto() { | ||
| return this.m_auto; | ||
| } | ||
| } |
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,91 @@ | ||
| package frc.lib; | ||
|
|
||
| import edu.wpi.first.wpilibj.DigitalInput; | ||
| import edu.wpi.first.wpilibj.DriverStation.Alliance; | ||
| import edu.wpi.first.wpilibj.event.BooleanEvent; | ||
| import edu.wpi.first.wpilibj.event.EventLoop; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import frc.robot.autos.ChoreoAuto; | ||
| import java.util.List; | ||
| import java.util.function.Supplier; | ||
|
|
||
| public class AutoSelector { | ||
|
|
||
| private ChoreoAuto m_currentAuto; | ||
| private DigitalInput[] m_switchPositions; | ||
| private Supplier<Alliance> m_allianceColorSupplier; | ||
| private List<AutoOption> m_autoOptions; | ||
| private EventLoop m_loop; | ||
|
|
||
| /** | ||
| * Constructs an autonomous selector switch | ||
| * | ||
| * @param ports An array of DIO ports for selecting an autonomous mode | ||
| * @param allianceColorSupplier A method that supplies the current alliance color | ||
| * @param autoOptions An array of autonomous mode options | ||
| */ | ||
| public AutoSelector( | ||
| int[] ports, Supplier<Alliance> allianceColorSupplier, List<AutoOption> autoOptions) { | ||
| this.m_allianceColorSupplier = allianceColorSupplier; | ||
| this.m_autoOptions = autoOptions; | ||
|
|
||
| m_switchPositions = new DigitalInput[ports.length]; | ||
|
|
||
| for (int i = 0; i < ports.length; i++) { | ||
| m_switchPositions[i] = new DigitalInput(ports[i]); | ||
| } | ||
| } | ||
|
|
||
| private int getSwitchPosition() { | ||
| for (int i = 0; i < m_switchPositions.length; i++) { | ||
| if (!m_switchPositions[i].get()) { | ||
| return i + 1; | ||
| } | ||
| } | ||
| return 0; // failure of the physical switch | ||
| } | ||
|
|
||
| private ChoreoAuto findMatchingOption() { | ||
| int switchPosition = getSwitchPosition(); | ||
| Alliance color = m_allianceColorSupplier.get(); | ||
|
|
||
| for (int i = 0; i < m_autoOptions.size(); i++) { | ||
| if (m_autoOptions.get(i).getColor() == color) | ||
| if (m_autoOptions.get(i).getOption() == switchPosition) { | ||
| return m_autoOptions.get(i).getChoreoAuto(); | ||
| } | ||
| } | ||
| return null; | ||
| } | ||
|
|
||
| private boolean updateAuto() { | ||
| ChoreoAuto m_newAuto = findMatchingOption(); | ||
| if (m_newAuto.equals(m_currentAuto)) return false; | ||
| else { | ||
| m_currentAuto = m_newAuto; | ||
| return true; | ||
| } | ||
| } | ||
|
|
||
| public BooleanEvent changedAuto = new BooleanEvent(m_loop, () -> updateAuto()); | ||
|
|
||
| /** Schedules the command corresponding to the selected autonomous mode */ | ||
| public void scheduleAuto() { | ||
| if (m_currentAuto != null) m_currentAuto.schedule(); | ||
| } | ||
|
|
||
| /** Deschedules the command corresponding to the selected autonomous mode */ | ||
| public void cancelAuto() { | ||
| if (m_currentAuto != null) m_currentAuto.cancel(); | ||
| } | ||
|
|
||
| public void disabledPeriodic() { | ||
| m_loop.poll(); | ||
|
|
||
| if (m_currentAuto != null) { | ||
| SmartDashboard.putString("Auto", m_currentAuto.getName()); | ||
| } else { | ||
| SmartDashboard.putString("Auto", "Null"); | ||
| } | ||
| } | ||
| } |
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
Oops, something went wrong.