mirror of
https://github.com/Team4388/2024AcrossTheRidgebotiverse.git
synced 2026-06-09 00:38:02 -06:00
mondays changes
This commit is contained in:
Abhishrek05
@@ -58,7 +58,9 @@ 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);
|
||||
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2);
|
||||
|
||||
|
||||
/* Virtual Controllers */
|
||||
private final VirtualController m_virtualDriver = new VirtualController(0);
|
||||
@@ -119,7 +121,9 @@ public class RobotContainer {
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
configureButtonBindings();
|
||||
configureVirtualButtonBindings();
|
||||
|
||||
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
|
||||
@@ -144,17 +148,9 @@ public class RobotContainer {
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
|
||||
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> getVirtualDriverController().setOutput(XboxController.A_BUTTON, true)))
|
||||
// .onFalse(new InstantCommand(() -> getVirtualDriverController().setOutput(XboxController.A_BUTTON, false)));
|
||||
|
||||
// new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
// //.onTrue(new InstantCommand(() -> System.out.println("Hello")));
|
||||
// .onTrue(new InstantCommand(() -> getVirtualDriverController().setRumble(RumbleType.kBothRumble, 1.0)))
|
||||
// .onFalse(new InstantCommand(() -> getVirtualDriverController().setRumble(RumbleType.kBothRumble, 0.0)));
|
||||
/* Auto Recording */
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
|
||||
@@ -168,78 +164,119 @@ public class RobotContainer {
|
||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
||||
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
|
||||
// .onFalse(new InstantCommand());
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
|
||||
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
|
||||
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
|
||||
"sample.auto"))
|
||||
"2note.auto"))
|
||||
.onFalse(new InstantCommand());
|
||||
|
||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
"sample.auto",
|
||||
"2note.auto",
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false))
|
||||
.onFalse(new InstantCommand());
|
||||
|
||||
new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> System.out.println("Pressed A")))
|
||||
.onFalse(new InstantCommand(() -> System.out.println("Released A")));
|
||||
|
||||
new JoystickButton(getVirtualDriverController(), XboxController.B_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> System.out.println("Pressed B")))
|
||||
.onFalse(new InstantCommand(() -> System.out.println("Released B")));
|
||||
|
||||
new JoystickButton(getVirtualDriverController(), XboxController.X_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> System.out.println("Pressed X")))
|
||||
.onFalse(new InstantCommand(() -> System.out.println("Released X")));
|
||||
|
||||
new JoystickButton(getVirtualDriverController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> System.out.println("Pressed Y")))
|
||||
.onFalse(new InstantCommand(() -> System.out.println("Released Y")));
|
||||
/* 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.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(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
|
||||
// .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
|
||||
|
||||
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotIntake.pidOut()))
|
||||
// .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.pidOut()))
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
|
||||
|
||||
|
||||
// // Override Intake Position encoder: out
|
||||
// new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake));
|
||||
// Override Intake Position encoder: out
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake));
|
||||
|
||||
// // Override Intake Position encoder: in
|
||||
// new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
|
||||
// .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake));
|
||||
// Override Intake Position encoder: in
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
|
||||
.onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(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));
|
||||
//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));
|
||||
|
||||
// new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||
// .onTrue(ejectToShoot)
|
||||
// .onFalse(turnOffShoot);
|
||||
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.pidIn()));
|
||||
new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||
.onTrue(i)
|
||||
.onFalse(new InstantCommand(() -> m_robotIntake.pidIn()));
|
||||
|
||||
//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));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -249,8 +286,11 @@ public class RobotContainer {
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
//no auto
|
||||
|
||||
return playbackChooser.getCommand();
|
||||
return new neoJoystickPlayback(m_robotSwerveDrive,
|
||||
"2note.auto",
|
||||
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
|
||||
true, false);
|
||||
//return playbackChooser.getCommand();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -68,13 +68,13 @@ public class PlaybackChooser {
|
||||
|
||||
String[] dirs = m_dir.list();
|
||||
|
||||
if(dirs == null){ // Fix funny error
|
||||
return;
|
||||
if(dirs != null){ // Fix funny error
|
||||
for (String auto : dirs) {
|
||||
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
|
||||
}
|
||||
}
|
||||
|
||||
for (String auto : dirs) {
|
||||
chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
|
||||
}
|
||||
|
||||
for (var cmd_name : m_commandPool.keySet()) {
|
||||
chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
|
||||
}
|
||||
|
||||
@@ -16,11 +16,11 @@ public class neoJoystickPlayback extends Command {
|
||||
private final String filename;
|
||||
private final VirtualController[] controllers;
|
||||
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
|
||||
private int frame_index = 0;
|
||||
private long startTime = 0;
|
||||
private long playbackTime = 0;
|
||||
private boolean m_finished = false; // ! There is no better way.
|
||||
private boolean m_shouldfree = false; // should free memory on ending
|
||||
private int frame_index = 0;
|
||||
private long startTime = 0;
|
||||
private long playbackTime = 0;
|
||||
private boolean m_finished = false; // ! There is no better way.
|
||||
private boolean m_shouldfree = false; // should free memory on ending
|
||||
|
||||
private byte m_numAxes = 0;
|
||||
private byte m_numPOVs = 0;
|
||||
@@ -42,7 +42,7 @@ public class neoJoystickPlayback extends Command {
|
||||
}
|
||||
|
||||
public boolean loadAuto() {
|
||||
try (FileInputStream stream = new FileInputStream("./" + filename)) {
|
||||
try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
|
||||
if (m_numFrames != -1 && m_numFrames == frames.size()) {
|
||||
System.out.println("AUTOPLAYBACK: Auto Already loaded.");
|
||||
return true;
|
||||
|
||||
@@ -69,7 +69,7 @@ public class neoJoystickRecorder extends Command {
|
||||
}
|
||||
@Override
|
||||
public void end(boolean interrupted) {
|
||||
try (FileOutputStream stream = new FileOutputStream("./" + filename)) {
|
||||
try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
|
||||
// header: size of 0x5
|
||||
// byte Number of axes per controller
|
||||
// byte Number of POVs per controller
|
||||
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
@@ -35,6 +36,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
|
||||
|
||||
public double rotTarget = 0.0;
|
||||
public Rotation2d orientRotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
/** Creates a new SwerveDrive. */
|
||||
@@ -55,6 +57,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
double rot = 0;
|
||||
|
||||
// ! drift correction
|
||||
if (rightStick.getNorm() > 0.05) {
|
||||
rotTarget = gyro.getAngle();
|
||||
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
|
||||
@@ -84,6 +87,24 @@ public class SwerveDrive extends SubsystemBase {
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
|
||||
|
||||
if(fieldRelative) {
|
||||
double rot = 0;
|
||||
if(rightStick.getNorm() > 0.5) {
|
||||
orientRotTarget = rightStick.getAngle().minus(new Rotation2d(0, 1));
|
||||
rot = orientRotTarget.minus(gyro.getRotation2d()).getRadians();
|
||||
}
|
||||
|
||||
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
|
||||
|
||||
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
|
||||
} else { // Create robot-relative speeds.
|
||||
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
|
||||
}
|
||||
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds));
|
||||
}
|
||||
|
||||
/**
|
||||
* Set each module of the swerve drive to the corresponding desired state.
|
||||
* @param desiredStates Array of module states to set.
|
||||
|
||||
Reference in New Issue
Block a user