mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Small refactoring
This commit is contained in:
nathanrsxtn
@@ -4,7 +4,6 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Objects;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
@@ -18,28 +17,26 @@ import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.Preferences;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
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.CommandGroupBase;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import frc4388.robot.Constants.AutoConstants;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.StorageConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.commands.PathRecorder;
|
||||
import frc4388.robot.commands.RunCommandForTime;
|
||||
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
|
||||
import frc4388.robot.commands.ExtenderIntakeCommands.RunExtender;
|
||||
import frc4388.robot.commands.ShooterCommands.TrackTarget;
|
||||
import frc4388.robot.commands.shooter.TimedWaitUntilCommand;
|
||||
import frc4388.robot.commands.shooter.TrackTarget;
|
||||
import frc4388.robot.commands.shuffleboard.CommandSchedule;
|
||||
import frc4388.robot.commands.shuffleboard.ShooterTuner;
|
||||
import frc4388.robot.subsystems.BoomBoom;
|
||||
@@ -55,579 +52,286 @@ import frc4388.robot.subsystems.Storage;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.VisionOdometry;
|
||||
import frc4388.utility.Vector2D;
|
||||
import frc4388.utility.controller.ButtonBox;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
//TODO: Try using ConditionalCommand for subsystem default commands.
|
||||
//TODO: Replace Path Recorder with Auto Chooser.
|
||||
//TODO: Add POV button pad bindings as an example.
|
||||
//XXX: Re-enable extender in autonomous.
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
* Command-based is a "declarative" paradigm, very little robot logic should
|
||||
* actually be handled in the {@link Robot} periodic methods (other than the
|
||||
* scheduler calls). Instead, the structure of the robot (including subsystems,
|
||||
* commands, and button mappings) should be declared here.
|
||||
* This class is where the bulk of the robot should be declared. Since Command-based is a
|
||||
* "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
|
||||
* periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
|
||||
* subsystems, commands, and button mappings) should be declared here.
|
||||
*/
|
||||
public class RobotContainer {
|
||||
private static final Logger LOGGER = Logger.getLogger(RobotContainer.class.getSimpleName());
|
||||
|
||||
// RobotMap
|
||||
/* Robot Map */
|
||||
public final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
public final Camera m_robotCamera = new Camera("driver", 0, 160, 120, 40);
|
||||
public final Climber m_robotClimber = new Climber(m_robotMap.elbow);
|
||||
public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam);
|
||||
public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw);
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.frontLeft, m_robotMap.frontRight, m_robotMap.backLeft, m_robotMap.backRight, m_robotMap.gyro);
|
||||
public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt);
|
||||
public final Intake m_robotIntake = new Intake(m_robotMap.intakeMotor);
|
||||
public final Extender m_robotExtender = new Extender(m_robotMap.extenderMotor);
|
||||
|
||||
public final Storage m_robotStorage = new Storage(m_robotMap.storageMotor);
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController); // ! no LED makes aarav sad
|
||||
public final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
public final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
|
||||
public final Hood m_robotHood = new Hood(m_robotMap.angleAdjusterMotor);
|
||||
public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret);
|
||||
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
|
||||
|
||||
/* Autonomous */
|
||||
/* Dashboard Tools */
|
||||
private final PathRecorder m_pathChooser = new PathRecorder(m_robotSwerveDrive);
|
||||
|
||||
private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
|
||||
private final ShooterTuner m_shooterTuner = new ShooterTuner(m_robotBoomBoom);
|
||||
private final CommandSchedule m_commandSchedule = new CommandSchedule(10, 5, false);
|
||||
// Controllers
|
||||
private final static DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final static DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
/* Controllers */
|
||||
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
|
||||
|
||||
public static boolean softLimits = true;
|
||||
|
||||
private static boolean softLimits = true;
|
||||
|
||||
// control mode switching
|
||||
public static enum ControlMode { SHOOTER, CLIMBER };
|
||||
public static ControlMode currentControlMode = ControlMode.SHOOTER;
|
||||
private enum ControlMode {
|
||||
SHOOTER, CLIMBER
|
||||
}
|
||||
|
||||
// turret mode switching
|
||||
private enum TurretMode { MANUAL, AUTONOMOUS };
|
||||
private TurretMode currentTurretMode = TurretMode.MANUAL;
|
||||
|
||||
// climber mode switching
|
||||
private enum ClimberMode { MANUAL, AUTONOMOUS };
|
||||
private ClimberMode currentClimberMode = ClimberMode.MANUAL;
|
||||
private ControlMode currentControlMode = ControlMode.SHOOTER;
|
||||
|
||||
// drive on off mode switching
|
||||
private enum DriveMode { ON, OFF };
|
||||
private enum DriveMode {
|
||||
ON, OFF
|
||||
}
|
||||
|
||||
private DriveMode currentDriveMode = DriveMode.ON;
|
||||
|
||||
// private SendableChooser<SequentialCommandGroup> quickAutoChooser = new SendableChooser<>();
|
||||
public SendableChooser<Command> autoChooser = new SendableChooser<Command>();
|
||||
|
||||
/**
|
||||
* SmartDash
|
||||
* - Limelight cam X
|
||||
* - Limit switches X
|
||||
* - Shooter RPM X
|
||||
* - Distance to target x
|
||||
* - target locked
|
||||
* - claws boolean
|
||||
* - field
|
||||
*/
|
||||
|
||||
// private SequentialCommandGroup makeTheWeirdGroup() {
|
||||
// SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
||||
// new ParallelCommandGroup(
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
|
||||
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0)
|
||||
// )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
// return weirdAutoShootingGroup;
|
||||
// }
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
autoChooser.addOption("DriveOffLineAndShoot", driveOffLineAndShoot);
|
||||
autoChooser.addOption("OneBallAuto", oneBallAuto);
|
||||
autoChooser.setDefaultOption("TwoBallAuto", twoBallAuto);
|
||||
autoChooser.addOption("ThreeBallAuto", threeBallAuto);
|
||||
// autoChooser.addOption("instantThreeBallSingleShotAutoSequence", instantThreeBallSingleShotAutoSequence);
|
||||
autoChooser.addOption("instantThreeBallDoubleSingleShotAutoSequence", instantThreeBallDoubleSingleShotAutoSequence);
|
||||
autoChooser.addOption("threeBallPlus", threeBallPlus);
|
||||
|
||||
SmartDashboard.putData("AutoChooser", autoChooser);
|
||||
|
||||
Preferences.initString("Autonomous", "Three Ball");
|
||||
|
||||
configureButtonBindings();
|
||||
/* Default Commands */
|
||||
// Swerve Drive with Input
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> {
|
||||
if (currentDriveMode == DriveMode.ON) {
|
||||
m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(),
|
||||
getDriverController().getRight(), true);
|
||||
}
|
||||
if (currentDriveMode == DriveMode.OFF) {
|
||||
m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false);
|
||||
}
|
||||
}, m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
if (currentDriveMode == DriveMode.ON) {
|
||||
m_robotSwerveDrive.driveWithInput(getDriverController().getLeft(), getDriverController().getRight(), true);
|
||||
}
|
||||
if (currentDriveMode == DriveMode.OFF) {
|
||||
m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, false);
|
||||
}
|
||||
}, m_robotSwerveDrive).withName("SwerveDrive DefaultCommand"));
|
||||
|
||||
// Intake with Triggers
|
||||
m_robotIntake.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotIntake.runWithTriggers(
|
||||
getOperatorController().getLeftTriggerAxis(),
|
||||
getOperatorController().getRightTriggerAxis()),
|
||||
m_robotIntake).withName("Intake runWithTriggers defaultCommand"));
|
||||
m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom));
|
||||
m_robotIntake.setDefaultCommand(new RunCommand(() -> m_robotIntake.runWithTriggers(getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake DefaultCommand"));
|
||||
|
||||
// Serializer Manual
|
||||
m_robotSerializer.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(),
|
||||
m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand"));
|
||||
// TODO: Comment
|
||||
m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom).withName("BoomBoom DefaultCommand"));
|
||||
|
||||
// Turret Manual
|
||||
m_robotTurret.setDefaultCommand(
|
||||
new RunCommand(() -> {
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
|
||||
}, m_robotTurret));
|
||||
// Serializer Manual
|
||||
m_robotSerializer.setDefaultCommand(new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8), m_robotSerializer).withName("Serializer DefaultCommand"));
|
||||
|
||||
// Hood Manual
|
||||
m_robotHood.setDefaultCommand(
|
||||
new RunCommand(() -> {
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getLeftY()); }
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); }
|
||||
}, m_robotHood));
|
||||
// Turret Manual
|
||||
m_robotTurret.setDefaultCommand(new RunCommand(() -> {
|
||||
if (currentControlMode == ControlMode.SHOOTER) {
|
||||
m_robotTurret.runTurretWithInput(getOperatorController().getLeftX());
|
||||
}
|
||||
if (currentControlMode == ControlMode.CLIMBER) {
|
||||
m_robotTurret.runTurretWithInput(0);
|
||||
}
|
||||
}, m_robotTurret));
|
||||
|
||||
// //Climber Manual
|
||||
m_robotClimber.setDefaultCommand(
|
||||
new RunCommand(() -> {
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); }
|
||||
if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getRightY()); }
|
||||
}, m_robotClimber));
|
||||
// m_robotClimber.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotClimber.setMotors(-getOperatorController().getRightY()), m_robotClimber));
|
||||
|
||||
m_robotBoomBoom.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
|
||||
);
|
||||
// Hood Manual
|
||||
m_robotHood.setDefaultCommand(new RunCommand(() -> {
|
||||
if (currentControlMode == ControlMode.SHOOTER) {
|
||||
m_robotHood.runHood(getOperatorController().getLeftY());
|
||||
}
|
||||
if (currentControlMode == ControlMode.CLIMBER) {
|
||||
m_robotHood.runHood(0);
|
||||
}
|
||||
}, m_robotHood));
|
||||
|
||||
// Climber Manual
|
||||
m_robotClimber.setDefaultCommand(new RunCommand(() -> {
|
||||
if (currentControlMode == ControlMode.SHOOTER) {
|
||||
m_robotClimber.setMotors(0.0);
|
||||
}
|
||||
if (currentControlMode == ControlMode.CLIMBER) {
|
||||
m_robotClimber.setMotors(-getOperatorController().getRightY());
|
||||
}
|
||||
}, m_robotClimber));
|
||||
|
||||
SmartDashboard.putData("Shooter Tuner", m_shooterTuner);
|
||||
SmartDashboard.putData("Shooter Tuner", m_shooterTuner);
|
||||
SmartDashboard.putData("Command Schedule", m_commandSchedule);
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
|
||||
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
* Use this method to define your button->command mappings. Buttons can be created by instantiating
|
||||
* a {@link GenericHID} or one of its subclasses ({@link edu.wpi.first.wpilibj.Joystick} or
|
||||
* {@link XboxController}), and then passing it to a
|
||||
* {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
//! Driver Buttons
|
||||
// ! Driver Buttons
|
||||
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kBack.value)
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kBack.value).whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kStart.value)
|
||||
.whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
// Start > Calibrate Odometry
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kStart.value).whenPressed(m_robotSwerveDrive::resetGyro);
|
||||
|
||||
// Left Bumper > Shift Down
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
// Left Bumper > Shift Down
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
|
||||
// Right Bumper > Shift Up
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
// Right Bumper > Shift Up
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value).whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
||||
.whenPressed(new InstantCommand(() -> switchControlMode()))
|
||||
.whenReleased(new InstantCommand(() -> switchControlMode()));
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value).whenPressed(new InstantCommand(() -> switchControlMode())).whenReleased(new InstantCommand(() -> switchControlMode()));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kB.value)
|
||||
.whenPressed(new InstantCommand(() -> switchDriveMode()))
|
||||
.whenReleased(new InstantCommand(() -> switchDriveMode()));
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> switchDriveMode())).whenReleased(new InstantCommand(() -> switchDriveMode()));
|
||||
|
||||
//! Operator Buttons
|
||||
// ! Operator Buttons
|
||||
|
||||
// Right Bumper > Storage Out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED)))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
// Right Bumper > Storage Out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
|
||||
// Left Bumper > Storage In
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
|
||||
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
// Left Bumper > Storage In
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value).whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))).whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0)));
|
||||
|
||||
// B > Toggle claws
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
.whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
|
||||
// B > Toggle claws
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kB.value).whenPressed(new InstantCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws));
|
||||
|
||||
// X > Toggles extender in and out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kX.value)
|
||||
.whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
||||
// X > Toggles extender in and out
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kX.value).whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender));
|
||||
|
||||
// A > Spit Out Ball
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret))
|
||||
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
|
||||
// A > Spit Out Ball
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kA.value).whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)).whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)));
|
||||
|
||||
// Y > Full aim command
|
||||
// Y > Full aim command
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry));
|
||||
|
||||
// .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood,
|
||||
// m_robotVisionOdometry));
|
||||
|
||||
//! Test Buttons
|
||||
// ! Test Buttons
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whenPressed(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0));
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED)); // * aim with turret to target);
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood,
|
||||
// m_robotVisionOdometry, false, false));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
|
||||
// .whenPressed(new RunCommandForTime(new RunCommand(() ->
|
||||
// m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)),
|
||||
// m_robotTurret), 1.0));
|
||||
new JoystickButton(getOperatorController(), XboxController.Button.kY.value).whileHeld(new TrackTarget(m_robotVisionOdometry, m_robotTurret, m_robotHood, m_robotBoomBoom)); // * aim with turret to target);
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
|
||||
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false)));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
|
||||
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));
|
||||
|
||||
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
|
||||
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
|
||||
// .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
|
||||
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
|
||||
|
||||
|
||||
|
||||
//! Button Box Buttons
|
||||
// Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender)
|
||||
// ! Button Box Buttons
|
||||
// Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood,
|
||||
// climber, and extender)
|
||||
|
||||
// SmartDashboard.putData("BB LEFT ON", new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret),
|
||||
|
||||
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood),
|
||||
|
||||
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)
|
||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret),
|
||||
|
||||
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood),
|
||||
|
||||
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)
|
||||
// ));
|
||||
|
||||
|
||||
// SmartDashboard.putData("BB LEFT OFF", new SequentialCommandGroup(
|
||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret),
|
||||
|
||||
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood),
|
||||
// new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret),
|
||||
|
||||
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender),
|
||||
// new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood),
|
||||
|
||||
// new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender),
|
||||
// new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender),
|
||||
// new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)
|
||||
// new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender),
|
||||
|
||||
// new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret),
|
||||
// new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood),
|
||||
// new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender),
|
||||
// new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake,
|
||||
// m_robotExtender),
|
||||
// new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)
|
||||
// ));
|
||||
|
||||
// new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value)
|
||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
|
||||
|
||||
// .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
|
||||
|
||||
// .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood))
|
||||
// .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
|
||||
// .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
|
||||
|
||||
// Middle Switch > Climber and Shooter mode switching
|
||||
// .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false),
|
||||
// m_robotExtender))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true),
|
||||
// m_robotExtender))
|
||||
|
||||
// .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0),
|
||||
// m_robotTurret))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender))
|
||||
// .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake,
|
||||
// m_robotExtender))
|
||||
// .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
|
||||
|
||||
// Middle Switch > Climber and Shooter mode switching
|
||||
// new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
|
||||
// .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER))
|
||||
// .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||
// // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
||||
// // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.))
|
||||
// // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||
|
||||
// .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.CLIMBER))
|
||||
// .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||
// // new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
||||
// // .whenPressed(new InstantCommand(() -> currentControlMode = ControlMode.))
|
||||
// // .whenReleased(new InstantCommand(() -> currentControlMode = ControlMode.SHOOTER));
|
||||
|
||||
// new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value)
|
||||
// .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF))
|
||||
// .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON));
|
||||
// .whileHeld(new InstantCommand(() -> currentDriveMode = DriveMode.OFF))
|
||||
// .whenReleased(new InstantCommand(() -> currentDriveMode = DriveMode.ON));
|
||||
|
||||
// // Left Button > Extender In
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kX.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
|
||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
// // Left Button > Extender In
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kX.value).whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)).whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
|
||||
// Left Button > Extender Out
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kY.value)
|
||||
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
|
||||
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
// Left Button > Extender Out
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kY.value).whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)).whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
|
||||
}
|
||||
|
||||
/**
|
||||
* Generate autonomous
|
||||
* @param maxVel max velocity for the path (null to override default value of 5.0)
|
||||
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
|
||||
* @param inputs strings (paths) or commands you want to run (in order)
|
||||
* @return array of commands
|
||||
*/
|
||||
public SequentialCommandGroup buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
|
||||
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY);
|
||||
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
|
||||
|
||||
ArrayList<Command> commands = new ArrayList<Command>();
|
||||
// commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
|
||||
// commands.add(new InstantCommand(() -> m_robotSwerveDrive.m, m_robotSwerveDrive));
|
||||
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
// parse input
|
||||
for (int i=0; i<inputs.length; i++) {
|
||||
if (inputs[i] instanceof String) {
|
||||
|
||||
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
|
||||
PathPlannerState initState = traj.getInitialState();
|
||||
|
||||
Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
|
||||
commands.add(new PPSwerveControllerCommand(
|
||||
traj,
|
||||
m_robotSwerveDrive::getOdometry,
|
||||
m_robotSwerveDrive.m_kinematics,
|
||||
xController,
|
||||
yController,
|
||||
thetaController,
|
||||
m_robotSwerveDrive::setModuleStates,
|
||||
m_robotSwerveDrive));
|
||||
}
|
||||
|
||||
if (inputs[i] instanceof Command) {
|
||||
commands.add((Command) inputs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive));
|
||||
Command[] ret = new Command[commands.size()];
|
||||
ret = commands.toArray(ret);
|
||||
SequentialCommandGroup seqCG = new SequentialCommandGroup(ret);
|
||||
return seqCG;
|
||||
private boolean isLockedOn() {
|
||||
return m_robotTurret.isLockedOn() || m_robotHood.isLockedOn() || m_robotBoomBoom.isLockedOn();
|
||||
}
|
||||
|
||||
// ! ways to not coast
|
||||
// // * 1. try zero joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.0, 0.0, false), m_robotSwerveDrive);
|
||||
// * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive);
|
||||
// * 3a. try permanently setting drive motors to brake, not coast, in RobotMap.java, and ask the driver how it feels.
|
||||
// * 3b. try to only set the drive motors to brake if in auto mode.
|
||||
// * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
|
||||
|
||||
// ? 1.0 input, 1 second: 134 inches
|
||||
// ? 0.75 input, 1 second: 48 inches
|
||||
// ! POSITIVE Y IS LEFT, POSITIVE X IS BACKWARDS
|
||||
|
||||
double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
|
||||
|
||||
double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
|
||||
double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
|
||||
|
||||
// ! ball positions are "unit tested"
|
||||
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
|
||||
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
|
||||
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
|
||||
|
||||
private SequentialCommandGroup shoot(double storageRunTime) {
|
||||
return new SequentialCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true)
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
private ParallelRaceGroup shoot(double storageRunTime, double timeout) {
|
||||
return new SequentialCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true)
|
||||
)
|
||||
).withTimeout(timeout + storageRunTime);
|
||||
}
|
||||
|
||||
SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 1.0, true)
|
||||
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
|
||||
SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.3, true)
|
||||
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
|
||||
SequentialCommandGroup weirdAutoShootingGroup3 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new ParallelCommandGroup(
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 4.0, true)
|
||||
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
|
||||
|
||||
Command resetGyro = new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive);
|
||||
Command brakeDrive = new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
|
||||
|
||||
private Command brakeStorage(double storageRunTime) {
|
||||
return new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotIntake), storageRunTime, true);
|
||||
}
|
||||
|
||||
SequentialCommandGroup extendThenAimTurret() {
|
||||
return new SequentialCommandGroup(
|
||||
new ExtenderIntakeGroup(m_robotIntake, m_robotExtender),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret), 0.5, true) // TODO: optimize time
|
||||
);
|
||||
}
|
||||
|
||||
ParallelDeadlineGroup idleDrumUntilShootingFirstBall() {
|
||||
return new ParallelDeadlineGroup(
|
||||
extendThenAimTurret(),
|
||||
new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(9000), m_robotBoomBoom)
|
||||
);
|
||||
}
|
||||
|
||||
private ParallelDeadlineGroup intakeWithPath1(double intakeRunTime) {
|
||||
return new ParallelDeadlineGroup(
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), intakeRunTime, true), // TODO: optimize time
|
||||
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
|
||||
buildAuto(3.0, 3.0, "JMove1") // TODO: make faster?
|
||||
);
|
||||
}
|
||||
|
||||
private ParallelDeadlineGroup intakeWithPath2(double intakeRunTime) {
|
||||
return new ParallelDeadlineGroup(
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), intakeRunTime, true), // TODO: optimize time
|
||||
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
|
||||
buildAuto(3.0, 3.0, "JMove2") // TODO: make faster?
|
||||
);
|
||||
}
|
||||
|
||||
ParallelCommandGroup intakeWithPathAndTrackTarget = new ParallelCommandGroup(intakeWithPath1(3.0), weirdAutoShootingGroup2);
|
||||
// ParallelCommandGroup intakeWithPath2AndTrackTarget = new ParallelCommandGroup(intakeWithPath2, weirdAutoShootingGroup3);
|
||||
|
||||
ParallelDeadlineGroup intakeWithPath2AndIdleShooterAndAimTurret = new ParallelDeadlineGroup(
|
||||
intakeWithPath2(2.8),
|
||||
new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom),
|
||||
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true)
|
||||
);
|
||||
|
||||
// ! DRIVE OFF LINE AND SHOOT (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE TARGET)
|
||||
SequentialCommandGroup driveOffLineAndShoot = new SequentialCommandGroup(
|
||||
resetGyro,
|
||||
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {1.0, 0.0, 0.0, 0.0}, (5.0 * 12) / distancePerSecond), // * go backwards five feet
|
||||
brakeDrive,
|
||||
shoot(1.0),
|
||||
brakeStorage(0.1)
|
||||
);
|
||||
|
||||
// ! ONE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE TARGET)
|
||||
SequentialCommandGroup oneBallAuto = new SequentialCommandGroup(
|
||||
shoot(1.0),
|
||||
brakeStorage(0.1)
|
||||
);
|
||||
|
||||
// ! TWO BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE)
|
||||
SequentialCommandGroup twoBallAuto = new SequentialCommandGroup(
|
||||
idleDrumUntilShootingFirstBall(),
|
||||
shoot(1.0, 4.0), // TODO: optimize time
|
||||
brakeStorage(0.1),
|
||||
intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget
|
||||
shoot(5.0), // TODO: optimize time
|
||||
brakeStorage(0.1)
|
||||
);
|
||||
|
||||
// ! THREE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE)
|
||||
SequentialCommandGroup threeBallAuto = new SequentialCommandGroup(
|
||||
idleDrumUntilShootingFirstBall(),
|
||||
shoot(0.8), // TODO: optimize time
|
||||
brakeStorage(0.1),
|
||||
intakeWithPathAndTrackTarget,
|
||||
// intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget
|
||||
shoot(0.8), // TODO: optimize time
|
||||
brakeStorage(0.1),
|
||||
intakeWithPath2AndIdleShooterAndAimTurret,
|
||||
shoot(4.0), // TODO: optimize time
|
||||
brakeStorage(0.1)
|
||||
);
|
||||
|
||||
// private final CommandGroupBase instantThreeBallSingleShotAutoSequence = CommandGroupBase.sequence(
|
||||
// // Preloaded Ball
|
||||
// new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
// new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"),
|
||||
// new RunExtender(m_robotExtender).withName("DeployExtender"),
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("FirstBallTarget"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("FirstBallFeed"),
|
||||
// new WaitCommand(3.0).withName("FirstBallShootTimer"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("FirstBallStopFeed"),
|
||||
// // Second Ball
|
||||
// new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
// new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"),
|
||||
// buildAuto(3.0, 3.0, "JMove1").withName("JMove1"),
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("SecondBallTarget"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("SecondBallFeed"),
|
||||
// new WaitCommand(3.0).withName("SecondBallShootTimer"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("SecondBallStopFeed"),
|
||||
// // Third Ball
|
||||
// new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
// new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"),
|
||||
// buildAuto(3.0, 3.0, "JMove2").withName("JMove2"),
|
||||
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("ThirdBallTarget"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("ThirdBallFeed"),
|
||||
// new WaitCommand(3.0).withName("ThirdBallShootTimer"),
|
||||
// new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StartRunningIntake"),
|
||||
// new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"),
|
||||
// new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed")
|
||||
// );
|
||||
|
||||
private static final class InstAutoConst {
|
||||
private static final double PATH_MAX_VEL = 3.0;
|
||||
private static final double PATH_MAX_ACCEL = 3.0;
|
||||
private static final double TRACK_TIME_ALLOWANCE = 1.0;
|
||||
private static final double STORAGE_TIME_TWO_BALLS = 1.5;
|
||||
private static final double STORAGE_TIME_ONE_BALL = 0.8;
|
||||
}
|
||||
private final CommandGroupBase instantThreeBallDoubleSingleShotAutoSequence = CommandGroupBase.sequence(
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"),
|
||||
new RunExtender(m_robotExtender).withName("DeployExtender"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"),
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"),
|
||||
// Preload and Second Ball
|
||||
buildAuto(InstAutoConst.PATH_MAX_VEL, InstAutoConst.PATH_MAX_ACCEL, "JMove1").withName("JMove1"),
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("FirstSecondBallTarget").withTimeout(InstAutoConst.TRACK_TIME_ALLOWANCE + 1.0),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("FirstSecondBallFeed"),
|
||||
new WaitCommand(InstAutoConst.STORAGE_TIME_TWO_BALLS).withName("FirstSecondBallShootTimer").deadlineWith(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, false).withName("FirstSecondBallTarget")),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("FirstSecondBallStopFeed"),
|
||||
// Third Ball
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"),
|
||||
buildAuto(InstAutoConst.PATH_MAX_VEL, InstAutoConst.PATH_MAX_ACCEL, "JMove2").withName("JMove2"),
|
||||
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withName("ThirdBallTarget").withTimeout(InstAutoConst.TRACK_TIME_ALLOWANCE + 1.0),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)).withName("ThirdBallFeed"),
|
||||
new WaitCommand(InstAutoConst.STORAGE_TIME_ONE_BALL).withName("ThirdBallShootTimer").deadlineWith(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, false).withName("FirstSecondBallTarget")),
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StartRunningIntake"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed")
|
||||
);
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
@@ -637,27 +341,74 @@ public class RobotContainer {
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
|
||||
private final CommandGroupBase threeBallPlus = CommandGroupBase.sequence(
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(-1), m_robotIntake).withName("StartRunningIntake"),
|
||||
// new RunExtender(m_robotExtender).withName("DeployExtender"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer).withName("StartRunningSerializer"),
|
||||
// Get Second Ball
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret).withName("StartTurningShooter"),
|
||||
makePathingGroup(AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, "JMove1").withName("JMove1"),
|
||||
// Shoot Preloaded and Second Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_TWO_BALLS, "FirstSecondBall"),
|
||||
// Get Third Ball
|
||||
new InstantCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom).withName("StartIdlingShooter"),
|
||||
new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret).withName("StartTurningShooter"),
|
||||
makePathingGroup(AutoConstants.PATH_MAX_VEL, AutoConstants.PATH_MAX_ACCEL, "JMove2").withName("JMove2"),
|
||||
// Shoot Third Ball
|
||||
makeTimeoutTrackShotGroup(AutoConstants.LOCK_ON_DURATION, AutoConstants.LOCK_ON_TIME_ALLOWANCE, AutoConstants.STORAGE_TIME_ONE_BALL, "ThirdBall"),
|
||||
// Stop
|
||||
new InstantCommand(() -> m_robotIntake.runAtOutput(0), m_robotIntake).withName("StopRunningIntake"),
|
||||
new InstantCommand(() -> m_robotSerializer.setSerializer(0.0), m_robotSerializer).withName("StopRunningSerializer"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName("ThirdBallStopFeed")
|
||||
);
|
||||
|
||||
private ParallelDeadlineGroup makeTimeoutTrackShotGroup(double lockOnDuration, double lockOnTimeAllowance, double storageRunTime, String name) {
|
||||
return CommandGroupBase.sequence(
|
||||
new TimedWaitUntilCommand(this::isLockedOn, lockOnDuration).withTimeout(lockOnTimeAllowance).withName(name + "LockOn"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage).withName(name + "Feed"),
|
||||
new WaitCommand(storageRunTime).withName(name + "ShootTimer"),
|
||||
new InstantCommand(() -> m_robotStorage.runStorage(0.0)).withName(name + "StopFeed")).deadlineWith(new TrackTarget(m_robotVisionOdometry, m_robotTurret, m_robotHood, m_robotBoomBoom).withName(name + "Track")
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Generate autonomous
|
||||
* @param maxVel max velocity for the path (null to override default value of 5.0)
|
||||
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
|
||||
* @param inputs strings (paths) or commands you want to run (in order)
|
||||
* @return array of commands
|
||||
*/
|
||||
private SequentialCommandGroup makePathingGroup(Double maxVel, Double maxAccel, String input) {
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
PathPlannerTrajectory pathTrajectory = PathPlanner.loadPath(input, Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY), Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION));
|
||||
PathPlannerState initialState = pathTrajectory.getInitialState();
|
||||
Pose2d initialPosition = new Pose2d(initialState.poseMeters.getTranslation(), initialState.holonomicRotation);
|
||||
|
||||
return new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initialPosition), m_robotSwerveDrive),
|
||||
new PPSwerveControllerCommand(pathTrajectory, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.m_kinematics, xController, yController, thetaController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive),
|
||||
new InstantCommand(m_robotSwerveDrive::stopModules, m_robotSwerveDrive)
|
||||
);
|
||||
}
|
||||
|
||||
public void switchControlMode() {
|
||||
if (currentControlMode == ControlMode.SHOOTER) {
|
||||
currentControlMode = ControlMode.CLIMBER;
|
||||
} else {
|
||||
currentControlMode = ControlMode.SHOOTER;
|
||||
}
|
||||
currentControlMode = currentControlMode == ControlMode.SHOOTER ? ControlMode.CLIMBER : ControlMode.SHOOTER;
|
||||
}
|
||||
|
||||
public void switchDriveMode() {
|
||||
if (currentDriveMode == DriveMode.ON) {
|
||||
currentDriveMode = DriveMode.OFF;
|
||||
} else {
|
||||
currentDriveMode = DriveMode.ON;
|
||||
}
|
||||
currentDriveMode = currentDriveMode == DriveMode.ON ? DriveMode.OFF : DriveMode.ON;
|
||||
}
|
||||
|
||||
public static DeadbandedXboxController getDriverController() {
|
||||
public DeadbandedXboxController getDriverController() {
|
||||
return m_driverXbox;
|
||||
}
|
||||
|
||||
public static DeadbandedXboxController getOperatorController() {
|
||||
public DeadbandedXboxController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
}
|
||||
|
||||
@@ -665,19 +416,6 @@ public class RobotContainer {
|
||||
return m_buttonBox;
|
||||
}
|
||||
|
||||
public static void setSoftLimits(boolean set) {
|
||||
softLimits = set;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get odometry.
|
||||
*
|
||||
* @return Odometry
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
return m_robotSwerveDrive.getOdometry();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set odometry to given pose.
|
||||
*
|
||||
|
||||
Reference in New Issue
Block a user