From 45574a2a06ff094a8ac19debfa6499ef2b2eeab6 Mon Sep 17 00:00:00 2001 From: aarav18 <57469559+aarav18@users.noreply.github.com> Date: Sat, 15 Jan 2022 14:55:07 -0700 Subject: [PATCH] Bumpers and drift protection --- src/main/java/frc4388/robot/Constants.java | 5 ++-- .../java/frc4388/robot/RobotContainer.java | 15 ++++++++---- .../frc4388/robot/subsystems/SwerveDrive.java | 24 +++++++++++++++---- .../robot/subsystems/SwerveModule.java | 6 +++-- 4 files changed, 36 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index eb9c841..592df37 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -22,11 +22,12 @@ import frc4388.utility.LEDPatterns; */ public final class Constants { public static final class SwerveDriveConstants { - public static final double ROTATION_SPEED = 0.1; + public static final double ROTATION_SPEED = 3; public static final double WHEEL_SPEED = 0.1; public static final double WIDTH = 22; public static final double HEIGHT = 22; - public static final double JOYSTICK_TO_METERS_PER_SECOND = 11; + public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11; + public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2; public static final double MAX_SPEED_FEET_PER_SEC = 20; public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; //IDs diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index aa244f3..f91a605 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -77,16 +77,21 @@ public class RobotContainer { /* Driver Buttons */ /*new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) .whenPressed(() -> m_robotSwerveDrive.resetGyro());*/ + + new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) + .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + + + new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); + + + /* Operator Buttons */ // activates "Lit Mode" new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - - // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - // .whenPressed(() -> { - // m_robotSwerveDrive.m_gyro.setYaw(0); - // }); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 3172120..dabc3df 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.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.interfaces.Gyro; @@ -54,6 +55,9 @@ public class SwerveDrive extends SubsystemBase { public SwerveModule[] modules; public WPI_PigeonIMU m_gyro; //TODO Add Gyro Lol + public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + public boolean ignoreAngles; + public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor, WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder, CANCoder rightFrontEncoder, @@ -93,6 +97,8 @@ public class SwerveDrive extends SubsystemBase { */ public void driveWithInput(double[] speeds, double rot, boolean fieldRelative) { + if (speeds[0] == 0 && speeds[1] == 0) ignoreAngles = true; + else ignoreAngles = false; // Temporary janky raw joysticks float[] driveController = new float[HAL.kMaxJoystickAxes]; HAL.getJoystickAxes((byte) 0, driveController); @@ -127,18 +133,18 @@ public class SwerveDrive extends SubsystemBase { // System.out.println("Inputx: " + speeds[0] + " Inputy: " + speeds[1]); /*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s ); driveFromSpeeds(speeds);*/ - double xSpeedMetersPerSecond = -speeds[0] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; - double ySpeedMetersPerSecond = speeds[1] * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND; + double xSpeedMetersPerSecond = -speeds[0] * speedAdjust; + double ySpeedMetersPerSecond = speeds[1] * speedAdjust; SwerveModuleState[] states = kinematics.toSwerveModuleStates( fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, m_gyro.getRotation2d()) - : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3)); + ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d()) + : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED)); SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC)); for (int i = 0; i < states.length; i++) { SwerveModule module = modules[i]; SwerveModuleState state = states[i]; - module.setDesiredState(state); + module.setDesiredState(state, ignoreAngle); } } @@ -151,4 +157,12 @@ public class SwerveDrive extends SubsystemBase { super.periodic(); } + public void highSpeed(boolean shift){ + if (shift){ + speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST; + } + else{ + speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + } + } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 0c38acf..450668c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -71,7 +71,7 @@ public class SwerveModule extends SubsystemBase { * Set the speed + rotation of the swerve module from a SwerveModuleState object * @param desiredState - A SwerveModuleState representing the desired new state of the module */ - public void setDesiredState(SwerveModuleState desiredState) { + public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) { Rotation2d currentRotation = getAngle(); SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); @@ -85,7 +85,9 @@ public class SwerveModule extends SubsystemBase { // Convert the CANCoder from it's position reading back to ticks double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient(); double desiredTicks = currentTicks + deltaTicks; - angleMotor.set(TalonFXControlMode.Position, desiredTicks); + if (!ignoreAngle){ + angleMotor.set(TalonFXControlMode.Position, desiredTicks); + } double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);