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

Updating to 2024.6.0.0

Browse Source
This commit is contained in:
thenetworkgrinch
2024-10-14 13:22:11 -05:00
parent 689634ab69
commit 8afd8526e9
126 changed files with 1841 additions and 460 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
swervelib/SwerveDrive.java
View File

@@ -30,6 +30,7 @@ import java.util.Optional;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import swervelib.encoders.CANCoderSwerve;
import swervelib.imu.IMUVelocity;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveMath;
@@ -97,6 +98,24 @@ public class SwerveDrive
* correction.
*/
public boolean chassisVelocityCorrection = true;
/**
* Correct chassis velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} (auto) using 254's
* correction during auto.
*/
public boolean autonomousChassisVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
*/
public boolean angularVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)}
* during auto.
*/
public boolean autonomousAngularVelocityCorrection = false;
/**
* Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).
*/
public double angularVelocityCoefficient = 0;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
@@ -113,6 +132,11 @@ public class SwerveDrive
* Swerve IMU device for sensing the heading of the robot.
*/
private SwerveIMU imu;
/**
* Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}.
*/
private IMUVelocity imuVelocity;
/**
* Simulation of the swerve drive.
*/
@@ -479,12 +503,7 @@ public class SwerveDrive
public void drive(ChassisSpeeds velocity, boolean isOpenLoop, Translation2d centerOfRotationMeters)
{
// Thank you to Jared Russell FRC254 for Open Loop Compensation Code
// https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
if (chassisVelocityCorrection)
{
velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
}
velocity = movementOptimizations(velocity, chassisVelocityCorrection, angularVelocityCorrection);
// Heading Angular Velocity Deadband, might make a configuration option later.
// Originally made by Team 1466 Webb Robotics.
@@ -518,7 +537,7 @@ public class SwerveDrive
// Calculate required module states via kinematics
SwerveModuleState[] swerveModuleStates = kinematics.toSwerveModuleStates(velocity, centerOfRotationMeters);
setRawModuleStates(swerveModuleStates, isOpenLoop);
setRawModuleStates(swerveModuleStates, velocity, isOpenLoop);
}
/**
@@ -568,14 +587,15 @@ public class SwerveDrive
* Set the module states (azimuth and velocity) directly.
*
* @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.
*/
private void setRawModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds desiredChassisSpeed, boolean isOpenLoop)
{
// Desaturates wheel speeds
if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0)
{
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, getRobotVelocity(),
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed,
maxSpeedMPS,
attainableMaxTranslationalSpeedMetersPerSecond,
attainableMaxRotationalVelocityRadiansPerSecond);
@@ -593,14 +613,23 @@ 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)
*
* @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.
*/
public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
{
setRawModuleStates(kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates)),
isOpenLoop);
desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates));
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxSpeedMPS);
// Sets states
for (SwerveModule module : swerveModules)
{
module.setDesiredState(desiredStates[module.moduleNumber], isOpenLoop, false);
}
}
/**
@@ -610,11 +639,14 @@ public class SwerveDrive
*/
public void setChassisSpeeds(ChassisSpeeds chassisSpeeds)
{
chassisSpeeds = movementOptimizations(chassisSpeeds, autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection);
SwerveDriveTelemetry.desiredChassisSpeeds[1] = chassisSpeeds.vyMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[0] = chassisSpeeds.vxMetersPerSecond;
SwerveDriveTelemetry.desiredChassisSpeeds[2] = Math.toDegrees(chassisSpeeds.omegaRadiansPerSecond);
setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), false);
setRawModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds), chassisSpeeds, false);
}
/**
@@ -836,9 +868,23 @@ 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
* @param deadband Deadband in degrees, default is 3 degrees.
*/
public void setModuleEncoderAutoSynchronize(boolean enabled, double deadband)
{
for(SwerveModule swerveModule : swerveModules)
{
swerveModule.setEncoderAutoSynchronize(enabled, deadband);
}
}
/**
* Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the
* {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and
* {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], ChassisSpeeds, boolean)} function and
* {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides
* what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates.
*
@@ -866,7 +912,7 @@ public class SwerveDrive
/**
* Set the maximum speed of the drive motors, modified {@link SwerveDrive#maxSpeedMPS} which is used for the
* {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], boolean)} function and
* {@link SwerveDrive#setRawModuleStates(SwerveModuleState[], ChassisSpeeds, boolean)} function and
* {@link SwerveController#getTargetSpeeds(double, double, double, double, double)} functions. This function overrides
* what was placed in the JSON and could damage your motor/robot if set too high or unachievable rates. Overwrites the
* {@link SwerveModule#setFeedforward(SimpleMotorFeedforward)}.
@@ -1205,7 +1251,7 @@ public class SwerveDrive
}
/**
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop
*
* @param enable Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following.
@@ -1213,8 +1259,101 @@ public class SwerveDrive
*/
public void setChassisDiscretization(boolean enable, double dtSeconds)
{
chassisVelocityCorrection = enable;
discretizationdtSeconds = dtSeconds;
if(!SwerveDriveTelemetry.isSimulation)
{
chassisVelocityCorrection = enable;
discretizationdtSeconds = dtSeconds;
}
}
/**
* Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction in teleop and/or auto
*
* @param useInTeleop Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in teleop.
* @param useInAuto Enable chassis velocity correction, which will use
* {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following in auto.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
*/
public void setChassisDiscretization(boolean useInTeleop, boolean useInAuto, double dtSeconds)
{
if(!SwerveDriveTelemetry.isSimulation)
{
chassisVelocityCorrection = useInTeleop;
autonomousChassisVelocityCorrection = useInAuto;
discretizationdtSeconds = dtSeconds;
}
}
/**
* Enables angular velocity skew correction in 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.
*/
public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAuto, double angularVelocityCoeff)
{
if(!SwerveDriveTelemetry.isSimulation)
{
imuVelocity = IMUVelocity.createIMUVelocity(imu);
angularVelocityCorrection = useInTeleop;
autonomousAngularVelocityCorrection = useInAuto;
angularVelocityCoefficient = angularVelocityCoeff;
}
}
/**
* Correct for skew that worsens as angular velocity increases
*
* @param velocity The chassis speeds to set the robot to achieve.
* @return {@link ChassisSpeeds} of the robot after angular velocity skew correction.
*/
public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds velocity)
{
var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
if(angularVelocity.getRadians() != 0.0){
velocity = ChassisSpeeds.fromRobotRelativeSpeeds(
velocity.vxMetersPerSecond,
velocity.vyMetersPerSecond,
velocity.omegaRadiansPerSecond,
getOdometryHeading());
velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading().plus(angularVelocity));
}
return velocity;
}
/**
* Enable desired drive corrections
*
* @param velocity The chassis speeds to set the robot to achieve.
* @param uesChassisDiscretize Correct chassis velocity using 254's correction.
* @param useAngularVelocitySkewCorrection Use the robot's angular velocity to correct for skew.
* @return The chassis speeds after optimizations.
*/
private ChassisSpeeds movementOptimizations(ChassisSpeeds velocity, boolean uesChassisDiscretize, boolean useAngularVelocitySkewCorrection)
{
if(useAngularVelocitySkewCorrection)
{
velocity = angularVelocitySkewCorrection(velocity);
}
// Thank you to Jared Russell FRC254 for Open Loop Compensation Code
// https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5
if (uesChassisDiscretize)
{
velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds);
}
return velocity;
}
}

4
swervelib/SwerveDriveTest.java
View File

@@ -356,8 +356,8 @@ public class SwerveDriveTest
public static void logAngularMotorActivity(SwerveModule module, SysIdRoutineLog log, Supplier<Double> powerSupplied)
{
double power = powerSupplied.get();
double angle = module.getAbsolutePosition();
double velocity = module.getAbsoluteEncoder().getVelocity();
double angle = module.getAngleMotor().getPosition();
double velocity = module.getAngleMotor().getVelocity();
SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Power", power);
SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Position", angle);
SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity",

36
swervelib/SwerveModule.java
View File

@@ -112,6 +112,14 @@ public class SwerveModule
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
/**
* Encoder, Absolute encoder synchronization enabled.
*/
private boolean synchronizeEncoderEnabled = false;
/**
* Encoder synchronization deadband in degrees.
*/
private double synchronizeEncoderDeadband = 3;
/**
@@ -242,12 +250,32 @@ public class SwerveModule
*/
public void queueSynchronizeEncoders()
{
if (absoluteEncoder != null)
if (absoluteEncoder != null && synchronizeEncoderEnabled)
{
synchronizeEncoderQueued = true;
}
}
/**
* 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)
{
synchronizeEncoderEnabled = enabled;
synchronizeEncoderDeadband = deadband;
}
/**
* 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)
{
synchronizeEncoderEnabled = enabled;
}
/**
* Set the antiJitter functionality, if true the modules will NOT auto center. Pushes the offsets to the angle motor
* controllers as well.
@@ -355,10 +383,12 @@ public class SwerveModule
// Prevent module rotation if angle is the same as the previous angle.
// Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
if (absoluteEncoder != null && synchronizeEncoderQueued)
if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled)
{
double absoluteEncoderPosition = getAbsolutePosition();
angleMotor.setPosition(absoluteEncoderPosition);
if(Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband) {
angleMotor.setPosition(absoluteEncoderPosition);
}
angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
synchronizeEncoderQueued = false;
} else

141
swervelib/imu/IMUVelocity.java Normal file
View File

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

10
swervelib/imu/Pigeon2Swerve.java
View File

@@ -26,11 +26,15 @@ public class Pigeon2Swerve extends SwerveIMU
/**
* Offset for the Pigeon 2.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* Pigeon2 configurator.
*/
private Pigeon2Configurator cfg;
/**
* Generate the SwerveIMU for pigeon.
@@ -41,6 +45,7 @@ public class Pigeon2Swerve extends SwerveIMU
public Pigeon2Swerve(int canid, String canbus)
{
imu = new Pigeon2(canid, canbus);
this.cfg = imu.getConfigurator();
SmartDashboard.putData(imu);
}
@@ -60,7 +65,6 @@ public class Pigeon2Swerve extends SwerveIMU
@Override
public void factoryDefault()
{
Pigeon2Configurator cfg = imu.getConfigurator();
Pigeon2Configuration config = new Pigeon2Configuration();
// Compass utilization causes readings to jump dramatically in some cases.

54
swervelib/math/IMULinearMovingAverageFilter.java Normal file
View File

@@ -0,0 +1,54 @@
package swervelib.math;
import edu.wpi.first.util.DoubleCircularBuffer;
/**
* A linear filter that does not calculate() each time a value is added to
* the DoubleCircularBuffer.
*/
public class IMULinearMovingAverageFilter {
/**
* Circular buffer storing the current IMU readings
*/
private final DoubleCircularBuffer m_inputs;
/**
* Gain on each reading.
*/
private final double m_inputGain;
/**
* Construct a linear moving average fitler
* @param bufferLength The number of values to average across
*/
public IMULinearMovingAverageFilter(int bufferLength)
{
m_inputs = new DoubleCircularBuffer(bufferLength);
m_inputGain = 1.0 / bufferLength;
}
/**
* Add a value to the DoubleCircularBuffer
* @param input 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
*/
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;
}
}

2
swervelib/math/SwerveMath.java
View File

@@ -136,7 +136,7 @@ public class SwerveMath
public static double calculateMaxAngularVelocity(
double maxSpeed, double furthestModuleX, double furthestModuleY)
{
return maxSpeed / new Rotation2d(furthestModuleX, furthestModuleY).getRadians();
return maxSpeed / Math.hypot(furthestModuleX, furthestModuleY);
}
/**

26
swervelib/motors/SparkMaxSwerve.java
View File

@@ -224,12 +224,38 @@ public class SparkMaxSwerve extends SwerveMotor
{
configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
configureSparkMax(() -> encoder.setMeasurementPeriod(10));
configureSparkMax(() -> encoder.setAverageDepth(2));
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
configureCANStatusFrames(10, 20, 20, 500, 500, 200, 200);
} else
{
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
// This needs to be set to 20ms or under to properly update the swerve module position for odometry
// Configuration taken from 3005, the team who helped develop the Max Swerve:
// https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
// Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
// From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
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)
{
configureCANStatusFrames(100, 20, 20, 19, 200, 200, 200);
}
configureSparkMax(() -> {
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{

12
swervelib/motors/TalonFXSwerve.java
View File

@@ -49,6 +49,10 @@ public class TalonFXSwerve extends SwerveMotor
* Current TalonFX configuration.
*/
private TalonFXConfiguration configuration = new TalonFXConfiguration();
/**
* Current TalonFX Configurator.
*/
private TalonFXConfigurator cfg;
/**
@@ -61,6 +65,7 @@ public class TalonFXSwerve extends SwerveMotor
{
this.isDriveMotor = isDriveMotor;
this.motor = motor;
this.cfg = motor.getConfigurator();
factoryDefaults();
clearStickyFaults();
@@ -102,7 +107,6 @@ public class TalonFXSwerve extends SwerveMotor
{
if (!factoryDefaultOccurred)
{
TalonFXConfigurator cfg = motor.getConfigurator();
configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
configuration.ClosedLoopGeneral.ContinuousWrap = true;
cfg.apply(configuration);
@@ -156,7 +160,6 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration);
positionConversionFactor = 1 / positionConversionFactor;
@@ -246,7 +249,6 @@ public class TalonFXSwerve extends SwerveMotor
public void configurePIDF(PIDFConfig config)
{
TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration.Slot0);
cfg.apply(
configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f));
@@ -263,7 +265,6 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration.ClosedLoopGeneral);
configuration.ClosedLoopGeneral.ContinuousWrap = true;
cfg.apply(configuration.ClosedLoopGeneral);
@@ -414,7 +415,6 @@ public class TalonFXSwerve extends SwerveMotor
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
position = position < 0 ? (position % 360) + 360 : position;
TalonFXConfigurator cfg = motor.getConfigurator();
cfg.setPosition(position / 360);
}
}
@@ -439,7 +439,6 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setCurrentLimit(int currentLimit)
{
TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration.CurrentLimits);
cfg.apply(
configuration.CurrentLimits.withStatorCurrentLimit(currentLimit)
@@ -454,7 +453,6 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public void setLoopRampRate(double rampRate)
{
TalonFXConfigurator cfg = motor.getConfigurator();
cfg.refresh(configuration.ClosedLoopRamps);
cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate));
}

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

@@ -74,6 +74,8 @@ public class DeviceJson
return new SparkMaxEncoderSwerve(motor, 360);
case "sparkmax_analog":
return new SparkMaxAnalogEncoderSwerve(motor, 3.3);
case "sparkmax_analog5v":
return new SparkMaxAnalogEncoderSwerve(motor, 5);
case "canandcoder_can":
case "canandmag_can":
return new CanAndMagSwerve(id);

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

@@ -142,7 +142,7 @@ public class ModuleJson
// Backwards compatibility, auto-optimization.
if (conversionFactor.angle == 360 && absEncoder != null &&
absEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder && angleMotor.getMotor() instanceof CANSparkMax)
absEncoder.getAbsoluteEncoder() instanceof MotorFeedbackSensor && angleMotor.getMotor() instanceof CANSparkMax)
{
angleMotor.setAbsoluteEncoder(absEncoder);
}