From 9fe6551d88dd593a1934f940a6020ba3c498d0b7 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Wed, 6 Nov 2024 00:10:07 +0000 Subject: [PATCH] Upgrading to 2024.7.0 --- swervelib/SwerveDrive.java | 109 ++++++++++-------- swervelib/SwerveModule.java | 21 ++-- swervelib/imu/CanandgyroSwerve.java | 27 ++--- swervelib/imu/IMUVelocity.java | 71 ++++++------ swervelib/imu/NavXSwerve.java | 13 ++- swervelib/imu/Pigeon2Swerve.java | 34 +++--- swervelib/imu/PigeonSwerve.java | 18 +-- .../math/IMULinearMovingAverageFilter.java | 47 ++++---- swervelib/motors/SparkFlexSwerve.java | 11 +- .../motors/SparkMaxBrushedMotorSwerve.java | 11 +- swervelib/motors/SparkMaxSwerve.java | 25 ++-- swervelib/motors/TalonFXSwerve.java | 2 +- swervelib/motors/TalonSRXSwerve.java | 2 +- swervelib/parser/json/ModuleJson.java | 2 - 14 files changed, 211 insertions(+), 182 deletions(-) diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index 415f0bb8..7d83e5d0 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -104,18 +104,19 @@ public class SwerveDrive */ public boolean autonomousChassisVelocityCorrection = false; /** - * Correct for skew that scales with angular velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} + * 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 angularVelocityCorrection = false; + /** + * Correct for skew that scales with angular velocity in + * {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto. */ - public boolean autonomousAngularVelocityCorrection = false; + public boolean autonomousAngularVelocityCorrection = false; /** * Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15). */ - public double angularVelocityCoefficient = 0; + public double angularVelocityCoefficient = 0; /** * Whether to correct heading when driving translationally. Set to true to enable. */ @@ -133,10 +134,10 @@ public class SwerveDrive */ private SwerveIMU imu; /** - * Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in + * Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}. */ - private IMUVelocity imuVelocity; + private IMUVelocity imuVelocity; /** * Simulation of the swerve drive. */ @@ -586,11 +587,12 @@ public double getMaximumAngularVelocity() /** * Set the module states (azimuth and velocity) directly. * - * @param desiredStates A list of SwerveModuleStates to send to the modules. + * @param desiredStates A list of SwerveModuleStates to send to the modules. * @param desiredChassisSpeed The desired chassis speeds to set the robot to achieve. - * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. + * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. */ - private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, boolean isOpenLoop) + private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, + boolean isOpenLoop) { // Desaturates wheel speeds if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) @@ -612,10 +614,10 @@ 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, - * double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond) + * 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, double attainableMaxTranslationalSpeedMetersPerSecond, double + * attainableMaxRotationalVelocityRadiansPerSecond) * * @param desiredStates A list of SwerveModuleStates to send to the modules. * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop. @@ -640,7 +642,9 @@ public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) { - chassisSpeeds = movementOptimizations(chassisSpeeds, autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection); + chassisSpeeds = movementOptimizations(chassisSpeeds, + autonomousChassisVelocityCorrection, + autonomousAngularVelocityCorrection); SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; @@ -869,13 +873,15 @@ public void setMotorIdleMode(boolean brake) } /** - * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds. - * @param enabled Enable state + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state * @param deadband Deadband in degrees, default is 3 degrees. */ public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband) { - for(SwerveModule swerveModule : swerveModules) + for (SwerveModule swerveModule : swerveModules) { swerveModule.setEncoderAutoSynchronize(enabled, deadband); } @@ -1259,7 +1265,7 @@ public void setCosineCompensator(boolean enabled) */ public void setChassisDiscretization(boolean enable, double dtSeconds) { - if(!SwerveDriveTelemetry.isSimulation) + if (!SwerveDriveTelemetry.isSimulation) { chassisVelocityCorrection = enable; discretizationdtSeconds = dtSeconds; @@ -1267,17 +1273,18 @@ public void setChassisDiscretization(boolean enable, double dtSeconds) } /** - * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto + * 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. + * @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) + if (!SwerveDriveTelemetry.isSimulation) { chassisVelocityCorrection = useInTeleop; autonomousChassisVelocityCorrection = useInAuto; @@ -1286,22 +1293,22 @@ public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, dou } /** - * Enables angular velocity skew correction in teleop and/or autonomous - * and sets the angular velocity coefficient for both modes + * Enables angular velocity skew correction in teleop 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. + * @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) + if (!SwerveDriveTelemetry.isSimulation) { imuVelocity = IMUVelocity.createIMUVelocity(imu); angularVelocityCorrection = useInTeleop; @@ -1313,20 +1320,21 @@ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAut /** * Correct for skew that worsens as angular velocity increases * - * @param velocity The chassis speeds to set the robot to achieve. + * @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)); - } + if (angularVelocity.getRadians() != 0.0) + { + velocity = ChassisSpeeds.fromRobotRelativeSpeeds( + velocity.vxMetersPerSecond, + velocity.vyMetersPerSecond, + velocity.omegaRadiansPerSecond, + getOdometryHeading()); + velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity)); + } return velocity; } @@ -1336,12 +1344,13 @@ public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity) * @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. + * @return The chassis speeds after optimizations. */ - private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, boolean useAngularVelocitySkewCorrection) + private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, + boolean useAngularVelocitySkewCorrection) { - if(useAngularVelocitySkewCorrection) + if (useAngularVelocitySkewCorrection) { velocity = angularVelocitySkewCorrection(velocity); } diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index 2cdbf449..8b5dec10 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -95,7 +95,7 @@ public class SwerveModule /** * Anti-Jitter AKA auto-centering disabled. */ - private boolean antiJitterEnabled = true; + private boolean antiJitterEnabled = true; /** * Last swerve module state applied. */ @@ -111,15 +111,15 @@ public class SwerveModule /** * Encoder synchronization queued. */ - private boolean synchronizeEncoderQueued = false; + private boolean synchronizeEncoderQueued = false; /** * Encoder, Absolute encoder synchronization enabled. */ - private boolean synchronizeEncoderEnabled = false; + private boolean synchronizeEncoderEnabled = false; /** * Encoder synchronization deadband in degrees. */ - private double synchronizeEncoderDeadband = 3; + private double synchronizeEncoderDeadband = 3; /** @@ -257,8 +257,10 @@ public void queueSynchronizeEncoders() } /** - * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds. - * @param enabled Enable state + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * + * @param enabled Enable state * @param deadband Deadband in degrees, default is 3 degrees. */ public void setEncoderAutoSynchronize(boolean enabled, double deadband) @@ -268,7 +270,9 @@ public void setEncoderAutoSynchronize(boolean enabled, double deadband) } /** - * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a few seconds. + * Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a + * few seconds. + * * @param enabled Enable state */ public void setEncoderAutoSynchronize(boolean enabled) @@ -386,7 +390,8 @@ public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled) { double absoluteEncoderPosition = getAbsolutePosition(); - if(Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) { + if (Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) + { angleMotor.setPosition(absoluteEncoderPosition); } angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); diff --git a/swervelib/imu/CanandgyroSwerve.java b/swervelib/imu/CanandgyroSwerve.java index 1dc09956..aed3a146 100644 --- a/swervelib/imu/CanandgyroSwerve.java +++ b/swervelib/imu/CanandgyroSwerve.java @@ -1,17 +1,12 @@ package swervelib.imu; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Pigeon2Configuration; -import com.ctre.phoenix6.configs.Pigeon2Configurator; -import com.ctre.phoenix6.hardware.Pigeon2; import com.reduxrobotics.sensors.canandgyro.Canandgyro; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import java.util.Optional; /** - * SwerveIMU interface for the Boron Candandgyro by Redux Robotics + * SwerveIMU interface for the Boron {@link Canandgyro} by Redux Robotics */ public class CanandgyroSwerve extends SwerveIMU { @@ -19,24 +14,24 @@ public class CanandgyroSwerve extends SwerveIMU /** * Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.04; + public static double STATUS_TIMEOUT_SECONDS = 0.04; /** - * Boron Canandgyro by Redux Robotics. + * Boron {@link Canandgyro} by Redux Robotics. */ - Canandgyro imu; + private final Canandgyro imu; /** - * Offset for the Boron Canandgyro. + * Offset for the Boron {@link Canandgyro}. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** * Generate the SwerveIMU for {@link Canandgyro}. * - * @param canid CAN ID for the Boron Canandgyro + * @param canid CAN ID for the Boron {@link Canandgyro} */ public CanandgyroSwerve(int canid) { @@ -44,7 +39,7 @@ public CanandgyroSwerve(int canid) } /** - * Reset IMU to factory default. + * Reset {@link Canandgyro} to factory default. */ @Override public void factoryDefault() @@ -53,7 +48,7 @@ public void factoryDefault() } /** - * Clear sticky faults on IMU. + * Clear sticky faults on {@link Canandgyro}. */ @Override public void clearStickyFaults() @@ -128,7 +123,7 @@ public double getRate() } /** - * Get the instantiated IMU object. + * Get the instantiated {@link Canandgyro} IMU object. * * @return IMU object. */ diff --git a/swervelib/imu/IMUVelocity.java b/swervelib/imu/IMUVelocity.java index fba47a34..c1c0f775 100644 --- a/swervelib/imu/IMUVelocity.java +++ b/swervelib/imu/IMUVelocity.java @@ -8,48 +8,48 @@ /** * Generic IMU Velocity filter. */ -public class IMUVelocity { +public class IMUVelocity +{ + /** * Swerve IMU. */ - private final SwerveIMU gyro; + private final SwerveIMU gyro; /** - * Linear filter used to calculate velocity, we use a custom filter class - * to prevent unwanted operations. + * 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; + private final Notifier notifier; /** * Prevents calculation when no previous measurement exists. */ - private boolean firstCycle = true; + private boolean firstCycle = true; /** * Tracks the previous loop's recorded time. */ - private double timestamp = 0.0; + private double timestamp = 0.0; /** - * Tracks the previous loop's position as a Rotation2d. + * Tracks the previous loop's position as a Rotation2d. */ - private Rotation2d position = new Rotation2d(); + private Rotation2d position = new Rotation2d(); /** * The calculated velocity of the robot based on averaged IMU measurements. */ - private double velocity = 0.0; + private double velocity = 0.0; /** * Constructor for the IMU Velocity. * - * @param gyro The SwerveIMU gyro. + * @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. + * 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. - * + * 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) { @@ -61,51 +61,51 @@ public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps) } /** - * 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. + * 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. * @return {@link IMUVelocity} for the given gyro with adjusted period readings for velocity. */ public static IMUVelocity createIMUVelocity(SwerveIMU gyro) { - double desiredNotifierPeriod = 1.0/50.0; + 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; + 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; + 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; + 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; + 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; + 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; + desiredNotifierPeriod = 1.0 / 100.0; } return new IMUVelocity(gyro, desiredNotifierPeriod, 5); } @@ -113,15 +113,17 @@ else if (gyro instanceof PigeonSwerve) /** * Update the robot's rotational velocity based on the current gyro position. */ - private void update() + private void update() { - double newTimestamp = HALUtil.getFPGATime(); - Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ()); + double newTimestamp = HALUtil.getFPGATime(); + Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ()); - synchronized (this) { - if (!firstCycle) { + synchronized (this) + { + if (!firstCycle) + { velocityFilter.addValue((newPosition.minus(position).getRadians()) / (newTimestamp - timestamp)); - } + } firstCycle = false; timestamp = newTimestamp; position = newPosition; @@ -129,12 +131,13 @@ private void update() } /** - * 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. + * 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() { + public synchronized double getVelocity() + { velocity = velocityFilter.calculate(); return velocity * 1e+6; } diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java index 1adb6e19..2d6454ef 100644 --- a/swervelib/imu/NavXSwerve.java +++ b/swervelib/imu/NavXSwerve.java @@ -11,7 +11,7 @@ import swervelib.telemetry.Alert; /** - * Communicates with the NavX as the IMU. + * Communicates with the NavX({@link AHRS}) as the IMU. */ public class NavXSwerve extends SwerveIMU { @@ -34,7 +34,7 @@ public class NavXSwerve extends SwerveIMU private Alert navXError; /** - * Constructor for the NavX swerve. + * Constructor for the NavX({@link AHRS}) swerve. * * @param port Serial Port to connect to. */ @@ -57,7 +57,7 @@ public NavXSwerve(SerialPort.Port port) } /** - * Constructor for the NavX swerve. + * Constructor for the NavX({@link AHRS}) swerve. * * @param port SPI Port to connect to. */ @@ -79,7 +79,7 @@ public NavXSwerve(SPI.Port port) } /** - * Constructor for the NavX swerve. + * Constructor for the NavX({@link AHRS}) swerve. * * @param port I2C Port to connect to. */ @@ -101,7 +101,8 @@ public NavXSwerve(I2C.Port port) } /** - * Reset IMU to factory default. + * Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be + * too slow. */ @Override public void factoryDefault() @@ -188,7 +189,7 @@ public double getRate() } /** - * Get the instantiated IMU object. + * Get the instantiated NavX({@link AHRS}) IMU object. * * @return IMU object. */ diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java index 22160d86..574da147 100644 --- a/swervelib/imu/Pigeon2Swerve.java +++ b/swervelib/imu/Pigeon2Swerve.java @@ -10,7 +10,7 @@ import java.util.Optional; /** - * SwerveIMU interface for the Pigeon2 + * SwerveIMU interface for the {@link Pigeon2} */ public class Pigeon2Swerve extends SwerveIMU { @@ -18,29 +18,29 @@ public class Pigeon2Swerve extends SwerveIMU /** * Wait time for status frames to show up. */ - public static double STATUS_TIMEOUT_SECONDS = 0.04; + public static double STATUS_TIMEOUT_SECONDS = 0.04; /** - * Pigeon2 IMU device. + * {@link Pigeon2} IMU device. */ - Pigeon2 imu; + private final Pigeon2 imu; /** - * Offset for the Pigeon 2. + * Offset for the {@link Pigeon2}. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** - * Pigeon2 configurator. + * {@link Pigeon2} configurator. */ - private Pigeon2Configurator cfg; + private Pigeon2Configurator cfg; /** - * Generate the SwerveIMU for pigeon. + * Generate the SwerveIMU for {@link Pigeon2}. * - * @param canid CAN ID for the pigeon - * @param canbus CAN Bus name the pigeon resides on. + * @param canid CAN ID for the {@link Pigeon2} + * @param canbus CAN Bus name the {@link Pigeon2} resides on. */ public Pigeon2Swerve(int canid, String canbus) { @@ -50,9 +50,9 @@ public Pigeon2Swerve(int canid, String canbus) } /** - * Generate the SwerveIMU for pigeon. + * Generate the SwerveIMU for {@link Pigeon2}. * - * @param canid CAN ID for the pigeon + * @param canid CAN ID for the {@link Pigeon2} */ public Pigeon2Swerve(int canid) { @@ -60,7 +60,7 @@ public Pigeon2Swerve(int canid) } /** - * Reset IMU to factory default. + * Reset {@link Pigeon2} to factory default. */ @Override public void factoryDefault() @@ -72,7 +72,7 @@ public void factoryDefault() } /** - * Clear sticky faults on IMU. + * Clear sticky faults on {@link Pigeon2}. */ @Override public void clearStickyFaults() @@ -153,7 +153,7 @@ public double getRate() } /** - * Get the instantiated IMU object. + * Get the instantiated {@link Pigeon2} object. * * @return IMU object. */ diff --git a/swervelib/imu/PigeonSwerve.java b/swervelib/imu/PigeonSwerve.java index 9515b0d1..e2dfafe0 100644 --- a/swervelib/imu/PigeonSwerve.java +++ b/swervelib/imu/PigeonSwerve.java @@ -8,28 +8,28 @@ import java.util.Optional; /** - * SwerveIMU interface for the Pigeon. + * SwerveIMU interface for the {@link WPI_PigeonIMU}. */ public class PigeonSwerve extends SwerveIMU { /** - * Pigeon v1 IMU device. + * {@link WPI_PigeonIMU} IMU device. */ - WPI_PigeonIMU imu; + private final WPI_PigeonIMU imu; /** - * Offset for the Pigeon. + * Offset for the {@link WPI_PigeonIMU}. */ - private Rotation3d offset = new Rotation3d(); + private Rotation3d offset = new Rotation3d(); /** * Inversion for the gyro */ - private boolean invertedIMU = false; + private boolean invertedIMU = false; /** - * Generate the SwerveIMU for pigeon. + * Generate the SwerveIMU for {@link WPI_PigeonIMU}. * - * @param canid CAN ID for the pigeon, does not support CANBus. + * @param canid CAN ID for the {@link WPI_PigeonIMU}, does not support CANBus. */ public PigeonSwerve(int canid) { @@ -126,7 +126,7 @@ public double getRate() } /** - * Get the instantiated IMU object. + * Get the instantiated {@link WPI_PigeonIMU} IMU object. * * @return IMU object. */ diff --git a/swervelib/math/IMULinearMovingAverageFilter.java b/swervelib/math/IMULinearMovingAverageFilter.java index f2a008a7..09509b75 100644 --- a/swervelib/math/IMULinearMovingAverageFilter.java +++ b/swervelib/math/IMULinearMovingAverageFilter.java @@ -3,10 +3,10 @@ import edu.wpi.first.util.DoubleCircularBuffer; /** - * A linear filter that does not calculate() each time a value is added to - * the DoubleCircularBuffer. + * A linear filter that does not calculate() each time a value is added to the DoubleCircularBuffer. */ -public class IMULinearMovingAverageFilter { +public class IMULinearMovingAverageFilter +{ /** * Circular buffer storing the current IMU readings @@ -16,39 +16,42 @@ public class IMULinearMovingAverageFilter { * Gain on each reading. */ private final double m_inputGain; - - /** - * Construct a linear moving average fitler - * @param bufferLength The number of values to average across - */ + + /** + * 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 Value to add - */ + /** + * Add a value to the DoubleCircularBuffer + * + * @param input Value to add + */ 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 - */ + /** + * 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; + for (int i = 0; i < m_inputs.size(); i++) + { + returnVal += m_inputs.get(i) * m_inputGain; + } + + return returnVal; } } diff --git a/swervelib/motors/SparkFlexSwerve.java b/swervelib/motors/SparkFlexSwerve.java index 40a27702..400de964 100644 --- a/swervelib/motors/SparkFlexSwerve.java +++ b/swervelib/motors/SparkFlexSwerve.java @@ -12,6 +12,7 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -25,9 +26,9 @@ public class SparkFlexSwerve extends SwerveMotor { /** - * SparkMAX Instance. + * {@link CANSparkFlex} Instance. */ - public CANSparkFlex motor; + private final CANSparkFlex motor; /** * Integrated encoder. */ @@ -108,6 +109,7 @@ private void configureSparkFlex(Supplier config) { return; } + Timer.delay(0.01); } failureConfiguring.set(true); } @@ -329,7 +331,10 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - motor.setInverted(inverted); + configureSparkFlex(() -> { + motor.setInverted(inverted); + return motor.getLastError(); + }); } /** diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java index d8fb2a8a..f384352f 100644 --- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -11,13 +11,14 @@ import com.revrobotics.SparkMaxAlternateEncoder; import com.revrobotics.SparkPIDController; import com.revrobotics.SparkRelativeEncoder.Type; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; import swervelib.telemetry.Alert; /** - * Brushed motor control with SparkMax. + * Brushed motor control with {@link CANSparkMax}. */ public class SparkMaxBrushedMotorSwerve extends SwerveMotor { @@ -25,7 +26,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor /** * SparkMAX Instance. */ - public CANSparkMax motor; + private final CANSparkMax motor; /** * Absolute encoder attached to the SparkMax (if exists) @@ -145,6 +146,7 @@ private void configureSparkMax(Supplier config) { return; } + Timer.delay(0.01); } failureConfiguringAlert.set(true); } @@ -351,7 +353,10 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - motor.setInverted(inverted); + configureSparkMax(() -> { + motor.setInverted(inverted); + return motor.getLastError(); + }); } /** diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java index 0f014173..14795716 100644 --- a/swervelib/motors/SparkMaxSwerve.java +++ b/swervelib/motors/SparkMaxSwerve.java @@ -12,6 +12,7 @@ import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Supplier; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.parser.PIDFConfig; @@ -24,33 +25,33 @@ public class SparkMaxSwerve extends SwerveMotor { /** - * SparkMAX Instance. + * {@link CANSparkMax} Instance. */ - public CANSparkMax motor; + private final CANSparkMax motor; /** * Integrated encoder. */ - public RelativeEncoder encoder; + public RelativeEncoder encoder; /** * Absolute encoder attached to the SparkMax (if exists) */ - public SwerveAbsoluteEncoder absoluteEncoder; + public SwerveAbsoluteEncoder absoluteEncoder; /** * Closed-loop PID controller. */ - public SparkPIDController pid; + public SparkPIDController pid; /** * Factory default already occurred. */ - private boolean factoryDefaultOccurred = false; + private boolean factoryDefaultOccurred = false; /** * Supplier for the velocity of the motor controller. */ - private Supplier velocity; + private Supplier velocity; /** * Supplier for the position of the motor controller. */ - private Supplier position; + private Supplier position; /** * Initialize the swerve motor. @@ -100,6 +101,7 @@ private void configureSparkMax(Supplier config) { return; } + Timer.delay(0.01); } DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); } @@ -252,7 +254,7 @@ public void configureIntegratedEncoder(double positionConversionFactor) configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50); } // Need to test on analog encoders but the same concept should apply for them, worst thing that could happen is slightly more can use - else if(absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor) + else if (absoluteEncoder.getAbsoluteEncoder() instanceof SparkAnalogSensor) { configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200); } @@ -357,7 +359,10 @@ public void setMotorBrake(boolean isBrakeMode) @Override public void setInverted(boolean inverted) { - motor.setInverted(inverted); + configureSparkMax(() -> { + motor.setInverted(inverted); + return motor.getLastError(); + }); } /** diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java index d2ddb4b2..233a868f 100644 --- a/swervelib/motors/TalonFXSwerve.java +++ b/swervelib/motors/TalonFXSwerve.java @@ -40,7 +40,7 @@ public class TalonFXSwerve extends SwerveMotor /** * TalonFX motor controller. */ - TalonFX motor; + private final TalonFX motor; /** * Conversion factor for the motor. */ diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java index 30db007e..98cf6e7c 100644 --- a/swervelib/motors/TalonSRXSwerve.java +++ b/swervelib/motors/TalonSRXSwerve.java @@ -33,7 +33,7 @@ public class TalonSRXSwerve extends SwerveMotor /** * TalonSRX motor controller. */ - WPI_TalonSRX motor; + private final WPI_TalonSRX motor; /** * The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees. */ diff --git a/swervelib/parser/json/ModuleJson.java b/swervelib/parser/json/ModuleJson.java index a2e5a6d8..2fa83c89 100644 --- a/swervelib/parser/json/ModuleJson.java +++ b/swervelib/parser/json/ModuleJson.java @@ -1,6 +1,5 @@ package swervelib.parser.json; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; import com.revrobotics.MotorFeedbackSensor; import edu.wpi.first.math.util.Units; @@ -12,7 +11,6 @@ import swervelib.parser.json.modules.BoolMotorJson; import swervelib.parser.json.modules.ConversionFactorsJson; import swervelib.parser.json.modules.LocationJson; -import swervelib.telemetry.Alert; /** * {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data.