Team4388/2024AcrossTheRidgebotiverse
1
0
Fork 0
mirror of https://github.com/Team4388/2024AcrossTheRidgebotiverse.git synced 2026-06-09 00:38:02 -06:00
Code Issues Packages Projects Releases Wiki Activity

mondays changes

Browse Source
This commit is contained in:
Abhishrek05
2024-02-19 15:04:55 -07:00
parent 2660adcd68
commit 83a4da3060
5 changed files with 133 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+100 -60
View File
@@ -58,7 +58,9 @@ public class RobotContainer {
/* Controllers */ /* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); 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 */ /* Virtual Controllers */
private final VirtualController m_virtualDriver = new VirtualController(0); 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. * The container for the robot. Contains subsystems, OI devices, and commands.
*/ */
public RobotContainer() { public RobotContainer() {
configureButtonBindings(); configureButtonBindings();
configureVirtualButtonBindings();
DriverStation.silenceJoystickConnectionWarning(true); DriverStation.silenceJoystickConnectionWarning(true);
@@ -144,17 +148,9 @@ public class RobotContainer {
*/ */
private void configureButtonBindings() { private void configureButtonBindings() {
/* Driver Buttons */ /* Driver Buttons */
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); .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 */ /* Auto Recording */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
// .whileTrue(new JoystickRecorder(m_robotSwerveDrive, // .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
@@ -168,78 +164,119 @@ public class RobotContainer {
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
// .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt")) // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
// .onFalse(new InstantCommand()); // .onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
.whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
"sample.auto")) "2note.auto"))
.onFalse(new InstantCommand()); .onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON)
.onTrue(new neoJoystickPlayback(m_robotSwerveDrive, .onTrue(new neoJoystickPlayback(m_robotSwerveDrive,
"sample.auto", "2note.auto",
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false)) true, false))
.onFalse(new InstantCommand()); .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 */ /* Speed */
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
// // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
/* Operator Buttons */ /* Operator Buttons */
// new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.pidIn())) .onTrue(new InstantCommand(() -> m_robotIntake.pidIn()))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.pidOut())) .onTrue(new InstantCommand(() -> m_robotIntake.pidOut()))
// .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor())); .onFalse(new InstantCommand(() -> m_robotIntake.stopArmMotor()));
// // Override Intake Position encoder: out // Override Intake Position encoder: out
// new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake)); .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(-53), m_robotIntake));
// // Override Intake Position encoder: in // Override Intake Position encoder: in
// new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake)); .onTrue(new InstantCommand(() -> m_robotIntake.setPivotEncoderPosition(0), m_robotIntake));
// //Spin Shooter Motors //Spin Shooter Motors
// new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter))
// .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)); .onFalse(new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(ejectToShoot) .onTrue(ejectToShoot)
// .onFalse(turnOffShoot); .onFalse(turnOffShoot);
// new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
// .onTrue(i) .onTrue(i)
// .onFalse(new InstantCommand(() -> m_robotIntake.pidIn())); .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() { public Command getAutonomousCommand() {
//no auto //no auto
return new neoJoystickPlayback(m_robotSwerveDrive,
return playbackChooser.getCommand(); "2note.auto",
new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()},
true, false);
//return playbackChooser.getCommand();
} }
/** /**
src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java
+5 -5
View File
@@ -68,13 +68,13 @@ public class PlaybackChooser {
String[] dirs = m_dir.list(); String[] dirs = m_dir.list();
if(dirs == null){ // Fix funny error if(dirs != null){ // Fix funny error
return; 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()) { for (var cmd_name : m_commandPool.keySet()) {
chooser.addOption(cmd_name, m_commandPool.get(cmd_name)); chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
} }
src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java
+6 -6
View File
@@ -16,11 +16,11 @@ public class neoJoystickPlayback extends Command {
private final String filename; private final String filename;
private final VirtualController[] controllers; private final VirtualController[] controllers;
private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>(); private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
private int frame_index = 0; private int frame_index = 0;
private long startTime = 0; private long startTime = 0;
private long playbackTime = 0; private long playbackTime = 0;
private boolean m_finished = false; // ! There is no better way. private boolean m_finished = false; // ! There is no better way.
private boolean m_shouldfree = false; // should free memory on ending private boolean m_shouldfree = false; // should free memory on ending
private byte m_numAxes = 0; private byte m_numAxes = 0;
private byte m_numPOVs = 0; private byte m_numPOVs = 0;
@@ -42,7 +42,7 @@ public class neoJoystickPlayback extends Command {
} }
public boolean loadAuto() { 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()) { if (m_numFrames != -1 && m_numFrames == frames.size()) {
System.out.println("AUTOPLAYBACK: Auto Already loaded."); System.out.println("AUTOPLAYBACK: Auto Already loaded.");
return true; return true;
src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java
+1 -1
View File
@@ -69,7 +69,7 @@ public class neoJoystickRecorder extends Command {
} }
@Override @Override
public void end(boolean interrupted) { public void end(boolean interrupted) {
try (FileOutputStream stream = new FileOutputStream("./" + filename)) { try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
// header: size of 0x5 // header: size of 0x5
// byte Number of axes per controller // byte Number of axes per controller
// byte Number of POVs per controller // byte Number of POVs per controller
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+21
View File
@@ -4,6 +4,7 @@
package frc4388.robot.subsystems; package frc4388.robot.subsystems;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics; 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 speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public double rotTarget = 0.0; public double rotTarget = 0.0;
public Rotation2d orientRotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */ /** Creates a new SwerveDrive. */
@@ -55,6 +57,7 @@ public class SwerveDrive extends SubsystemBase {
double rot = 0; double rot = 0;
// ! drift correction
if (rightStick.getNorm() > 0.05) { if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle(); rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
@@ -84,6 +87,24 @@ public class SwerveDrive extends SubsystemBase {
setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); 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. * Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set. * @param desiredStates Array of module states to set.