diff --git a/src/main/java/frc/constants/DriveConstants.java b/src/main/java/frc/constants/DriveConstants.java index 4894ea9..a5fd1d5 100644 --- a/src/main/java/frc/constants/DriveConstants.java +++ b/src/main/java/frc/constants/DriveConstants.java @@ -14,9 +14,10 @@ public class DriveConstants { public static final double MAX_SPEED_METERS_PER_SECOND = 4.8; public static final double MAX_ANGULAR_SPEED = 2 * Math.PI; // radians per second - public static final double DIRECTION_SLEW_RATE = 1.2; // radians per second - public static final double MAGNITUDE_SLEW_RATE = 1.8; // percent per second (1 = 100%) - public static final double ROTATIONAL_SLEW_RATE = 2.0; // percent per second (1 = 100%) + // TODO: Tune these values + public static final double MAX_ACCELERATION = 9.6; // meters per second per second + public static final double MAX_DRIVE_ANGULAR_VELOCITY = 8; // radians per second + public static final double MAX_ROTATIONAL_ACCELERATION = 2; // percent per second // Chassis configuration public static final double TRACK_WIDTH_METERS = Units.inchesToMeters(27.5); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 597735c..039b0cb 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -5,11 +5,11 @@ package frc.robot.subsystems; import com.kauailabs.navx.frc.AHRS; +import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.*; -import edu.wpi.first.util.WPIUtilJNI; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.constants.CANMappings; @@ -45,14 +45,17 @@ public class DriveSubsystem extends SubsystemBase { // The gyro sensor private final AHRS gyro = new AHRS(); - // Slew rate filter variables for controlling lateral acceleration private double currentRotation = 0.0; - private double currentTranslationDir = 0.0; - private double currentTranslationMag = 0.0; - private SlewRateLimiter magLimiter = new SlewRateLimiter(DriveConstants.MAGNITUDE_SLEW_RATE); - private SlewRateLimiter rotLimiter = new SlewRateLimiter(DriveConstants.ROTATIONAL_SLEW_RATE); - private double prevTime = WPIUtilJNI.now() * 1e-6; + // Slew rate filter variables for controlling lateral acceleration + private double currentTranslationDir = 0.0; // radian + private double currentTranslationMag = 0.0; // meter per second + private SlewRateLimiter magLimiter = new SlewRateLimiter(DriveConstants.MAX_ACCELERATION); + + private double prevTime = MathSharedStore.getTimestamp(); + + private SlewRateLimiter rotLimiter = + new SlewRateLimiter(DriveConstants.MAX_ROTATIONAL_ACCELERATION); // Odometry class for tracking robot pose SwerveDriveOdometry odometry = @@ -123,7 +126,8 @@ public void resetOdometry(Pose2d pose) { } /** - * Method to drive the robot using joystick info. + * Method to drive the robot using joystick info. This method may not work as intended if the + * joystick does not move in a circle * * @param xSpeed Speed of the robot in the x direction (forward). * @param ySpeed Speed of the robot in the y direction (sideways). @@ -139,94 +143,73 @@ public void drive( if (rateLimit) { // Convert XY to polar(theta and magnitude) for rate limiting - double inputTranslationDir = Math.atan2(ySpeed, xSpeed); - double inputTranslationMag = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)); - - // Calculate the direction slew rate based on an estimate of the lateral acceleration - // if driving set the rotation slew rate to a constant divided by current translation speed so - // the faster you are, the slower you turn - // else practically don't limit rotational speed - double directionSlewRate; - if (this.currentTranslationMag != 0.0) { - directionSlewRate = - Math.abs(DriveConstants.DIRECTION_SLEW_RATE / this.currentTranslationMag); - } else { - directionSlewRate = - 500.0; // some high number that means the slew rate is effectively instantaneous + double inputTranslationDir = SwerveUtils.wrapAngle(Math.atan2(ySpeed, xSpeed)); + double inputTranslationMag = + Math.min( + Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2)) + * DriveConstants.MAX_SPEED_METERS_PER_SECOND, + DriveConstants.MAX_SPEED_METERS_PER_SECOND); + + // If either translation magnitudes are low enough, it's direction is irrelevant + if (currentTranslationMag < 0.1) { + currentTranslationDir = inputTranslationDir; + } else if (inputTranslationMag < 0.1) { + inputTranslationDir = currentTranslationDir; } - double currentTime = WPIUtilJNI.now() * 1e-6; - double elapsedTime = currentTime - this.prevTime; - double angleDif = - SwerveUtils.angleDifference(inputTranslationDir, this.currentTranslationDir); - - /* - * Description of the if below - * - * if the difference between the target direction and current direction is smallish, - * step the current direction slowly to match the input based on the slew rate and time. - * Allow the translation speed to change based on the limiter. This will result in the robot turning while it maintains speed - * - * if the difference is large and the robot is moving, slow down - * if stopped swap the direction and accelerate away. - * This performs a 180 flip that is then corrected by the first case. The robot stops, then accelerates backward - * - * if the difference is in the middle, step the current angle towards the target and slow the robot. - * This will eventually switch to the first case when the robot accelerates again. - */ - - // if angle is less than 81 degrees, step the current translation direction to the input with - // a step size the size of the slew rate, this will prevent the dir from changing too fast - // then limit the translation speed based on the limiter - if (angleDif < 0.45 * Math.PI) { - this.currentTranslationDir = - SwerveUtils.stepTowardsCircular( - currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); - this.currentTranslationMag = magLimiter.calculate(inputTranslationMag); - } else if (angleDif > 0.85 * Math.PI) { - // if the angle is greater than 153 degrees and the requested translation speed is non-zero - // slow the robot speed to zero - if (this.currentTranslationMag - > 1e-4) { // some small number to avoid floating-point errors with equality checking - // keep currentTranslationDir unchanged - this.currentTranslationMag = magLimiter.calculate(0.0); - } else { - // if the robot speed is already at zero flip the angle so the robot reverses direction - // and begin to re-accelerate - this.currentTranslationDir = SwerveUtils.wrapAngle(currentTranslationDir + Math.PI); - this.currentTranslationMag = magLimiter.calculate(inputTranslationMag); - } - } else { - // if the angle is between the two checks, slow the robot to zero and begin to turn - this.currentTranslationDir = - SwerveUtils.stepTowardsCircular( - currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); - this.currentTranslationMag = magLimiter.calculate(0.0); + // If the new direction is over 90º from the current direction, slow the robot + // to a near stop before continuing. The block above activates when the robot + // has slowed down enough + if (SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) > Math.PI / 2.0) { + inputTranslationDir = currentTranslationDir; + inputTranslationMag *= -1; } - this.prevTime = currentTime; + // Decrease the requested speed based on how far the current translation + // direction is from the requested one. A 90º angle means the requested + // magnitude is 0, a 0º angle means the magnitude is unchanged. + inputTranslationMag *= + 1 + - SwerveUtils.angleDifference(inputTranslationDir, currentTranslationDir) + / (Math.PI / 2); + + // Set the direction limit to a percent of the maxiumum based on the percent of + // max speed we are currently going at. This is limited to prevent fully + // disabling steering and leaves it at 10%. + double dirLimit = + Math.max( + 0, + DriveConstants.MAX_DRIVE_ANGULAR_VELOCITY + * ((1 - currentTranslationMag / DriveConstants.MAX_SPEED_METERS_PER_SECOND) * 0.90 + + 0.10)); + + double currentTime = MathSharedStore.getTimestamp(); + + this.currentTranslationMag = magLimiter.calculate(inputTranslationMag); + this.currentTranslationDir = + SwerveUtils.stepTowardsCircular( + currentTranslationDir, inputTranslationDir, dirLimit * (currentTime - prevTime)); + + prevTime = currentTime; xSpeedCommanded = this.currentTranslationMag * Math.cos(currentTranslationDir); ySpeedCommanded = this.currentTranslationMag * Math.sin(currentTranslationDir); - this.currentRotation = rotLimiter.calculate(rot); + this.currentRotation = rotLimiter.calculate(rot) * DriveConstants.MAX_ANGULAR_SPEED; } else { - xSpeedCommanded = xSpeed; - ySpeedCommanded = ySpeed; - this.currentRotation = rot; + xSpeedCommanded = xSpeed * DriveConstants.MAX_SPEED_METERS_PER_SECOND; + ySpeedCommanded = ySpeed * DriveConstants.MAX_SPEED_METERS_PER_SECOND; + this.currentRotation = rot * DriveConstants.MAX_ANGULAR_SPEED; } - // Convert the commanded speeds into the correct units for the drivetrain - double xSpeedDelivered = xSpeedCommanded * DriveConstants.MAX_SPEED_METERS_PER_SECOND; - double ySpeedDelivered = ySpeedCommanded * DriveConstants.MAX_SPEED_METERS_PER_SECOND; - double rotDelivered = this.currentRotation * DriveConstants.MAX_ANGULAR_SPEED; - var swerveModuleStates = DriveConstants.DRIVE_KINEMATICS.toSwerveModuleStates( fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds( - xSpeedDelivered, ySpeedDelivered, rotDelivered, getHeading()) - : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); + xSpeedCommanded, ySpeedCommanded, this.currentRotation, getHeading()) + : new ChassisSpeeds(xSpeedCommanded, ySpeedCommanded, this.currentRotation)); + SwerveDriveKinematics.desaturateWheelSpeeds( swerveModuleStates, DriveConstants.MAX_SPEED_METERS_PER_SECOND); + this.frontLeft.setDesiredState(swerveModuleStates[0]); this.frontRight.setDesiredState(swerveModuleStates[1]); this.rearLeft.setDesiredState(swerveModuleStates[2]); diff --git a/src/main/java/frc/utils/SwerveUtils.java b/src/main/java/frc/utils/SwerveUtils.java index 9d0221d..5f32b67 100644 --- a/src/main/java/frc/utils/SwerveUtils.java +++ b/src/main/java/frc/utils/SwerveUtils.java @@ -1,7 +1,6 @@ package frc.utils; public class SwerveUtils { - /** * Steps a value towards a target with a specified step size. *