diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 9139bcb6..01904fd8 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -104,6 +104,7 @@ public SwerveSubsystem(File directory) } swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. + swerveDrive.setAngularVelocityCompensation(true, true, 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. if (visionDriveTest) { setupPhotonVision(); @@ -375,14 +376,14 @@ public Command centerModulesCommand() { * @param distanceInMeters the distance to drive in meters * @param speedInMetersPerSecond the speed at which to drive in meters per second * @return a Command that drives the swerve drive to a specific distance at a given speed - */ + */ public Command driveToDistanceCommand(double distanceInMeters, double speedInMetersPerSecond) { return Commands.deferredProxy( () -> Commands.run(() -> drive(new ChassisSpeeds(speedInMetersPerSecond, 0, 0)), this) .until(() -> swerveDrive.getPose().getTranslation().getDistance(new Translation2d(0, 0)) > distanceInMeters) ); } - + /** * Sets the maximum speed of the swerve drive. * @@ -398,7 +399,7 @@ public void setMaximumSpeed(double maximumSpeedInMetersPerSecond) { * @param kS the static gain of the feedforward * @param kV the velocity gain of the feedforward * @param kA the acceleration gain of the feedforward - */ + */ public void replaceSwerveModuleFeedforward(double kS, double kV, double kA) { swerveDrive.replaceSwerveModuleFeedforward(new SimpleMotorFeedforward(kS, kV, kA)); } diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 09097143..0605f6e9 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -30,6 +30,7 @@ import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import swervelib.encoders.CANCoderSwerve; +import swervelib.imu.IMUVelocity; import swervelib.imu.Pigeon2Swerve; import swervelib.imu.SwerveIMU; import swervelib.math.SwerveMath; @@ -97,6 +98,24 @@ public class SwerveDrive * correction. */ public boolean chassisVelocityCorrection = true; + /** + * Correct chassis velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} (auto) using 254's + * correction during auto. + */ + public boolean autonomousChassisVelocityCorrection = false; + /** + * Correct for skew that scales with angular velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} + */ + public boolean angularVelocityCorrection = false; + /** + * Correct for skew that scales with angular velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} + * during auto. + */ + public boolean autonomousAngularVelocityCorrection = false; + /** + * Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15). + */ + public double angularVelocityCoefficient = 0; /** * Whether to correct heading when driving translationally. Set to true to enable. */ @@ -113,6 +132,11 @@ public class SwerveDrive * Swerve IMU device for sensing the heading of the robot. */ private SwerveIMU imu; + /** + * Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in + * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}. + */ + private IMUVelocity imuVelocity; /** * Simulation of the swerve drive. */ @@ -479,12 +503,7 @@ public void drive( public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters) { - // Thank you to Jared Russell FRC254 for Open Loop Compensation Code - // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 - if (chassisVelocityCorrection) - { - velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds); - } + velocity = movementOptimizations(velocity, chassisVelocityCorrection, angularVelocityCorrection); // Heading Angular Velocity Deadband, might make a configuration option later. // Originally made by Team 1466 Webb Robotics. @@ -594,8 +613,8 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds /** * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. - * Does not allow for usage of desaturateWheelSpeeds(SwerveModuleState[] moduleStates, - * ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, + * Does not allow for usage of desaturateWheelSpeeds(SwerveModuleState[] moduleStates, + * ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, * double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond) * * @param desiredStates A list of SwerveModuleStates to send to the modules. @@ -620,6 +639,9 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo */ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { + + chassisSpeeds = movementOptimizations(chassisSpeeds, autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection); + SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond); @@ -1215,7 +1237,7 @@ public void setCosineCompensator(boolean enabled) } /** - * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop * * @param enable Enable chassis velocity correction, which will use * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following. @@ -1223,8 +1245,101 @@ public void setCosineCompensator(boolean enabled) */ public void setChassisDiscretization(boolean enable, double dtSeconds) { - chassisVelocityCorrection = enable; - discretizationdtSeconds = dtSeconds; + if(!SwerveDriveTelemetry.isSimulation) + { + chassisVelocityCorrection = enable; + discretizationdtSeconds = dtSeconds; + } + } + + /** + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto + * + * @param useInTeleop Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop. + * @param useInAuto Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto. + * @param dtSeconds The duration of the timestep the speeds should be applied for. + */ + public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds) + { + if(!SwerveDriveTelemetry.isSimulation) + { + chassisVelocityCorrection = useInTeleop; + autonomousChassisVelocityCorrection = useInAuto; + discretizationdtSeconds = dtSeconds; + } + } + + /** + * Enables angular velocity skew correction in telop and/or autonomous + * and sets the angular velocity coefficient for both modes + * + * @param useInTeleop Enables angular velocity correction in teleop. + * @param useInAuto Enables angular velocity correction in autonomous. + * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15. + * Start with a value of 0.1, test in teleop. + * When enabling for the first time if the skew is significantly worse try inverting the value. + * Tune by moving in a straight line while rotating. Testing is best done with angular velocity controls on the right stick. + * Change the value until you are visually happy with the skew. + * Ensure your tune works with different translational and rotational magnitudes. + * If this reduces skew in teleop, it may improve auto. + */ + public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff) + { + if(!SwerveDriveTelemetry.isSimulation) + { + imuVelocity = IMUVelocity.createIMUVelocity(imu); + angularVelocityCorrection = useInTeleop; + autonomousAngularVelocityCorrection = useInAuto; + angularVelocityCoefficient = angularVelocityCoeff; + } + } + + /** + * Correct for skew that worsens as angular velocity increases + * + * @param velocity The chassis speeds to set the robot to achieve. + * @return {@link ChassisSpeeds} of the robot after angular velocity skew correction. + */ + public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity) + { + var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient); + if(angularVelocity.getRadians() != 0.0){ + velocity = ChassisSpeeds.fromRobotRelativeSpeeds( + velocity.vxMetersPerSecond, + velocity.vyMetersPerSecond, + velocity.omegaRadiansPerSecond, + getOdometryHeading()); + velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity)); + } + return velocity; + } + + /** + * Enable desired drive corrections + * + * @param velocity The chassis speeds to set the robot to achieve. + * @param uesChassisDiscretize Correct chassis velocity using 254's correction. + * @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew. + * @return The chassis speeds after optimizations. + */ + private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, boolean useAngularVelocitySkewCorrection) + { + + if(useAngularVelocitySkewCorrection) + { + velocity = angularVelocitySkewCorrection(velocity); + } + + // Thank you to Jared Russell FRC254 for Open Loop Compensation Code + // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 + if (uesChassisDiscretize) + { + velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds); + } + + return velocity; } } diff --git a/src/main/java/swervelib/imu/IMUVelocity.java b/src/main/java/swervelib/imu/IMUVelocity.java new file mode 100644 index 00000000..998f7375 --- /dev/null +++ b/src/main/java/swervelib/imu/IMUVelocity.java @@ -0,0 +1,137 @@ +package swervelib.imu; + +import edu.wpi.first.hal.HALUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Notifier; +import swervelib.math.IMULinearMovingAverageFilter; + +public class IMUVelocity { + /** + * Swerve IMU. + */ + private final SwerveIMU gyro; + /** + * Linear filter used to calculate velocity, we use a custom filter class + * to prevent unwanted operations. + */ + private final IMULinearMovingAverageFilter velocityFilter; + /** + * WPILib {@link Notifier} to keep IMU velocity up to date. + */ + private final Notifier notifier; + + /** + * Prevents calculation when no previous measurement exists. + */ + private boolean firstCycle = true; + /** + * Tracks the previous loop's recorded time. + */ + private double timestamp = 0.0; + /** + * Tracks the previous loop's position as a Rotation2d. + */ + private Rotation2d position = new Rotation2d(); + /** + * The calculated velocity of the robot based on averaged IMU measurements. + */ + private double velocity = 0.0; + + /** + * Constructor for the IMU Velocity. + * + * @param gyro The SwerveIMU gyro. + * @param periodSeconds The rate to collect measurements from the gyro, in the form (1/number of samples per second), + * make sure this does not exceed the update rate of your IMU. + * @param averagingTaps The number of samples to used for the moving average linear filter. Higher values will not + * allow the system to update to changes in velocity, lower values may create a less smooth signal. Expected taps + * will probably be ~2-8, with the goal of having the lowest smooth value. + * + */ + public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps) + { + this.gyro = gyro; + velocityFilter = new IMULinearMovingAverageFilter(averagingTaps); + notifier = new Notifier(this::update); + notifier.startPeriodic(periodSeconds); + timestamp = HALUtil.getFPGATime(); + } + + /** + * Static factory for IMU Velocity. Supported IMU rates will be as quick as possible + * but will not exceed 100hz and will use 5 taps (supported IMUs are listed in swervelib's IMU folder). + * Other gyroscopes will default to 50hz and 5 taps. For custom rates please use the IMUVelocity constructor. + * + * @param gyro The SwerveIMU gyro. + */ + public static IMUVelocity createIMUVelocity(SwerveIMU gyro) + { + double desiredNotifierPeriod = 1.0/50.0; + // ADIS16448_IMU ~200HZ: + // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277 + if (gyro instanceof ADIS16448Swerve) + { + desiredNotifierPeriod = 1.0/100.0; + } + // ADIS16470_IMU 200HZ + // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345 + else if (gyro instanceof ADIS16470Swerve) + { + desiredNotifierPeriod = 1.0/100.0; + } + // ADXRS450_Gyro 2000HZ? + // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31 + else if (gyro instanceof ADXRS450Swerve) + { + desiredNotifierPeriod = 1.0/100.0; + } + // NAX (AHRS): 60HZ + // https://github.com/kauailabs/navxmxp/blob/5e010ba810bb7f7eaab597e0b708e34f159984db/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java#L119C25-L119C61 + else if (gyro instanceof NavXSwerve) + { + desiredNotifierPeriod = 1.0/60.0; + } + // Pigeon2 100HZ + // https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf + else if (gyro instanceof Pigeon2Swerve) + { + desiredNotifierPeriod = 1.0/100.0; + } + // Pigeon 100HZ + // https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf + else if (gyro instanceof PigeonSwerve) + { + desiredNotifierPeriod = 1.0/100.0; + } + return new IMUVelocity(gyro, desiredNotifierPeriod, 5); + } + + /** + * Update the robot's rotational velocity based on the current gyro position. + */ + private void update() + { + double newTimestamp = HALUtil.getFPGATime(); + Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ()); + + synchronized (this) { + if (!firstCycle) { + velocityFilter.addValue((newPosition.minus(position).getRadians()) / (newTimestamp - timestamp)); + } + firstCycle = false; + timestamp = newTimestamp; + position = newPosition; + } + } + + /** + * Get the robot's angular velocity based on averaged meaasurements from the IMU. + * Velocity is multiplied by 1e+6 (1,000,000) because the timestamps are in microseconds. + * + * @return robot's angular velocity in rads/s as a double. + */ + public synchronized double getVelocity() { + velocity = velocityFilter.calculate(); + return velocity * 1e+6; + } +} diff --git a/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java new file mode 100644 index 00000000..552ae620 --- /dev/null +++ b/src/main/java/swervelib/math/IMULinearMovingAverageFilter.java @@ -0,0 +1,47 @@ +package swervelib.math; + +import edu.wpi.first.util.DoubleCircularBuffer; + +/** + * A linear filter that does not calculate() each time a value is added to + * the DoubleCircularBuffer. + */ +public class IMULinearMovingAverageFilter { + private final DoubleCircularBuffer m_inputs; + private final double m_inputGain; + + /** + * Construct a linear moving average fitler + * @param bufferLength The number of values to average across + */ + public IMULinearMovingAverageFilter(int bufferLength) + { + m_inputs = new DoubleCircularBuffer(bufferLength); + m_inputGain = 1.0 / bufferLength; + } + + /** + * Add a value to the DoubleCircularBuffer + * @param input + */ + public void addValue(double input) + { + m_inputs.addFirst(input); + } + + /** + * Calculate the average of the samples in the buffer + * @return The average of the values in the buffer + */ + public double calculate() + { + double returnVal = 0.0; + + for(int i = 0; i < m_inputs.size(); i++) + { + returnVal += m_inputs.get(i) * m_inputGain; + } + + return returnVal; + } +}