Skip to content

Commit

Permalink
Auto selector cleanup (#60)
Browse files Browse the repository at this point in the history 
* expose initial pose of autonomous mode

* A little cleanup

* A little cleanup

* add pose getter

---------

Co-authored-by: nlaverdure <[email protected]>
  • Loading branch information
@caparomula @nlaverdure
caparomula and nlaverdure authored Jan 29, 2025
1 parent 224c1db commit 9e20bb2
Show file tree
Hide file tree
Showing 2 changed files with 73 additions and 83 deletions.
45 changes: 27 additions & 18 deletions src/main/java/frc/lib/AutoSelector.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,18 +10,19 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
import java.util.function.Supplier;

public class AutoSelector {

private Optional<AutoOption> currentAutoOption;
private DigitalInput[] m_switchPositions;
private Supplier<Alliance> m_allianceColorSupplier;
private List<AutoOption> m_autoOptions;
private EventLoop m_loop = new EventLoop();
private BooleanEvent m_changedAutoSelection;
private DigitalInput[] switchPositions;
private Supplier<Alliance> allianceColorSupplier;
private List<AutoOption> autoOptions = new ArrayList<>();
private EventLoop eventLoop = new EventLoop();
private BooleanEvent autoSelectionChanged;
private StructPublisher<Pose2d> initialPosePublisher =
NetworkTableInstance.getDefault()
.getStructTopic("AutonomousInitialPose", Pose2d.struct)
Expand All @@ -34,37 +35,39 @@ public class AutoSelector {
* @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;
public AutoSelector(int[] ports, Supplier<Alliance> allianceColorSupplier) {
this.allianceColorSupplier = allianceColorSupplier;

m_switchPositions = new DigitalInput[ports.length];
switchPositions = new DigitalInput[ports.length];
for (int i = 0; i < ports.length; i++) {
m_switchPositions[i] = new DigitalInput(ports[i]);
switchPositions[i] = new DigitalInput(ports[i]);
}

m_changedAutoSelection = new BooleanEvent(m_loop, () -> updateAuto());
autoSelectionChanged = new BooleanEvent(eventLoop, () -> updateAuto());
}

public void addAuto(AutoOption newAuto) {
autoOptions.add(newAuto);
}

/**
* @return The position of the autonomous selection switch
*/
public int getSwitchPosition() {
for (int i = 0; i < m_switchPositions.length; i++) {
if (!m_switchPositions[i].get()) {
for (int i = 0; i < switchPositions.length; i++) {
if (!switchPositions[i].get()) {
return i + 1;
}
}
return 0; // failure of the physical switch
}

private Alliance getAllianceColor() {
return m_allianceColorSupplier.get();
return allianceColorSupplier.get();
}

private Optional<AutoOption> findMatchingOption() {
return m_autoOptions.stream()
return autoOptions.stream()
.filter(o -> o.getColor() == getAllianceColor())
.filter(o -> o.getOption() == getSwitchPosition())
.findFirst();
Expand All @@ -83,7 +86,7 @@ private boolean updateAuto() {
* @return Object for binding a command to a change in autonomous mode selection
*/
public Trigger getChangedAutoSelection() {
return m_changedAutoSelection.castTo(Trigger::new);
return autoSelectionChanged.castTo(Trigger::new);
}

/** Schedules the command corresponding to the selected autonomous mode */
Expand All @@ -97,7 +100,7 @@ public void cancelAuto() {
}

public void disabledPeriodic() {
m_loop.poll();
eventLoop.poll();
SmartDashboard.putNumber("AutoSelectorSwitchPosition", getSwitchPosition());

currentAutoOption.ifPresentOrElse(
Expand All @@ -113,4 +116,10 @@ public void disabledPeriodic() {
initialPosePublisher.set(new Pose2d());
});
}

public Optional<Pose2d> getInitialPose() {
return currentAutoOption.isPresent()
? currentAutoOption.get().getInitialPose()
: Optional.empty();
}
}
111 changes: 46 additions & 65 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,29 +28,22 @@
import frc.robot.drivetrain.commands.DriveToPoseCommand;
import frc.robot.drivetrain.commands.ZorroDriveCommand;
import frc.robot.vision.Vision;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

public class Robot extends TimedRobot {

private List<AutoOption> m_autoOptions = new ArrayList<>();

private final PowerDistribution m_powerDistribution = new PowerDistribution(1, ModuleType.kRev);
private final AllianceSelector m_allianceSelector;
private final AutoSelector m_autoSelector;
private final Drivetrain m_swerve;
private final LEDs m_LEDs;
private final Vision m_vision;

// private final AutoFactory m_autoFactory;

private CommandZorroController m_driver;
private CommandXboxController m_operator;

private int m_usb_check_delay = OIConstants.kUSBCheckNumLoops;

private final PowerDistribution powerDistribution = new PowerDistribution(1, ModuleType.kRev);
private final AllianceSelector allianceSelector =
new AllianceSelector(AutoConstants.kAllianceColorSelectorPort);
private final AutoSelector autoSelector =
new AutoSelector(
AutoConstants.kAutonomousModeSelectorPorts, allianceSelector::getAllianceColor);
private final Drivetrain swerve = new Drivetrain(allianceSelector::fieldRotated);
private final LEDs leds = new LEDs();
private final Vision vision = new Vision();
private CommandZorroController driver;
private CommandXboxController operator;
private int usbCheckDelay = OIConstants.kUSBCheckNumLoops;
private Map<String, StructPublisher<Pose2d>> posePublishers = new HashMap<>();

private Pose2d[] targetPoses = {
Expand All @@ -59,26 +52,14 @@ public class Robot extends TimedRobot {
};

public Robot() {
m_allianceSelector = new AllianceSelector(AutoConstants.kAllianceColorSelectorPort);

m_autoSelector =
new AutoSelector(
AutoConstants.kAutonomousModeSelectorPorts,
m_allianceSelector::getAllianceColor,
m_autoOptions);

m_swerve = new Drivetrain(m_allianceSelector::fieldRotated);
m_LEDs = new LEDs();
m_vision = new Vision();

configureButtonBindings();
configureEventBindings();
configureAutoOptions();

// Create a button on Smart Dashboard to reset the encoders.
SmartDashboard.putData(
"Align Encoders",
new InstantCommand(() -> m_swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true));
new InstantCommand(() -> swerve.zeroAbsTurningEncoderOffsets()).ignoringDisable(true));
}

@Override
Expand All @@ -88,26 +69,26 @@ public void robotInit() {
DataLogManager.start();
DriverStation.startDataLog(DataLogManager.getLog());

m_swerve.setDefaultCommand(
new ZorroDriveCommand(m_swerve, DriveConstants.kDriveKinematics, m_driver.getHID()));
swerve.setDefaultCommand(
new ZorroDriveCommand(swerve, DriveConstants.kDriveKinematics, driver.getHID()));
}

@Override
public void robotPeriodic() {
CommandScheduler.getInstance().run();
checkVision();
SmartDashboard.putData(m_driver.getHID());
SmartDashboard.putData(m_operator.getHID());
SmartDashboard.putData(m_powerDistribution);
SmartDashboard.putData(driver.getHID());
SmartDashboard.putData(operator.getHID());
SmartDashboard.putData(powerDistribution);
}

@Override
public void disabledInit() {
m_LEDs.setDefaultCommand(
m_LEDs.createDisabledCommand(
m_autoSelector::getSwitchPosition,
m_allianceSelector::getAllianceColor,
m_allianceSelector::agreementInAllianceInputs));
leds.setDefaultCommand(
leds.createDisabledCommand(
autoSelector::getSwitchPosition,
allianceSelector::getAllianceColor,
allianceSelector::agreementInAllianceInputs));
}

@Override
Expand All @@ -118,33 +99,33 @@ public void disabledPeriodic() {
* Only check if controllers changed every kUSBCheckNumLoops loops of disablePeriodic().
* This prevents us from hammering on some routines that cause the RIO to lock up.
*/
m_usb_check_delay--;
if (0 >= m_usb_check_delay) {
m_usb_check_delay = OIConstants.kUSBCheckNumLoops;
usbCheckDelay--;
if (0 >= usbCheckDelay) {
usbCheckDelay = OIConstants.kUSBCheckNumLoops;
if (ControllerPatroller.getInstance().controllersChanged()) {
// Reset the joysticks & button mappings.
configureButtonBindings();
}
}

m_allianceSelector.disabledPeriodic();
m_autoSelector.disabledPeriodic();
allianceSelector.disabledPeriodic();
autoSelector.disabledPeriodic();
}

@Override
public void autonomousInit() {
m_autoSelector.scheduleAuto();
m_LEDs.setDefaultCommand(m_LEDs.createEnabledCommand());
autoSelector.scheduleAuto();
leds.setDefaultCommand(leds.createEnabledCommand());
}

@Override
public void autonomousPeriodic() {}

@Override
public void teleopInit() {
m_autoSelector.cancelAuto();
m_LEDs.setDefaultCommand(m_LEDs.createEnabledCommand());
m_swerve.resetHeadingOffset();
autoSelector.cancelAuto();
leds.setDefaultCommand(leds.createEnabledCommand());
swerve.resetHeadingOffset();
}

@Override
Expand All @@ -167,8 +148,8 @@ public void configureButtonBindings() {

// We use two different types of controllers - Joystick & XboxController.
// Create objects of the specific types.
m_driver = new CommandZorroController(cp.findDriverPort());
m_operator = new CommandXboxController(cp.findOperatorPort());
driver = new CommandZorroController(cp.findDriverPort());
operator = new CommandXboxController(cp.findOperatorPort());

configureDriverButtonBindings();
configureOperatorButtonBindings();
Expand All @@ -178,26 +159,26 @@ public void configureButtonBindings() {
private void configureDriverButtonBindings() {

// Reset heading
m_driver.DIn()
.onTrue(new InstantCommand(() -> m_swerve.setHeadingOffset())
driver.DIn()
.onTrue(new InstantCommand(() -> swerve.setHeadingOffset())
.ignoringDisable(true));

m_driver.AIn()
.whileTrue(new DriveToPoseCommand(m_swerve, m_vision, () -> getNearestPose(m_swerve.getPose(), targetPoses)));
driver.AIn()
.whileTrue(new DriveToPoseCommand(swerve, vision, () -> getNearestPose(swerve.getPose(), targetPoses)));

}
// spotless:on

private void configureOperatorButtonBindings() {}

private void configureEventBindings() {
m_autoSelector.getChangedAutoSelection().onTrue(m_LEDs.createChangeAutoAnimationCommand());
autoSelector.getChangedAutoSelection().onTrue(leds.createChangeAutoAnimationCommand());
}

private void configureAutoOptions() {
m_autoOptions.add(new AutoOption(Alliance.Red, 4));
m_autoOptions.add(new AutoOption(Alliance.Blue, 1, new ExampleAuto(m_swerve)));
m_autoOptions.add(new AutoOption(Alliance.Blue, 2));
autoSelector.addAuto(new AutoOption(Alliance.Red, 4));
autoSelector.addAuto(new AutoOption(Alliance.Blue, 1, new ExampleAuto(swerve)));
autoSelector.addAuto(new AutoOption(Alliance.Blue, 2));
}

/**
Expand All @@ -208,7 +189,7 @@ private void configureAutoOptions() {
* @return Current in Amps on the PDH channel corresponding to the motor channel
*/
public double getPDHCurrent(int CANBusPort) {
return m_powerDistribution.getCurrent(CANBusPort - 10);
return powerDistribution.getCurrent(CANBusPort - 10);
}

public Pose2d getNearestPose(Pose2d currentPose, Pose2d[] targetPoses) {
Expand Down Expand Up @@ -236,11 +217,11 @@ private synchronized StructPublisher<Pose2d> getPose2dPublisher(String name) {
}

protected void checkVision() {
m_vision
vision
.getPoseEstimates()
.forEach(
est -> {
m_swerve.addVisionMeasurement(
swerve.addVisionMeasurement(
est.pose().estimatedPose.toPose2d(), est.pose().timestampSeconds, est.stdev());
getPose2dPublisher(est.name()).set(est.pose().estimatedPose.toPose2d());
});
Expand Down

0 comments on commit 9e20bb2

Please sign in to comment.