From 5e53702ff47cb298523ca86f9c4da30dba685b16 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 2 Apr 2024 16:34:54 -0600 Subject: [PATCH 1/5] update rotation speed --- src/main/java/frc4388/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 58513e1..fbaa647 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -23,7 +23,7 @@ import frc4388.utility.LEDPatterns; public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 3.5; + public static final double MAX_ROT_SPEED = 2.5; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; public static double ROTATION_SPEED = MAX_ROT_SPEED; From 6a8630f14756cd0122c98c1a4c549beec5ba5507 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 4 Apr 2024 17:34:41 -0600 Subject: [PATCH 2/5] revert --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index f0429f8..cdd52a1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -160,7 +160,7 @@ public class RobotContainer { private String lastAutoName = "final_red_center_4note_taxi.auto"; private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName); private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, - lastAutoName, // () -> autoplaybackName.get(), + () -> autoplaybackName.get(), // lastAutoName, // () -> autoplaybackName.get(), new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, true); From 05b3ce17863e9a29a081e22b5d44560a1ef3ab5e Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 4 Apr 2024 17:35:26 -0600 Subject: [PATCH 3/5] false --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index cdd52a1..4188fe1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -162,7 +162,7 @@ public class RobotContainer { private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, () -> autoplaybackName.get(), // lastAutoName, // () -> autoplaybackName.get(), new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, true); + true, false); //Help Simplify Shooting From 1313e17ae6b1d775ba2566e5377b4f3eeb198be7 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 4 Apr 2024 17:37:45 -0600 Subject: [PATCH 4/5] negative one --- src/main/java/frc4388/utility/controller/VirtualController.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/utility/controller/VirtualController.java b/src/main/java/frc4388/utility/controller/VirtualController.java index 709bc1d..85adb64 100644 --- a/src/main/java/frc4388/utility/controller/VirtualController.java +++ b/src/main/java/frc4388/utility/controller/VirtualController.java @@ -39,7 +39,7 @@ public class VirtualController extends GenericHID { m_axes = new double[6]; m_buttonStates = 0; m_buttonStatesLastFrame = 0; - m_pov = new short[1]; + m_pov = new short[] {-1}; } /** From 18688868016aaf82696b92532e5bfc3152691555 Mon Sep 17 00:00:00 2001 From: Daniel Carta <79732052+immortaldan10@users.noreply.github.com> Date: Thu, 2 May 2024 17:29:41 -0600 Subject: [PATCH 5/5] Added 12 speed transmission, no clutch (for now...) --- src/main/java/frc4388/robot/Constants.java | 9 ++- .../java/frc4388/robot/RobotContainer.java | 35 ---------- .../frc4388/robot/subsystems/SwerveDrive.java | 67 +++---------------- 3 files changed, 15 insertions(+), 96 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index fbaa647..386c734 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -33,9 +33,12 @@ public final class Constants { public static final double CORRECTION_MIN = 10; public static final double CORRECTION_MAX = 50; - public static final double SLOW_SPEED = 0.5; - public static final double FAST_SPEED = 1.0; - public static final double TURBO_SPEED = 4.0; + public static final double[] SPEEDS = {0.25, 0.5, 0.75, 1.0, 1.25, 1.5, 1.75, 2, 2.5, 3, 3.5, 4}; + // public static final double[] SPEEDS = {0.5, 1.0, 4.0}; + + // public static final double SLOW_SPEED = 0.5; + // public static final double FAST_SPEED = 1.0; + // public static final double TURBO_SPEED = 4.0; public static final class DefaultSwerveRotOffsets { public static final double FRONT_LEFT_ROT_OFFSET = 220; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4188fe1..1f0434c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -310,7 +310,6 @@ public class RobotContainer { true); }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); - m_robotSwerveDrive.setToSlow(); // ! Swerve Drive Default Command (Orientation Rotation) // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { @@ -505,40 +504,6 @@ public class RobotContainer { new JoystickButton(getVirtualDriverController(), XboxController.START_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - - - // * /* D-Pad Stuff */ - // new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); - - // new Trigger(() -> getVirtualDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0), - // new Translation2d(0, 0), - // true))) - // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), - // new Translation2d(0, 0), - // true))); // ! /* Auto Recording */ new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 8e00166..98feae6 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -92,13 +92,10 @@ public class SwerveDrive extends SubsystemBase { public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { - double rot = 0; - // ! drift correction if (rightStick.getNorm() > 0.05) { rotTarget = gyro.getAngle(); - rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - // SmartDashboard.putBoolean("drift correction", false); + // SmartDashboard.putBoolean("drift correction", false); stopped = false; } else if(leftStick.getNorm() > 0.05) { if (!stopped) { @@ -107,8 +104,7 @@ public class SwerveDrive extends SubsystemBase { } // SmartDashboard.putBoolean("drift correction", true); - - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + } @@ -129,7 +125,6 @@ public class SwerveDrive extends SubsystemBase { Translation2d rightStick = new Translation2d(-rightX, rightY); if(fieldRelative) { - double rot = 0; if(rightStick.getNorm() > 0.5) { orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); @@ -139,7 +134,6 @@ public class SwerveDrive extends SubsystemBase { if(tmp.getDegrees() < 0) min*=-1; tmp = new Rotation2d(min * Math.PI / 180); - rot = tmp.getRadians(); // x x - y/x } Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); @@ -228,61 +222,18 @@ public class SwerveDrive extends SubsystemBase { // SmartDashboard.putNumber("Gyro", getGyroAngle()); } + private int GEAR = SwerveDriveConstants.SPEEDS.length /2; public void shiftDown() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; - } else { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - + if(GEAR > 0){ + GEAR--; + this.speedAdjust = SwerveDriveConstants.SPEEDS[GEAR]; } } - public void setToSlow() { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - } - - public void setToFast() { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - } - - public void setToTurbo() { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - } - public void shiftUp() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; - } else { - - } - } - - public void toggleGear(double angle) { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } else { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; + if(GEAR < SwerveDriveConstants.SPEEDS.length){ + GEAR++; + this.speedAdjust = SwerveDriveConstants.SPEEDS[GEAR]; } }