Merge branch 'shooter-test' into AutoAlign

This commit is contained in:
Abhishrek05
2024-02-20 19:42:53 -07:00
24 changed files with 969 additions and 198 deletions
+197 -81
View File
@@ -7,12 +7,10 @@
package frc4388.robot;
import java.time.Instant;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.ConditionalCommand;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -24,14 +22,16 @@ import frc4388.robot.commands.Autos.AutoAlign;
import frc4388.robot.commands.Autos.PlaybackChooser;
import frc4388.robot.commands.Swerve.JoystickPlayback;
import frc4388.robot.commands.Swerve.JoystickRecorder;
import frc4388.robot.commands.Swerve.neoJoystickPlayback;
import frc4388.robot.commands.Swerve.neoJoystickRecorder;
import frc4388.robot.commands.Intake.ArmIntakeIn;
import frc4388.robot.commands.Intake.RotateIntakeToPosition;
import frc4388.robot.subsystems.LED;
import frc4388.robot.subsystems.Limelight;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Shooter;
import frc4388.robot.subsystems.Intake;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.VirtualController;
import frc4388.utility.controller.XboxController;
/**
@@ -65,17 +65,19 @@ public class RobotContainer {
/* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
/* Autos */
// This shoud be in a SequentialCommandGroup vvv
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2);
private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
private Command MoveToSpeaker = new JoystickPlayback(m_robotSwerveDrive, "MoveToSpeaker.txt");
/* Virtual Controllers */
private final VirtualController m_virtualDriver = new VirtualController(0);
private final VirtualController m_virtualOperator = new VirtualController(1);
private Limelight limelight = new Limelight();
private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter);
private SequentialCommandGroup intakeToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotIntake.pidIn()),
new InstantCommand(() -> m_robotIntake.talonPIDIn()),
new InstantCommand(() -> m_robotShooter.spin())
);
@@ -94,7 +96,7 @@ public class RobotContainer {
private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight);
private SequentialCommandGroup autoShoot = new SequentialCommandGroup(
MoveToSpeaker,
// MoveToSpeaker,
autoAlign,
new InstantCommand(() -> m_robotShooter.spin()),
new WaitCommand(3.0),
@@ -112,27 +114,88 @@ public class RobotContainer {
private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake)
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake)
);
private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake)
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
/* Autos */
private Command taxi = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt");
private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt");
private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup (
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
new WaitCommand(1).asProxy(),
new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")
);
private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup (
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup(
startLeftMoveRight.asProxy(),
ejectToShoot.asProxy(),
taxi.asProxy()
);
private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup(
startRightMoveLeft.asProxy(),
ejectToShoot.asProxy(),
taxi.asProxy()
);
private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup(
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake),
intakeToShootStuff.asProxy(),
new WaitCommand(1).asProxy(),
new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt1.txt"),
intakeToShoot.asProxy(),
new WaitCommand(1).asProxy(),
new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"),
new WaitCommand(0.5).asProxy(),
new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake),
new WaitCommand(1).asProxy(),
new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
);
private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
.addOption("Taxi Auto", new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
.addOption("Taxi Auto", taxi.asProxy())
.addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker)
.addOption("One Note Auto Starting in Front of Speaker, But Stay", oneNoteStartingSpeakerStationary)
.addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft)
.addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight)
.buildDisplay();
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
configureButtonBindings();
configureButtonBindings();
configureVirtualButtonBindings();
DriverStation.silenceJoystickConnectionWarning(true);
CameraServer.startAutomaticCapture();
/* Default Commands */
// drives the robot with a two-axis input from the driver controller
@@ -145,10 +208,6 @@ public class RobotContainer {
// continually sends updates to the Blinkin LED controller to keep the lights on
m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
SmartDashboard.putNumber("Velocity Output", m_robotIntake.getVelocity());
// m_robotIntake.resetPostion();
}
/**
@@ -162,8 +221,6 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
/* Auto Recording */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
@@ -171,84 +228,131 @@ public class RobotContainer {
// () -> getDeadbandedDriverController().getLeftY(),
// () -> getDeadbandedDriverController().getRightX(),
// () -> getDeadbandedDriverController().getRightY(),
// "IntenseTaxi.txt"))
// () -> getDeadbandedOperatorController().getLeftBumper(),
// () -> getDeadbandedOperatorController().getRightBumper(),
// "TwoNotePrt1.txt"))
// .onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
.onFalse(new InstantCommand());
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
// .onFalse(new InstantCommand());
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
"2note.auto"))
.onFalse(new InstantCommand());
/* Speed */
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
"2note.auto",
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false))
.onFalse(new InstantCommand());
// /* Speed */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
/* Operator Buttons */
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 360).execute(), m_robotIntake))
// .onFalse(new InstantCommand(() -> new RotateIntakeToPosition(m_robotIntake, 0).execute(), m_robotShooter));
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
.onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.pidOut()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
// //Pull arm in
// new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.pidIn(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake));
// //Pull arm out
// new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.pidOut(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor(), m_robotIntake));
// //Intake Note
// new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.spinIntakeMotor(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
// //Outtake Note
// new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.handoff(), m_robotIntake))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopIntakeMotors(), m_robotIntake));
//Spin Shooter Motors
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut()))
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonSpinIntakeMotor()))
.onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors()));
// Override Intake Position encoder: out
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake));
// Override Intake Position encoder: in
new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(0), m_robotIntake));
//Spin Shooter Motors
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter))
.onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter));
// //Intake Note and ramp up shooter to 40%
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
// .onTrue(intakeToShoot);
// //Ramps up shooter to 100% to Shooter
// new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
// .onTrue(outtakeToShootFull);
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(ejectToShoot)
.onFalse(turnOffShoot);
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(i)
.onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn()));
//spins up shooter, no wind down
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
}
private void configureVirtualButtonBindings() {
/* Driver Buttons */
new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
/* Speed */
new JoystickButton(getVirtualDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
new JoystickButton(getVirtualDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
/* Operator Buttons */
new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
new JoystickButton(getVirtualOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.pidOut()))
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
// Override Intake Position encoder: out
new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake));
// Override Intake Position encoder: in
new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON)
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake));
//Spin Shooter Motors
new JoystickButton(getVirtualOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter))
.onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter));
new JoystickButton(getVirtualOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(ejectToShoot)
.onFalse(turnOffShoot);
new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(i)
.onFalse(new InstantCommand(() -> m_robotIntake.pidIn()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON)
.onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter));
}
/**
@@ -257,8 +361,12 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
//no auto
return playbackChooser.getCommand();
//no auto
return new neoJoystickPlayback(m_robotSwerveDrive,
"2note.auto",
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false);
//return playbackChooser.getCommand();
}
/**
@@ -271,4 +379,12 @@ public class RobotContainer {
public DeadbandedXboxController getDeadbandedOperatorController() {
return this.m_operatorXbox;
}
public VirtualController getVirtualDriverController() {
return m_virtualDriver;
}
public VirtualController getVirtualOperatorController() {
return m_virtualOperator;
}
}