mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-08 16:28:03 -06:00
Cleanup and re-enable auto recording
This commit is contained in:
C4llSiqn
@@ -11,46 +11,33 @@ package frc4388.robot;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.cameraserver.CameraServer;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.GenericHID.RumbleType;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
|
||||
// Commands
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
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.WaitUntilCommand;
|
||||
|
||||
// Autos
|
||||
import frc4388.robot.commands.Autos.PlaybackChooser;
|
||||
import frc4388.robot.commands.Intake.ArmIntakeIn;
|
||||
import frc4388.robot.commands.Swerve.JoystickPlayback;
|
||||
import frc4388.robot.commands.Swerve.JoystickRecorder;
|
||||
import frc4388.utility.controller.VirtualController;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
|
||||
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
|
||||
// import frc4388.robot.commands.Intake.ArmIntakeIn;
|
||||
//import frc4388.robot.commands.Autos.AutoAlign;
|
||||
|
||||
// Subsystems
|
||||
import frc4388.robot.subsystems.Climber;
|
||||
import frc4388.robot.subsystems.Intake;
|
||||
import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.Shooter;
|
||||
// Subsystems
|
||||
// import frc4388.robot.subsystems.LED;
|
||||
// import frc4388.robot.subsystems.Limelight;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
// import frc4388.robot.subsystems.Shooter;
|
||||
// import frc4388.robot.subsystems.Climber;
|
||||
// import frc4388.robot.subsystems.Intake;
|
||||
|
||||
// Utilites
|
||||
import frc4388.utility.DeferredBlock;
|
||||
@@ -98,28 +85,13 @@ public class RobotContainer {
|
||||
private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
|
||||
private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter);
|
||||
|
||||
// ! Teleop Commands
|
||||
|
||||
private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
|
||||
new InstantCommand(() -> m_robotIntake.PIDIn()),
|
||||
new InstantCommand(() -> m_robotShooter.idle())
|
||||
// new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))),
|
||||
// new InstantCommand(() -> m_robotShooter.spin())
|
||||
);
|
||||
|
||||
|
||||
// ! Teleop Commands
|
||||
private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
|
||||
// MoveToSpeaker,
|
||||
//autoAlign,
|
||||
new InstantCommand(() -> m_robotShooter.spin()),
|
||||
new WaitCommand(3.0),
|
||||
new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake),
|
||||
new WaitCommand(3.0),
|
||||
new InstantCommand(() -> m_robotShooter.idle())
|
||||
// new InstantCommand(() -> autoAlign.reverse()),
|
||||
// autoAlign.asProxy()
|
||||
);
|
||||
|
||||
|
||||
private SequentialCommandGroup i = new SequentialCommandGroup(
|
||||
intakeToShootStuff, intakeToShoot,
|
||||
new InstantCommand(() -> m_robotShooter.idle())
|
||||
@@ -148,7 +120,6 @@ public class RobotContainer {
|
||||
);
|
||||
|
||||
// ! /* Autos */
|
||||
// private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
|
||||
private String lastAutoName = "final_red_center_4note_taxi.auto";
|
||||
private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
|
||||
private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
@@ -156,58 +127,31 @@ public class RobotContainer {
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false);
|
||||
|
||||
// private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
|
||||
// .addOption("Taxi Auto", taxi.asProxy())
|
||||
// .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy())
|
||||
// .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy())
|
||||
// // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy())
|
||||
// // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy())
|
||||
// .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy())
|
||||
// .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy())
|
||||
// .buildDisplay();
|
||||
|
||||
// private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
|
||||
// // MoveToSpeaker,
|
||||
// //autoAlign,
|
||||
// new InstantCommand(() -> m_robotShooter.spin()),
|
||||
// new WaitCommand(3.0),
|
||||
// new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake),
|
||||
// new WaitCommand(3.0),
|
||||
// new InstantCommand(() -> m_robotShooter.idle())
|
||||
// // new InstantCommand(() -> autoAlign.reverse()),
|
||||
// // autoAlign.asProxy()
|
||||
// );
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
// configureVirtualButtonBindings();
|
||||
// new Trigger(() -> autoplaybackName.get().equals(lastAutoName)).onTrue(new InstantCommand(() -> changeAuto()));
|
||||
// new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
|
||||
configureVirtualButtonBindings();
|
||||
new DeferredBlock(() -> m_robotSwerveDrive.resetGyro());
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
// CameraServer.startAutomaticCapture();
|
||||
|
||||
/* Default Commands */
|
||||
// ! Swerve Drive Default Command (Regular Rotation)
|
||||
// drives the robot with a two-axis input from the driver controller
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
|
||||
getDeadbandedDriverController().getRight(),
|
||||
true);
|
||||
}, m_robotSwerveDrive)
|
||||
.withName("SwerveDrive DefaultCommand"));
|
||||
|
||||
// DriverStation.silenceJoystickConnectionWarning(true);
|
||||
// // CameraServer.startAutomaticCapture();
|
||||
|
||||
// /* Default Commands */
|
||||
// // drives the robot with a two-axis input from the driver controller
|
||||
// ! Swerve Drive Default Command (Regular Rotation)
|
||||
|
||||
// ! Swerve Drive One Module Test
|
||||
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
// m_robotMap.rightFront.go(getDeadbandedDriverController().getLeft());
|
||||
// }
|
||||
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
// m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(),
|
||||
// getDeadbandedDriverController().getRight(),
|
||||
// true);
|
||||
// }, m_robotSwerveDrive)
|
||||
// .withName("SwerveDrive DefaultCommand"));
|
||||
// m_robotSwerveDrive.setToSlow();
|
||||
|
||||
// ! Swerve Drive Default Command (Orientation Rotation)
|
||||
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
@@ -215,7 +159,10 @@ public class RobotContainer {
|
||||
// getDeadbandedDriverController().getRightX(),
|
||||
// getDeadbandedDriverController().getRightY(),
|
||||
// true);
|
||||
// }, m_robotSwerveDrive));
|
||||
// }, m_robotSwerveDrive))
|
||||
// .withName("SwerveDrive OrientationCommand"));
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
|
||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||
m_robotSwerveDrive.driveWithInput(
|
||||
@@ -226,18 +173,9 @@ public class RobotContainer {
|
||||
|
||||
|
||||
|
||||
// .withName("SwerveDrive OrientationCommand"));
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
|
||||
|
||||
}
|
||||
|
||||
// private void changeAuto() {
|
||||
// autoPlayback.unloadAuto();
|
||||
// autoPlayback.loadAuto();
|
||||
// lastAutoName = autoplaybackName.get();
|
||||
// System.out.println("AUTO: Changed auto to; `" + lastAutoName + "`");
|
||||
// }
|
||||
/**
|
||||
* Use this method to define your button->command mappings. Buttons can be
|
||||
* created by instantiating a {@link GenericHID} or one of its subclasses
|
||||
@@ -251,65 +189,10 @@ public class RobotContainer {
|
||||
DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
|
||||
|
||||
// DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()))
|
||||
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180()));
|
||||
|
||||
// DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()))
|
||||
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180()));
|
||||
|
||||
// * /* D-Pad Stuff */
|
||||
// new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
|
||||
// new Translation2d(0, 0),
|
||||
// true)))
|
||||
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
|
||||
// new Translation2d(0, 0),
|
||||
// true)));
|
||||
|
||||
// new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
|
||||
// new Translation2d(0, 0),
|
||||
// true)))
|
||||
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
|
||||
// new Translation2d(0, 0),
|
||||
// true)));
|
||||
|
||||
// new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
|
||||
// new Translation2d(0, 0),
|
||||
// true)))
|
||||
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
|
||||
// new Translation2d(0, 0),
|
||||
// true)));
|
||||
|
||||
// new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
|
||||
// new Translation2d(0, 0),
|
||||
// true)))
|
||||
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0),
|
||||
// new Translation2d(0, 0),
|
||||
// true)));
|
||||
|
||||
// ! /* Auto Recording */
|
||||
// new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
|
||||
// .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
|
||||
// new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
|
||||
// () -> autoplaybackName.get()))
|
||||
// .onFalse(new InstantCommand());
|
||||
|
||||
// new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
|
||||
// .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
// () -> autoplaybackName.get(),
|
||||
// new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
// true, false))
|
||||
// .onFalse(new InstantCommand());
|
||||
|
||||
// ! /* Speed */
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
|
||||
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
|
||||
.onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
|
||||
@@ -321,11 +204,10 @@ public class RobotContainer {
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot()));
|
||||
|
||||
|
||||
|
||||
//? /* Operator Buttons */
|
||||
// ? /* Operator Buttons */
|
||||
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.PIDIn()))
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.PIDIn()))
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
|
||||
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.X_BUTTON)
|
||||
@@ -357,7 +239,7 @@ public class RobotContainer {
|
||||
.onTrue(i)
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.PIDIn()));
|
||||
|
||||
//spins up shooter, no wind down
|
||||
// Spins up shooter, no wind down
|
||||
DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
|
||||
|
||||
@@ -375,6 +257,21 @@ public class RobotContainer {
|
||||
new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5)));
|
||||
|
||||
// ? /* Programer Buttons (Controller 3)*/
|
||||
|
||||
// * /* Auto Recording */
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
|
||||
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
|
||||
() -> autoplaybackName.get()))
|
||||
.onFalse(new InstantCommand());
|
||||
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
() -> autoplaybackName.get(),
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false))
|
||||
.onFalse(new InstantCommand());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -399,16 +296,8 @@ public class RobotContainer {
|
||||
* Climbing controls : We don't need to climb in auto.
|
||||
*/
|
||||
|
||||
// new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
|
||||
// .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
|
||||
// .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
|
||||
// ? Notice: the Programer Buttons are not to be replicated because they are designed for debuging the robot, and do not need to be replicated in auto.
|
||||
|
||||
// new Trigger(() -> getVirtualOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
|
||||
// .onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
|
||||
// .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
|
||||
|
||||
new Trigger(() -> getVirtualOperatorController().getPOV() == 0)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.ampOuttake(0.5)));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -418,9 +307,9 @@ public class RobotContainer {
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
//no auto
|
||||
// return autoPlayback;
|
||||
return autoPlayback;
|
||||
//return playbackChooser.getCommand();
|
||||
return new Command() {};
|
||||
// return new Command() {};
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -433,9 +322,6 @@ public class RobotContainer {
|
||||
return new Trigger(() -> (joystickA.getRawButton(buttonNumber) || joystickB.getRawButton(buttonNumber)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public DeadbandedXboxController getDeadbandedDriverController() {
|
||||
return this.m_driverXbox;
|
||||
}
|
||||
|
||||
@@ -7,7 +7,6 @@ package frc4388.robot.commands.Intake;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import frc4388.robot.subsystems.Intake;
|
||||
import frc4388.robot.subsystems.Shooter;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
|
||||
public class ArmIntakeIn extends Command {
|
||||
/** Creates a new ArmIntakeIn. */
|
||||
|
||||
@@ -37,7 +37,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
private RobotGyro gyro;
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MIN_ROT_SPEED;
|
||||
public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
|
||||
public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
|
||||
Reference in New Issue
Block a user