From 721f160f3779d3506aba585a87cddaa14e05b7d1 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 27 Aug 2024 17:45:51 -0600 Subject: [PATCH] update gearshifting logic --- src/main/java/frc4388/robot/Constants.java | 29 ++++++++++--------- .../java/frc4388/robot/RobotContainer.java | 16 +++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 11 +++---- 3 files changed, 29 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index aee6cae..3376b08 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -35,19 +35,20 @@ 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 SLOW_SPEED = 0.25; + public static final double FAST_SPEED = 0.5; + public static final double TURBO_SPEED = 1.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = -0.130126953125 + 0.25 + 0.25; - public static final double FRONT_RIGHT_ROT_OFFSET = 0.364990234375 + 0.25;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; - public static final double BACK_LEFT_ROT_OFFSET = -0.4775390625 + 0.25; - public static final double BACK_RIGHT_ROT_OFFSET = 0.356201171875 + 0.25; + public static final double FRONT_LEFT_ROT_OFFSET = 0.36962890625 +0.5 ;//+ 0.5; + public static final double FRONT_RIGHT_ROT_OFFSET = 0.61474609375 +0.5 ;//+ 0.5; + public static final double BACK_LEFT_ROT_OFFSET = -0.227294921875+0.5 ; + public static final double BACK_RIGHT_ROT_OFFSET = 0.60595703125 +0.5 ; } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 2; + + public static final int RIGHT_FRONT_WHEEL_ID = 2; public static final int RIGHT_FRONT_STEER_ID = 3; public static final int RIGHT_FRONT_ENCODER_ID = 10; @@ -59,7 +60,7 @@ public final class Constants { public static final int LEFT_BACK_STEER_ID = 7; public static final int LEFT_BACK_ENCODER_ID = 12; - public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_WHEEL_ID = 8; public static final int RIGHT_BACK_STEER_ID = 9; public static final int RIGHT_BACK_ENCODER_ID = 13; } @@ -84,15 +85,15 @@ public final class Constants { } public static final class Conversions { - public static final int CANCODER_TICKS_PER_ROTATION = 4096; + public static final int CANCODER_TICKS_PER_ROTATION = 1; - public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8; - public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8; + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 6.22; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = JOYSTICK_TO_METERS_PER_SECOND_FAST * 0.3; public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; public static final double MOTOR_REV_PER_STEER_REV = 12.8; - public static final double TICKS_PER_MOTOR_REV = 2048; + public static final double TICKS_PER_MOTOR_REV = 0.5; public static final double WHEEL_DIAMETER_INCHES = 3.9; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; @@ -111,7 +112,7 @@ public final class Constants { public static final double NEUTRAL_DEADBAND = 0.04; // TODO: find the actual value } - public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value + public static final double MAX_SPEED_FEET_PER_SECOND = 20.4; // TODO: find the actual value public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value // dimensions diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c3287b7..3a2cab0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -251,13 +251,13 @@ public class RobotContainer { DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive)); - DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue())) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); - DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); + // DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.START_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp())) + // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.add180())); // * /* D-Pad Stuff */ // new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9) @@ -308,11 +308,11 @@ public class RobotContainer { // ! /* Speed */ new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo())); // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow())); new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 45f68f7..243093e 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.SwerveDriveConstants.Conversions; import frc4388.utility.RobotGyro; import frc4388.utility.RobotUnits; @@ -70,7 +71,7 @@ public class SwerveDrive extends SubsystemBase { boolean stopped = false; public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 1.0; + double rot_correction = RobotUnits.degreesToRadians(gyro.getRotation2d().getDegrees() - rotTarget) * 0.0; SmartDashboard.putNumber("Rot_correct", RobotUnits.radiansToDegrees(rot_correction)); if (fieldRelative) { @@ -102,7 +103,7 @@ public class SwerveDrive extends SubsystemBase { // Convert field-relative speeds to robot-relative speeds. // chassisSpeeds = chassisSpeeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction, gyro.getRotation2d().times(-1)); } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -267,7 +268,7 @@ public class SwerveDrive extends SubsystemBase { } public void setToSlow() { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; + this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.SLOW_SPEED; System.out.println("SLOW"); System.out.println("SLOW"); System.out.println("SLOW"); @@ -276,7 +277,7 @@ public class SwerveDrive extends SubsystemBase { } public void setToFast() { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.FAST_SPEED; System.out.println("FAST"); System.out.println("FAST"); System.out.println("FAST"); @@ -285,7 +286,7 @@ public class SwerveDrive extends SubsystemBase { } public void setToTurbo() { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; + this.speedAdjust = Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST * SwerveDriveConstants.TURBO_SPEED; System.out.println("TURBO"); System.out.println("TURBO"); System.out.println("TURBO");