Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Upgrading to 2024.7.0

Browse Source
This commit is contained in:
thenetworkgrinch
2024-11-06 00:10:07 +00:00
parent b8e06f205e
commit 9fe6551d88
14 changed files with 211 additions and 182 deletions
Download Patch File Download Diff File Expand all files Collapse all files

109
swervelib/SwerveDrive.java
View File

@@ -104,18 +104,19 @@ public class SwerveDrive
*/ */
public boolean autonomousChassisVelocityCorrection = false; 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; public boolean angularVelocityCorrection = false;
/** /**
* Correct for skew that scales with angular velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} * Correct for skew that scales with angular velocity in
* during auto. * {@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). * 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. * Whether to correct heading when driving translationally. Set to true to enable.
*/ */
@@ -133,10 +134,10 @@ public class SwerveDrive
*/ */
private SwerveIMU imu; 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)}. * {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}.
*/ */
private IMUVelocity imuVelocity; private IMUVelocity imuVelocity;
/** /**
* Simulation of the swerve drive. * Simulation of the swerve drive.
*/ */
@@ -586,11 +587,12 @@ public class SwerveDrive
/** /**
* Set the module states (azimuth and velocity) directly. * 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 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 // Desaturates wheel speeds
if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0)
@@ -612,10 +614,10 @@ public class SwerveDrive
} }
/** /**
* Set the module states (azimuth and velocity) directly. Used primarily for auto paths. * Set the module states (azimuth and velocity) directly. Used primarily for auto paths. Does not allow for usage of
* Does not allow for usage of desaturateWheelSpeeds(SwerveModuleState[] moduleStates, * desaturateWheelSpeeds(SwerveModuleState[] moduleStates, ChassisSpeeds desiredChassisSpeed, double
* ChassisSpeeds desiredChassisSpeed, double attainableMaxModuleSpeedMetersPerSecond, * attainableMaxModuleSpeedMetersPerSecond, double attainableMaxTranslationalSpeedMetersPerSecond, double
* double attainableMaxTranslationalSpeedMetersPerSecond, double attainableMaxRotationalVelocityRadiansPerSecond) * attainableMaxRotationalVelocityRadiansPerSecond)
* *
* @param desiredStates A list of SwerveModuleStates to send to the modules. * @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. * @param isOpenLoop Whether to use closed-loop velocity control. Set to true to disable closed-loop.
@@ -640,7 +642,9 @@ public class SwerveDrive
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
{ {
chassisSpeeds = movementOptimizations(chassisSpeeds, autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection); chassisSpeeds = movementOptimizations(chassisSpeeds,
autonomousChassisVelocityCorrection,
autonomousAngularVelocityCorrection);
SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
@@ -869,13 +873,15 @@ public class SwerveDrive
} }
/** /**
* 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
* @param enabled Enable state * few seconds.
*
* @param enabled Enable state
* @param deadband Deadband in degrees, default is 3 degrees. * @param deadband Deadband in degrees, default is 3 degrees.
*/ */
public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband) public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband)
{ {
for(SwerveModule swerveModule : swerveModules) for (SwerveModule swerveModule : swerveModules)
{ {
swerveModule.setEncoderAutoSynchronize(enabled, deadband); swerveModule.setEncoderAutoSynchronize(enabled, deadband);
} }
@@ -1259,7 +1265,7 @@ public class SwerveDrive
*/ */
public void setChassisDiscretization(boolean enable, double dtSeconds) public void setChassisDiscretization(boolean enable, double dtSeconds)
{ {
if(!SwerveDriveTelemetry.isSimulation) if (!SwerveDriveTelemetry.isSimulation)
{ {
chassisVelocityCorrection = enable; chassisVelocityCorrection = enable;
discretizationdtSeconds = dtSeconds; discretizationdtSeconds = dtSeconds;
@@ -1267,17 +1273,18 @@ public class SwerveDrive
} }
/** /**
* 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 * @param useInTeleop Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop. * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
* @param useInAuto Enable chassis velocity correction, which will use * @param useInAuto Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto. * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
* @param dtSeconds The duration of the timestep the speeds should be applied for. * @param dtSeconds The duration of the timestep the speeds should be applied for.
*/ */
public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds) public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
{ {
if(!SwerveDriveTelemetry.isSimulation) if (!SwerveDriveTelemetry.isSimulation)
{ {
chassisVelocityCorrection = useInTeleop; chassisVelocityCorrection = useInTeleop;
autonomousChassisVelocityCorrection = useInAuto; autonomousChassisVelocityCorrection = useInAuto;
@@ -1286,22 +1293,22 @@ public class SwerveDrive
} }
/** /**
* Enables angular velocity skew correction in teleop and/or autonomous * Enables angular velocity skew correction in teleop and/or autonomous and sets the angular velocity coefficient for
* and sets the angular velocity coefficient for both modes * both modes
* *
* @param useInTeleop Enables angular velocity correction in teleop. * @param useInTeleop Enables angular velocity correction in teleop.
* @param useInAuto Enables angular velocity correction in autonomous. * @param useInAuto Enables angular velocity correction in autonomous.
* @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15. * @param angularVelocityCoeff The angular velocity coefficient. Expected values between -0.15 to 0.15. Start with a
* Start with a value of 0.1, test in teleop. * value of 0.1, test in teleop. When enabling for the first time if the skew is
* When enabling for the first time if the skew is significantly worse try inverting the value. * significantly worse try inverting the value. Tune by moving in a straight line while
* Tune by moving in a straight line while rotating. Testing is best done with angular velocity controls on the right stick. * rotating. Testing is best done with angular velocity controls on the right stick.
* Change the value until you are visually happy with the skew. * Change the value until you are visually happy with the skew. Ensure your tune works
* Ensure your tune works with different translational and rotational magnitudes. * with different translational and rotational magnitudes. If this reduces skew in teleop,
* If this reduces skew in teleop, it may improve auto. * it may improve auto.
*/ */
public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff) public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff)
{ {
if(!SwerveDriveTelemetry.isSimulation) if (!SwerveDriveTelemetry.isSimulation)
{ {
imuVelocity = IMUVelocity.createIMUVelocity(imu); imuVelocity = IMUVelocity.createIMUVelocity(imu);
angularVelocityCorrection = useInTeleop; angularVelocityCorrection = useInTeleop;
@@ -1313,20 +1320,21 @@ public class SwerveDrive
/** /**
* Correct for skew that worsens as angular velocity increases * 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. * @return {@link ChassisSpeeds} of the robot after angular velocity skew correction.
*/ */
public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity) public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
{ {
var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient); var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
if(angularVelocity.getRadians() != 0.0){ if (angularVelocity.getRadians() != 0.0)
velocity = ChassisSpeeds.fromRobotRelativeSpeeds( {
velocity.vxMetersPerSecond, velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
velocity.vyMetersPerSecond, velocity.vxMetersPerSecond,
velocity.omegaRadiansPerSecond, velocity.vyMetersPerSecond,
getOdometryHeading()); velocity.omegaRadiansPerSecond,
velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity)); getOdometryHeading());
} velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity));
}
return velocity; return velocity;
} }
@@ -1336,12 +1344,13 @@ public class SwerveDrive
* @param velocity The chassis speeds to set the robot to achieve. * @param velocity The chassis speeds to set the robot to achieve.
* @param uesChassisDiscretize Correct chassis velocity using 254's correction. * @param uesChassisDiscretize Correct chassis velocity using 254's correction.
* @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew. * @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); velocity = angularVelocitySkewCorrection(velocity);
} }

21
swervelib/SwerveModule.java
View File

@@ -95,7 +95,7 @@ public class SwerveModule
/** /**
* Anti-Jitter AKA auto-centering disabled. * Anti-Jitter AKA auto-centering disabled.
*/ */
private boolean antiJitterEnabled = true; private boolean antiJitterEnabled = true;
/** /**
* Last swerve module state applied. * Last swerve module state applied.
*/ */
@@ -111,15 +111,15 @@ public class SwerveModule
/** /**
* Encoder synchronization queued. * Encoder synchronization queued.
*/ */
private boolean synchronizeEncoderQueued = false; private boolean synchronizeEncoderQueued = false;
/** /**
* Encoder, Absolute encoder synchronization enabled. * Encoder, Absolute encoder synchronization enabled.
*/ */
private boolean synchronizeEncoderEnabled = false; private boolean synchronizeEncoderEnabled = false;
/** /**
* Encoder synchronization deadband in degrees. * Encoder synchronization deadband in degrees.
*/ */
private double synchronizeEncoderDeadband = 3; private double synchronizeEncoderDeadband = 3;
/** /**
@@ -257,8 +257,10 @@ public class SwerveModule
} }
/** /**
* 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
* @param enabled Enable state * few seconds.
*
* @param enabled Enable state
* @param deadband Deadband in degrees, default is 3 degrees. * @param deadband Deadband in degrees, default is 3 degrees.
*/ */
public void setEncoderAutoSynchronize(boolean enabled, double deadband) public void setEncoderAutoSynchronize(boolean enabled, double deadband)
@@ -268,7 +270,9 @@ public class SwerveModule
} }
/** /**
* 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 * @param enabled Enable state
*/ */
public void setEncoderAutoSynchronize(boolean enabled) public void setEncoderAutoSynchronize(boolean enabled)
@@ -386,7 +390,8 @@ public class SwerveModule
if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled) if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled)
{ {
double absoluteEncoderPosition = getAbsolutePosition(); double absoluteEncoderPosition = getAbsolutePosition();
if(Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) { if (Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband)
{
angleMotor.setPosition(absoluteEncoderPosition); angleMotor.setPosition(absoluteEncoderPosition);
} }
angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition); angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);

27
swervelib/imu/CanandgyroSwerve.java
View File

@@ -1,17 +1,12 @@
package swervelib.imu; 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 com.reduxrobotics.sensors.canandgyro.Canandgyro;
import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional; 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 public class CanandgyroSwerve extends SwerveIMU
{ {
@@ -19,24 +14,24 @@ public class CanandgyroSwerve extends SwerveIMU
/** /**
* Wait time for status frames to show up. * 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 * Inversion for the gyro
*/ */
private boolean invertedIMU = false; private boolean invertedIMU = false;
/** /**
* Generate the SwerveIMU for {@link Canandgyro}. * 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) public CanandgyroSwerve(int canid)
{ {
@@ -44,7 +39,7 @@ public class CanandgyroSwerve extends SwerveIMU
} }
/** /**
* Reset IMU to factory default. * Reset {@link Canandgyro} to factory default.
*/ */
@Override @Override
public void factoryDefault() public void factoryDefault()
@@ -53,7 +48,7 @@ public class CanandgyroSwerve extends SwerveIMU
} }
/** /**
* Clear sticky faults on IMU. * Clear sticky faults on {@link Canandgyro}.
*/ */
@Override @Override
public void clearStickyFaults() public void clearStickyFaults()
@@ -128,7 +123,7 @@ public class CanandgyroSwerve extends SwerveIMU
} }
/** /**
* Get the instantiated IMU object. * Get the instantiated {@link Canandgyro} IMU object.
* *
* @return IMU object. * @return IMU object.
*/ */

71
swervelib/imu/IMUVelocity.java
View File

@@ -8,48 +8,48 @@ import swervelib.math.IMULinearMovingAverageFilter;
/** /**
* Generic IMU Velocity filter. * Generic IMU Velocity filter.
*/ */
public class IMUVelocity { public class IMUVelocity
{
/** /**
* Swerve IMU. * Swerve IMU.
*/ */
private final SwerveIMU gyro; private final SwerveIMU gyro;
/** /**
* Linear filter used to calculate velocity, we use a custom filter class * Linear filter used to calculate velocity, we use a custom filter class to prevent unwanted operations.
* to prevent unwanted operations.
*/ */
private final IMULinearMovingAverageFilter velocityFilter; private final IMULinearMovingAverageFilter velocityFilter;
/** /**
* WPILib {@link Notifier} to keep IMU velocity up to date. * 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. * Prevents calculation when no previous measurement exists.
*/ */
private boolean firstCycle = true; private boolean firstCycle = true;
/** /**
* Tracks the previous loop's recorded time. * 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. * 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. * 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), * @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 * @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 * allow the system to update to changes in velocity, lower values may create a less smooth
* will probably be ~2-8, with the goal of having the lowest smooth value. * 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) public IMUVelocity(SwerveIMU gyro, double periodSeconds, int averagingTaps)
{ {
@@ -61,51 +61,51 @@ public class IMUVelocity {
} }
/** /**
* Static factory for IMU Velocity. Supported IMU rates will be as quick as possible * Static factory for IMU Velocity. Supported IMU rates will be as quick as possible but will not exceed 100hz and
* but will not exceed 100hz and will use 5 taps (supported IMUs are listed in swervelib's IMU folder). * will use 5 taps (supported IMUs are listed in swervelib's IMU folder). Other gyroscopes will default to 50hz and 5
* Other gyroscopes will default to 50hz and 5 taps. For custom rates please use the IMUVelocity constructor. * taps. For custom rates please use the IMUVelocity constructor.
* *
* @param gyro The SwerveIMU gyro. * @param gyro The SwerveIMU gyro.
* @return {@link IMUVelocity} for the given gyro with adjusted period readings for velocity. * @return {@link IMUVelocity} for the given gyro with adjusted period readings for velocity.
*/ */
public static IMUVelocity createIMUVelocity(SwerveIMU gyro) public static IMUVelocity createIMUVelocity(SwerveIMU gyro)
{ {
double desiredNotifierPeriod = 1.0/50.0; double desiredNotifierPeriod = 1.0 / 50.0;
// ADIS16448_IMU ~200HZ: // ADIS16448_IMU ~200HZ:
// https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277 // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java#L277
if (gyro instanceof ADIS16448Swerve) if (gyro instanceof ADIS16448Swerve)
{ {
desiredNotifierPeriod = 1.0/100.0; desiredNotifierPeriod = 1.0 / 100.0;
} }
// ADIS16470_IMU 200HZ // ADIS16470_IMU 200HZ
// https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345 // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java#L345
else if (gyro instanceof ADIS16470Swerve) else if (gyro instanceof ADIS16470Swerve)
{ {
desiredNotifierPeriod = 1.0/100.0; desiredNotifierPeriod = 1.0 / 100.0;
} }
// ADXRS450_Gyro 2000HZ? // ADXRS450_Gyro 2000HZ?
// https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31 // https://github.com/wpilibsuite/allwpilib/blob/f82e1c9d4807f4c0fa832fd5bd9f9e90848eb8eb/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java#L31
else if (gyro instanceof ADXRS450Swerve) else if (gyro instanceof ADXRS450Swerve)
{ {
desiredNotifierPeriod = 1.0/100.0; desiredNotifierPeriod = 1.0 / 100.0;
} }
// NAX (AHRS): 60HZ // NAX (AHRS): 60HZ
// https://github.com/kauailabs/navxmxp/blob/5e010ba810bb7f7eaab597e0b708e34f159984db/roborio/java/navx_frc/src/com/kauailabs/navx/frc/AHRS.java#L119C25-L119C61 // 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) else if (gyro instanceof NavXSwerve)
{ {
desiredNotifierPeriod = 1.0/60.0; desiredNotifierPeriod = 1.0 / 60.0;
} }
// Pigeon2 100HZ // Pigeon2 100HZ
// https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf // https://store.ctr-electronics.com/content/user-manual/Pigeon2%20User's%20Guide.pdf
else if (gyro instanceof Pigeon2Swerve) else if (gyro instanceof Pigeon2Swerve)
{ {
desiredNotifierPeriod = 1.0/100.0; desiredNotifierPeriod = 1.0 / 100.0;
} }
// Pigeon 100HZ // Pigeon 100HZ
// https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf // https://store.ctr-electronics.com/content/user-manual/Pigeon%20IMU%20User's%20Guide.pdf
else if (gyro instanceof PigeonSwerve) else if (gyro instanceof PigeonSwerve)
{ {
desiredNotifierPeriod = 1.0/100.0; desiredNotifierPeriod = 1.0 / 100.0;
} }
return new IMUVelocity(gyro, desiredNotifierPeriod, 5); return new IMUVelocity(gyro, desiredNotifierPeriod, 5);
} }
@@ -113,15 +113,17 @@ public class IMUVelocity {
/** /**
* Update the robot's rotational velocity based on the current gyro position. * Update the robot's rotational velocity based on the current gyro position.
*/ */
private void update() private void update()
{ {
double newTimestamp = HALUtil.getFPGATime(); double newTimestamp = HALUtil.getFPGATime();
Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ()); Rotation2d newPosition = Rotation2d.fromRadians(gyro.getRotation3d().getZ());
synchronized (this) { synchronized (this)
if (!firstCycle) { {
if (!firstCycle)
{
velocityFilter.addValue((newPosition.minus(position).getRadians()) / (newTimestamp - timestamp)); velocityFilter.addValue((newPosition.minus(position).getRadians()) / (newTimestamp - timestamp));
} }
firstCycle = false; firstCycle = false;
timestamp = newTimestamp; timestamp = newTimestamp;
position = newPosition; position = newPosition;
@@ -129,12 +131,13 @@ public class IMUVelocity {
} }
/** /**
* Get the robot's angular velocity based on averaged meaasurements from the IMU. * Get the robot's angular velocity based on averaged meaasurements from the IMU. Velocity is multiplied by 1e+6
* Velocity is multiplied by 1e+6 (1,000,000) because the timestamps are in microseconds. * (1,000,000) because the timestamps are in microseconds.
* *
* @return robot's angular velocity in rads/s as a double. * @return robot's angular velocity in rads/s as a double.
*/ */
public synchronized double getVelocity() { public synchronized double getVelocity()
{
velocity = velocityFilter.calculate(); velocity = velocityFilter.calculate();
return velocity * 1e+6; return velocity * 1e+6;
} }

13
swervelib/imu/NavXSwerve.java
View File

@@ -11,7 +11,7 @@ import java.util.Optional;
import swervelib.telemetry.Alert; 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 public class NavXSwerve extends SwerveIMU
{ {
@@ -34,7 +34,7 @@ public class NavXSwerve extends SwerveIMU
private Alert navXError; private Alert navXError;
/** /**
* Constructor for the NavX swerve. * Constructor for the NavX({@link AHRS}) swerve.
* *
* @param port Serial Port to connect to. * @param port Serial Port to connect to.
*/ */
@@ -57,7 +57,7 @@ public class NavXSwerve extends SwerveIMU
} }
/** /**
* Constructor for the NavX swerve. * Constructor for the NavX({@link AHRS}) swerve.
* *
* @param port SPI Port to connect to. * @param port SPI Port to connect to.
*/ */
@@ -79,7 +79,7 @@ public class NavXSwerve extends SwerveIMU
} }
/** /**
* Constructor for the NavX swerve. * Constructor for the NavX({@link AHRS}) swerve.
* *
* @param port I2C Port to connect to. * @param port I2C Port to connect to.
*/ */
@@ -101,7 +101,8 @@ public class NavXSwerve extends SwerveIMU
} }
/** /**
* 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 @Override
public void factoryDefault() public void factoryDefault()
@@ -188,7 +189,7 @@ public class NavXSwerve extends SwerveIMU
} }
/** /**
* Get the instantiated IMU object. * Get the instantiated NavX({@link AHRS}) IMU object.
* *
* @return IMU object. * @return IMU object.
*/ */

34
swervelib/imu/Pigeon2Swerve.java
View File

@@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional; import java.util.Optional;
/** /**
* SwerveIMU interface for the Pigeon2 * SwerveIMU interface for the {@link Pigeon2}
*/ */
public class Pigeon2Swerve extends SwerveIMU public class Pigeon2Swerve extends SwerveIMU
{ {
@@ -18,29 +18,29 @@ public class Pigeon2Swerve extends SwerveIMU
/** /**
* Wait time for status frames to show up. * 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 * 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 canid CAN ID for the {@link Pigeon2}
* @param canbus CAN Bus name the pigeon resides on. * @param canbus CAN Bus name the {@link Pigeon2} resides on.
*/ */
public Pigeon2Swerve(int canid, String canbus) public Pigeon2Swerve(int canid, String canbus)
{ {
@@ -50,9 +50,9 @@ public class Pigeon2Swerve extends SwerveIMU
} }
/** /**
* 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) public Pigeon2Swerve(int canid)
{ {
@@ -60,7 +60,7 @@ public class Pigeon2Swerve extends SwerveIMU
} }
/** /**
* Reset IMU to factory default. * Reset {@link Pigeon2} to factory default.
*/ */
@Override @Override
public void factoryDefault() public void factoryDefault()
@@ -72,7 +72,7 @@ public class Pigeon2Swerve extends SwerveIMU
} }
/** /**
* Clear sticky faults on IMU. * Clear sticky faults on {@link Pigeon2}.
*/ */
@Override @Override
public void clearStickyFaults() public void clearStickyFaults()
@@ -153,7 +153,7 @@ public class Pigeon2Swerve extends SwerveIMU
} }
/** /**
* Get the instantiated IMU object. * Get the instantiated {@link Pigeon2} object.
* *
* @return IMU object. * @return IMU object.
*/ */

18
swervelib/imu/PigeonSwerve.java
View File

@@ -8,28 +8,28 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional; import java.util.Optional;
/** /**
* SwerveIMU interface for the Pigeon. * SwerveIMU interface for the {@link WPI_PigeonIMU}.
*/ */
public class PigeonSwerve extends SwerveIMU 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 * 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) public PigeonSwerve(int canid)
{ {
@@ -126,7 +126,7 @@ public class PigeonSwerve extends SwerveIMU
} }
/** /**
* Get the instantiated IMU object. * Get the instantiated {@link WPI_PigeonIMU} IMU object.
* *
* @return IMU object. * @return IMU object.
*/ */

47
swervelib/math/IMULinearMovingAverageFilter.java
View File

@@ -3,10 +3,10 @@ package swervelib.math;
import edu.wpi.first.util.DoubleCircularBuffer; import edu.wpi.first.util.DoubleCircularBuffer;
/** /**
* A linear filter that does not calculate() each time a value is added to * A linear filter that does not calculate() each time a value is added to the DoubleCircularBuffer.
* the DoubleCircularBuffer.
*/ */
public class IMULinearMovingAverageFilter { public class IMULinearMovingAverageFilter
{
/** /**
* Circular buffer storing the current IMU readings * Circular buffer storing the current IMU readings
@@ -16,39 +16,42 @@ public class IMULinearMovingAverageFilter {
* Gain on each reading. * Gain on each reading.
*/ */
private final double m_inputGain; private final double m_inputGain;
/** /**
* Construct a linear moving average fitler * Construct a linear moving average fitler
* @param bufferLength The number of values to average across *
*/ * @param bufferLength The number of values to average across
*/
public IMULinearMovingAverageFilter(int bufferLength) public IMULinearMovingAverageFilter(int bufferLength)
{ {
m_inputs = new DoubleCircularBuffer(bufferLength); m_inputs = new DoubleCircularBuffer(bufferLength);
m_inputGain = 1.0 / bufferLength; m_inputGain = 1.0 / bufferLength;
} }
/** /**
* Add a value to the DoubleCircularBuffer * Add a value to the DoubleCircularBuffer
* @param input Value to add *
*/ * @param input Value to add
*/
public void addValue(double input) public void addValue(double input)
{ {
m_inputs.addFirst(input); m_inputs.addFirst(input);
} }
/** /**
* Calculate the average of the samples in the buffer * Calculate the average of the samples in the buffer
* @return The average of the values in the buffer *
*/ * @return The average of the values in the buffer
*/
public double calculate() public double calculate()
{ {
double returnVal = 0.0; double returnVal = 0.0;
for(int i = 0; i < m_inputs.size(); i++) for (int i = 0; i < m_inputs.size(); i++)
{ {
returnVal += m_inputs.get(i) * m_inputGain; returnVal += m_inputs.get(i) * m_inputGain;
} }
return returnVal; return returnVal;
} }
} }

11
swervelib/motors/SparkFlexSwerve.java
View File

@@ -12,6 +12,7 @@ import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder; import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkAnalogSensor;
import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier; import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig; 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. * Integrated encoder.
*/ */
@@ -108,6 +109,7 @@ public class SparkFlexSwerve extends SwerveMotor
{ {
return; return;
} }
Timer.delay(0.01);
} }
failureConfiguring.set(true); failureConfiguring.set(true);
} }
@@ -329,7 +331,10 @@ public class SparkFlexSwerve extends SwerveMotor
@Override @Override
public void setInverted(boolean inverted) public void setInverted(boolean inverted)
{ {
motor.setInverted(inverted); configureSparkFlex(() -> {
motor.setInverted(inverted);
return motor.getLastError();
});
} }
/** /**

11
swervelib/motors/SparkMaxBrushedMotorSwerve.java
View File

@@ -11,13 +11,14 @@ import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxAlternateEncoder; import com.revrobotics.SparkMaxAlternateEncoder;
import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController;
import com.revrobotics.SparkRelativeEncoder.Type; import com.revrobotics.SparkRelativeEncoder.Type;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier; import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig; import swervelib.parser.PIDFConfig;
import swervelib.telemetry.Alert; import swervelib.telemetry.Alert;
/** /**
* Brushed motor control with SparkMax. * Brushed motor control with {@link CANSparkMax}.
*/ */
public class SparkMaxBrushedMotorSwerve extends SwerveMotor public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{ {
@@ -25,7 +26,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
/** /**
* SparkMAX Instance. * SparkMAX Instance.
*/ */
public CANSparkMax motor; private final CANSparkMax motor;
/** /**
* Absolute encoder attached to the SparkMax (if exists) * Absolute encoder attached to the SparkMax (if exists)
@@ -145,6 +146,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{ {
return; return;
} }
Timer.delay(0.01);
} }
failureConfiguringAlert.set(true); failureConfiguringAlert.set(true);
} }
@@ -351,7 +353,10 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override @Override
public void setInverted(boolean inverted) public void setInverted(boolean inverted)
{ {
motor.setInverted(inverted); configureSparkMax(() -> {
motor.setInverted(inverted);
return motor.getLastError();
});
} }
/** /**

25
swervelib/motors/SparkMaxSwerve.java
View File

@@ -12,6 +12,7 @@ import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkAnalogSensor;
import com.revrobotics.SparkPIDController; import com.revrobotics.SparkPIDController;
import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.function.Supplier; import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig; 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. * Integrated encoder.
*/ */
public RelativeEncoder encoder; public RelativeEncoder encoder;
/** /**
* Absolute encoder attached to the SparkMax (if exists) * Absolute encoder attached to the SparkMax (if exists)
*/ */
public SwerveAbsoluteEncoder absoluteEncoder; public SwerveAbsoluteEncoder absoluteEncoder;
/** /**
* Closed-loop PID controller. * Closed-loop PID controller.
*/ */
public SparkPIDController pid; public SparkPIDController pid;
/** /**
* Factory default already occurred. * Factory default already occurred.
*/ */
private boolean factoryDefaultOccurred = false; private boolean factoryDefaultOccurred = false;
/** /**
* Supplier for the velocity of the motor controller. * Supplier for the velocity of the motor controller.
*/ */
private Supplier<Double> velocity; private Supplier<Double> velocity;
/** /**
* Supplier for the position of the motor controller. * Supplier for the position of the motor controller.
*/ */
private Supplier<Double> position; private Supplier<Double> position;
/** /**
* Initialize the swerve motor. * Initialize the swerve motor.
@@ -100,6 +101,7 @@ public class SparkMaxSwerve extends SwerveMotor
{ {
return; return;
} }
Timer.delay(0.01);
} }
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
} }
@@ -252,7 +254,7 @@ public class SparkMaxSwerve extends SwerveMotor
configureCANStatusFrames(100, 20, 20, 200, 20, 8, 50); 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 // 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); configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200);
} }
@@ -357,7 +359,10 @@ public class SparkMaxSwerve extends SwerveMotor
@Override @Override
public void setInverted(boolean inverted) public void setInverted(boolean inverted)
{ {
motor.setInverted(inverted); configureSparkMax(() -> {
motor.setInverted(inverted);
return motor.getLastError();
});
} }
/** /**

2
swervelib/motors/TalonFXSwerve.java
View File

@@ -40,7 +40,7 @@ public class TalonFXSwerve extends SwerveMotor
/** /**
* TalonFX motor controller. * TalonFX motor controller.
*/ */
TalonFX motor; private final TalonFX motor;
/** /**
* Conversion factor for the motor. * Conversion factor for the motor.
*/ */

2
swervelib/motors/TalonSRXSwerve.java
View File

@@ -33,7 +33,7 @@ public class TalonSRXSwerve extends SwerveMotor
/** /**
* TalonSRX motor controller. * 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. * The position conversion factor to convert raw sensor units to Meters Per 100ms, or Ticks to Degrees.
*/ */

2
swervelib/parser/json/ModuleJson.java
View File

@@ -1,6 +1,5 @@
package swervelib.parser.json; package swervelib.parser.json;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax;
import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.MotorFeedbackSensor;
import edu.wpi.first.math.util.Units; import edu.wpi.first.math.util.Units;
@@ -12,7 +11,6 @@ import swervelib.parser.SwerveModulePhysicalCharacteristics;
import swervelib.parser.json.modules.BoolMotorJson; import swervelib.parser.json.modules.BoolMotorJson;
import swervelib.parser.json.modules.ConversionFactorsJson; import swervelib.parser.json.modules.ConversionFactorsJson;
import swervelib.parser.json.modules.LocationJson; import swervelib.parser.json.modules.LocationJson;
import swervelib.telemetry.Alert;
/** /**
* {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data. * {@link swervelib.SwerveModule} JSON parsed class. Used to access the JSON data.