Skip to content

Commit

Permalink
Merge branch 'spark-fused-encoder' into develop
Browse files Browse the repository at this point in the history
  • Loading branch information
@viggy96
viggy96 committed Sep 6, 2024
2 parents 8b552ae + 1125b53 commit b3cefe0
Show file tree
Hide file tree
Showing 3 changed files with 95 additions and 32 deletions.
18 changes: 10 additions & 8 deletions src/main/java/org/lasarobotics/drive/MAXSwerveModule.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,6 @@ private GearRatio(double value) {
public static final Measure<Time> DEFAULT_PERIOD = Units.Milliseconds.of(10.0);

private final double EPSILON = 5e-3;
private final double DRIVE_FF_SCALAR = 1.5;
private final Measure<Current> DRIVE_MOTOR_CURRENT_LIMIT;
private final Measure<Current> ROTATE_MOTOR_CURRENT_LIMIT = Units.Amps.of(18.0);
private final Rotation2d LOCK_POSITION = Rotation2d.fromRadians(Math.PI / 4);
Expand All @@ -112,16 +111,16 @@ private GearRatio(double value) {
private final int COSINE_CORRECTION;

// Swerve velocity PID settings
private static final double DRIVE_VELOCITY_kP = 0.18;
private static final double DRIVE_VELOCITY_kD = 0.001;
private static final double DRIVE_VELOCITY_kP = 0.4;
private static final double DRIVE_VELOCITY_kD = 0.002;
private static final double DRIVE_VELOCITY_kS = 0.2;
private static final double DRIVE_VELOCITY_kA = 0.5;
private static final double DRIVE_VELOCITY_TOLERANCE = 0.01;
private static final boolean DRIVE_VELOCITY_SENSOR_PHASE = false;
private static final boolean DRIVE_INVERT_MOTOR = false;

// Swerve rotate PID settings
private static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(2.1, 0.0, 0.2, 0.0, 0.0);
private static final PIDConstants DRIVE_ROTATE_PID = new PIDConstants(0.9, 0.0, 0.1, 0.0, 0.0);
private static final double DRIVE_ROTATE_kS = 0.2;
private static final double DRIVE_ROTATE_kA = 0.01;
private static final double DRIVE_ROTATE_TOLERANCE = 0.01;
Expand Down Expand Up @@ -206,8 +205,8 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat

// Set rotate encoder conversion factor
m_rotateConversionFactor = 2 * Math.PI;
m_rotateMotor.setPositionConversionFactor(Spark.FeedbackSensor.THROUGH_BORE_ENCODER, m_rotateConversionFactor);
m_rotateMotor.setVelocityConversionFactor(Spark.FeedbackSensor.THROUGH_BORE_ENCODER, m_rotateConversionFactor / 60);
m_rotateMotor.setPositionConversionFactor(Spark.FeedbackSensor.ABSOLUTE_ENCODER, m_rotateConversionFactor);
m_rotateMotor.setVelocityConversionFactor(Spark.FeedbackSensor.ABSOLUTE_ENCODER, m_rotateConversionFactor / 60);

// Enable PID wrapping
m_rotateMotor.enablePIDWrapping(0.0, m_rotateConversionFactor);
Expand All @@ -218,7 +217,7 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat
DRIVE_VELOCITY_kP,
0.0,
DRIVE_VELOCITY_kD,
(1 / ((m_driveMotor.getKind().getMaxRPM() / 60) * m_driveConversionFactor)) * DRIVE_FF_SCALAR,
1 / ((m_driveMotor.getKind().getMaxRPM() / 60) * m_driveConversionFactor),
0.0
),
DRIVE_VELOCITY_SENSOR_PHASE,
Expand All @@ -237,7 +236,10 @@ public MAXSwerveModule(Hardware swerveHardware, ModuleLocation location, GearRat

// Initialize PID
m_driveMotor.initializeSparkPID(m_driveMotorConfig, Spark.FeedbackSensor.NEO_ENCODER);
m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, Spark.FeedbackSensor.THROUGH_BORE_ENCODER);
m_rotateMotor.initializeSparkPID(m_rotateMotorConfig, Spark.FeedbackSensor.FUSED_ENCODER);

// Set gear ratio between motor and rotate encoder
m_rotateMotor.setMotorToSensorRatio(DRIVE_ROTATE_GEAR_RATIO);

// Set drive motor to coast
m_driveMotor.setIdleMode(IdleMode.kCoast);
Expand Down
14 changes: 5 additions & 9 deletions src/main/java/org/lasarobotics/drive/TractionControlController.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ private State(int value) {
private final double MAX_SLIP_RATIO = 0.40;
private final int SIGMOID_K = 10;
private final Measure<Velocity<Distance>> INERTIAL_VELOCITY_THRESHOLD = Units.MetersPerSecond.of(0.01);
private final Measure<Time> MIN_SLIPPING_TIME = Units.Seconds.of(1.1);
private final Measure<Time> MIN_SLIPPING_TIME = Units.Seconds.of(0.9);

private double m_optimalSlipRatio;
private double m_mass;
Expand Down Expand Up @@ -118,18 +118,14 @@ public Measure<Velocity<Distance>> calculate(Measure<Velocity<Distance>> velocit
& isEnabled()
);

// Get desired acceleration, capping at max possible acceleration
double requestedAcceleration = velocityRequest.minus(inertialVelocity).per(Units.Seconds.of(GlobalConstants.ROBOT_LOOP_PERIOD)).in(Units.MetersPerSecondPerSecond);
var desiredAcceleration = Units.MetersPerSecond.of(Math.copySign(
Math.min(Math.abs(requestedAcceleration), m_maxAcceleration / GlobalConstants.ROBOT_LOOP_HZ),
requestedAcceleration
)).per(Units.Seconds.of(GlobalConstants.ROBOT_LOOP_PERIOD));
// Get desired acceleration
var desiredAcceleration = velocityRequest.minus(inertialVelocity).per(Units.Seconds.of(GlobalConstants.ROBOT_LOOP_PERIOD));

// Get sigmoid value
double sigmoid = 1 / (1 + Math.exp(-SIGMOID_K * MathUtil.clamp(2 * (currentSlipRatio - m_optimalSlipRatio) - 1, -1.0, +1.0)));

// Scale CoF based on whether or not robot is currently slipping
double effectiveCoF = m_isSlipping ? m_staticCoF * (1 - sigmoid) + m_dynamicCoF * sigmoid : m_staticCoF;
double effectiveCoF = m_isSlipping ? m_staticCoF * (1 - sigmoid) + m_dynamicCoF * sigmoid : m_staticCoF;

// Simplified prediction of future slip ratio based on desired acceleration
double predictedSlipRatio = Math.abs(
Expand All @@ -138,7 +134,7 @@ & isEnabled()
+ effectiveCoF * m_mass * GlobalConstants.GRAVITATIONAL_ACCELERATION.in(Units.MetersPerSecondPerSecond))
) / m_maxPredictedSlipRatio;

// Calculate correction based on difference between optimal and weighted slip ratio, which combines the predicted and current slip ratios
// Calculate correction based on difference between optimal and predicted slip ratio, which combines the predicted and current slip ratios
var velocityCorrection = velocityOutput.times((m_optimalSlipRatio - predictedSlipRatio) * m_state.value);

// Update output, clamping to max linear speed
Expand Down
95 changes: 80 additions & 15 deletions src/main/java/org/lasarobotics/hardware/revrobotics/Spark.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,6 @@
import com.revrobotics.CANSparkFlex;
import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
import com.revrobotics.SparkPIDController.ArbFFUnits;
import com.revrobotics.CANSparkMax;
import com.revrobotics.MotorFeedbackSensor;
import com.revrobotics.REVLibError;
Expand All @@ -37,6 +36,7 @@
import com.revrobotics.SparkHelpers;
import com.revrobotics.SparkLimitSwitch;
import com.revrobotics.SparkPIDController;
import com.revrobotics.SparkPIDController.ArbFFUnits;
import com.revrobotics.SparkRelativeEncoder;

import edu.wpi.first.math.filter.Debouncer;
Expand Down Expand Up @@ -97,7 +97,7 @@ public double getMaxRPM() {

/** Feedback sensor */
public enum FeedbackSensor {
NEO_ENCODER, ANALOG, THROUGH_BORE_ENCODER;
NEO_ENCODER, ANALOG, ABSOLUTE_ENCODER, FUSED_ENCODER;
}

/** Spark output */
Expand Down Expand Up @@ -158,7 +158,8 @@ public static class SparkInputs {

private ID m_id;
private MotorKind m_kind;
private Notifier m_inputsThread;
private Notifier m_inputThread;
private Measure<Time> m_inputThreadPeriod;

private int m_errorCount;
private boolean m_isSmoothMotionEnabled;
Expand Down Expand Up @@ -199,12 +200,14 @@ public Spark(ID id, MotorKind kind, SparkLimitSwitch.Type limitSwitchType, Measu
this.m_kind = kind;
this.m_output = new SparkOutput(0.0, ControlType.kDutyCycle, 0.0, ArbFFUnits.kVoltage);
this.m_inputs = new SparkInputsAutoLogged();
this.m_inputsThread = new Notifier(this::updateInputs);
this.m_inputThread = new Notifier(this::updateInputs);
this.m_inputThreadPeriod = inputThreadPeriod;
this.m_isSmoothMotionEnabled = false;
this.m_errorCount = 0;
this.m_limitSwitchType = limitSwitchType;
this.m_parameterChain = new LinkedHashSet<>();
this.m_invertedRunner = () -> {};
this.m_feedbackSensor = FeedbackSensor.NEO_ENCODER;

// Set CAN timeout
m_spark.setCANTimeout(CAN_TIMEOUT_MS);
Expand Down Expand Up @@ -238,8 +241,8 @@ public Spark(ID id, MotorKind kind, SparkLimitSwitch.Type limitSwitchType, Measu
PurpleManager.add(this);

// Start sensor input thread
m_inputsThread.setName(m_id.name);
if (!Logger.hasReplaySource()) m_inputsThread.startPeriodic(inputThreadPeriod.in(Units.Seconds));
m_inputThread.setName(m_id.name);
if (!Logger.hasReplaySource()) m_inputThread.startPeriodic(m_inputThreadPeriod.in(Units.Seconds));
}

/**
Expand Down Expand Up @@ -530,22 +533,43 @@ private REVLibError enableVoltageCompensation(Measure<Voltage> nominalVoltage) {
return status;
}

/**
* Fuse absolute encoder to NEO built-in encoder
* @return {@link REVLibError#kOk} if successful
*/
private REVLibError fuseEncoders() {
// Return if not using fused mode
if (!m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) return REVLibError.kOk;

// Fuse encoder if required
m_inputs.encoderPosition = m_inputs.absoluteEncoderPosition;
m_inputs.encoderVelocity = m_inputs.absoluteEncoderVelocity;

// Set encoder to fused value
return getEncoder().setPosition(m_inputs.encoderPosition);
}

/**
* Update sensor input readings
*/
private void updateInputs() {
synchronized (m_inputs) {
// Get sensor inputs
m_inputs.analogPosition = getAnalogPosition();
m_inputs.analogVelocity = getAnalogVelocity();
m_inputs.absoluteEncoderPosition = getAbsoluteEncoderPosition();
m_inputs.absoluteEncoderVelocity = getAbsoluteEncoderVelocity();
m_inputs.forwardLimitSwitch = getForwardLimitSwitch().isPressed();
m_inputs.reverseLimitSwitch = getReverseLimitSwitch().isPressed();

if (!getMotorType().equals(MotorType.kBrushed)) {
// Get motor encoder
if (!getMotorType().equals(MotorType.kBrushed) && !m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) {
m_inputs.encoderPosition = getEncoderPosition();
m_inputs.encoderVelocity = getEncoderVelocity();
}

// Fuse encoder if required
fuseEncoders();
}
}

Expand Down Expand Up @@ -741,20 +765,31 @@ public void initializeSparkPID(SparkPIDConfig config, FeedbackSensor feedbackSen
selectedSensor = getAnalog();
m_currentStateSupplier = () -> new TrapezoidProfile.State(getInputs().analogPosition, getInputs().analogVelocity);
break;
case THROUGH_BORE_ENCODER:
case ABSOLUTE_ENCODER:
selectedSensor = getAbsoluteEncoder();
m_currentStateSupplier = () -> new TrapezoidProfile.State(getInputs().absoluteEncoderPosition, getInputs().absoluteEncoderVelocity);
break;
case NEO_ENCODER:
case FUSED_ENCODER:
default:
selectedSensor = getEncoder();
m_currentStateSupplier = () -> new TrapezoidProfile.State(getInputs().encoderPosition, getInputs().encoderVelocity);
break;
}

// If fused, seed NEO encoder with absolute and increase relevant status frame rates
if (m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) {
setPeriodicFrameRate(PeriodicFrame.kStatus1, m_inputThreadPeriod);
setPeriodicFrameRate(PeriodicFrame.kStatus2, m_inputThreadPeriod);
setPeriodicFrameRate(PeriodicFrame.kStatus5, m_inputThreadPeriod);
setPeriodicFrameRate(PeriodicFrame.kStatus6, m_inputThreadPeriod);
Timer.delay(APPLY_PARAMETER_WAIT_TIME);
resetEncoder(getAbsoluteEncoderPosition());
}

// Configure feedback sensor and set sensor phase
m_spark.getPIDController().setFeedbackDevice(selectedSensor);
if (!m_feedbackSensor.equals(FeedbackSensor.NEO_ENCODER)) {
if (!m_feedbackSensor.equals(FeedbackSensor.NEO_ENCODER) && !m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) {
parameterApplier = () -> applyParameter(
() -> selectedSensor.setInverted(m_config.getSensorPhase()),
() -> selectedSensor.getInverted() == m_config.getSensorPhase(),
Expand Down Expand Up @@ -883,6 +918,9 @@ public void set(double value, ControlType ctrl, double arbFeedforward, SparkPIDC
/**
* Set the conversion factor for position of the sensor. Multiplied by the native output units to
* give you position.
* <p>
* Passing in {@link FeedbackSensor#ABSOLUTE_ENCODER} or {@link FeedbackSensor#FUSED_ENCODER}
* will set the conversion factor for a connected absolute encoder.
* @param sensor Sensor to set conversion factor for
* @param factor The conversion factor to multiply the native units by
* @return {@link REVLibError#kOk} if successful
Expand All @@ -900,7 +938,8 @@ public REVLibError setPositionConversionFactor(FeedbackSensor sensor, double fac
parameterSetter = () -> getAnalog().setPositionConversionFactor(factor);
parameterCheckSupplier = () -> Precision.equals(getAnalog().getPositionConversionFactor(), factor, EPSILON);
break;
case THROUGH_BORE_ENCODER:
case FUSED_ENCODER:
case ABSOLUTE_ENCODER:
parameterSetter = () -> getAbsoluteEncoder().setPositionConversionFactor(factor);
parameterCheckSupplier = () -> Precision.equals(getAbsoluteEncoder().getPositionConversionFactor(), factor, EPSILON);
break;
Expand All @@ -925,12 +964,11 @@ public REVLibError setPositionConversionFactor(FeedbackSensor sensor, double fac
*/
public double getPositionConversionFactor(FeedbackSensor sensor) {
switch (sensor) {
case NEO_ENCODER:
return getEncoder().getPositionConversionFactor();
case ANALOG:
return getAnalog().getPositionConversionFactor();
case THROUGH_BORE_ENCODER:
case ABSOLUTE_ENCODER:
return getAbsoluteEncoder().getPositionConversionFactor();
case NEO_ENCODER:
default:
return getEncoder().getPositionConversionFactor();
}
Expand All @@ -939,6 +977,9 @@ public double getPositionConversionFactor(FeedbackSensor sensor) {
/**
* Set the conversion factor for velocity of the sensor. Multiplied by the native output units to
* give you velocity.
* <p>
* Passing in {@link FeedbackSensor#ABSOLUTE_ENCODER} or {@link FeedbackSensor#FUSED_ENCODER}
* will set the conversion factor for a connected absolute encoder.
* @param sensor Sensor to set conversion factor for
* @param factor The conversion factor to multiply the native units by
* @return {@link REVLibError#kOk} if successful
Expand All @@ -956,7 +997,8 @@ public REVLibError setVelocityConversionFactor(FeedbackSensor sensor, double fac
parameterSetter = () -> getAnalog().setVelocityConversionFactor(factor);
parameterCheckSupplier = () -> Precision.equals(getAnalog().getVelocityConversionFactor(), factor, EPSILON);
break;
case THROUGH_BORE_ENCODER:
case FUSED_ENCODER:
case ABSOLUTE_ENCODER:
parameterSetter = () -> getAbsoluteEncoder().setVelocityConversionFactor(factor);
parameterCheckSupplier = () -> Precision.equals(getAnalog().getVelocityConversionFactor(), factor, EPSILON);
break;
Expand Down Expand Up @@ -985,7 +1027,7 @@ public double getVelocityConversionFactor(FeedbackSensor sensor) {
return getEncoder().getVelocityConversionFactor();
case ANALOG:
return getAnalog().getVelocityConversionFactor();
case THROUGH_BORE_ENCODER:
case ABSOLUTE_ENCODER:
return getAbsoluteEncoder().getVelocityConversionFactor();
default:
return getEncoder().getVelocityConversionFactor();
Expand Down Expand Up @@ -1115,6 +1157,29 @@ public void smoothMotion(double value, TrapezoidProfile.Constraints motionConstr
smoothMotion(value, motionConstraint, (motionProfileState) -> 0.0);
}

/**
* Set gear ratio between motor shaft and external absolute encoder
* <p>
* Sets the NEO encoder conversion factor to the absolute encoder conversion factor
* divided by the specified ratio.
* Call {@link Spark#setPositionConversionFactor(FeedbackSensor, double)} and/or {@link Spark#setVelocityConversionFactor(FeedbackSensor, double)}
* <i>before</i> this method.
* <p>
* {@link Spark#setPositionConversionFactor(FeedbackSensor, double)} and {@link Spark#setVelocityConversionFactor(FeedbackSensor, double)}
* should be called for the absolute encoder before this.
* @param ratio Gear ratio
*/
public void setMotorToSensorRatio(double ratio) {
if (!m_feedbackSensor.equals(FeedbackSensor.FUSED_ENCODER)) {
org.tinylog.Logger.tag(LOG_TAG)
.warn(
"Feedback sensor is not set to fused, currently set to {}",
m_feedbackSensor.toString()
);
}
setPositionConversionFactor(FeedbackSensor.NEO_ENCODER, getPositionConversionFactor(FeedbackSensor.ABSOLUTE_ENCODER) / ratio);
setVelocityConversionFactor(FeedbackSensor.NEO_ENCODER, getVelocityConversionFactor(FeedbackSensor.ABSOLUTE_ENCODER) / ratio);
}

/**
* Reset NEO built-in encoder to desired value
Expand Down

0 comments on commit b3cefe0

Please sign in to comment.