From 6ab8b13056a56eb3fa57b0fcd8ca897dd9f2fc7f Mon Sep 17 00:00:00 2001 From: Aarav Date: Thu, 16 Mar 2023 12:58:55 -0600 Subject: [PATCH 1/4] fixes for driving --- src/main/java/frc4388/robot/Constants.java | 5 +- .../java/frc4388/robot/RobotContainer.java | 34 ++++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 62 +++++++++++++++++-- .../controller/DeadbandedXboxController.java | 8 ++- 4 files changed, 90 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f82c168..29a76af 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -23,7 +23,6 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 0.8; public static double ROTATION_SPEED = MAX_ROT_SPEED; @@ -32,6 +31,10 @@ 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.8; + public static final double FAST_SPEED = 1.0; + public static final double TURBO_SPEED = 4.0; + public static final class IDs { public static final int LEFT_FRONT_WHEEL_ID = 2; public static final int LEFT_FRONT_STEER_ID = 3; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1e721ef..45ca574 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -122,25 +122,33 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(m_robotArm.getArmRotation()-135), m_robotSwerveDrive)); + new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())) + .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); + // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .whileTrue(new JoystickRecorder(m_robotSwerveDrive, - () -> getDeadbandedDriverController().getLeftX(), - () -> getDeadbandedDriverController().getLeftY(), - () -> getDeadbandedDriverController().getRightX(), - () -> getDeadbandedDriverController().getRightY(), - "Blue1Path.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .whileTrue(new JoystickRecorder(m_robotSwerveDrive, + // () -> getDeadbandedDriverController().getLeftX(), + // () -> getDeadbandedDriverController().getLeftY(), + // () -> getDeadbandedDriverController().getRightX(), + // () -> getDeadbandedDriverController().getRightY(), + // "Blue1Path.txt")) + // .onFalse(new InstantCommand()); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) - .onFalse(new InstantCommand()); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt")) + // .onFalse(new InstantCommand()); // * Operator Buttons // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 2edb77d..556c63e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -53,6 +53,7 @@ public class SwerveDrive extends SubsystemBase { this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } + boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { @@ -61,13 +62,20 @@ public class SwerveDrive extends SubsystemBase { rotTarget = gyro.getAngle(); rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; SmartDashboard.putBoolean("drift correction", false); - } else { + stopped = false; + } else if(leftStick.getNorm() > 0.05) { + if (!stopped) { + stopModules(); + stopped = true; + } + SmartDashboard.putBoolean("drift correction", true); rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; } // Use the left joystick to set speed. Apply a cubic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + // Translation2d speed = leftStick.times(speedAdjust / leftStick.getNorm()); Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); // Convert field-relative speeds to robot-relative speeds. @@ -116,11 +124,57 @@ public class SwerveDrive extends SubsystemBase { // This method will be called once per scheduler run } + 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; + } + } + + 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 (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW - && Math.abs(angle) < 2) { + 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_MAX; + 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; diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index bad1605..d339841 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -20,8 +20,14 @@ public class DeadbandedXboxController extends XboxController { public static Translation2d skewToDeadzonedCircle(double x, double y) { Translation2d translation2d = new Translation2d(x, y); double magnitude = translation2d.getNorm(); - if (OIConstants.SKEW_STICKS && magnitude >= 1) return translation2d.div(magnitude); + + // if (OIConstants.SKEW_STICKS && magnitude >= 1) { + // System.out.println("if statement running"); + // return translation2d.div(magnitude); + // } + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); + return translation2d; } } \ No newline at end of file From f2a8832958f2843e9aaafc006dc32b3c1acbca19 Mon Sep 17 00:00:00 2001 From: Aarav Date: Thu, 16 Mar 2023 16:37:31 -0600 Subject: [PATCH 2/4] Delete LimeAlign.java --- .../frc4388/robot/commands/LimeAlign.java | 34 ------------------- 1 file changed, 34 deletions(-) delete mode 100644 src/main/java/frc4388/robot/commands/LimeAlign.java diff --git a/src/main/java/frc4388/robot/commands/LimeAlign.java b/src/main/java/frc4388/robot/commands/LimeAlign.java deleted file mode 100644 index c811a3e..0000000 --- a/src/main/java/frc4388/robot/commands/LimeAlign.java +++ /dev/null @@ -1,34 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc4388.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.SwerveDrive; - -public class LimeAlign extends CommandBase { - public LimeAlign(SwerveDrive drive) { - addRequirements(drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} From ef3273620c9088abd07ae523bae4dc77dcc6fba0 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Thu, 16 Mar 2023 16:45:50 -0600 Subject: [PATCH 3/4] Update SwerveDrive.java --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6db02ad..77466fb 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.math.kinematics.SwerveModulePosition; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.RobotGyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class SwerveDrive extends SubsystemBase { @@ -37,7 +38,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default - public Rotation2d rotTarget = new Rotation2d(); + public double rotTarget = 0.0; public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); /** Creates a new SwerveDrive. */ From ec004f6de849bc9bf050505b619206c8c1ca2460 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Thu, 16 Mar 2023 16:50:02 -0600 Subject: [PATCH 4/4] Update SwerveDrive.java --- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 77466fb..f65dd00 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -107,7 +107,7 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - rotTarget = new Rotation2d(0); + rotTarget = 0.0; } public void stopModules() {