From b545717a85ff51985189855600956b2e4e86ed6d Mon Sep 17 00:00:00 2001 From: yapplejack Date: Wed, 2 Oct 2024 14:57:43 -0400 Subject: [PATCH] reverted dev to YAGSL dev --- .../swervedrive/SwerveSubsystem.java | 3 - src/main/java/swervelib/SwerveDrive.java | 94 ------------------ src/main/java/swervelib/imu/IMUVelocity.java | 99 ------------------- 3 files changed, 196 deletions(-) delete mode 100644 src/main/java/swervelib/imu/IMUVelocity.java diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 0b5ee486..9ce2de4f 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -96,9 +96,6 @@ 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, 0.1); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. - //Setting to true by default for testing, below is how I would push it - //swerveDrive.setAngularVelocityCompensation(false, 0); //Correct for skew that gets worse as angular velocity increases. Start with a coefficient of 0.1. if (visionDriveTest) { setupPhotonVision(); diff --git a/src/main/java/swervelib/SwerveDrive.java b/src/main/java/swervelib/SwerveDrive.java index 4fe49f5f..c6460e98 100644 --- a/src/main/java/swervelib/SwerveDrive.java +++ b/src/main/java/swervelib/SwerveDrive.java @@ -30,7 +30,6 @@ 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; @@ -98,15 +97,6 @@ public class SwerveDrive * correction. */ public boolean chassisVelocityCorrection = true; - /** - * Correct for skew that scales with angular velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} - * TODO: Still working on the explanation, I will have it ready soon (hopefully) - */ - public boolean angularVelocityCorrection = 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. */ @@ -123,11 +113,6 @@ 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. */ @@ -494,22 +479,6 @@ public void drive( public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters) { - // Explanation coming soon - if(angularVelocityCorrection) - { - // TODO: I still haven't done the math on the 1 million multipler, I will do that asap but I am pushing it for now - // This multipler should be built into the imuVelocity.getVelocity() function and is almost certainly dependand on the timestep used - var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * 1000000 * angularVelocityCoefficient); - if(angularVelocity.getRadians() != 0.0){ - velocity = ChassisSpeeds.fromRobotRelativeSpeeds( - velocity.vxMetersPerSecond, - velocity.vyMetersPerSecond, - velocity.omegaRadiansPerSecond, - getOdometryHeading()); - velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity)); - } - } - // 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) @@ -647,46 +616,6 @@ public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false); } - /** - * Set chassis speeds with closed-loop velocity control, using the optimizations present - * in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}. This may improve - * performance during autonomous, if the optimizations make you drive straight in teleop then - * they should help in autonomous, think of this as a feedforward that reduces the correction needed - * from PID due to skew. - * - * @param chassisSpeeds Chassis speeds to set. - */ - public void setChassisSpeedsWithDriveOptimizations(ChassisSpeeds chassisSpeeds) - { - // Explanation coming soon - if(angularVelocityCorrection) - { - // TODO: I still haven't done the math on the 1 million multipler, I will do that asap but I am pushing it for now - // This multipler should be built into the imuVelocity.getVelocity() function and is almost certainly dependand on the timestep used - var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * 1000000 * angularVelocityCoefficient); - if(angularVelocity.getRadians() != 0.0){ - chassisSpeeds = ChassisSpeeds.fromRobotRelativeSpeeds( - chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond, - chassisSpeeds.omegaRadiansPerSecond, - getOdometryHeading()); - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, getOdometryHeading().plus(angularVelocity)); - } - } - - // 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) - { - chassisSpeeds = ChassisSpeeds.discretize(chassisSpeeds, discretizationdtSeconds); - } - - SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; - SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond); - - setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false); - } /** * Gets the current pose (position and rotation) of the robot, as reported by odometry. @@ -1288,27 +1217,4 @@ public void setChassisDiscretization(boolean enable, double dtSeconds) discretizationdtSeconds = dtSeconds; } - /** - * Enables angular velocity correction and sets the angular velocity coefficient - * - * @param enable Enables angular velocity correction. - * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15, although higher values may be needed. - * Start with a value of 0.1. - * 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. Change the value until you are visually happy with the skew. - * Ensure your tune works with different translational and rotational magnitudes. - */ - public void setAngularVelocityCompensation(boolean enable, double angularVelocityCoeff) - { - /** TODO: How should it behave in the case of simulation? Skew doesn't really exist in simulation, - * discretize could also be removed in simulation as it creates skew in the simulation, - * the skew it corrects is only present on a real bot. - * This makes the robot continously rotate due to a lack of friction. - * My vote is to disable both discretize and angular velocity correction. - */ - imuVelocity = new IMUVelocity(imu); - angularVelocityCorrection = true; - angularVelocityCoefficient = angularVelocityCoeff; - } - } diff --git a/src/main/java/swervelib/imu/IMUVelocity.java b/src/main/java/swervelib/imu/IMUVelocity.java deleted file mode 100644 index 3a1df320..00000000 --- a/src/main/java/swervelib/imu/IMUVelocity.java +++ /dev/null @@ -1,99 +0,0 @@ -package swervelib.imu; - -import edu.wpi.first.math.filter.LinearFilter; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.wpilibj.Notifier; -import edu.wpi.first.wpilibj.RobotController; - -public class IMUVelocity { - /** - * Swerve IMU. - */ - private final SwerveIMU gyro; - /** - * Linear filter used to calculate velocity. - */ - private final LinearFilter 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. Defaults to an IMU polling rate of 50hz - * and 5 taps for moving average linear filter. - * - * @param gyro The SwerveIMU gyro. - */ - public IMUVelocity(SwerveIMU gyro) - { - /* TODO: For now I am going to assume that gyros operate at 60hz or more by default */ - /* NAVX2.0 defaults to 60hz, Pigeon2 is 100hz */ - this(gyro, 1.0/60.0, 5); - } - - /** - * 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 = LinearFilter.movingAverage(averagingTaps); - notifier = new Notifier(this::update); - notifier.startPeriodic(periodSeconds); - timestamp = RobotController.getFPGATime(); - } - - /** - * Update the robot's rotational velocity based on the current gyro position. - */ - private void update() - { - double newTimestamp = RobotController.getFPGATime(); - Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ()); - - synchronized (this) { - if (!firstCycle) { - velocity = velocityFilter.calculate( - (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. - * - * @return robot's angular velocity in rads/s as a double. - */ - public synchronized double getVelocity() { - return velocity; - } -}