diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0e87403f..8549b607 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -30,9 +30,8 @@ public static final class DriveConstants { public static final double kMaxSpeedMetersPerSecond = 4.8; public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second - public static final double kDirectionSlewRate = 1.2; // radians per second - public static final double kMagnitudeSlewRate = 1.8; // percent per second (1 = 100%) - public static final double kRotationalSlewRate = 2.0; // percent per second (1 = 100%) + public static final double kMagnitudeSlewRate = 1.8 * kMaxSpeedMetersPerSecond; // meters per second^2 + public static final double kRotationalSlewRate = 2.0 * kMaxAngularSpeed; // radians per second^2 // Chassis configuration public static final double kTrackWidth = Units.inchesToMeters(26.5); diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index 8557c427..d85b60e0 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -4,7 +4,6 @@ package frc.robot.subsystems; -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.ChassisSpeeds; @@ -44,14 +43,8 @@ public class DriveSubsystem extends SubsystemBase { // The gyro sensor private final ADIS16470_IMU m_gyro = new ADIS16470_IMU(); - // Slew rate filter variables for controlling lateral acceleration - private double m_currentRotation = 0.0; - private double m_currentTranslationDir = 0.0; - private double m_currentTranslationMag = 0.0; - - private SlewRateLimiter m_magLimiter = new SlewRateLimiter(DriveConstants.kMagnitudeSlewRate); - private SlewRateLimiter m_rotLimiter = new SlewRateLimiter(DriveConstants.kRotationalSlewRate); private double m_prevTime = WPIUtilJNI.now() * 1e-6; + private ChassisSpeeds m_prevTarget = new ChassisSpeeds(); // Odometry class for tracking robot pose SwerveDriveOdometry m_odometry = new SwerveDriveOdometry( @@ -118,73 +111,38 @@ public void resetOdometry(Pose2d pose) { * @param rateLimit Whether to enable rate limiting for smoother control. */ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative, boolean rateLimit) { - - double xSpeedCommanded; - double ySpeedCommanded; - - if (rateLimit) { - // Convert XY to polar 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 - double directionSlewRate; - if (m_currentTranslationMag != 0.0) { - directionSlewRate = Math.abs(DriveConstants.kDirectionSlewRate / m_currentTranslationMag); - } else { - directionSlewRate = 500.0; //some high number that means the slew rate is effectively instantaneous - } - - - double currentTime = WPIUtilJNI.now() * 1e-6; - double elapsedTime = currentTime - m_prevTime; - double angleDif = SwerveUtils.AngleDifference(inputTranslationDir, m_currentTranslationDir); - if (angleDif < 0.45*Math.PI) { - m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); - m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); - } - else if (angleDif > 0.85*Math.PI) { - if (m_currentTranslationMag > 1e-4) { //some small number to avoid floating-point errors with equality checking - // keep currentTranslationDir unchanged - m_currentTranslationMag = m_magLimiter.calculate(0.0); - } - else { - m_currentTranslationDir = SwerveUtils.WrapAngle(m_currentTranslationDir + Math.PI); - m_currentTranslationMag = m_magLimiter.calculate(inputTranslationMag); - } - } - else { - m_currentTranslationDir = SwerveUtils.StepTowardsCircular(m_currentTranslationDir, inputTranslationDir, directionSlewRate * elapsedTime); - m_currentTranslationMag = m_magLimiter.calculate(0.0); - } - m_prevTime = currentTime; - - xSpeedCommanded = m_currentTranslationMag * Math.cos(m_currentTranslationDir); - ySpeedCommanded = m_currentTranslationMag * Math.sin(m_currentTranslationDir); - m_currentRotation = m_rotLimiter.calculate(rot); - - } else { - xSpeedCommanded = xSpeed; - ySpeedCommanded = ySpeed; - m_currentRotation = rot; + // Convert the commanded speeds into the correct units for the drivetrain + xSpeed *= DriveConstants.kMaxSpeedMetersPerSecond; + ySpeed *= DriveConstants.kMaxSpeedMetersPerSecond; + rot *= DriveConstants.kMaxAngularSpeed; + + // Get the target chassis speeds relative to the robot + final ChassisSpeeds vel = ( fieldRelative ? + ChassisSpeeds.fromFieldRelativeSpeeds( xSpeed, ySpeed, rot, Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)) ) + : new ChassisSpeeds( xSpeed, ySpeed, rot ) + ); + + // Rate limit if applicable + if(rateLimit) { + final double + currentTime = WPIUtilJNI.now() * 1e-6, + elapsedTime = currentTime - m_prevTime; + SwerveUtils.RateLimitVelocity( + vel, m_prevTarget, elapsedTime, + DriveConstants.kMagnitudeSlewRate, DriveConstants.kRotationalSlewRate); + m_prevTime = currentTime; + m_prevTarget = vel; } - // Convert the commanded speeds into the correct units for the drivetrain - double xSpeedDelivered = xSpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond; - double ySpeedDelivered = ySpeedCommanded * DriveConstants.kMaxSpeedMetersPerSecond; - double rotDelivered = m_currentRotation * DriveConstants.kMaxAngularSpeed; - - var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates( - fieldRelative - ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ))) - : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered)); - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); + var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(vel); + SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond); + m_frontLeft.setDesiredState(swerveModuleStates[0]); m_frontRight.setDesiredState(swerveModuleStates[1]); m_rearLeft.setDesiredState(swerveModuleStates[2]); m_rearRight.setDesiredState(swerveModuleStates[3]); + } /** diff --git a/src/main/java/frc/utils/SwerveUtils.java b/src/main/java/frc/utils/SwerveUtils.java index 867fe0b4..5c8e6ddd 100644 --- a/src/main/java/frc/utils/SwerveUtils.java +++ b/src/main/java/frc/utils/SwerveUtils.java @@ -1,89 +1,34 @@ package frc.utils; -public class SwerveUtils { - - /** - * Steps a value towards a target with a specified step size. - * @param _current The current or starting value. Can be positive or negative. - * @param _target The target value the algorithm will step towards. Can be positive or negative. - * @param _stepsize The maximum step size that can be taken. - * @return The new value for {@code _current} after performing the specified step towards the specified target. - */ - public static double StepTowards(double _current, double _target, double _stepsize) { - if (Math.abs(_current - _target) <= _stepsize) { - return _target; - } - else if (_target < _current) { - return _current - _stepsize; - } - else { - return _current + _stepsize; - } - } - - /** - * Steps a value (angle) towards a target (angle) taking the shortest path with a specified step size. - * @param _current The current or starting angle (in radians). Can lie outside the 0 to 2*PI range. - * @param _target The target angle (in radians) the algorithm will step towards. Can lie outside the 0 to 2*PI range. - * @param _stepsize The maximum step size that can be taken (in radians). - * @return The new angle (in radians) for {@code _current} after performing the specified step towards the specified target. - * This value will always lie in the range 0 to 2*PI (exclusive). - */ - public static double StepTowardsCircular(double _current, double _target, double _stepsize) { - _current = WrapAngle(_current); - _target = WrapAngle(_target); +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.kinematics.ChassisSpeeds; - double stepDirection = Math.signum(_target - _current); - double difference = Math.abs(_current - _target); - - if (difference <= _stepsize) { - return _target; - } - else if (difference > Math.PI) { //does the system need to wrap over eventually? - //handle the special case where you can reach the target in one step while also wrapping - if (_current + 2*Math.PI - _target < _stepsize || _target + 2*Math.PI - _current < _stepsize) { - return _target; - } - else { - return WrapAngle(_current - stepDirection * _stepsize); //this will handle wrapping gracefully - } - - } - else { - return _current + stepDirection * _stepsize; - } - } +public class SwerveUtils { /** - * Finds the (unsigned) minimum difference between two angles including calculating across 0. - * @param _angleA An angle (in radians). - * @param _angleB An angle (in radians). - * @return The (unsigned) minimum difference between the two angles (in radians). + * Limit the linear and angular accelerations of the chassis between setpoints. + * @param target - the target chassis velocity for the next period + * @param previous - the previous target chassis velocity + * @param dt - the time interval between periods + * @param linear_acc_lim - the maximum linear acceleration allowed + * @param rotational_acc_lim - the maximum rotational acceleration allowed */ - public static double AngleDifference(double _angleA, double _angleB) { - double difference = Math.abs(_angleA - _angleB); - return difference > Math.PI? (2 * Math.PI) - difference : difference; + public static void RateLimitVelocity(ChassisSpeeds target, ChassisSpeeds previous, double dt, double linear_acc_lim, double rotational_acc_lim) { + final double + dvlim = linear_acc_lim * dt, // the maximum change in velocity allowed for the given time interval + drlim = rotational_acc_lim * dt, + dvx = target.vxMetersPerSecond - previous.vxMetersPerSecond, // the current change in velocity (components) + dvy = target.vyMetersPerSecond - previous.vyMetersPerSecond, + dvr = target.omegaRadiansPerSecond - previous.omegaRadiansPerSecond, + dv = Math.hypot(dvx, dvy), // the current change in velocity (vector magnitude) + _dv = MathUtil.clamp(dv, -dvlim, dvlim), // the clamped magnitude + _dr = MathUtil.clamp(dvr, -drlim, drlim), + scale = dv == 0.0 ? 1.0 : _dv / dv, // protect against div by 0 when delta velocity was (0, 0) + _dvx = dvx * scale, // rescale component deltas based on clamped magnitude + _dvy = dvy * scale; + target.vxMetersPerSecond = previous.vxMetersPerSecond + _dvx; // reapply clamped changes in velocity + target.vyMetersPerSecond = previous.vyMetersPerSecond + _dvy; + target.omegaRadiansPerSecond = previous.omegaRadiansPerSecond + _dr; } - /** - * Wraps an angle until it lies within the range from 0 to 2*PI (exclusive). - * @param _angle The angle (in radians) to wrap. Can be positive or negative and can lie multiple wraps outside the output range. - * @return An angle (in radians) from 0 and 2*PI (exclusive). - */ - public static double WrapAngle(double _angle) { - double twoPi = 2*Math.PI; - - if (_angle == twoPi) { // Handle this case separately to avoid floating point errors with the floor after the division in the case below - return 0.0; - } - else if (_angle > twoPi) { - return _angle - twoPi*Math.floor(_angle / twoPi); - } - else if (_angle < 0.0) { - return _angle + twoPi*(Math.floor((-_angle) / twoPi)+1); - } - else { - return _angle; - } - } } \ No newline at end of file