Skip to content

Commit

Permalink
Added lock for thread safety, repetitively configure spark max's unti…
Browse files Browse the repository at this point in the history 
…l success
  • Loading branch information
@thenetworkgrinch
thenetworkgrinch committed Aug 30, 2023
1 parent 3cdec3f commit 8711ec2
Show file tree
Hide file tree
Showing 32 changed files with 186 additions and 105 deletions.
10 changes: 8 additions & 2 deletions src/main/java/swervelib/SwerveDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@
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;
Expand All @@ -47,6 +49,10 @@ public class SwerveDrive {
public final SwerveDrivePoseEstimator swerveDrivePoseEstimator;
/** 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. */
public Field2d field = new Field2d();
/** Swerve controller for controlling heading of the robot. */
Expand Down Expand Up @@ -76,8 +82,6 @@ public class SwerveDrive {
private int moduleSynchronizationCounter = 0;
/** 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}
Expand Down Expand Up @@ -644,6 +648,7 @@ public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward feedforward) {
* SmartDashboard with module encoder readings and states.
*/
public void updateOdometry() {
odometryLock.lock();
// Update odometry
swerveDrivePoseEstimator.update(getYaw(), getModulePositions());

Expand Down Expand Up @@ -697,6 +702,7 @@ public void updateOdometry() {
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) {
SwerveDriveTelemetry.updateData();
}
odometryLock.unlock();
}

/** Synchronize angle motor integrated encoders with data from absolute encoders. */
Expand Down
3 changes: 1 addition & 2 deletions src/main/java/swervelib/encoders/CANCoderSwerve.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,8 +90,7 @@ public double getAbsolutePosition() {
// 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++) {
for (int i = 0; i < maximumRetries; i++) {
if (code == ErrorCode.OK) {
break;
}
Expand Down
21 changes: 19 additions & 2 deletions src/main/java/swervelib/encoders/SparkMaxEncoderSwerve.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,10 @@

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;

/** SparkMax absolute encoder, attached through the data port. */
Expand All @@ -20,13 +23,27 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder {
public SparkMaxEncoderSwerve(SwerveMotor motor, int conversionFactor) {
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<REVLibError> 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. */
@Override
public void factoryDefault() {
Expand Down
5 changes: 5 additions & 0 deletions src/main/java/swervelib/encoders/SwerveAbsoluteEncoder.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,11 @@
*/
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;

Expand Down
105 changes: 65 additions & 40 deletions src/main/java/swervelib/motors/SparkMaxBrushedMotorSwerve.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,14 @@
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;

/** Brushed motor control with SparkMax. */
Expand Down Expand Up @@ -77,10 +78,11 @@ public SparkMaxBrushedMotorSwerve(
: 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.
}

/**
Expand Down Expand Up @@ -108,14 +110,28 @@ public SparkMaxBrushedMotorSwerve(
useDataPortQuadEncoder);
}

/**
* Run the configuration until it succeeds or times out.
*
* @param config Lambda supplier returning the error state.
*/
private void configureSparkMax(Supplier<REVLibError> 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.
*
* @param nominalVoltage Nominal voltage for operation to output to.
*/
@Override
public void setVoltageCompensation(double nominalVoltage) {
motor.enableVoltageCompensation(nominalVoltage);
configureSparkMax(() -> motor.enableVoltageCompensation(nominalVoltage));
}

/**
Expand All @@ -126,7 +142,7 @@ public void setVoltageCompensation(double nominalVoltage) {
*/
@Override
public void setCurrentLimit(int currentLimit) {
motor.setSmartCurrentLimit(currentLimit);
configureSparkMax(() -> motor.setSmartCurrentLimit(currentLimit));
}

/**
Expand All @@ -136,8 +152,8 @@ public void setCurrentLimit(int currentLimit) {
*/
@Override
public void setLoopRampRate(double rampRate) {
motor.setOpenLoopRampRate(rampRate);
motor.setClosedLoopRampRate(rampRate);
configureSparkMax(() -> motor.setOpenLoopRampRate(rampRate));
configureSparkMax(() -> motor.setClosedLoopRampRate(rampRate));
}

/**
Expand All @@ -164,15 +180,15 @@ public boolean isAttachedAbsoluteEncoder() {
@Override
public void factoryDefaults() {
if (!factoryDefaultOccurred) {
motor.restoreFactoryDefaults();
configureSparkMax(motor::restoreFactoryDefaults);
factoryDefaultOccurred = true;
}
}

/** Clear the sticky faults on the motor controller. */
@Override
public void clearStickyFaults() {
motor.clearFaults();
configureSparkMax(motor::clearFaults);
}

/**
Expand All @@ -185,7 +201,7 @@ public void clearStickyFaults() {
public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) {
if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder) {
absoluteEncoder = (AbsoluteEncoder) encoder.getAbsoluteEncoder();
pid.setFeedbackDevice(absoluteEncoder);
configureSparkMax(() -> pid.setFeedbackDevice(absoluteEncoder));
}
if (absoluteEncoder == null && this.encoder == null) {
DriverStation.reportError("An encoder MUST be defined to work with a SparkMAX", true);
Expand All @@ -203,15 +219,17 @@ public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder) {
@Override
public void configureIntegratedEncoder(double positionConversionFactor) {
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));
}
}

Expand All @@ -222,14 +240,17 @@ public void configureIntegratedEncoder(double positionConversionFactor) {
*/
@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));
}

/**
Expand All @@ -240,9 +261,9 @@ public void configurePIDF(PIDFConfig config) {
*/
@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));
}

/**
Expand All @@ -256,11 +277,11 @@ public void configurePIDWrapping(double minInput, double maxInput) {
*/
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
}
Expand All @@ -272,7 +293,7 @@ public void configureCANStatusFrames(
*/
@Override
public void setMotorBrake(boolean isBrakeMode) {
motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast);
configureSparkMax(() -> motor.setIdleMode(isBrakeMode ? IdleMode.kBrake : IdleMode.kCoast));
}

/**
Expand All @@ -288,7 +309,7 @@ public void setInverted(boolean inverted) {
/** Save the configurations from flash to EEPROM. */
@Override
public void burnFlash() {
motor.burnFlash();
configureSparkMax(() -> motor.burnFlash());
}

/**
Expand All @@ -309,13 +330,17 @@ public void set(double percentOutput) {
*/
@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));
}

/**
Expand Down Expand Up @@ -358,7 +383,7 @@ public double getPosition() {
@Override
public void setPosition(double position) {
if (absoluteEncoder == null) {
encoder.setPosition(position);
configureSparkMax(() -> encoder.setPosition(position));
}
}
}
Loading

0 comments on commit 8711ec2

Please sign in to comment.