From c5f5e6c57fc4c5a8a74b6efa70fd63d0bfad81d0 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 29 Feb 2024 06:45:18 -0700 Subject: [PATCH] ready for comp (im geekin) --- .../java/frc4388/robot/RobotContainer.java | 145 +++++++++++------- .../java/frc4388/robot/subsystems/Intake.java | 4 +- .../frc4388/robot/subsystems/SwerveDrive.java | 6 +- 3 files changed, 94 insertions(+), 61 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 90aac51..a82dec8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -68,12 +68,12 @@ 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_autoRecorderXbox = new DeadbandedXboxController(2); + // private final DeadbandedXboxController m_autoRecorderXbox = new DeadbandedXboxController(2); /* Virtual Controllers */ - private final VirtualController m_virtualDriver = new VirtualController(0); - private final VirtualController m_virtualOperator = new VirtualController(1); + // private final VirtualController m_virtualDriver = new VirtualController(0); + // private final VirtualController m_virtualOperator = new VirtualController(1); private Command intakeToShootStuff = new ArmIntakeIn(m_robotIntake, m_robotShooter); @@ -124,12 +124,23 @@ public class RobotContainer { new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); - /* Autos */ + // ! /* Autos */ private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); //new InstantCommand(); private Command startLeftMoveRight = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartLeftMoveRight.txt"); private Command startRightMoveLeft = new InstantCommand(); // new JoystickPlayback(m_robotSwerveDrive, "StartRightMoveLeft.txt"); private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); + //Help Simplify Shooting + private SequentialCommandGroup pullInArmtoShoot = new SequentialCommandGroup( + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1.4).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(0.5), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + ); + private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), new WaitCommand(1).asProxy(), @@ -161,7 +172,8 @@ public class RobotContainer { private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( interrupt, - new InstantCommand(() -> m_robotIntake.talonPIDIn()) + new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup( @@ -172,13 +184,7 @@ public class RobotContainer { new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + pullInArmtoShoot, new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") // new WaitCommand(1).asProxy(), // new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"), @@ -190,6 +196,21 @@ public class RobotContainer { // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) ); + + private SequentialCommandGroup threeNoteStartingFromSpeaker = 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 ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), + pullInArmtoShoot, + //? Create Another Parallel Command Group :( + pullInArmtoShoot, + new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") + ); + private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) .addOption("Taxi Auto", taxi.asProxy()) .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker) @@ -214,12 +235,22 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller + // ! Swerve Drive Default Command (Regular Rotation) m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), getDeadbandedDriverController().getRight(), true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); + + // ! Swerve Drive Default Command (Orientation Rotation) + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + // m_robotSwerveDrive.driveWithInputOrientation(getDeadbandedDriverController().getLeft(), + // getDeadbandedDriverController().getRightX(), + // getDeadbandedDriverController().getRightY(), + // true); + // }, 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)); @@ -281,27 +312,27 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut())) .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.talonHandoff())) .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); + + new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) + .onTrue(emergencyRetract); // Override Intake Position encoder: out new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-57), m_robotIntake)); + .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-59), 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)); + new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)) @@ -328,55 +359,55 @@ public class RobotContainer { private void configureVirtualButtonBindings() { /* Driver Buttons */ - new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); + // 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())); + // /* 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())); + // new JoystickButton(getVirtualDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - /* Operator Buttons */ + // /* Operator Buttons */ - new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) - .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); + // new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) + // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); - new JoystickButton(getVirtualOperatorController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut())) - .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); + // new JoystickButton(getVirtualOperatorController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDOut())) + // .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); - // Override Intake Position encoder: out - new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake)); + // // Override Intake Position encoder: out + // new JoystickButton(getVirtualOperatorController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(-53), m_robotIntake)); - // Override Intake Position encoder: in - new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(0), m_robotIntake)); + // // Override Intake Position encoder: in + // new JoystickButton(getVirtualOperatorController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotIntake.talonSetPivotEncoderPosition(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)); + // //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.LEFT_BUMPER_BUTTON) + // .onTrue(ejectToShoot) + // .onFalse(turnOffShoot); - // new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) - // .onTrue(i) - // .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); + // // new JoystickButton(getVirtualOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) + // // .onTrue(i) + // // .onFalse(new InstantCommand(() -> m_robotIntake.talonPIDIn())); - new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); + // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_JOYSTICK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter)); } /** @@ -404,11 +435,11 @@ public class RobotContainer { return this.m_operatorXbox; } - public VirtualController getVirtualDriverController() { - return m_virtualDriver; - } + // public VirtualController getVirtualDriverController() { + // return m_virtualDriver; + // } - public VirtualController getVirtualOperatorController() { - return m_virtualOperator; - } + // public VirtualController getVirtualOperatorController() { + // return m_virtualOperator; + // } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 8ba0205..684fb8f 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -129,12 +129,12 @@ public class Intake extends SubsystemBase { // ! Talon Methods public void talonPIDIn() { PositionVoltage request = new PositionVoltage(-59); - talonPivot.setControl(request.withPosition(0)); //TODO: Find actual value + talonPivot.setControl(request.withPosition(0)); } public void talonPIDOut() { PositionVoltage request = new PositionVoltage(0); - talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value + talonPivot.setControl(request.withPosition(-59)); } public void talonHandoff() { diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index cac8f93..6ef7843 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -87,12 +87,14 @@ public class SwerveDrive extends SubsystemBase { setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } - public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { + + Translation2d rightStick = new Translation2d(rightX, rightY); if(fieldRelative) { double rot = 0; if(rightStick.getNorm() > 0.5) { - orientRotTarget = rightStick.getAngle().minus(new Rotation2d(0, 1)); + orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); rot = orientRotTarget.minus(gyro.getRotation2d()).getRadians(); }