(SwerveDrive swerveDrive,
+
+powerAngleMotorsDutyCycle
+public static void powerAngleMotorsDutyCycle (SwerveDrive swerveDrive,
double percentage) Power the angle motors for the swerve drive to a set percentage.
@@ -299,6 +447,161 @@ loadScripts(document, 'script');
+
+
+createConfigCustomTimeout
+public static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config createConfigCustomTimeout (double timeout)
+Creates a SysIdRoutine.Config with a custom final timeout
+
+Parameters:
+timeout - - the most a SysIdRoutine should runReturns:
+A custom SysIdRoutine.Config
+
+
+
+
+
+logDriveMotorDutyCycle
+public static void logDriveMotorDutyCycle (SwerveModule module,
+ edu.wpi.first.wpilibj.sysid.SysIdRoutineLog log) Logs output, position and velocuty info form the drive motor to the SysIdRoutineLog
+
+Parameters:
+module - - the swerve module being loggedlog - - the logger
+
+
+
+
+logDriveMotorVoltage
+public static void logDriveMotorVoltage (SwerveModule module,
+ edu.wpi.first.wpilibj.sysid.SysIdRoutineLog log) Logs voltage, position and velocuty info form the drive motor to the SysIdRoutineLog
+
+Parameters:
+module - - the swerve module being loggedlog - - the logger
+
+
+
+
+logDriveMotorActivity
+public static void logDriveMotorActivity (SwerveModule module,
+ edu.wpi.first.wpilibj.sysid.SysIdRoutineLog log,
+ Supplier <Double > powerSupplied) Logs power, position and velocuty info form the drive motor to the SysIdRoutineLog
+
+Parameters:
+module - - the swerve module being loggedlog - - the loggerpowerSupplied - - a functional supplier of the power to be logged
+
+
+
+
+setDriveSysIdRoutine
+public static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine setDriveSysIdRoutine (edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config config,
+ edu.wpi.first.wpilibj2.command.SubsystemBase swerveSubsystem,
+ SwerveDrive swerveDrive,
+ double maxVolts) Sets up the SysId runner and logger for the drive motors
+
+Parameters:
+config - - The SysIdRoutine.Config to useswerveSubsystem - - the subsystem to add to requirementsswerveDrive - - the SwerveDrive from which to access motor infomaxVolts - - The maximum voltage that should be applied to the drive motors.Returns:
+A SysIdRoutine runner
+
+
+
+
+
+logAngularMotorDutyCycle
+public static void logAngularMotorDutyCycle (SwerveModule module,
+ edu.wpi.first.wpilibj.sysid.SysIdRoutineLog log) Logs info about the angle motor to the SysIdRoutineLog.
+
+Parameters:
+module - - the swerve module being loggedlog - - the logger
+
+
+
+
+logAngularMotorVoltage
+public static void logAngularMotorVoltage (SwerveModule module,
+ edu.wpi.first.wpilibj.sysid.SysIdRoutineLog log) Logs info about the angle motor to the SysIdRoutineLog
+
+Parameters:
+module - - the swerve module being loggedlog - - the logger
+
+
+
+
+logAngularMotorActivity
+public static void logAngularMotorActivity (SwerveModule module,
+ edu.wpi.first.wpilibj.sysid.SysIdRoutineLog log,
+ Supplier <Double > powerSupplied) Logs info about the angle motor to the SysIdRoutineLog
+
+Parameters:
+module - - the swerve module being loggedlog - - the loggerpowerSupplied - - a functional supplier of the power to be logged
+
+
+
+
+setAngleSysIdRoutine
+public static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine setAngleSysIdRoutine (edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config config,
+ edu.wpi.first.wpilibj2.command.SubsystemBase swerveSubsystem,
+ SwerveDrive swerveDrive) Sets up the SysId runner and logger for the angle motors
+
+Parameters:
+config - - The SysIdRoutine.Config to useswerveSubsystem - - the subsystem to add to requirementsswerveDrive - - the SwerveDrive from which to access motor infoReturns:
+A SysIdRoutineRunner
+
+
+
+
+
+generateSysIdCommand
+public static edu.wpi.first.wpilibj2.command.Command generateSysIdCommand (edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine sysIdRoutine,
+ double delay,
+ double quasiTimeout,
+ double dynamicTimeout)
+Creates a command that can be mapped to a button or other trigger. Delays can be set to customize the length of
+ each part of the SysId Routine
+
+Parameters:
+sysIdRoutine - - The Sys Id routine runnerdelay - - seconds between each portion to allow motors to spin down, etc...quasiTimeout - - seconds to run the Quasistatic routines, so robot doesn't get too fardynamicTimeout - - seconds to run the Dynamic routines, 2-3 secs should be enoughReturns:
+A command that can be mapped to a button or other trigger
+
+
+
diff --git a/docs/swervelib/SwerveModule.html b/docs/swervelib/SwerveModule.html
index 5ae5925..3e66e42 100644
--- a/docs/swervelib/SwerveModule.html
+++ b/docs/swervelib/SwerveModule.html
@@ -1,11 +1,11 @@
-
+
SwerveModule
Absolute encoder for swerve drive.
-
-
+
+
-private double
-
-
-
Angle offset from the absolute encoder.
-
-
Swerve module configuration options.
+
Absolute encoder position cache.
private double
+
-
An
Alert for if pushing the Absolute Encoder offset to the encoder fails.
+
Angle offset from the absolute encoder.
edu.wpi.first.math.controller.SimpleMotorFeedforward
-
+
+
-
Feedforward for drive motor during closed loop control.
+
Swerve module configuration options.
private edu.wpi.first.math.kinematics.SwerveModuleState
-
+
+
-
Last swerve module state applied.
+
Swerve Motors.
double
-
+
+
-
Maximum speed of the drive motors in meters per second.
+
Drive motor position cache.
int
-
+
+
-
Module number for kinematics, usually 0 to 3.
+
Drive motor velocity cache.
+
An
Alert for if pushing the Absolute Encoder offset to the encoder fails.
+
edu.wpi.first.math.controller.SimpleMotorFeedforward
+
+
+
Feedforward for drive motor during closed loop control.
+
private edu.wpi.first.math.kinematics.SwerveModuleState
+
+
+
Last swerve module state applied.
+
double
+
+
+
Maximum speed of the drive motors in meters per second.
+
int
+
+
+
Module number for kinematics, usually 0 to 3.
+
An
Alert for if there is no Absolute Encoder on the module.
-
-
-
+
+
+
-
private boolean
-
-
+
private boolean
+
+
Encoder synchronization queued.
@@ -228,70 +243,75 @@ loadScripts(document, 'script');
Get the position of the swerve module.
double
-
+
+
Get the absolute position.
+
double
+
+
Get the relative angle in degrees.
-edu.wpi.first.math.kinematics.SwerveModuleState
-
-
+
edu.wpi.first.math.kinematics.SwerveModuleState
+
+
Get the Swerve Module state.
-
void
-
-
+
void
+
+
Push absolute encoder offset in the memory of the encoder or controller.
-
void
-
-
+
void
+
+
Queue synchronization of the integrated angle encoder with the absolute encoder.
-
void
-
-
+
void
+
+
Restore internal offset in YAGSL and either sets absolute encoder offset to 0 or restores old value.
-
void
-
-
+
void
+
+
Set the angle for the module.
-
void
-
-
+
void
+
+
Set the conversion factor for the angle/azimuth motor controller.
-
void
-
-
+
void
+
+
Set the voltage compensation for the swerve module motor.
-
void
-
setDesiredState (edu.wpi.first.math.kinematics.SwerveModuleState desiredState,
+void
+setDesiredState (edu.wpi.first.math.kinematics.SwerveModuleState desiredState,
boolean isOpenLoop,
boolean force)
+
Set the desired state of the swerve module.
-
void
-
-
+
void
+
+
Set the conversion factor for the drive motor controller.
-
void
-
-
+
void
+
+
Set the voltage compensation for the swerve module motor.
-
void
-
-
+
void
+
+
-
void
-
-
+
void
+
+
Update data sent to SmartDashboard.
@@ -319,6 +339,27 @@ loadScripts(document, 'script');
+
+absolutePositionCache
+
+Absolute encoder position cache.
+
+
+
+
+drivePositionCache
+
+Drive motor position cache.
+
+
+
+
+driveVelocityCache
+
+Drive motor velocity cache.
+
+
+
angleMotor
@@ -538,6 +579,17 @@ loadScripts(document, 'script');
+
+getRawAbsolutePosition
+public double getRawAbsolutePosition ()
+Get the absolute position. Falls back to relative position on reading failure.
+
+Returns:
+Absolute encoder angle in degrees in the range [0, 360).
+
+
+
+
getRelativePosition
public double getRelativePosition ()
diff --git a/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html b/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html
index 8ce56ba..aa6a1f1 100644
--- a/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html
+++ b/docs/swervelib/encoders/AnalogAbsoluteEncoderSwerve.html
@@ -1,11 +1,11 @@
-
+
AnalogAbsoluteEncoderSwerve
CANCoderSwerve
An
Alert for if the CANCoder reading is faulty and the reading is ignored.
+private final double
+
+
+
Wait time for status frames to show up.
+
Fields inherited from class swervelib.encoders.SwerveAbsoluteEncoder
@@ -208,6 +213,21 @@ loadScripts(document, 'script');
Field Details
+
+STATUS_TIMEOUT_SECONDS
+private final double STATUS_TIMEOUT_SECONDS
+Wait time for status frames to show up.
+
+See Also:
+
+
+
+
+
+
+
encoder
public com.ctre.phoenix6.hardware.CANcoder encoder
diff --git a/docs/swervelib/encoders/CanAndCoderSwerve.html b/docs/swervelib/encoders/CanAndCoderSwerve.html
index 5231c18..2996cb7 100644
--- a/docs/swervelib/encoders/CanAndCoderSwerve.html
+++ b/docs/swervelib/encoders/CanAndCoderSwerve.html
@@ -1,11 +1,11 @@
-
+
CanAndCoderSwerve
PWMDutyCycleEncoderSwerve
SparkMaxAnalogEncoderSwerve
SparkMaxEncoderSwerve
SwerveAbsoluteEncoder
swervelib.encoders
swervelib.encoders Class Hierarchy
ADIS16448Swerve
ADIS16470Swerve
ADXRS450Swerve
AnalogGyroSwerve
NavXSwerve
Pigeon2Swerve
private final double
+
+
+
Wait time for status frames to show up.
+
@@ -200,6 +205,21 @@ loadScripts(document, 'script');
Field Details
+
+STATUS_TIMEOUT_SECONDS
+private final double STATUS_TIMEOUT_SECONDS
+Wait time for status frames to show up.
+
+See Also:
+
+
+
+
+
+
+
imu
com.ctre.phoenix6.hardware.Pigeon2 imu
diff --git a/docs/swervelib/imu/PigeonSwerve.html b/docs/swervelib/imu/PigeonSwerve.html
index d8da28b..1ee1b73 100644
--- a/docs/swervelib/imu/PigeonSwerve.html
+++ b/docs/swervelib/imu/PigeonSwerve.html
@@ -1,11 +1,11 @@
-
+
PigeonSwerve
SwerveIMU
swervelib.imu
swervelib.imu Class Hierarchy
Matter
SwerveMath
swervelib.math
swervelib.math Class Hierarchy
SparkFlexSwerve.SparkMAX_slotIdx
SparkFlexSwerve
SparkMaxBrushedMotorSwerve
SparkMaxSwerve.SparkMAX_slotIdx
SparkMaxSwerve
Closed-loop PID controller.
+
+
+
+
Supplier for the position of the motor controller.
+
+
Supplier for the velocity of the motor controller.
+
Fields inherited from class swervelib.motors.SwerveMotor
@@ -361,6 +371,20 @@ loadScripts(document, 'script');
Factory default already occurred.
+
+
+velocity
+
+Supplier for the velocity of the motor controller.
+
+
+
+
+position
+
+Supplier for the position of the motor controller.
+
+
diff --git a/docs/swervelib/motors/SwerveMotor.html b/docs/swervelib/motors/SwerveMotor.html
index 076a1b8..807e02e 100644
--- a/docs/swervelib/motors/SwerveMotor.html
+++ b/docs/swervelib/motors/SwerveMotor.html
@@ -1,11 +1,11 @@
-
+
SwerveMotor
-
+
diff --git a/docs/swervelib/motors/TalonFXSwerve.html b/docs/swervelib/motors/TalonFXSwerve.html
index 77817cf..376f5d7 100644
--- a/docs/swervelib/motors/TalonFXSwerve.html
+++ b/docs/swervelib/motors/TalonFXSwerve.html
@@ -1,11 +1,11 @@
-
+
TalonFXSwerve
-
+
@@ -128,6 +128,11 @@ loadScripts(document, 'script');
TalonFX motor controller.
+
private final double
+
+
+
Wait time for status frames to show up.
+
Fields inherited from class swervelib.motors.SwerveMotor
@@ -374,6 +379,21 @@ loadScripts(document, 'script');
+
+STATUS_TIMEOUT_SECONDS
+private final double STATUS_TIMEOUT_SECONDS
+Wait time for status frames to show up.
+
+See Also:
+
+
+
+
+
+
+
motor
com.ctre.phoenix6.hardware.TalonFX motor
diff --git a/docs/swervelib/motors/TalonSRXSwerve.html b/docs/swervelib/motors/TalonSRXSwerve.html
index faf9849..e8cdbfa 100644
--- a/docs/swervelib/motors/TalonSRXSwerve.html
+++ b/docs/swervelib/motors/TalonSRXSwerve.html
@@ -1,11 +1,11 @@
-
+
TalonSRXSwerve
swervelib.motors
swervelib.motors Class Hierarchy
swervelib
swervelib Class Hierarchy
Cache
+
+JavaScript is disabled on your browser.
+
+
+
+
+
+
+
+
+
+public class Cache<T>
+
extends Object Cache for frequently requested data.
+
+
+
+
+
+
+Field Summary
+Fields
+
+
+
+
+
+
+
+
Supplier for cached value.
+
+
private long
+
+
+
Timestamp in microseconds.
+
+
private long
+
+
+
Validity period in microseconds.
+
+
+
+
+
+
+
+
+
+Constructor Summary
+Constructors
+
+
+
+
+
+
Cache for arbitrary values.
+
+
+
+
+
+
+Method Summary
+
+
All Methods Instance Methods Concrete Methods
+
+
+
+
+
+
+
+
+
Get the most up to date cached value.
+
+
boolean
+
+
+
Return whether the cache is stale.
+
+
+
+
+
Update the cache value and timestamp.
+
+
+
+
+
Update the supplier to a new source.
+
+
+
+
+
Update the validity period for the cached value, also updates the value.
+
+
+
+
+
Methods inherited from class java.lang.Object
+
clone , equals , finalize , getClass , hashCode , notify , notifyAll , toString , wait , wait , wait
+
+
+
+
+
+
+
+
+Field Details
+
+
+
+value
+
+Cached value.
+
+
+
+
+supplier
+
+Supplier for cached value.
+
+
+
+
+timestamp
+private long timestamp
+Timestamp in microseconds.
+
+
+
+
+validityPeriod
+private long validityPeriod
+Validity period in microseconds.
+
+
+
+
+
+
+
+
+Constructor Details
+
+
+
+Cache
+public Cache (Supplier <T > val,
+ long validityPeriod) Cache for arbitrary values.
+
+Parameters:
+val - Value to cache.validityPeriod - Validity period in milliseconds.
+
+
+
+
+
+
+
+
+Method Details
+
+
+
+isStale
+public boolean isStale ()
+Return whether the cache is stale.
+
+Returns:
+The stale state of the cache.
+
+
+
+
+
+update
+
+Update the cache value and timestamp.
+
+Returns:
+Cache
+
+
+
+
+updateSupplier
+
+Update the supplier to a new source. Updates the value and timestamp as well.
+
+Parameters:
+supplier - new supplier source.Returns:
+Cache
+
+
+
+
+updateValidityPeriod
+public Cache <T >updateValidityPeriod (long validityPeriod) Update the validity period for the cached value, also updates the value.
+
+Parameters:
+validityPeriod - The new validity period in milliseconds.Returns:
+Cache
+
+
+
+
+getValue
+
+Get the most up to date cached value.
+
+Returns:
+Cache
+
+
+
+
+
+
+
+
+
+
+
PIDFConfig
SwerveControllerConfiguration
SwerveDriveConfiguration
SwerveModuleConfiguration
SwerveModulePhysicalCharacteristics
SwerveParser
PIDFRange
swervelib.parser.deserializer
swervelib.parser.deserializer Class Hierarchy
ControllerPropertiesJson
DeviceJson
ModuleJson
MotorConfigDouble
MotorConfigInt
PIDFPropertiesJson
PhysicalPropertiesJson
SwerveDriveJson
BoolMotorJson
LocationJson
swervelib.parser.json.modules
swervelib.parser.json.modules Class Hierarchy
swervelib.parser.json
swervelib.parser.json Class Hierarchy
swervelib.parser
-
+
+
Cache for frequently requested data.
+
+
+
Hold the PIDF and Integral Zone values for a PID.
-
-
+
+
Swerve Controller configuration class which is used to configure
SwerveController.
-
-
+
+
Swerve drive configurations used during SwerveDrive construction.
-
-
+
+
Swerve Module configuration class which is used to configure
SwerveModule.
-
-
+
+
Configuration class which stores physical characteristics shared between every swerve module.
-
-
+
+
Helper class used to parse the JSON directory with specified configuration options.
diff --git a/docs/swervelib/parser/package-tree.html b/docs/swervelib/parser/package-tree.html
index b67911c..5117e7a 100644
--- a/docs/swervelib/parser/package-tree.html
+++ b/docs/swervelib/parser/package-tree.html
@@ -1,11 +1,11 @@
-
+
swervelib.parser Class Hierarchy
-
+
@@ -59,6 +59,7 @@ loadScripts(document, 'script');
java.lang.Object
+swervelib.parser.Cache <T>
swervelib.parser.PIDFConfig
swervelib.parser.SwerveControllerConfiguration
swervelib.parser.SwerveDriveConfiguration
diff --git a/docs/swervelib/simulation/SwerveIMUSimulation.html b/docs/swervelib/simulation/SwerveIMUSimulation.html
index 3bc0164..f267bf3 100644
--- a/docs/swervelib/simulation/SwerveIMUSimulation.html
+++ b/docs/swervelib/simulation/SwerveIMUSimulation.html
@@ -1,11 +1,11 @@
-
+
SwerveIMUSimulation
SwerveModuleSimulation
swervelib.simulation
swervelib.simulation Class Hierarchy
Alert.AlertType
Alert.SendableAlerts
Alert
SwerveDriveTelemetry.TelemetryVerbosity
SwerveDriveTelemetry
swervelib.telemetry
swervelib.telemetry Class Hierarchy
imuReadingCache;
/**
* Swerve modules.
*/
@@ -155,10 +162,12 @@ public class SwerveDrive
if (SwerveDriveTelemetry.isSimulation)
{
simIMU = new SwerveIMUSimulation();
+ imuReadingCache = new Cache<>(simIMU::getGyroRotation3d, 5L);
} else
{
imu = config.imu;
imu.factoryDefault();
+ imuReadingCache = new Cache<>(imu::getRotation3d, 5L);
}
this.swerveModules = config.modules;
@@ -213,6 +222,24 @@ public class SwerveDrive
checkIfTunerXCompatible();
}
+ /**
+ * Update the cache validity period for the robot.
+ *
+ * @param imu IMU reading cache validity period in milliseconds.
+ * @param driveMotor Drive motor reading cache in milliseconds.
+ * @param absoluteEncoder Absolute encoder reading cache in milliseconds.
+ */
+ public void updateCacheValidityPeriods(long imu, long driveMotor, long absoluteEncoder)
+ {
+ imuReadingCache.updateValidityPeriod(imu);
+ for (SwerveModule module : swerveModules)
+ {
+ module.drivePositionCache.updateValidityPeriod(driveMotor);
+ module.driveVelocityCache.updateValidityPeriod(driveMotor);
+ module.absolutePositionCache.updateValidityPeriod(absoluteEncoder);
+ }
+ }
+
/**
* Check all components to ensure that Tuner X Swerve Generator is recommended instead.
*/
@@ -623,7 +650,7 @@ public class SwerveDrive
odometryLock.lock();
swerveDrivePoseEstimator.resetPosition(getYaw(), getModulePositions(), pose);
odometryLock.unlock();
- kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, pose.getRotation()));
+ kinematics.toSwerveModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(0, 0, 0, getYaw()));
}
/**
@@ -695,6 +722,7 @@ public class SwerveDrive
{
setGyroOffset(imu.getRawRotation3d().minus(gyro));
}
+ imuReadingCache.update();
}
/**
@@ -704,13 +732,14 @@ public class SwerveDrive
{
// Resets the real gyro or the angle accumulator, depending on whether the robot is being
// simulated
- if (!SwerveDriveTelemetry.isSimulation)
- {
- imu.setOffset(imu.getRawRotation3d());
- } else
+ if (SwerveDriveTelemetry.isSimulation)
{
simIMU.setAngle(0);
+ } else
+ {
+ setGyroOffset(imu.getRawRotation3d());
}
+ imuReadingCache.update();
swerveController.lastAngleScalar = 0;
lastHeadingRadians = 0;
resetOdometry(new Pose2d(getPose().getTranslation(), new Rotation2d()));
@@ -724,13 +753,7 @@ public class SwerveDrive
public Rotation2d getYaw()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
- if (!SwerveDriveTelemetry.isSimulation)
- {
- return Rotation2d.fromRadians(imu.getRotation3d().getZ());
- } else
- {
- return simIMU.getYaw();
- }
+ return Rotation2d.fromRadians(imuReadingCache.getValue().getZ());
}
/**
@@ -741,13 +764,7 @@ public class SwerveDrive
public Rotation2d getPitch()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
- if (!SwerveDriveTelemetry.isSimulation)
- {
- return Rotation2d.fromRadians(imu.getRotation3d().getY());
- } else
- {
- return simIMU.getPitch();
- }
+ return Rotation2d.fromRadians(imuReadingCache.getValue().getY());
}
/**
@@ -758,13 +775,7 @@ public class SwerveDrive
public Rotation2d getRoll()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
- if (!SwerveDriveTelemetry.isSimulation)
- {
- return Rotation2d.fromRadians(imu.getRotation3d().getX());
- } else
- {
- return simIMU.getRoll();
- }
+ return Rotation2d.fromRadians(imuReadingCache.getValue().getX());
}
/**
@@ -775,13 +786,7 @@ public class SwerveDrive
public Rotation3d getGyroRotation3d()
{
// Read the imu if the robot is real or the accumulator if the robot is simulated.
- if (!SwerveDriveTelemetry.isSimulation)
- {
- return imu.getRotation3d();
- } else
- {
- return simIMU.getGyroRotation3d();
- }
+ return imuReadingCache.getValue();
}
/**
@@ -1016,6 +1021,7 @@ public class SwerveDrive
{
imu.setOffset(offset);
}
+ imuReadingCache.update();
}
/**
@@ -1085,6 +1091,21 @@ public class SwerveDrive
return swerveDriveConfiguration.modules;
}
+ /**
+ * Get the {@link SwerveModule}'s as a {@link HashMap} where the key is the swerve module configuration name.
+ *
+ * @return {@link HashMap}(Module Name, SwerveModule)
+ */
+ public Map getModuleMap()
+ {
+ Map map = new HashMap();
+ for (SwerveModule module : swerveModules)
+ {
+ map.put(module.configuration.name, module);
+ }
+ return map;
+ }
+
/**
* Reset the drive encoders on the robot, useful when manually resetting the robot without a reboot, like in
* autonomous.
diff --git a/swervelib/SwerveDriveTest.java b/swervelib/SwerveDriveTest.java
index 37030a9..947601d 100644
--- a/swervelib/SwerveDriveTest.java
+++ b/swervelib/SwerveDriveTest.java
@@ -1,9 +1,31 @@
package swervelib;
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Degrees;
+import static edu.wpi.first.units.Units.DegreesPerSecond;
+import static edu.wpi.first.units.Units.Meters;
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+import static edu.wpi.first.units.Units.Volts;
+
import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Distance;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
+import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
/**
@@ -12,6 +34,27 @@ import swervelib.encoders.SwerveAbsoluteEncoder;
public class SwerveDriveTest
{
+ /**
+ * Tracks the voltage being applied to a motor
+ */
+ private static final MutableMeasure m_appliedVoltage = mutable(Volts.of(0));
+ /**
+ * Tracks the distance travelled of a position motor
+ */
+ private static final MutableMeasure m_distance = mutable(Meters.of(0));
+ /**
+ * Tracks the velocity of a positional motor
+ */
+ private static final MutableMeasure> m_velocity = mutable(MetersPerSecond.of(0));
+ /**
+ * Tracks the rotations of an angular motor
+ */
+ private static final MutableMeasure m_anglePosition = mutable(Degrees.of(0));
+ /**
+ * Tracks the velocity of an angular motor
+ */
+ private static final MutableMeasure> m_angVelocity = mutable(DegreesPerSecond.of(0));
+
/**
* Set the angle of the modules to a given {@link Rotation2d}
*
@@ -22,7 +65,7 @@ public class SwerveDriveTest
{
for (SwerveModule swerveModule : swerveDrive.getModules())
{
- swerveModule.setDesiredState(new SwerveModuleState(0, moduleAngle), false, true);
+ swerveModule.getAngleMotor().setReference(moduleAngle.getDegrees(), 0);
}
}
@@ -46,7 +89,7 @@ public class SwerveDriveTest
* @param swerveDrive {@link SwerveDrive} to control.
* @param percentage DutyCycle percentage to send to angle motors.
*/
- public static void powerAngleMotors(SwerveDrive swerveDrive, double percentage)
+ public static void powerAngleMotorsDutyCycle(SwerveDrive swerveDrive, double percentage)
{
for (SwerveModule swerveModule : swerveDrive.getModules())
{
@@ -195,4 +238,178 @@ public class SwerveDriveTest
DriverStation.reportWarning("Average Coupling Ratio: " + (couplingRatioSum / 4.0), false);
return (couplingRatioSum / 4.0);
}
+
+ /**
+ * Creates a SysIdRoutine.Config with a custom final timeout
+ *
+ * @param timeout - the most a SysIdRoutine should run
+ * @return A custom SysIdRoutine.Config
+ */
+ public static Config createConfigCustomTimeout(double timeout)
+ {
+ return new Config(null, null, Seconds.of(timeout));
+ }
+
+ /**
+ * Logs output, position and velocuty info form the drive motor to the SysIdRoutineLog powerSupplied)
+ {
+ double power = powerSupplied.get();
+ double distance = module.getPosition().distanceMeters;
+ double velocity = module.getDriveMotor().getVelocity();
+ SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Drive Power", power);
+ SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Drive Position", distance);
+ SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Drive Velocity", velocity);
+ log.motor("drive-" + module.configuration.name)
+ .voltage(m_appliedVoltage.mut_replace(power, Volts))
+ .linearPosition(m_distance.mut_replace(distance, Meters))
+ .linearVelocity(m_velocity.mut_replace(velocity, MetersPerSecond));
+ }
+
+ /**
+ * Sets up the SysId runner and logger for the drive motors
+ *
+ * @param config - The SysIdRoutine.Config to use
+ * @param swerveSubsystem - the subsystem to add to requirements
+ * @param swerveDrive - the SwerveDrive from which to access motor info
+ * @param maxVolts - The maximum voltage that should be applied to the drive motors.
+ * @return A SysIdRoutine runner
+ */
+ public static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swerveSubsystem,
+ SwerveDrive swerveDrive, double maxVolts)
+ {
+ return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
+ (Measure voltage) -> {
+ SwerveDriveTest.centerModules(swerveDrive);
+ SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
+ },
+ log -> {
+ for (SwerveModule module : swerveDrive.getModules())
+ {
+ logDriveMotorVoltage(module, log);
+ }
+ }, swerveSubsystem));
+ }
+
+ /**
+ * Logs info about the angle motor to the SysIdRoutineLog. powerSupplied)
+ {
+ double power = powerSupplied.get();
+ double angle = module.getAbsolutePosition();
+ double velocity = module.getAbsoluteEncoder().getVelocity();
+ SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Angle Power", power);
+ SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Angle Position", angle);
+ SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Absolute Encoder Velocity", velocity);
+ log.motor("angle-" + module.configuration.name)
+ .voltage(m_appliedVoltage.mut_replace(power, Volts))
+ .angularPosition(m_anglePosition.mut_replace(angle, Degrees))
+ .angularVelocity(m_angVelocity.mut_replace(velocity, DegreesPerSecond));
+ }
+
+ /**
+ * Sets up the SysId runner and logger for the angle motors
+ *
+ * @param config - The SysIdRoutine.Config to use
+ * @param swerveSubsystem - the subsystem to add to requirements
+ * @param swerveDrive - the SwerveDrive from which to access motor info
+ * @return A SysIdRoutineRunner
+ */
+ public static SysIdRoutine setAngleSysIdRoutine(Config config, SubsystemBase swerveSubsystem,
+ SwerveDrive swerveDrive)
+ {
+ return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
+ (Measure voltage) -> {
+ SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
+ SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
+ },
+ log -> {
+ for (SwerveModule module : swerveDrive.getModules())
+ {
+ logAngularMotorVoltage(module, log);
+ }
+ }, swerveSubsystem));
+ }
+
+ /**
+ * Creates a command that can be mapped to a button or other trigger. Delays can be set to customize the length of
+ * each part of the SysId Routine
+ *
+ * @param sysIdRoutine - The Sys Id routine runner
+ * @param delay - seconds between each portion to allow motors to spin down, etc...
+ * @param quasiTimeout - seconds to run the Quasistatic routines, so robot doesn't get too far
+ * @param dynamicTimeout - seconds to run the Dynamic routines, 2-3 secs should be enough
+ * @return A command that can be mapped to a button or other trigger
+ */
+ public static Command generateSysIdCommand(SysIdRoutine sysIdRoutine, double delay, double quasiTimeout,
+ double dynamicTimeout)
+ {
+ return sysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward).withTimeout(quasiTimeout)
+ .andThen(Commands.waitSeconds(delay))
+ .andThen(sysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse).withTimeout(quasiTimeout))
+ .andThen(Commands.waitSeconds(delay))
+ .andThen(sysIdRoutine.dynamic(SysIdRoutine.Direction.kForward).withTimeout(dynamicTimeout))
+ .andThen(Commands.waitSeconds(delay))
+ .andThen(sysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse).withTimeout(dynamicTimeout));
+ }
}
diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java
index fb488a9..b7b3c01 100644
--- a/swervelib/SwerveModule.java
+++ b/swervelib/SwerveModule.java
@@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.math.SwerveMath;
import swervelib.motors.SwerveMotor;
+import swervelib.parser.Cache;
import swervelib.parser.SwerveModuleConfiguration;
import swervelib.simulation.SwerveModuleSimulation;
import swervelib.telemetry.Alert;
@@ -25,6 +26,18 @@ public class SwerveModule
* Swerve module configuration options.
*/
public final SwerveModuleConfiguration configuration;
+ /**
+ * Absolute encoder position cache.
+ */
+ public final Cache absolutePositionCache;
+ /**
+ * Drive motor position cache.
+ */
+ public final Cache drivePositionCache;
+ /**
+ * Drive motor velocity cache.
+ */
+ public final Cache driveVelocityCache;
/**
* Swerve Motors.
*/
@@ -70,6 +83,7 @@ public class SwerveModule
*/
private boolean synchronizeEncoderQueued = false;
+
/**
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
*
@@ -114,6 +128,9 @@ public class SwerveModule
absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
}
+ // Setup the cache for the absolute encoder position.
+ absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 15);
+
// Config angle motor/controller
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle);
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
@@ -136,11 +153,20 @@ public class SwerveModule
driveMotor.burnFlash();
angleMotor.burnFlash();
+ drivePositionCache = new Cache<>(driveMotor::getPosition, 15);
+ driveVelocityCache = new Cache<>(driveMotor::getVelocity, 15);
+
if (SwerveDriveTelemetry.isSimulation)
{
simModule = new SwerveModuleSimulation();
}
+ // Force a cache update on init.
+ driveVelocityCache.update();
+ drivePositionCache.update();
+ absolutePositionCache.update();
+
+ // Save the current state.
lastState = getState();
noEncoderWarning = new Alert("Motors",
@@ -198,8 +224,9 @@ public class SwerveModule
{
desiredState = SwerveModuleState.optimize(desiredState, Rotation2d.fromDegrees(getAbsolutePosition()));
// Cosine compensation.
- double velocity = configuration.useCosineCompensator ? getCosineCompensatedVelocity(desiredState)
- : desiredState.speedMetersPerSecond;
+ double velocity = configuration.useCosineCompensator
+ ? getCosineCompensatedVelocity(desiredState)
+ : desiredState.speedMetersPerSecond;
if (isOpenLoop)
{
@@ -298,7 +325,7 @@ public class SwerveModule
Rotation2d azimuth;
if (!SwerveDriveTelemetry.isSimulation)
{
- velocity = driveMotor.getVelocity();
+ velocity = driveVelocityCache.getValue();
azimuth = Rotation2d.fromDegrees(getAbsolutePosition());
} else
{
@@ -318,7 +345,7 @@ public class SwerveModule
Rotation2d azimuth;
if (!SwerveDriveTelemetry.isSimulation)
{
- position = driveMotor.getPosition();
+ position = drivePositionCache.getValue();
azimuth = Rotation2d.fromDegrees(getAbsolutePosition());
} else
{
@@ -333,6 +360,16 @@ public class SwerveModule
* @return Absolute encoder angle in degrees in the range [0, 360).
*/
public double getAbsolutePosition()
+ {
+ return absolutePositionCache.getValue();
+ }
+
+ /**
+ * Get the absolute position. Falls back to relative position on reading failure.
+ *
+ * @return Absolute encoder angle in degrees in the range [0, 360).
+ */
+ public double getRawAbsolutePosition()
{
double angle;
if (absoluteEncoder != null)
diff --git a/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java
index 011560c..da6c465 100644
--- a/swervelib/encoders/CANCoderSwerve.java
+++ b/swervelib/encoders/CANCoderSwerve.java
@@ -17,26 +17,30 @@ import swervelib.telemetry.Alert;
public class CANCoderSwerve extends SwerveAbsoluteEncoder
{
+ /**
+ * Wait time for status frames to show up.
+ */
+ private final double STATUS_TIMEOUT_SECONDS = 0.02;
/**
* CANCoder with WPILib sendable and support.
*/
- public CANcoder encoder;
+ public CANcoder encoder;
/**
* An {@link Alert} for if the CANCoder magnet field is less than ideal.
*/
- private Alert magnetFieldLessThanIdeal;
+ private Alert magnetFieldLessThanIdeal;
/**
* An {@link Alert} for if the CANCoder reading is faulty.
*/
- private Alert readingFaulty;
+ private Alert readingFaulty;
/**
* An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored.
*/
- private Alert readingIgnored;
+ private Alert readingIgnored;
/**
* An {@link Alert} for if the absolute encoder offset cannot be set.
*/
- private Alert cannotSetOffset;
+ private Alert cannotSetOffset;
/**
* Initialize the CANCoder on the standard CANBus.
@@ -45,7 +49,8 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
*/
public CANCoderSwerve(int id)
{
- encoder = new CANcoder(id);
+ // Empty string uses the default canbus for the system
+ this(id, "");
}
/**
@@ -134,7 +139,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
readingFaulty.set(false);
}
- StatusSignal angle = encoder.getAbsolutePosition().refresh();
+ StatusSignal angle = encoder.getAbsolutePosition();
// Taken from democat's library.
// Source: https://github.com/democat3457/swerve-lib/blob/7c03126b8c22f23a501b2c2742f9d173a5bcbc40/src/main/java/com/swervedrivespecialties/swervelib/ctre/CanCoderFactoryBuilder.java#L51-L74
@@ -144,7 +149,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
{
break;
}
- angle = angle.waitForUpdate(0.01);
+ angle = angle.waitForUpdate(STATUS_TIMEOUT_SECONDS);
}
if (angle.getStatus() != StatusCode.OK)
{
diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java
index 1ff4e18..0a3e552 100644
--- a/swervelib/imu/Pigeon2Swerve.java
+++ b/swervelib/imu/Pigeon2Swerve.java
@@ -16,6 +16,10 @@ import java.util.Optional;
public class Pigeon2Swerve extends SwerveIMU
{
+ /**
+ * Wait time for status frames to show up.
+ */
+ private final double STATUS_TIMEOUT_SECONDS = 0.02;
/**
* Pigeon2 IMU device.
*/
@@ -23,11 +27,11 @@ 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;
/**
* Generate the SwerveIMU for pigeon.
@@ -106,10 +110,10 @@ public class Pigeon2Swerve extends SwerveIMU
StatusSignal x = imu.getQuatX();
StatusSignal y = imu.getQuatY();
StatusSignal z = imu.getQuatZ();
- Rotation3d reading = new Rotation3d(new Quaternion(w.refresh().getValue(),
- x.refresh().getValue(),
- y.refresh().getValue(),
- z.refresh().getValue()));
+ Rotation3d reading = new Rotation3d(new Quaternion(w.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+ x.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+ y.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+ z.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()));
return invertedIMU ? reading.unaryMinus() : reading;
}
@@ -138,9 +142,9 @@ public class Pigeon2Swerve extends SwerveIMU
StatusSignal yAcc = imu.getAccelerationY();
StatusSignal zAcc = imu.getAccelerationZ();
- return Optional.of(new Translation3d(xAcc.refresh().getValue(),
- yAcc.refresh().getValue(),
- zAcc.refresh().getValue()).times(9.81 / 16384.0));
+ return Optional.of(new Translation3d(xAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+ yAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue(),
+ zAcc.waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue()).times(9.81 / 16384.0));
}
/**
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index 766b798..aa19a0c 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -43,6 +43,14 @@ public class SparkMaxSwerve extends SwerveMotor
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
+ /**
+ * Supplier for the velocity of the motor controller.
+ */
+ private Supplier velocity;
+ /**
+ * Supplier for the position of the motor controller.
+ */
+ private Supplier position;
/**
* Initialize the swerve motor.
@@ -62,6 +70,8 @@ public class SparkMaxSwerve extends SwerveMotor
pid.setFeedbackDevice(
encoder); // Configure feedback of the PID controller as the integrated encoder.
+ velocity = encoder::getVelocity;
+ position = encoder::getPosition;
// Spin off configurations in a different thread.
// configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
}
@@ -190,6 +200,8 @@ public class SparkMaxSwerve extends SwerveMotor
false);
absoluteEncoder = encoder;
configureSparkMax(() -> pid.setFeedbackDevice((MotorFeedbackSensor) absoluteEncoder.getAbsoluteEncoder()));
+ velocity = absoluteEncoder::getVelocity;
+ position = absoluteEncoder::getAbsolutePosition;
}
return this;
}
@@ -433,7 +445,7 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public double getVelocity()
{
- return absoluteEncoder == null ? encoder.getVelocity() : absoluteEncoder.getVelocity();
+ return velocity.get();
}
/**
@@ -444,7 +456,7 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public double getPosition()
{
- return absoluteEncoder == null ? encoder.getPosition() : absoluteEncoder.getAbsolutePosition();
+ return position.get();
}
/**
diff --git a/swervelib/motors/TalonFXSwerve.java b/swervelib/motors/TalonFXSwerve.java
index d6959ce..9c44c72 100644
--- a/swervelib/motors/TalonFXSwerve.java
+++ b/swervelib/motors/TalonFXSwerve.java
@@ -33,6 +33,10 @@ public class TalonFXSwerve extends SwerveMotor
* Velocity voltage setter for controlling drive motor.
*/
private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
+ /**
+ * Wait time for status frames to show up.
+ */
+ private final double STATUS_TIMEOUT_SECONDS = 0.02;
/**
* TalonFX motor controller.
*/
@@ -40,11 +44,12 @@ public class TalonFXSwerve extends SwerveMotor
/**
* Conversion factor for the motor.
*/
- private double conversionFactor;
+ private double conversionFactor;
/**
* Current TalonFX configuration.
*/
- private TalonFXConfiguration configuration = new TalonFXConfiguration();
+ private TalonFXConfiguration configuration = new TalonFXConfiguration();
+
/**
* Constructor for TalonFX swerve motor.
@@ -351,7 +356,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public double getVoltage()
{
- return motor.getMotorVoltage().refresh().getValue();
+ return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue();
}
/**
@@ -373,7 +378,7 @@ public class TalonFXSwerve extends SwerveMotor
@Override
public double getAppliedOutput()
{
- return motor.getDutyCycle().refresh().getValue();
+ return motor.getDutyCycle().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue();
}
/**
diff --git a/swervelib/parser/Cache.java b/swervelib/parser/Cache.java
new file mode 100644
index 0000000..830a08c
--- /dev/null
+++ b/swervelib/parser/Cache.java
@@ -0,0 +1,105 @@
+package swervelib.parser;
+
+import edu.wpi.first.wpilibj.RobotController;
+import java.util.function.Supplier;
+
+/**
+ * Cache for frequently requested data.
+ */
+public class Cache
+{
+
+ /**
+ * Cached value.
+ */
+ private T value;
+ /**
+ * Supplier for cached value.
+ */
+ private Supplier supplier;
+ /**
+ * Timestamp in microseconds.
+ */
+ private long timestamp;
+ /**
+ * Validity period in microseconds.
+ */
+ private long validityPeriod;
+
+ /**
+ * Cache for arbitrary values.
+ *
+ * @param val Value to cache.
+ * @param validityPeriod Validity period in milliseconds.
+ */
+ public Cache(Supplier val, long validityPeriod)
+ {
+ supplier = val;
+ value = supplier.get();
+ timestamp = RobotController.getFPGATime();
+ this.validityPeriod = validityPeriod * 1000L;
+ }
+
+ /**
+ * Return whether the cache is stale.
+ *
+ * @return The stale state of the cache.
+ */
+ public boolean isStale()
+ {
+ return (RobotController.getFPGATime() - timestamp) > validityPeriod;
+ }
+
+ /**
+ * Update the cache value and timestamp.
+ *
+ * @return {@link Cache} used.
+ */
+ public Cache update()
+ {
+ this.value = supplier.get();
+ this.timestamp = RobotController.getFPGATime();
+ return this;
+ }
+
+ /**
+ * Update the supplier to a new source. Updates the value and timestamp as well.
+ *
+ * @param supplier new supplier source.
+ * @return {@link Cache} for chaining.
+ */
+ public Cache updateSupplier(Supplier supplier)
+ {
+ this.supplier = supplier;
+ update();
+ return this;
+ }
+
+ /**
+ * Update the validity period for the cached value, also updates the value.
+ *
+ * @param validityPeriod The new validity period in milliseconds.
+ * @return {@link Cache} for chaining.
+ */
+ public Cache updateValidityPeriod(long validityPeriod)
+ {
+ this.validityPeriod = validityPeriod * 1000L;
+ update();
+ return this;
+ }
+
+ /**
+ * Get the most up to date cached value.
+ *
+ * @return {@link T} updated to the latest cached version.
+ */
+ public T getValue()
+ {
+ if (isStale())
+ {
+ update();
+ }
+ return value;
+ }
+
+}