522 lines
21 KiB
Java
522 lines
21 KiB
Java
// Copyright (c) FIRST and other WPILib contributors.
|
|
// Open Source Software; you can modify and/or share it under the terms of
|
|
// the WPILib BSD license file in the root directory of this project.
|
|
|
|
package frc.robot;
|
|
|
|
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
|
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
|
|
|
|
import java.util.Optional;
|
|
import java.util.OptionalDouble;
|
|
|
|
import org.littletonrobotics.junction.Logger;
|
|
|
|
import com.pathplanner.lib.auto.AutoBuilder;
|
|
import com.pathplanner.lib.auto.NamedCommands;
|
|
import com.pathplanner.lib.commands.PathPlannerAuto;
|
|
import com.pathplanner.lib.events.EventTrigger;
|
|
import com.pathplanner.lib.path.EventMarker;
|
|
|
|
import edu.wpi.first.math.MathUtil;
|
|
import edu.wpi.first.math.geometry.Pose2d;
|
|
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
|
|
import edu.wpi.first.math.util.Units;
|
|
import edu.wpi.first.util.sendable.Sendable;
|
|
import edu.wpi.first.util.sendable.SendableBuilder;
|
|
import edu.wpi.first.wpilibj.Timer;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
import edu.wpi.first.wpilibj2.command.Command;
|
|
import edu.wpi.first.wpilibj2.command.Commands;
|
|
import edu.wpi.first.wpilibj2.command.FunctionalCommand;
|
|
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
|
import edu.wpi.first.wpilibj2.command.PrintCommand;
|
|
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
|
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
|
|
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
|
import frc.robot.constants.AutoConstants;
|
|
import frc.robot.constants.CompetitionConstants;
|
|
import frc.robot.constants.HoodConstants;
|
|
import frc.robot.constants.OIConstants;
|
|
import frc.robot.constants.ShooterConstants;
|
|
import frc.robot.constants.IntakePivotConstants.IntakePivotPosition;
|
|
import frc.robot.constants.ShooterConstants.ShooterSpeeds;
|
|
import frc.robot.subsystems.Climber;
|
|
import frc.robot.subsystems.Drivetrain;
|
|
import frc.robot.subsystems.Hood;
|
|
import frc.robot.subsystems.IntakePivot;
|
|
import frc.robot.subsystems.IntakeRoller;
|
|
import frc.robot.subsystems.PhotonVision;
|
|
import frc.robot.subsystems.Shooter;
|
|
import frc.robot.subsystems.Spindexer;
|
|
import frc.robot.utilities.Elastic;
|
|
import frc.robot.utilities.Utilities;
|
|
|
|
public class RobotContainer {
|
|
private PhotonVision vision;
|
|
private Drivetrain drivetrain;
|
|
private Hood hood;
|
|
private Shooter shooter;
|
|
private IntakePivot intakePivot;
|
|
private IntakeRoller intakeRoller;
|
|
private Spindexer spindexer;
|
|
//private Climber climber;
|
|
|
|
private CommandXboxController driver;
|
|
private CommandXboxController secondary;
|
|
|
|
private SendableChooser<Command> autoChooser;
|
|
|
|
private Timer shiftTimer;
|
|
|
|
private boolean resetOdometryToVisualPose;
|
|
|
|
public RobotContainer() {
|
|
vision = new PhotonVision();
|
|
drivetrain = new Drivetrain(null);
|
|
hood = new Hood();
|
|
shooter = new Shooter();
|
|
intakePivot = new IntakePivot();
|
|
intakeRoller = new IntakeRoller();
|
|
spindexer = new Spindexer();
|
|
//climber = new Climber();
|
|
configureNamedCommands();
|
|
|
|
resetOdometryToVisualPose = false;
|
|
|
|
vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
|
|
vision.addPoseEstimateConsumer((vp) -> {
|
|
Logger.recordOutput(
|
|
"Vision/" + vp.cameraName() + "/Pose",
|
|
vp.visualPose()
|
|
);
|
|
});
|
|
|
|
vision.addPoseEstimateConsumer((vp) -> {
|
|
if(resetOdometryToVisualPose) {
|
|
drivetrain.resetOdometry(vp.visualPose());
|
|
resetOdometryToVisualPose = false;
|
|
}
|
|
});
|
|
|
|
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
|
|
secondary = new CommandXboxController(OIConstants.kOperatorControllerPort);
|
|
|
|
shiftTimer = new Timer();
|
|
shiftTimer.reset();
|
|
|
|
configureBindings();
|
|
configureShiftDisplay();
|
|
//testConfigureBindings();
|
|
|
|
|
|
if(AutoConstants.kAutoConfigOk) {
|
|
autoChooser = AutoBuilder.buildAutoChooser();
|
|
SmartDashboard.putData("Auto Chooser", autoChooser);
|
|
|
|
autoChooser.addOption(
|
|
"MOVE B____ right to center",
|
|
new PathPlannerAuto("MOVE B____ left to center", true)
|
|
);
|
|
|
|
autoChooser.addOption(
|
|
"right to center",
|
|
new PathPlannerAuto("left to center", true)
|
|
);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Before you @ mention me for terrible match controls, these are <i>TEST BINDINGS</i>
|
|
*
|
|
* The are configured in such a way to make testing of multiple systems possible without
|
|
* having to constantly change bindings.
|
|
*
|
|
* Most of the configurations here won't make sense for actual competition play. See
|
|
* {@link #configureBindings()} for actual competition play
|
|
*
|
|
* The intent of each binding is outlined by comments above each binding.
|
|
*/
|
|
private void testConfigureBindings() {
|
|
// This should just work, if it doesn't it's likely modules aren't assigned the right IDs
|
|
// after the electronics rebuild. For testing normal operation nothing about the Drivetrain
|
|
// class should need to change
|
|
//drivetrain.setDefaultCommand(drivetrain.drive(() -> 0, () -> 0, () -> 0, () -> true));
|
|
drivetrain.setDefaultCommand(
|
|
drivetrain.drive(
|
|
driver::getLeftY,
|
|
driver::getLeftX,
|
|
driver::getRightX,
|
|
() -> true
|
|
)
|
|
);
|
|
|
|
driver.y().whileTrue(drivetrain.zeroHeading());
|
|
/*
|
|
// This needs to be tested after a prolonged amount of driving around <i>aggressively</i>.
|
|
// Do things like going over the bump repeatedly, spin around a bunch, etc.
|
|
// If this works well over time, then this is likely all we need
|
|
driver.a().whileTrue(
|
|
drivetrain.lockRotationToHub(
|
|
driver::getLeftY,
|
|
driver::getLeftX,
|
|
false
|
|
)
|
|
);
|
|
|
|
// This can be tested as an alternative to the above, it's less dynamic, but is a simpler
|
|
// alternative.
|
|
driver.b().whileTrue(
|
|
drivetrain.lockToYaw(
|
|
() -> {
|
|
OptionalDouble maybeYaw = vision.getBestYawForTag(Utilities.getHubCenterAprilTagID());
|
|
|
|
return maybeYaw.isEmpty() ? 0 : maybeYaw.getAsDouble();
|
|
},
|
|
driver::getLeftY,
|
|
driver::getLeftX
|
|
)
|
|
);*/
|
|
|
|
// Stop everything by default other than the drivetrain
|
|
shooter.setDefaultCommand(shooter.stop());
|
|
intakePivot.setDefaultCommand(intakePivot.stop());
|
|
intakeRoller.setDefaultCommand(intakeRoller.stop());
|
|
hood.setDefaultCommand(hood.stop());
|
|
spindexer.setDefaultCommand(spindexer.stop());
|
|
//climber.setDefaultCommand(climber.stop());
|
|
|
|
// While holding POV up of the driver controller, the climber
|
|
// should move such that its motor moves the climber down with the left
|
|
// driver controller trigger axis, and up with the right driver controller
|
|
// trigger axis.
|
|
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted
|
|
// from the constants file for the subsystem having the problem.
|
|
//driver.povUp().whileTrue(climber.manualSpeed(() -> {
|
|
// return driver.getLeftTriggerAxis() * -1 + driver.getRightTriggerAxis();
|
|
//}));
|
|
|
|
// While holding the right bumper of the driver controller, the intake rollers
|
|
// and the spindexer and feeder should move such that all motors are moving in such a way
|
|
// that it would draw balls from the floor, through the spindexer, and into the
|
|
// feeder.
|
|
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
|
|
// constants file for the subsystem having the problem
|
|
driver.rightBumper().whileTrue(
|
|
spindexer.spinToShooter()
|
|
);
|
|
|
|
// While holding the left bumper of the driver controller, the intake rollers
|
|
// and the spindexer and feeder should move such that all motors are moving in such a way
|
|
// that it would try to eject balls through the intake.
|
|
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
|
|
// constants file for the subsystem having the problem
|
|
driver.leftBumper().whileTrue(
|
|
intakeRoller.runIn()
|
|
//intakeRoller.runOut().alongWith(spindexer.spinToIntake())
|
|
);
|
|
|
|
// While holding D-Pad up on the secondary controller, the shooter should spin
|
|
// while holding down the secondary controllers right trigger some amount.
|
|
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
|
|
// constants file for the subsystem having the problem
|
|
secondary.povUp().whileTrue(
|
|
shooter.manualSpeed(secondary::getRightTriggerAxis)
|
|
);
|
|
|
|
// While holding D-Pad down on the seconadry controller, the intakePivot should move
|
|
// such that left trigger on the secondary controller moves the pivot down, and
|
|
// right trigger on the secondary controller moves the pivot up.
|
|
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
|
|
// constants file for the subsystem having the problem
|
|
secondary.povDown().whileTrue(
|
|
intakePivot.manualSpeed(() -> {
|
|
return secondary.getLeftTriggerAxis() * -1 + secondary.getRightTriggerAxis();
|
|
})
|
|
);
|
|
|
|
// While holding D-Pad left on the secondary controller, the hood should move
|
|
// such that the left trigger on the secondary controller moves the hood down, and
|
|
// right trigger on the secondary controller moves the hood up.
|
|
// DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the
|
|
// constants file for the subsystem having the problem
|
|
secondary.povLeft().whileTrue(
|
|
hood.manualSpeed(() -> {
|
|
return secondary.getLeftTriggerAxis() * -1 + secondary.getRightTriggerAxis();
|
|
})
|
|
);
|
|
|
|
// STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP STOP
|
|
// Don't proceed unless you've verified by hand or with the above bindings, that sensing
|
|
// systems are providing correct values in the correct direction of rotation.
|
|
|
|
|
|
// Useful for testing PID and FF responses of the shooter
|
|
// You need to have graphs up of the logged data to make sure the response is correct
|
|
//secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
|
|
secondary.b().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
|
|
|
|
// Useful for testing PID and FF responses of the intake pivot
|
|
// You need to have graphs up of the logged data to make sure the response is correct
|
|
secondary.x().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kDown));
|
|
secondary.y().whileTrue(intakePivot.maintainPosition(IntakePivotPosition.kUp));
|
|
|
|
secondary.povUp().whileTrue(hood.trackToAngle(() -> Math.toRadians(40.0)));
|
|
secondary.povDown().whileTrue(hood.trackToAngle(() -> Math.toRadians(0.0)));
|
|
|
|
|
|
// TODO Some means of testing hood PIDF
|
|
// TODO Some means of testing climber PIDF
|
|
}
|
|
|
|
private void configureBindings() {
|
|
drivetrain.setDefaultCommand(
|
|
drivetrain.drive(
|
|
driver::getLeftY,
|
|
driver::getLeftX,
|
|
driver::getRightX,
|
|
() -> true
|
|
)
|
|
);
|
|
shooter.setDefaultCommand(shooter.stop());
|
|
intakeRoller.setDefaultCommand(intakeRoller.stop());
|
|
spindexer.setDefaultCommand(spindexer.stop());
|
|
|
|
intakePivot.setDefaultCommand(intakePivot.manualSpeed(() -> secondary.getLeftY()));
|
|
|
|
driver.a().onTrue(new InstantCommand(() -> resetOdometryToVisualPose = true));
|
|
driver.y().whileTrue(drivetrain.zeroHeading());
|
|
driver.x().whileTrue(drivetrain.setX());
|
|
|
|
driver.leftTrigger().whileTrue(
|
|
drivetrain.lockRotationToHub(
|
|
driver::getLeftY,
|
|
driver::getLeftX,
|
|
false
|
|
)
|
|
);
|
|
|
|
driver.leftBumper().whileTrue(intakeRoller.runOut());
|
|
driver.rightBumper().whileTrue(intakeRoller.runIn());
|
|
|
|
driver.rightTrigger().whileTrue(
|
|
spindexer.spinToShooter(shooter::getAverageActualSpeeds, 2000).alongWith(
|
|
intakeRoller.runIn(),
|
|
intakePivot.jimmy(.5)
|
|
)
|
|
);
|
|
|
|
driver.rightTrigger().onFalse(
|
|
intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5)
|
|
);
|
|
|
|
driver.b().whileTrue(spindexer.spinToIntake());
|
|
/* driver.b().whileTrue(
|
|
drivetrain.lockToYaw(
|
|
() -> {
|
|
OptionalDouble maybeYaw = vision.getBestYawForTag(Utilities.getHubCenterAprilTagID());
|
|
|
|
return maybeYaw.isEmpty() ? 0 : maybeYaw.getAsDouble();
|
|
},
|
|
driver::getLeftY,
|
|
driver::getLeftX
|
|
)
|
|
);*/
|
|
|
|
secondary.a().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
|
|
secondary.x().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
|
|
|
|
//hood.setDefaultCommand(hood.trackToAngle(() -> Units.degreesToRadians(MathUtil.clamp(hoodAngle, 0, 40))));
|
|
//secondary.y().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(40)));
|
|
//40 good for feeding
|
|
//secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
|
|
//30 degrees good for shooter far near outpost
|
|
secondary.rightBumper().whileTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
|
|
//10 degrees good for shooting ~33in away from hub
|
|
|
|
hood.setDefaultCommand(hood.trackToAnglePoseBased(drivetrain, shooter));
|
|
|
|
/*hood.setDefaultCommand(hood.trackToAngle(() -> {
|
|
Pose2d drivetrainPose = drivetrain.getPose();
|
|
Pose2d hubPose = Utilities.getHubPose();
|
|
|
|
double distance = drivetrainPose.getTranslation()
|
|
.getDistance(hubPose.getTranslation());
|
|
|
|
Logger.recordOutput("Hood/DistanceToHub", distance);
|
|
|
|
Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
|
|
|
|
if(currentSpeeds.isPresent()) {
|
|
InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
|
|
|
|
if(map != null) {
|
|
return MathUtil.clamp(map.get(distance), 0, 40);
|
|
} else {
|
|
return 0;
|
|
}
|
|
} else {
|
|
return 0;
|
|
}
|
|
}));*/
|
|
|
|
new Trigger(() -> MathUtil.isNear(
|
|
shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(),
|
|
shooter.getAverageActualSpeeds(),
|
|
150)).onTrue(
|
|
new FunctionalCommand(
|
|
() -> {},
|
|
() -> {
|
|
driver.setRumble(RumbleType.kBothRumble, .75);
|
|
secondary.setRumble(RumbleType.kBothRumble, .75);
|
|
},
|
|
(b) -> {
|
|
driver.setRumble(RumbleType.kBothRumble, 0);
|
|
secondary.setRumble(RumbleType.kBothRumble, 0);
|
|
},
|
|
() -> false
|
|
).withTimeout(1)
|
|
);
|
|
}
|
|
|
|
private void configureNamedCommands() {
|
|
NamedCommands.registerCommand(
|
|
"Drivetrain Set X",
|
|
drivetrain.setX()
|
|
);
|
|
|
|
NamedCommands.registerCommand(
|
|
"Drivetrain Face Hub",
|
|
drivetrain.rotateToPose(
|
|
Utilities.getHubPose(),
|
|
false // TODO Should this be true by default?
|
|
)
|
|
);
|
|
|
|
NamedCommands.registerCommand(
|
|
"intake down",
|
|
intakePivot.manualSpeed(()->0.75)
|
|
.withTimeout(1)
|
|
);
|
|
|
|
NamedCommands.registerCommand("spinup",
|
|
shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
|
|
.withTimeout(2));
|
|
|
|
NamedCommands.registerCommand("shoot close",
|
|
spindexer.spinToShooter()
|
|
.alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed))
|
|
.alongWith(hood.trackToAngle(() -> Units.degreesToRadians(10)))
|
|
.withTimeout(3).andThen(spindexer.instantaneousStop()));
|
|
|
|
// NamedCommands.registerCommand("Intake Start", intakeRoller.runIn());
|
|
|
|
new EventTrigger("Intake Start")
|
|
.onTrue(
|
|
intakeRoller.runIn());
|
|
|
|
new EventTrigger("windup trigger")
|
|
.onTrue(
|
|
shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
|
|
|
|
NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop());
|
|
|
|
NamedCommands.registerCommand("jimmy",
|
|
intakePivot.jimmy(0.2)
|
|
);
|
|
|
|
NamedCommands.registerCommand("shoot N jimmy",
|
|
Commands.parallel(
|
|
intakePivot.jimmy(0.5),
|
|
spindexer.spinToShooter()
|
|
.alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed),
|
|
hood.trackToAngle(() -> Units.degreesToRadians(10)))
|
|
|
|
).withTimeout(3).andThen(spindexer.instantaneousStop()));
|
|
|
|
NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter)
|
|
.alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), spindexer.spinToShooter(), intakePivot.jimmy(0.5)));
|
|
}
|
|
|
|
public Command getAutonomousCommand() {
|
|
return autoChooser.getSelected();
|
|
}
|
|
|
|
/**
|
|
* The "shift display" relies on Elastic's ability to show 1 or more colors
|
|
* in a box on the dashboard.
|
|
*
|
|
* Using the RobotModeTriggers and a Timer based on the FPGA, a reasonably
|
|
* accurate "shift display" can be created to indicate whose hub is active
|
|
* and when.
|
|
*
|
|
* During autonomous, and the first 10 seconds of teleop, the shift display
|
|
* will display a gradient of both red and blue, indicating the fact that
|
|
* both hubs are active.
|
|
*
|
|
* For the rest of teleop, with the exception of the endgame, the display
|
|
* will present either the color red, or the color blue, based on the returned
|
|
* value of Utilities.whoHasFirstShift(). Because shifts change on a known cycle,
|
|
* we can use the known state of who has first shift, to determine the remaining three shifts
|
|
* that come after.
|
|
*
|
|
* For the endgame portion of teleop, the shift display returns to the gradient
|
|
* of both red and blue.
|
|
*
|
|
* Because this relies on the RobotModeTriggers and an FPGA timer, it should be
|
|
* <i>reasonably</i> accurate, it's unlikely to be perfect relative to field time
|
|
* but it will be very very very (likely unnoticably) close.
|
|
*/
|
|
private void configureShiftDisplay() {
|
|
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
|
|
|
|
RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> {
|
|
shiftTimer.stop();
|
|
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
|
|
}));
|
|
|
|
RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> {
|
|
Elastic.selectTab(OIConstants.kTeleopTab);
|
|
shiftTimer.reset();
|
|
shiftTimer.start();
|
|
}));
|
|
|
|
new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> {
|
|
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
|
|
}));
|
|
|
|
new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
|
|
SmartDashboard.putStringArray(
|
|
OIConstants.kCurrentActiveHub,
|
|
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
|
|
);
|
|
}));
|
|
|
|
new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
|
|
SmartDashboard.putStringArray(
|
|
OIConstants.kCurrentActiveHub,
|
|
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
|
|
);
|
|
}));
|
|
|
|
new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
|
|
SmartDashboard.putStringArray(
|
|
OIConstants.kCurrentActiveHub,
|
|
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
|
|
);
|
|
}));
|
|
|
|
new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
|
|
SmartDashboard.putStringArray(
|
|
OIConstants.kCurrentActiveHub,
|
|
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
|
|
);
|
|
}));
|
|
|
|
new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> {
|
|
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
|
|
}));
|
|
}
|
|
}
|