+
void
+
set(double percentOutput)
+
Set the percentage output.
-
-
-
+
+
+
Set the absolute encoder to be a compatible absolute encoder.
-
void
-
-
+
void
+
+
Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
voltage compensation.
-
void
-
-
+
void
+
+
Set the motor to be inverted.
-
void
-
-
+
void
+
+
Set the maximum rate the open/closed loop output can change by.
-
void
-
-
+
void
+
+
-
void
-
-
+
void
+
+
Set the integrated encoder position.
-
void
-
-
-
Set the closed loop PID controller reference point.
-
void
-
setReference(double setpoint,
- double feedforward,
- double position)
+
Set the closed loop PID controller reference point.
void
-
+
setReference(double setpoint,
+ double feedforward,
+ double position)
+
Set the closed loop PID controller reference point.
+
+
void
+
+
Set the voltage compensation for the swerve module motor.
@@ -382,6 +387,17 @@ loadScripts(document, 'script');
Method Details
-
+
+
+-
setVoltageCompensation
public void setVoltageCompensation(double nominalVoltage)
diff --git a/docs/swervelib/motors/SwerveMotor.html b/docs/swervelib/motors/SwerveMotor.html
index d4b91d4..97af3e4 100644
--- a/docs/swervelib/motors/SwerveMotor.html
+++ b/docs/swervelib/motors/SwerveMotor.html
@@ -1,7 +1,7 @@
-
+
SwerveMotor
@@ -100,6 +100,11 @@ loadScripts(document, 'script');
Whether the swerve motor is a drive motor.
+final int
+
+
+
The maximum amount of times the swerve motor will attempt to configure a motor if failures occur.
+
@@ -250,6 +255,21 @@ loadScripts(document, 'script');
Field Details
-
+
+
maximumRetries
+public final int maximumRetries
+The maximum amount of times the swerve motor will attempt to configure a motor if failures occur.
+
+- See Also:
+-
+
+
+
+
+
+-
isDriveMotor
protected boolean isDriveMotor
diff --git a/docs/swervelib/motors/TalonFXSwerve.html b/docs/swervelib/motors/TalonFXSwerve.html
index d7fe916..7dccd31 100644
--- a/docs/swervelib/motors/TalonFXSwerve.html
+++ b/docs/swervelib/motors/TalonFXSwerve.html
@@ -1,7 +1,7 @@
-
+
TalonFXSwerve
@@ -131,7 +131,7 @@ loadScripts(document, 'script');
+
isDriveMotor, maximumRetries
diff --git a/docs/swervelib/motors/TalonSRXSwerve.html b/docs/swervelib/motors/TalonSRXSwerve.html
index 62cfcfe..f14e2ea 100644
--- a/docs/swervelib/motors/TalonSRXSwerve.html
+++ b/docs/swervelib/motors/TalonSRXSwerve.html
@@ -1,7 +1,7 @@
-
+
TalonSRXSwerve
@@ -131,7 +131,7 @@ loadScripts(document, 'script');
+
isDriveMotor, maximumRetries
diff --git a/docs/swervelib/motors/package-summary.html b/docs/swervelib/motors/package-summary.html
index db083c3..8da41b9 100644
--- a/docs/swervelib/motors/package-summary.html
+++ b/docs/swervelib/motors/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.motors
diff --git a/docs/swervelib/motors/package-tree.html b/docs/swervelib/motors/package-tree.html
index 8c74c7c..d4fba61 100644
--- a/docs/swervelib/motors/package-tree.html
+++ b/docs/swervelib/motors/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.motors Class Hierarchy
diff --git a/docs/swervelib/package-summary.html b/docs/swervelib/package-summary.html
index 760bd4e..15ffa5f 100644
--- a/docs/swervelib/package-summary.html
+++ b/docs/swervelib/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib
diff --git a/docs/swervelib/package-tree.html b/docs/swervelib/package-tree.html
index 5f00661..f952505 100644
--- a/docs/swervelib/package-tree.html
+++ b/docs/swervelib/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib Class Hierarchy
diff --git a/docs/swervelib/parser/PIDFConfig.html b/docs/swervelib/parser/PIDFConfig.html
index dd69ded..3f59b5a 100644
--- a/docs/swervelib/parser/PIDFConfig.html
+++ b/docs/swervelib/parser/PIDFConfig.html
@@ -1,7 +1,7 @@
-
+
PIDFConfig
diff --git a/docs/swervelib/parser/SwerveControllerConfiguration.html b/docs/swervelib/parser/SwerveControllerConfiguration.html
index 3b48b3c..0895f21 100644
--- a/docs/swervelib/parser/SwerveControllerConfiguration.html
+++ b/docs/swervelib/parser/SwerveControllerConfiguration.html
@@ -1,7 +1,7 @@
-
+
SwerveControllerConfiguration
diff --git a/docs/swervelib/parser/SwerveDriveConfiguration.html b/docs/swervelib/parser/SwerveDriveConfiguration.html
index 23e189f..fc5cf4b 100644
--- a/docs/swervelib/parser/SwerveDriveConfiguration.html
+++ b/docs/swervelib/parser/SwerveDriveConfiguration.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveConfiguration
diff --git a/docs/swervelib/parser/SwerveModuleConfiguration.html b/docs/swervelib/parser/SwerveModuleConfiguration.html
index 2f6718c..382cf58 100644
--- a/docs/swervelib/parser/SwerveModuleConfiguration.html
+++ b/docs/swervelib/parser/SwerveModuleConfiguration.html
@@ -1,7 +1,7 @@
-
+
SwerveModuleConfiguration
diff --git a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
index c942361..e209cd2 100644
--- a/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
+++ b/docs/swervelib/parser/SwerveModulePhysicalCharacteristics.html
@@ -1,7 +1,7 @@
-
+
SwerveModulePhysicalCharacteristics
diff --git a/docs/swervelib/parser/SwerveParser.html b/docs/swervelib/parser/SwerveParser.html
index 113c4fb..9fa30dd 100644
--- a/docs/swervelib/parser/SwerveParser.html
+++ b/docs/swervelib/parser/SwerveParser.html
@@ -1,7 +1,7 @@
-
+
SwerveParser
diff --git a/docs/swervelib/parser/deserializer/PIDFRange.html b/docs/swervelib/parser/deserializer/PIDFRange.html
index 74e0cb5..73b4f14 100644
--- a/docs/swervelib/parser/deserializer/PIDFRange.html
+++ b/docs/swervelib/parser/deserializer/PIDFRange.html
@@ -1,7 +1,7 @@
-
+
PIDFRange
diff --git a/docs/swervelib/parser/deserializer/package-summary.html b/docs/swervelib/parser/deserializer/package-summary.html
index 404ebcc..c4c8538 100644
--- a/docs/swervelib/parser/deserializer/package-summary.html
+++ b/docs/swervelib/parser/deserializer/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.deserializer
diff --git a/docs/swervelib/parser/deserializer/package-tree.html b/docs/swervelib/parser/deserializer/package-tree.html
index e657e95..d4864d9 100644
--- a/docs/swervelib/parser/deserializer/package-tree.html
+++ b/docs/swervelib/parser/deserializer/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.deserializer Class Hierarchy
diff --git a/docs/swervelib/parser/json/ControllerPropertiesJson.html b/docs/swervelib/parser/json/ControllerPropertiesJson.html
index 8614ff0..34e03e6 100644
--- a/docs/swervelib/parser/json/ControllerPropertiesJson.html
+++ b/docs/swervelib/parser/json/ControllerPropertiesJson.html
@@ -1,7 +1,7 @@
-
+
ControllerPropertiesJson
diff --git a/docs/swervelib/parser/json/DeviceJson.html b/docs/swervelib/parser/json/DeviceJson.html
index 21d1f7d..cb8f720 100644
--- a/docs/swervelib/parser/json/DeviceJson.html
+++ b/docs/swervelib/parser/json/DeviceJson.html
@@ -1,7 +1,7 @@
-
+
DeviceJson
diff --git a/docs/swervelib/parser/json/ModuleJson.html b/docs/swervelib/parser/json/ModuleJson.html
index 87375ab..4512cdf 100644
--- a/docs/swervelib/parser/json/ModuleJson.html
+++ b/docs/swervelib/parser/json/ModuleJson.html
@@ -1,7 +1,7 @@
-
+
ModuleJson
diff --git a/docs/swervelib/parser/json/MotorConfigDouble.html b/docs/swervelib/parser/json/MotorConfigDouble.html
index 90f6976..82cd63b 100644
--- a/docs/swervelib/parser/json/MotorConfigDouble.html
+++ b/docs/swervelib/parser/json/MotorConfigDouble.html
@@ -1,7 +1,7 @@
-
+
MotorConfigDouble
diff --git a/docs/swervelib/parser/json/MotorConfigInt.html b/docs/swervelib/parser/json/MotorConfigInt.html
index 4f1a44f..0314931 100644
--- a/docs/swervelib/parser/json/MotorConfigInt.html
+++ b/docs/swervelib/parser/json/MotorConfigInt.html
@@ -1,7 +1,7 @@
-
+
MotorConfigInt
diff --git a/docs/swervelib/parser/json/PIDFPropertiesJson.html b/docs/swervelib/parser/json/PIDFPropertiesJson.html
index 3b0ba95..c8c0a32 100644
--- a/docs/swervelib/parser/json/PIDFPropertiesJson.html
+++ b/docs/swervelib/parser/json/PIDFPropertiesJson.html
@@ -1,7 +1,7 @@
-
+
PIDFPropertiesJson
diff --git a/docs/swervelib/parser/json/PhysicalPropertiesJson.html b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
index ffc751a..3375b5b 100644
--- a/docs/swervelib/parser/json/PhysicalPropertiesJson.html
+++ b/docs/swervelib/parser/json/PhysicalPropertiesJson.html
@@ -1,7 +1,7 @@
-
+
PhysicalPropertiesJson
diff --git a/docs/swervelib/parser/json/SwerveDriveJson.html b/docs/swervelib/parser/json/SwerveDriveJson.html
index 0e00034..c7c3a47 100644
--- a/docs/swervelib/parser/json/SwerveDriveJson.html
+++ b/docs/swervelib/parser/json/SwerveDriveJson.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveJson
diff --git a/docs/swervelib/parser/json/modules/BoolMotorJson.html b/docs/swervelib/parser/json/modules/BoolMotorJson.html
index 4c5f2e5..59fa361 100644
--- a/docs/swervelib/parser/json/modules/BoolMotorJson.html
+++ b/docs/swervelib/parser/json/modules/BoolMotorJson.html
@@ -1,7 +1,7 @@
-
+
BoolMotorJson
diff --git a/docs/swervelib/parser/json/modules/LocationJson.html b/docs/swervelib/parser/json/modules/LocationJson.html
index 0bb30c3..9f5a230 100644
--- a/docs/swervelib/parser/json/modules/LocationJson.html
+++ b/docs/swervelib/parser/json/modules/LocationJson.html
@@ -1,7 +1,7 @@
-
+
LocationJson
diff --git a/docs/swervelib/parser/json/modules/package-summary.html b/docs/swervelib/parser/json/modules/package-summary.html
index 039ff0e..cbaa118 100644
--- a/docs/swervelib/parser/json/modules/package-summary.html
+++ b/docs/swervelib/parser/json/modules/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json.modules
diff --git a/docs/swervelib/parser/json/modules/package-tree.html b/docs/swervelib/parser/json/modules/package-tree.html
index b0074a0..5b8621f 100644
--- a/docs/swervelib/parser/json/modules/package-tree.html
+++ b/docs/swervelib/parser/json/modules/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json.modules Class Hierarchy
diff --git a/docs/swervelib/parser/json/package-summary.html b/docs/swervelib/parser/json/package-summary.html
index 3586aeb..29de3f0 100644
--- a/docs/swervelib/parser/json/package-summary.html
+++ b/docs/swervelib/parser/json/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json
diff --git a/docs/swervelib/parser/json/package-tree.html b/docs/swervelib/parser/json/package-tree.html
index ae8086a..e949b22 100644
--- a/docs/swervelib/parser/json/package-tree.html
+++ b/docs/swervelib/parser/json/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser.json Class Hierarchy
diff --git a/docs/swervelib/parser/package-summary.html b/docs/swervelib/parser/package-summary.html
index 9d06773..b852749 100644
--- a/docs/swervelib/parser/package-summary.html
+++ b/docs/swervelib/parser/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser
diff --git a/docs/swervelib/parser/package-tree.html b/docs/swervelib/parser/package-tree.html
index fcf3bee..88acea6 100644
--- a/docs/swervelib/parser/package-tree.html
+++ b/docs/swervelib/parser/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.parser Class Hierarchy
diff --git a/docs/swervelib/simulation/SwerveIMUSimulation.html b/docs/swervelib/simulation/SwerveIMUSimulation.html
index 0a3868f..7b27bee 100644
--- a/docs/swervelib/simulation/SwerveIMUSimulation.html
+++ b/docs/swervelib/simulation/SwerveIMUSimulation.html
@@ -1,7 +1,7 @@
-
+
SwerveIMUSimulation
diff --git a/docs/swervelib/simulation/SwerveModuleSimulation.html b/docs/swervelib/simulation/SwerveModuleSimulation.html
index 411398a..5e5a4d0 100644
--- a/docs/swervelib/simulation/SwerveModuleSimulation.html
+++ b/docs/swervelib/simulation/SwerveModuleSimulation.html
@@ -1,7 +1,7 @@
-
+
SwerveModuleSimulation
diff --git a/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html b/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
index bdb929e..8416871 100644
--- a/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
+++ b/docs/swervelib/simulation/ctre/PhysicsSim.SimProfile.html
@@ -1,7 +1,7 @@
-
+
PhysicsSim.SimProfile
diff --git a/docs/swervelib/simulation/ctre/PhysicsSim.html b/docs/swervelib/simulation/ctre/PhysicsSim.html
index 4ec4f0d..6af2064 100644
--- a/docs/swervelib/simulation/ctre/PhysicsSim.html
+++ b/docs/swervelib/simulation/ctre/PhysicsSim.html
@@ -1,7 +1,7 @@
-
+
PhysicsSim
diff --git a/docs/swervelib/simulation/ctre/TalonFXSimProfile.html b/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
index b1f401f..e5ec4dc 100644
--- a/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/TalonFXSimProfile.html
@@ -1,7 +1,7 @@
-
+
TalonFXSimProfile
diff --git a/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html b/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
index af3f4a1..b38119e 100644
--- a/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/TalonSRXSimProfile.html
@@ -1,7 +1,7 @@
-
+
TalonSRXSimProfile
diff --git a/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html b/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
index 119d8da..049413e 100644
--- a/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
+++ b/docs/swervelib/simulation/ctre/VictorSPXSimProfile.html
@@ -1,7 +1,7 @@
-
+
VictorSPXSimProfile
diff --git a/docs/swervelib/simulation/ctre/package-summary.html b/docs/swervelib/simulation/ctre/package-summary.html
index ea113ae..942f44e 100644
--- a/docs/swervelib/simulation/ctre/package-summary.html
+++ b/docs/swervelib/simulation/ctre/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation.ctre
diff --git a/docs/swervelib/simulation/ctre/package-tree.html b/docs/swervelib/simulation/ctre/package-tree.html
index 8459df1..c097493 100644
--- a/docs/swervelib/simulation/ctre/package-tree.html
+++ b/docs/swervelib/simulation/ctre/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation.ctre Class Hierarchy
diff --git a/docs/swervelib/simulation/package-summary.html b/docs/swervelib/simulation/package-summary.html
index a52ef83..194972b 100644
--- a/docs/swervelib/simulation/package-summary.html
+++ b/docs/swervelib/simulation/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation
diff --git a/docs/swervelib/simulation/package-tree.html b/docs/swervelib/simulation/package-tree.html
index 0d0bb46..4cdae6e 100644
--- a/docs/swervelib/simulation/package-tree.html
+++ b/docs/swervelib/simulation/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.simulation Class Hierarchy
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
index 0f40282..9e69b83 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.TelemetryVerbosity.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveTelemetry.TelemetryVerbosity
diff --git a/docs/swervelib/telemetry/SwerveDriveTelemetry.html b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
index 9adecac..164f740 100644
--- a/docs/swervelib/telemetry/SwerveDriveTelemetry.html
+++ b/docs/swervelib/telemetry/SwerveDriveTelemetry.html
@@ -1,7 +1,7 @@
-
+
SwerveDriveTelemetry
diff --git a/docs/swervelib/telemetry/package-summary.html b/docs/swervelib/telemetry/package-summary.html
index df7cdd3..83509d4 100644
--- a/docs/swervelib/telemetry/package-summary.html
+++ b/docs/swervelib/telemetry/package-summary.html
@@ -1,7 +1,7 @@
-
+
swervelib.telemetry
diff --git a/docs/swervelib/telemetry/package-tree.html b/docs/swervelib/telemetry/package-tree.html
index 4c64838..40e4a55 100644
--- a/docs/swervelib/telemetry/package-tree.html
+++ b/docs/swervelib/telemetry/package-tree.html
@@ -1,7 +1,7 @@
-
+
swervelib.telemetry Class Hierarchy
diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java
index b764128..2f9f681 100644
--- a/swervelib/SwerveDrive.java
+++ b/swervelib/SwerveDrive.java
@@ -28,6 +28,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.ArrayList;
import java.util.List;
import java.util.Optional;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveMath;
import swervelib.parser.SwerveControllerConfiguration;
@@ -58,6 +60,14 @@ public class SwerveDrive
* Swerve modules.
*/
private final SwerveModule[] swerveModules;
+ /**
+ * WPILib {@link Notifier} to keep odometry up to date.
+ */
+ private final Notifier odometryThread;
+ /**
+ * Odometry lock to ensure thread safety.
+ */
+ private final Lock odometryLock = new ReentrantLock();
/**
* Field object.
*/
@@ -101,10 +111,6 @@ public class SwerveDrive
* The last heading set in radians.
*/
private double lastHeadingRadians = 0;
- /**
- * WPILib {@link Notifier} to keep odometry up to date.
- */
- private final Notifier odometryThread;
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -720,6 +726,7 @@ public class SwerveDrive
*/
public void updateOdometry()
{
+ odometryLock.lock();
// Update odometry
swerveDrivePoseEstimator.update(getYaw(), getModulePositions());
@@ -780,6 +787,7 @@ public class SwerveDrive
{
SwerveDriveTelemetry.updateData();
}
+ odometryLock.unlock();
}
/**
diff --git a/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java
index b5abf4f..176d005 100644
--- a/swervelib/encoders/CANCoderSwerve.java
+++ b/swervelib/encoders/CANCoderSwerve.java
@@ -102,9 +102,8 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
// 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
- ErrorCode code = encoder.getLastError();
- int ATTEMPTS = 3;
- for (int i = 0; i < ATTEMPTS; i++)
+ ErrorCode code = encoder.getLastError();
+ for (int i = 0; i < maximumRetries; i++)
{
if (code == ErrorCode.OK)
{
diff --git a/swervelib/encoders/SparkMaxEncoderSwerve.java b/swervelib/encoders/SparkMaxEncoderSwerve.java
index 0e95953..a55d9e9 100644
--- a/swervelib/encoders/SparkMaxEncoderSwerve.java
+++ b/swervelib/encoders/SparkMaxEncoderSwerve.java
@@ -2,7 +2,10 @@ package swervelib.encoders;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.CANSparkMax;
+import com.revrobotics.REVLibError;
import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.function.Supplier;
import swervelib.motors.SwerveMotor;
/**
@@ -27,14 +30,31 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
if (motor.getMotor() instanceof CANSparkMax)
{
encoder = ((CANSparkMax) motor.getMotor()).getAbsoluteEncoder(Type.kDutyCycle);
- encoder.setVelocityConversionFactor(conversionFactor);
- encoder.setPositionConversionFactor(conversionFactor);
+ configureSparkMax(() -> encoder.setVelocityConversionFactor(conversionFactor));
+ configureSparkMax(() -> encoder.setPositionConversionFactor(conversionFactor));
} else
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
}
}
+ /**
+ * Run the configuration until it succeeds or times out.
+ *
+ * @param config Lambda supplier returning the error state.
+ */
+ private void configureSparkMax(Supplier
config)
+ {
+ for (int i = 0; i < maximumRetries; i++)
+ {
+ if (config.get() == REVLibError.kOk)
+ {
+ return;
+ }
+ }
+ DriverStation.reportWarning("Failure configuring encoder", true);
+ }
+
/**
* Reset the encoder to factory defaults.
*/
diff --git a/swervelib/encoders/SwerveAbsoluteEncoder.java b/swervelib/encoders/SwerveAbsoluteEncoder.java
index bec2bf0..38a82ac 100644
--- a/swervelib/encoders/SwerveAbsoluteEncoder.java
+++ b/swervelib/encoders/SwerveAbsoluteEncoder.java
@@ -6,10 +6,14 @@ package swervelib.encoders;
public abstract class SwerveAbsoluteEncoder
{
+ /**
+ * The maximum amount of times the swerve encoder will attempt to configure itself if failures occur.
+ */
+ public final int maximumRetries = 5;
/**
* Last angle reading was faulty.
*/
- public boolean readingError = false;
+ public boolean readingError = false;
/**
* Reset the encoder to factory defaults.
diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
index fd03677..bab8719 100644
--- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java
+++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java
@@ -6,13 +6,14 @@ import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
+import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxAlternateEncoder;
import com.revrobotics.SparkMaxPIDController;
import com.revrobotics.SparkMaxRelativeEncoder.Type;
import edu.wpi.first.wpilibj.DriverStation;
+import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
-import swervelib.motors.SparkMaxSwerve.SparkMAX_slotIdx;
import swervelib.parser.PIDFConfig;
/**
@@ -85,10 +86,10 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
motor.getEncoder(encoderType, countsPerRevolution);
// Configure feedback of the PID controller as the integrated encoder.
- pid.setFeedbackDevice(encoder);
+ configureSparkMax(() -> pid.setFeedbackDevice(encoder));
}
- motor.setCANTimeout(0); // Spin off configurations in a different thread.
+ configureSparkMax(() -> motor.setCANTimeout(0)); // Spin off configurations in a different thread.
}
/**
@@ -107,6 +108,23 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
useDataPortQuadEncoder);
}
+ /**
+ * Run the configuration until it succeeds or times out.
+ *
+ * @param config Lambda supplier returning the error state.
+ */
+ private void configureSparkMax(Supplier config)
+ {
+ for (int i = 0; i < maximumRetries; i++)
+ {
+ if (config.get() == REVLibError.kOk)
+ {
+ return;
+ }
+ }
+ DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
+ }
+
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -115,7 +133,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setVoltageCompensation(double nominalVoltage)
{
- motor.enableVoltageCompensation(nominalVoltage);
+ configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage));
}
/**
@@ -127,7 +145,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setCurrentLimit(int currentLimit)
{
- motor.setSmartCurrentLimit(currentLimit);
+ configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit));
}
/**
@@ -138,8 +156,8 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setLoopRampRate(double rampRate)
{
- motor.setOpenLoopRampRate(rampRate);
- motor.setClosedLoopRampRate(rampRate);
+ configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate));
+ configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate));
}
/**
@@ -172,7 +190,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
if (!factoryDefaultOccurred)
{
- motor.restoreFactoryDefaults();
+ configureSparkMax(motor::restoreFactoryDefaults);
factoryDefaultOccurred = true;
}
}
@@ -183,7 +201,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void clearStickyFaults()
{
- motor.clearFaults();
+ configureSparkMax(motor::clearFaults);
}
/**
@@ -198,7 +216,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder();
- pid.setFeedbackDevice(absoluteEncoder);
+ configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder));
}
if (absoluteEncoder == null && this.encoder == null)
{
@@ -218,16 +236,16 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
if (absoluteEncoder == null)
{
- encoder.setPositionConversionFactor(positionConversionFactor);
- encoder.setVelocityConversionFactor(positionConversionFactor / 60);
+ configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
+ configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
configureCANStatusFrames(10, 20, 20, 500, 500);
} else
{
- absoluteEncoder.setPositionConversionFactor(positionConversionFactor);
- absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60);
+ configureSparkMax(() -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor));
+ configureSparkMax(() -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60));
}
}
@@ -239,14 +257,16 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void configurePIDF(PIDFConfig config)
{
- int pidSlot =
- isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
- pid.setP(config.p, pidSlot);
- pid.setI(config.i, pidSlot);
- pid.setD(config.d, pidSlot);
- pid.setFF(config.f, pidSlot);
- pid.setIZone(config.iz, pidSlot);
- pid.setOutputRange(config.output.min, config.output.max, pidSlot);
+// int pidSlot =
+// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ int pidSlot = 0;
+
+ configureSparkMax(() -> pid.setP(config.p, pidSlot));
+ configureSparkMax(() -> pid.setI(config.i, pidSlot));
+ configureSparkMax(() -> pid.setD(config.d, pidSlot));
+ configureSparkMax(() -> pid.setFF(config.f, pidSlot));
+ configureSparkMax(() -> pid.setIZone(config.iz, pidSlot));
+ configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
}
/**
@@ -258,9 +278,9 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
- pid.setPositionPIDWrappingEnabled(true);
- pid.setPositionPIDWrappingMinInput(minInput);
- pid.setPositionPIDWrappingMaxInput(maxInput);
+ configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true));
+ configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput));
+ configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
}
/**
@@ -275,11 +295,11 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
public void configureCANStatusFrames(
int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
{
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0);
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1);
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2);
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3);
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4);
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
// TODO: Configure Status Frame 5 and 6 if necessary
// https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
@@ -292,7 +312,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setMotorBrake(boolean isBrakeMode)
{
- motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
+ configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
}
/**
@@ -312,7 +332,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void burnFlash()
{
- motor.burnFlash();
+ configureSparkMax(() -> motor.burnFlash());
}
/**
@@ -335,13 +355,16 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setReference(double setpoint, double feedforward)
{
- int pidSlot =
- isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
- pid.setReference(
- setpoint,
- isDriveMotor ? ControlType.kVelocity : ControlType.kPosition,
- pidSlot,
- feedforward);
+// int pidSlot =
+// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ int pidSlot = 0;
+ configureSparkMax(() ->
+ pid.setReference(
+ setpoint,
+ isDriveMotor ? ControlType.kVelocity : ControlType.kPosition,
+ pidSlot,
+ feedforward)
+ );
}
/**
@@ -389,7 +412,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
if (absoluteEncoder == null)
{
- encoder.setPosition(position);
+ configureSparkMax(() -> encoder.setPosition(position));
}
}
}
diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java
index fadc733..0df0b4c 100644
--- a/swervelib/motors/SparkMaxSwerve.java
+++ b/swervelib/motors/SparkMaxSwerve.java
@@ -6,8 +6,11 @@ import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
+import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxPIDController;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
@@ -56,7 +59,8 @@ public class SparkMaxSwerve extends SwerveMotor
pid.setFeedbackDevice(
encoder); // Configure feedback of the PID controller as the integrated encoder.
- motor.setCANTimeout(0); // Spin off configurations in a different thread.
+ // Spin off configurations in a different thread.
+ configureSparkMax(() -> motor.setCANTimeout(0));
}
/**
@@ -70,6 +74,23 @@ public class SparkMaxSwerve extends SwerveMotor
this(new CANSparkMax(id, MotorType.kBrushless), isDriveMotor);
}
+ /**
+ * Run the configuration until it succeeds or times out.
+ *
+ * @param config Lambda supplier returning the error state.
+ */
+ private void configureSparkMax(Supplier config)
+ {
+ for (int i = 0; i < maximumRetries; i++)
+ {
+ if (config.get() == REVLibError.kOk)
+ {
+ return;
+ }
+ }
+ DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
+ }
+
/**
* Set the voltage compensation for the swerve module motor.
*
@@ -78,7 +99,7 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setVoltageCompensation(double nominalVoltage)
{
- motor.enableVoltageCompensation(nominalVoltage);
+ configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage));
}
/**
@@ -90,7 +111,7 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setCurrentLimit(int currentLimit)
{
- motor.setSmartCurrentLimit(currentLimit);
+ configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit));
}
/**
@@ -101,8 +122,8 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setLoopRampRate(double rampRate)
{
- motor.setOpenLoopRampRate(rampRate);
- motor.setClosedLoopRampRate(rampRate);
+ configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate));
+ configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate));
}
/**
@@ -135,7 +156,7 @@ public class SparkMaxSwerve extends SwerveMotor
{
if (!factoryDefaultOccurred)
{
- motor.restoreFactoryDefaults();
+ configureSparkMax(motor::restoreFactoryDefaults);
factoryDefaultOccurred = true;
}
}
@@ -146,7 +167,7 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void clearStickyFaults()
{
- motor.clearFaults();
+ configureSparkMax(motor::clearFaults);
}
/**
@@ -161,7 +182,7 @@ public class SparkMaxSwerve extends SwerveMotor
if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder();
- pid.setFeedbackDevice(absoluteEncoder);
+ configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder));
}
return this;
}
@@ -176,16 +197,16 @@ public class SparkMaxSwerve extends SwerveMotor
{
if (absoluteEncoder == null)
{
- encoder.setPositionConversionFactor(positionConversionFactor);
- encoder.setVelocityConversionFactor(positionConversionFactor / 60);
+ configureSparkMax(() -> encoder.setPositionConversionFactor(positionConversionFactor));
+ configureSparkMax(() -> encoder.setVelocityConversionFactor(positionConversionFactor / 60));
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/CANSparkMaxUtil.java#L67
configureCANStatusFrames(10, 20, 20, 500, 500);
} else
{
- absoluteEncoder.setPositionConversionFactor(positionConversionFactor);
- absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60);
+ configureSparkMax(() -> absoluteEncoder.setPositionConversionFactor(positionConversionFactor));
+ configureSparkMax(() -> absoluteEncoder.setVelocityConversionFactor(positionConversionFactor / 60));
}
}
@@ -197,15 +218,15 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void configurePIDF(PIDFConfig config)
{
- int pidSlot =
- isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
- pidSlot = 0;
- pid.setP(config.p, pidSlot);
- pid.setI(config.i, pidSlot);
- pid.setD(config.d, pidSlot);
- pid.setFF(config.f, pidSlot);
- pid.setIZone(config.iz, pidSlot);
- pid.setOutputRange(config.output.min, config.output.max, pidSlot);
+// int pidSlot =
+// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ int pidSlot = 0;
+ configureSparkMax(() -> pid.setP(config.p, pidSlot));
+ configureSparkMax(() -> pid.setI(config.i, pidSlot));
+ configureSparkMax(() -> pid.setD(config.d, pidSlot));
+ configureSparkMax(() -> pid.setFF(config.f, pidSlot));
+ configureSparkMax(() -> pid.setIZone(config.iz, pidSlot));
+ configureSparkMax(() -> pid.setOutputRange(config.output.min, config.output.max, pidSlot));
}
/**
@@ -217,9 +238,9 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
- pid.setPositionPIDWrappingEnabled(true);
- pid.setPositionPIDWrappingMinInput(minInput);
- pid.setPositionPIDWrappingMaxInput(maxInput);
+ configureSparkMax(() -> pid.setPositionPIDWrappingEnabled(true));
+ configureSparkMax(() -> pid.setPositionPIDWrappingMinInput(minInput));
+ configureSparkMax(() -> pid.setPositionPIDWrappingMaxInput(maxInput));
}
/**
@@ -234,11 +255,11 @@ public class SparkMaxSwerve extends SwerveMotor
public void configureCANStatusFrames(
int CANStatus0, int CANStatus1, int CANStatus2, int CANStatus3, int CANStatus4)
{
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0);
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1);
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2);
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3);
- motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4);
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus0, CANStatus0));
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus1, CANStatus1));
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, CANStatus2));
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, CANStatus3));
+ configureSparkMax(() -> motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, CANStatus4));
// TODO: Configure Status Frame 5 and 6 if necessary
// https://docs.revrobotics.com/sparkmax/operating-modes/control-interfaces
}
@@ -251,7 +272,7 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setMotorBrake(boolean isBrakeMode)
{
- motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
+ configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
}
/**
@@ -277,7 +298,7 @@ public class SparkMaxSwerve extends SwerveMotor
} catch (Exception e)
{
}
- motor.burnFlash();
+ configureSparkMax(() -> motor.burnFlash());
}
/**
@@ -301,24 +322,26 @@ public class SparkMaxSwerve extends SwerveMotor
public void setReference(double setpoint, double feedforward)
{
boolean possibleBurnOutIssue = true;
- int pidSlot =
- isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
- pidSlot = 0;
+// int pidSlot =
+// isDriveMotor ? SparkMAX_slotIdx.Velocity.ordinal() : SparkMAX_slotIdx.Position.ordinal();
+ int pidSlot = 0;
if (isDriveMotor)
{
- pid.setReference(
- setpoint,
- ControlType.kVelocity,
- pidSlot,
- feedforward);
+ configureSparkMax(() ->
+ pid.setReference(
+ setpoint,
+ ControlType.kVelocity,
+ pidSlot,
+ feedforward));
} else
{
- pid.setReference(
- setpoint,
- ControlType.kPosition,
- pidSlot,
- feedforward);
+ configureSparkMax(() ->
+ pid.setReference(
+ setpoint,
+ ControlType.kPosition,
+ pidSlot,
+ feedforward));
}
}
@@ -367,7 +390,7 @@ public class SparkMaxSwerve extends SwerveMotor
{
if (absoluteEncoder == null)
{
- encoder.setPosition(position);
+ configureSparkMax(() -> encoder.setPosition(position));
}
}
diff --git a/swervelib/motors/SwerveMotor.java b/swervelib/motors/SwerveMotor.java
index 415b79e..c7470eb 100644
--- a/swervelib/motors/SwerveMotor.java
+++ b/swervelib/motors/SwerveMotor.java
@@ -9,6 +9,10 @@ import swervelib.parser.PIDFConfig;
public abstract class SwerveMotor
{
+ /**
+ * The maximum amount of times the swerve motor will attempt to configure a motor if failures occur.
+ */
+ public final int maximumRetries = 5;
/**
* Whether the swerve motor is a drive motor.
*/