diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 56230c6..6ef4e27 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -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(); } /** diff --git a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java index a270d7b..4fdf02a 100644 --- a/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java +++ b/src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java @@ -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)); } diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java index 529bd34..206a2b5 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -16,11 +16,11 @@ public class neoJoystickPlayback extends Command { private final String filename; private final VirtualController[] controllers; private final ArrayList 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; diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java index 7cae811..0ad4100 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -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 diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 8cb65d9..7554671 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -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.