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;
/**
* 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 class SwerveDrive
/**
* 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 @@ public class SwerveDrive
}
/**
* 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 class SwerveDrive
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 class SwerveDrive
}
/**
* 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 class SwerveDrive
*/
public void setChassisDiscretization(boolean enable, double dtSeconds)
{
if(!SwerveDriveTelemetry.isSimulation)
if (!SwerveDriveTelemetry.isSimulation)
{
chassisVelocityCorrection = enable;
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
* {@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 class SwerveDrive
}
/**
* 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 class SwerveDrive
/**
* 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 class SwerveDrive
* @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);
}

21
swervelib/SwerveModule.java
View File

@@ -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 class SwerveModule
}
/**
* 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 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
*/
public void setEncoderAutoSynchronize(boolean enabled)
@@ -386,7 +390,8 @@ public class SwerveModule
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);

27
swervelib/imu/CanandgyroSwerve.java
View File

@@ -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 class CanandgyroSwerve extends SwerveIMU
}
/**
* Reset IMU to factory default.
* Reset {@link Canandgyro} to factory default.
*/
@Override
public void factoryDefault()
@@ -53,7 +48,7 @@ public class CanandgyroSwerve extends SwerveIMU
}
/**
* Clear sticky faults on IMU.
* Clear sticky faults on {@link Canandgyro}.
*/
@Override
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.
*/

71
swervelib/imu/IMUVelocity.java
View File

@@ -8,48 +8,48 @@ import swervelib.math.IMULinearMovingAverageFilter;
/**
* 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 class IMUVelocity {
}
/**
* 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 @@ public class IMUVelocity {
/**
* 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 @@ public class IMUVelocity {
}
/**
* 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;
}

13
swervelib/imu/NavXSwerve.java
View File

@@ -11,7 +11,7 @@ import java.util.Optional;
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 class NavXSwerve extends SwerveIMU
}
/**
* Constructor for the NavX swerve.
* Constructor for the NavX({@link AHRS}) swerve.
*
* @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.
*/
@@ -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
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.
*/

34
swervelib/imu/Pigeon2Swerve.java
View File

@@ -10,7 +10,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
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 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)
{
@@ -60,7 +60,7 @@ public class Pigeon2Swerve extends SwerveIMU
}
/**
* Reset IMU to factory default.
* Reset {@link Pigeon2} to factory default.
*/
@Override
public void factoryDefault()
@@ -72,7 +72,7 @@ public class Pigeon2Swerve extends SwerveIMU
}
/**
* Clear sticky faults on IMU.
* Clear sticky faults on {@link Pigeon2}.
*/
@Override
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.
*/

18
swervelib/imu/PigeonSwerve.java
View File

@@ -8,28 +8,28 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
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 class PigeonSwerve extends SwerveIMU
}
/**
* Get the instantiated IMU object.
* Get the instantiated {@link WPI_PigeonIMU} 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;
/**
* 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;
}
}

11
swervelib/motors/SparkFlexSwerve.java
View File

@@ -12,6 +12,7 @@ import com.revrobotics.REVLibError;
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 @@ public class SparkFlexSwerve extends SwerveMotor
{
return;
}
Timer.delay(0.01);
}
failureConfiguring.set(true);
}
@@ -329,7 +331,10 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
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.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 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
return;
}
Timer.delay(0.01);
}
failureConfiguringAlert.set(true);
}
@@ -351,7 +353,10 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
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.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<Double> velocity;
private Supplier<Double> velocity;
/**
* Supplier for the position of the motor controller.
*/
private Supplier<Double> position;
private Supplier<Double> position;
/**
* Initialize the swerve motor.
@@ -100,6 +101,7 @@ public class SparkMaxSwerve extends SwerveMotor
{
return;
}
Timer.delay(0.01);
}
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);
}
// 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 class SparkMaxSwerve extends SwerveMotor
@Override
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;
private final TalonFX 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.
*/
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.
*/

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

@@ -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.SwerveModulePhysicalCharacteristics;
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.