Skip to content
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.

Sign up for GitHub

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

Auto selector cleanup #60

Merged
merged 5 commits into from
Jan 29, 2025
Merged

Auto selector cleanup #60

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
d6b76a7
expose initial pose of autonomous mode
nlaverdure Jan 25, 2025
a03fb21
A little cleanup
caparomula Jan 27, 2025
c4552ab
A little cleanup
caparomula Jan 27, 2025
195716e
Merge branch 'auto-selector-cleanup' of https://github.com/TripleHeli…
nlaverdure Jan 29, 2025
90c6c96
add pose getter
nlaverdure Jan 29, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 27 additions & 18 deletions src/main/java/frc/lib/AutoSelector.java
View file
Open in desktop
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
Open in desktop
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