mirror of
https://github.com/BroncBotz3481/YAGSL
synced 2026-06-19 06:21:40 +00:00
1017 lines
33 KiB
Java
1017 lines
33 KiB
Java
package swervelib;
|
|
|
|
import static edu.wpi.first.units.Units.InchesPerSecond;
|
|
import static edu.wpi.first.units.Units.MetersPerSecond;
|
|
import static edu.wpi.first.units.Units.RadiansPerSecond;
|
|
import static edu.wpi.first.units.Units.RotationsPerSecond;
|
|
|
|
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
|
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
|
import edu.wpi.first.networktables.BooleanPublisher;
|
|
import edu.wpi.first.networktables.DoublePublisher;
|
|
import edu.wpi.first.networktables.NetworkTableInstance;
|
|
import edu.wpi.first.units.measure.AngularVelocity;
|
|
import edu.wpi.first.units.measure.LinearVelocity;
|
|
import edu.wpi.first.wpilibj.Alert;
|
|
import edu.wpi.first.wpilibj.Alert.AlertType;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
import swervelib.encoders.SparkMaxEncoderSwerve;
|
|
import swervelib.encoders.SwerveAbsoluteEncoder;
|
|
import swervelib.math.SwerveMath;
|
|
import swervelib.motors.SparkMaxBrushedMotorSwerve;
|
|
import swervelib.motors.SparkMaxSwerve;
|
|
import swervelib.motors.SwerveMotor;
|
|
import swervelib.parser.Cache;
|
|
import swervelib.parser.PIDFConfig;
|
|
import swervelib.parser.SwerveModuleConfiguration;
|
|
import swervelib.parser.SwerveModulePhysicalCharacteristics;
|
|
import swervelib.simulation.SwerveModuleSimulation;
|
|
import swervelib.telemetry.SwerveDriveTelemetry;
|
|
import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
|
|
|
|
/**
|
|
* The Swerve Module class which represents and controls Swerve Modules for the swerve drive.
|
|
*/
|
|
public class SwerveModule implements AutoCloseable
|
|
{
|
|
|
|
/**
|
|
* Swerve module configuration options.
|
|
*/
|
|
public final SwerveModuleConfiguration configuration;
|
|
/**
|
|
* Absolute encoder position cache.
|
|
*/
|
|
public final Cache<Double> absolutePositionCache;
|
|
/**
|
|
* Drive motor position cache.
|
|
*/
|
|
public final Cache<Double> drivePositionCache;
|
|
/**
|
|
* Drive motor velocity cache.
|
|
*/
|
|
public final Cache<Double> driveVelocityCache;
|
|
/**
|
|
* Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right.
|
|
*/
|
|
public final int moduleNumber;
|
|
/**
|
|
* Swerve Motors.
|
|
*/
|
|
private final SwerveMotor angleMotor, driveMotor;
|
|
/**
|
|
* Absolute encoder for swerve drive.
|
|
*/
|
|
private final SwerveAbsoluteEncoder absoluteEncoder;
|
|
/**
|
|
* An {@link Alert} for if pushing the Absolute Encoder offset to the encoder fails.
|
|
*/
|
|
private final Alert encoderOffsetWarning;
|
|
/**
|
|
* An {@link Alert} for if there is no Absolute Encoder on the module.
|
|
*/
|
|
private final Alert noEncoderWarning;
|
|
/**
|
|
* An {@link Alert} for if there is no Absolute Encoder on the module.
|
|
*/
|
|
private final Alert externalSensorIsNull = new Alert("No absolute Encoder found.",
|
|
AlertType.kError);
|
|
/**
|
|
* An {@link Alert} for if the offset is 0 degrees.
|
|
*/
|
|
private final Alert internalOffsetIsZero = new Alert(
|
|
"Absolute encoder offset is 0, this may be a problem.",
|
|
AlertType.kWarning);
|
|
/**
|
|
* An {@link Alert} for if the angle/steer/azimuth motor is incompatible with the absolute encoder.
|
|
*/
|
|
private final Alert externalFeedbackIncompatible = new Alert(
|
|
"Absolute encoder is incompatible, cannot set as an external feedback device.",
|
|
AlertType.kError);
|
|
/**
|
|
* An {@link Alert} for if the absolute encoder cannot set an offset.
|
|
*/
|
|
private final Alert externalOffsetIncompatible = new Alert(
|
|
"Absolute encoder is incompatible, cannot set an offset internally.",
|
|
AlertType.kError);
|
|
/**
|
|
* NT4 Raw Absolute Angle publisher for the absolute encoder.
|
|
*/
|
|
private final DoublePublisher rawAbsoluteAnglePublisher;
|
|
/**
|
|
* NT4 Adjusted Absolute angle publisher for the absolute encoder.
|
|
*/
|
|
private final DoublePublisher adjAbsoluteAnglePublisher;
|
|
/**
|
|
* NT4 Absolute encoder read issue.
|
|
*/
|
|
private final BooleanPublisher absoluteEncoderIssuePublisher;
|
|
/**
|
|
* NT4 raw angle motor.
|
|
*/
|
|
private final DoublePublisher rawAnglePublisher;
|
|
/**
|
|
* NT4 Raw drive motor.
|
|
*/
|
|
private final DoublePublisher rawDriveEncoderPublisher;
|
|
/**
|
|
* NT4 Raw drive motor.
|
|
*/
|
|
private final DoublePublisher rawDriveVelocityPublisher;
|
|
/**
|
|
* Speed setpoint publisher for the module motor-controller PID.
|
|
*/
|
|
private final DoublePublisher speedSetpointPublisher;
|
|
/**
|
|
* Angle setpoint publisher for the module motor-controller PID.
|
|
*/
|
|
private final DoublePublisher angleSetpointPublisher;
|
|
/**
|
|
* Maximum {@link LinearVelocity} for the drive motor of the swerve module.
|
|
*/
|
|
private LinearVelocity maxDriveVelocity;
|
|
/**
|
|
* Maximum velocity for the drive motor of the swerve module.
|
|
*/
|
|
private double maxDriveVelocityMetersPerSecond;
|
|
/**
|
|
* Maximum {@link AngularVelocity} for the azimuth/angle motor of the swerve module.
|
|
*/
|
|
private AngularVelocity maxAngularVelocity;
|
|
/**
|
|
* Feedforward for the drive motor during closed loop control.
|
|
*/
|
|
private SimpleMotorFeedforward driveMotorFeedforward;
|
|
/**
|
|
* Anti-Jitter AKA auto-centering disabled.
|
|
*/
|
|
private boolean antiJitterEnabled = true;
|
|
/**
|
|
* Last swerve module state applied.
|
|
*/
|
|
private SwerveModuleState lastState;
|
|
/**
|
|
* Angle offset from the absolute encoder.
|
|
*/
|
|
private double angleOffset;
|
|
/**
|
|
* Simulated swerve module.
|
|
*/
|
|
private SwerveModuleSimulation simModule;
|
|
/**
|
|
* Enables utilization off {@link SwerveModuleState#optimize(Rotation2d)}
|
|
*/
|
|
private boolean optimizeSwerveModuleState = true;
|
|
/**
|
|
* Encoder synchronization queued.
|
|
*/
|
|
private boolean synchronizeEncoderQueued = false;
|
|
/**
|
|
* Encoder, Absolute encoder synchronization enabled.
|
|
*/
|
|
private boolean synchronizeEncoderEnabled = false;
|
|
/**
|
|
* Encoder synchronization deadband in degrees.
|
|
*/
|
|
private double synchronizeEncoderDeadband = 3;
|
|
|
|
|
|
/**
|
|
* Construct the swerve module and initialize the swerve module motors and absolute encoder.
|
|
*
|
|
* @param moduleNumber Module number for kinematics.
|
|
* @param moduleConfiguration Module constants containing CAN ID's and offsets.
|
|
*/
|
|
public SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfiguration)
|
|
{
|
|
// angle = 0;
|
|
// speed = 0;
|
|
// omega = 0;
|
|
// fakePos = 0;
|
|
this.moduleNumber = moduleNumber;
|
|
configuration = moduleConfiguration;
|
|
angleOffset = moduleConfiguration.angleOffset;
|
|
|
|
// Create motors from configuration and reset them to defaults.
|
|
angleMotor = moduleConfiguration.angleMotor;
|
|
driveMotor = moduleConfiguration.driveMotor;
|
|
angleMotor.factoryDefaults();
|
|
driveMotor.factoryDefaults();
|
|
|
|
// Initialize Feedforwards.
|
|
driveMotorFeedforward = getDefaultFeedforward();
|
|
|
|
// Configure voltage comp, current limit, and ramp rate.
|
|
angleMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage);
|
|
driveMotor.setVoltageCompensation(configuration.physicalCharacteristics.optimalVoltage);
|
|
angleMotor.setCurrentLimit(configuration.physicalCharacteristics.angleMotorCurrentLimit);
|
|
driveMotor.setCurrentLimit(configuration.physicalCharacteristics.driveMotorCurrentLimit);
|
|
angleMotor.setLoopRampRate(configuration.physicalCharacteristics.angleMotorRampRate);
|
|
driveMotor.setLoopRampRate(configuration.physicalCharacteristics.driveMotorRampRate);
|
|
|
|
// Config angle encoders
|
|
absoluteEncoder = moduleConfiguration.absoluteEncoder;
|
|
if (absoluteEncoder != null)
|
|
{
|
|
absoluteEncoder.factoryDefault();
|
|
absoluteEncoder.configure(moduleConfiguration.absoluteEncoderInverted);
|
|
}
|
|
|
|
if (SwerveDriveTelemetry.isSimulation)
|
|
{
|
|
simModule = new SwerveModuleSimulation();
|
|
}
|
|
|
|
// Setup the cache for the absolute encoder position.
|
|
absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20);
|
|
|
|
// Config angle motor/controller
|
|
if (!angleMotor.usingExternalFeedbackSensor())
|
|
{
|
|
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor);
|
|
}
|
|
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
|
|
angleMotor.configurePIDWrapping(0, 360);
|
|
angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
|
|
angleMotor.setMotorBrake(false);
|
|
|
|
// Set the position AFTER settings the conversion factor.
|
|
if (absoluteEncoder != null)
|
|
{
|
|
angleMotor.setPosition(getAbsolutePosition());
|
|
}
|
|
|
|
// Config drive motor/controller
|
|
driveMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.drive.factor);
|
|
driveMotor.configurePIDF(moduleConfiguration.velocityPIDF);
|
|
driveMotor.setInverted(moduleConfiguration.driveMotorInverted);
|
|
driveMotor.setMotorBrake(true);
|
|
|
|
driveMotor.burnFlash();
|
|
angleMotor.burnFlash();
|
|
|
|
drivePositionCache = new Cache<>(driveMotor::getPosition, 20);
|
|
driveVelocityCache = new Cache<>(driveMotor::getVelocity, 20);
|
|
|
|
// Force a cache update on init.
|
|
driveVelocityCache.update();
|
|
drivePositionCache.update();
|
|
absolutePositionCache.update();
|
|
|
|
// Save the current state.
|
|
lastState = getState();
|
|
|
|
noEncoderWarning = new Alert("Motors",
|
|
"There is no Absolute Encoder on module #" +
|
|
moduleNumber,
|
|
AlertType.kWarning);
|
|
encoderOffsetWarning = new Alert("Motors",
|
|
"Pushing the Absolute Encoder offset to the encoder failed on module #" +
|
|
moduleNumber,
|
|
AlertType.kWarning);
|
|
|
|
rawAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
|
|
"swerve/modules/" + configuration.name + "/Raw Absolute Encoder").publish();
|
|
adjAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
|
|
"swerve/modules/" + configuration.name + "/Adjusted Absolute Encoder").publish();
|
|
absoluteEncoderIssuePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getBooleanTopic(
|
|
"swerve/modules/" + configuration.name + "/Absolute Encoder Read Issue").publish();
|
|
rawAnglePublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
|
|
"swerve/modules/" + configuration.name + "/Raw Angle Encoder").publish();
|
|
rawDriveEncoderPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
|
|
"swerve/modules/" + configuration.name + "/Raw Drive Encoder").publish();
|
|
rawDriveVelocityPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
|
|
"swerve/modules/" + configuration.name + "/Raw Drive Velocity").publish();
|
|
speedSetpointPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
|
|
"swerve/modules/" + configuration.name + "/Speed Setpoint").publish();
|
|
angleSetpointPublisher = NetworkTableInstance.getDefault().getTable("SmartDashboard").getDoubleTopic(
|
|
"swerve/modules/" + configuration.name + "/Angle Setpoint").publish();
|
|
}
|
|
|
|
@Override
|
|
public void close()
|
|
{
|
|
angleMotor.close();
|
|
driveMotor.close();
|
|
absoluteEncoder.close();
|
|
}
|
|
|
|
/**
|
|
* Get the default {@link SimpleMotorFeedforward} for the swerve module drive motor.
|
|
*
|
|
* @return {@link SimpleMotorFeedforward} using motor details.
|
|
*/
|
|
public SimpleMotorFeedforward getDefaultFeedforward()
|
|
{
|
|
double nominalVoltage = driveMotor.getSimMotor().nominalVoltageVolts;
|
|
double maxDriveSpeedMPS = getMaxVelocity().in(MetersPerSecond);
|
|
return SwerveMath.createDriveFeedforward(nominalVoltage,
|
|
maxDriveSpeedMPS,
|
|
configuration.physicalCharacteristics.wheelGripCoefficientOfFriction);
|
|
}
|
|
|
|
/**
|
|
* Set utilization of {@link SwerveModuleState#optimize(Rotation2d)} which should be disabled for some debugging.
|
|
*
|
|
* @param optimizationState Optimization enabled.
|
|
*/
|
|
public void setModuleStateOptimization(boolean optimizationState)
|
|
{
|
|
optimizeSwerveModuleState = optimizationState;
|
|
if (!optimizeSwerveModuleState)
|
|
{
|
|
angleMotor.disablePIDWrapping();
|
|
angleMotor.burnFlash();
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Check if the module state optimization used by {@link SwerveModuleState#optimize(Rotation2d)} is enabled.
|
|
*
|
|
* @return optimization state.
|
|
*/
|
|
public boolean getModuleStateOptimization()
|
|
{
|
|
return optimizeSwerveModuleState;
|
|
}
|
|
|
|
/**
|
|
* Set the voltage compensation for the swerve module motor.
|
|
*
|
|
* @param optimalVoltage Nominal voltage for operation to output to.
|
|
*/
|
|
public void setAngleMotorVoltageCompensation(double optimalVoltage)
|
|
{
|
|
angleMotor.setVoltageCompensation(optimalVoltage);
|
|
}
|
|
|
|
/**
|
|
* Set the voltage compensation for the swerve module motor.
|
|
*
|
|
* @param optimalVoltage Nominal voltage for operation to output to.
|
|
*/
|
|
public void setDriveMotorVoltageCompensation(double optimalVoltage)
|
|
{
|
|
driveMotor.setVoltageCompensation(optimalVoltage);
|
|
}
|
|
|
|
|
|
/**
|
|
* Queue synchronization of the integrated angle encoder with the absolute encoder.
|
|
*/
|
|
public void queueSynchronizeEncoders()
|
|
{
|
|
if (absoluteEncoder != null && synchronizeEncoderEnabled)
|
|
{
|
|
synchronizeEncoderQueued = true;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a
|
|
* few seconds.
|
|
*
|
|
* @param enabled Enable state
|
|
* @param deadband Deadband in degrees, default is 3 degrees.
|
|
*/
|
|
public void setEncoderAutoSynchronize(boolean enabled, double deadband)
|
|
{
|
|
synchronizeEncoderEnabled = enabled;
|
|
synchronizeEncoderDeadband = deadband;
|
|
}
|
|
|
|
/**
|
|
* Enable auto synchronization for encoders during a match. This will only occur when the modules are not moving for a
|
|
* few seconds.
|
|
*
|
|
* @param enabled Enable state
|
|
*/
|
|
public void setEncoderAutoSynchronize(boolean enabled)
|
|
{
|
|
synchronizeEncoderEnabled = enabled;
|
|
}
|
|
|
|
/**
|
|
* Set the antiJitter functionality, if true the modules will NOT auto center. Pushes the offsets to the angle motor
|
|
* controllers as well.
|
|
*
|
|
* @param antiJitter Anti-Jitter state desired.
|
|
*/
|
|
public void setAntiJitter(boolean antiJitter)
|
|
{
|
|
this.antiJitterEnabled = antiJitter;
|
|
if (antiJitter)
|
|
{
|
|
pushOffsetsToEncoders();
|
|
} else
|
|
{
|
|
restoreInternalOffset();
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Set the feedforward attributes to the given parameters.
|
|
*
|
|
* @param drive Drive motor feedforward for the module.
|
|
*/
|
|
public void setFeedforward(SimpleMotorFeedforward drive)
|
|
{
|
|
this.driveMotorFeedforward = drive;
|
|
}
|
|
|
|
/**
|
|
* Get the current drive motor PIDF values.
|
|
*
|
|
* @return {@link PIDFConfig} of the drive motor.
|
|
*/
|
|
public PIDFConfig getDrivePIDF()
|
|
{
|
|
return configuration.velocityPIDF;
|
|
}
|
|
|
|
/**
|
|
* Set the drive PIDF values.
|
|
*
|
|
* @param config {@link PIDFConfig} of that should be set.
|
|
*/
|
|
public void setDrivePIDF(PIDFConfig config)
|
|
{
|
|
configuration.velocityPIDF = config;
|
|
driveMotor.configurePIDF(config);
|
|
}
|
|
|
|
/**
|
|
* Get the current angle/azimuth/steering motor PIDF values.
|
|
*
|
|
* @return {@link PIDFConfig} of the angle motor.
|
|
*/
|
|
public PIDFConfig getAnglePIDF()
|
|
{
|
|
return configuration.anglePIDF;
|
|
}
|
|
|
|
/**
|
|
* Set the angle/azimuth/steering motor PID
|
|
*
|
|
* @param config {@link PIDFConfig} of that should be set.
|
|
*/
|
|
public void setAnglePIDF(PIDFConfig config)
|
|
{
|
|
configuration.anglePIDF = config;
|
|
angleMotor.configurePIDF(config);
|
|
}
|
|
|
|
/**
|
|
* Set the desired state of the swerve module. <br /><b>WARNING: If you are not using one of the functions from
|
|
* {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics}</b>
|
|
*
|
|
* @param desiredState Desired swerve module state.
|
|
* @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control.
|
|
* @param force Disables optimizations that prevent movement in the angle motor and forces the desired state
|
|
* onto the swerve module.
|
|
*/
|
|
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
|
|
{
|
|
applyStateOptimizations(desiredState);
|
|
applyAntiJitter(desiredState, force);
|
|
|
|
// Cosine compensation.
|
|
double nextVelocityMetersPerSecond = configuration.useCosineCompensator
|
|
? getCosineCompensatedVelocity(desiredState)
|
|
: desiredState.speedMetersPerSecond;
|
|
double curVelocityMetersPerSecond = lastState.speedMetersPerSecond;
|
|
desiredState.speedMetersPerSecond = nextVelocityMetersPerSecond;
|
|
|
|
setDesiredState(desiredState,
|
|
isOpenLoop,
|
|
driveMotorFeedforward.calculateWithVelocities(curVelocityMetersPerSecond,
|
|
nextVelocityMetersPerSecond));
|
|
}
|
|
|
|
/**
|
|
* Set the desired state of the swerve module. <br /><b>WARNING: If you are not using one of the functions from
|
|
* {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics}</b>
|
|
*
|
|
* @param desiredState Desired swerve module state.
|
|
* @param isOpenLoop Whether to use open loop (direct percent) or direct velocity control.
|
|
* @param driveFeedforwardVoltage Drive motor controller feedforward as a voltage.
|
|
*/
|
|
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
|
|
double driveFeedforwardVoltage)
|
|
{
|
|
if (isOpenLoop)
|
|
{
|
|
double percentOutput = desiredState.speedMetersPerSecond / maxDriveVelocity.in(MetersPerSecond);
|
|
driveMotor.setVoltage(percentOutput * 12);
|
|
} else
|
|
{
|
|
driveMotor.setReference(desiredState.speedMetersPerSecond, driveFeedforwardVoltage);
|
|
}
|
|
|
|
// Prevent module rotation if angle is the same as the previous angle.
|
|
// Synchronize encoders if queued and send in the current position as the value from the absolute encoder.
|
|
if (absoluteEncoder != null && synchronizeEncoderQueued && synchronizeEncoderEnabled)
|
|
{
|
|
double absoluteEncoderPosition = getAbsolutePosition();
|
|
if (Math.abs(angleMotor.getPosition() - absoluteEncoderPosition) >= synchronizeEncoderDeadband)
|
|
{
|
|
angleMotor.setPosition(absoluteEncoderPosition);
|
|
}
|
|
angleMotor.setReference(desiredState.angle.getDegrees(), 0, absoluteEncoderPosition);
|
|
synchronizeEncoderQueued = false;
|
|
} else
|
|
{
|
|
angleMotor.setReference(desiredState.angle.getDegrees(), 0);
|
|
}
|
|
|
|
lastState = desiredState;
|
|
|
|
if (SwerveDriveTelemetry.isSimulation)
|
|
{
|
|
simModule.updateStateAndPosition(desiredState);
|
|
}
|
|
|
|
// TODO: Change and move to SwerveDriveTelemetry
|
|
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
|
|
{
|
|
SwerveDriveTelemetry.desiredStatesObj[moduleNumber] = desiredState;
|
|
}
|
|
|
|
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
|
|
{
|
|
speedSetpointPublisher.set(desiredState.speedMetersPerSecond);
|
|
angleSetpointPublisher.set(desiredState.angle.getDegrees());
|
|
}
|
|
|
|
if (moduleNumber == SwerveDriveTelemetry.moduleCount - 1)
|
|
{
|
|
SwerveDriveTelemetry.endCtrlCycle();
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Get the cosine compensated velocity to set the swerve module to.
|
|
*
|
|
* @param desiredState Desired {@link SwerveModuleState} to use.
|
|
* @return Cosine compensated velocity in meters/second.
|
|
*/
|
|
private double getCosineCompensatedVelocity(SwerveModuleState desiredState)
|
|
{
|
|
double cosineScalar = 1.0;
|
|
// Taken from the CTRE SwerveModule class.
|
|
// https://api.ctr-electronics.com/phoenix6/release/java/src-html/com/ctre/phoenix6/mechanisms/swerve/SwerveModule.html#line.46
|
|
/* From FRC 900's whitepaper, we add a cosine compensator to the applied drive velocity */
|
|
/* To reduce the "skew" that occurs when changing direction */
|
|
/* If error is close to 0 rotations, we're already there, so apply full power */
|
|
/* If the error is close to 0.25 rotations, then we're 90 degrees, so movement doesn't help us at all */
|
|
cosineScalar = Rotation2d.fromDegrees(desiredState.angle.getDegrees())
|
|
.minus(Rotation2d.fromDegrees(getAbsolutePosition()))
|
|
.getCos(); // TODO: Investigate angle modulus by 180.
|
|
/* Make sure we don't invert our drive, even though we shouldn't ever target over 90 degrees anyway */
|
|
if (cosineScalar < 0.0)
|
|
{
|
|
cosineScalar = 1;
|
|
}
|
|
|
|
return desiredState.speedMetersPerSecond * cosineScalar;
|
|
}
|
|
|
|
/**
|
|
* Apply the {@link SwerveModuleState#optimize(Rotation2d)} function if the module state optimization is enabled while
|
|
* debugging.
|
|
*
|
|
* @param desiredState The desired state to apply the optimization to.
|
|
*/
|
|
public void applyStateOptimizations(SwerveModuleState desiredState)
|
|
{
|
|
// SwerveModuleState optimization might be desired to be disabled while debugging.
|
|
if (optimizeSwerveModuleState)
|
|
{
|
|
desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition()));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Apply anti-jitter to the desired state. This will prevent the module from rotating if the speed requested is too
|
|
* low. If force is true, the anti-jitter will not be applied.
|
|
*
|
|
* @param desiredState The desired state to apply the anti-jitter to.
|
|
* @param force Whether to ignore the {@link SwerveModule#antiJitterEnabled} state and apply the anti-jitter
|
|
* anyway.
|
|
*/
|
|
public void applyAntiJitter(SwerveModuleState desiredState, boolean force)
|
|
{
|
|
if (!force && antiJitterEnabled)
|
|
{
|
|
// Prevents module rotation if speed is less than 1%
|
|
SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocityMetersPerSecond, 4));
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Set the angle for the module.
|
|
*
|
|
* @param angle Angle in degrees.
|
|
*/
|
|
public void setAngle(double angle)
|
|
{
|
|
angleMotor.setReference(angle, 0);
|
|
lastState.angle = Rotation2d.fromDegrees(angle);
|
|
}
|
|
|
|
/**
|
|
* Get the Swerve Module state.
|
|
*
|
|
* @return Current SwerveModule state.
|
|
*/
|
|
public SwerveModuleState getState()
|
|
{
|
|
double velocity;
|
|
Rotation2d azimuth;
|
|
if (!SwerveDriveTelemetry.isSimulation)
|
|
{
|
|
velocity = driveVelocityCache.getValue();
|
|
azimuth = Rotation2d.fromDegrees(getAbsolutePosition());
|
|
} else
|
|
{
|
|
return simModule.getState();
|
|
}
|
|
return new SwerveModuleState(velocity, azimuth);
|
|
}
|
|
|
|
/**
|
|
* Get the position of the swerve module.
|
|
*
|
|
* @return {@link SwerveModulePosition} of the swerve module.
|
|
*/
|
|
public SwerveModulePosition getPosition()
|
|
{
|
|
double position;
|
|
Rotation2d azimuth;
|
|
if (!SwerveDriveTelemetry.isSimulation)
|
|
{
|
|
position = drivePositionCache.getValue();
|
|
azimuth = Rotation2d.fromDegrees(getAbsolutePosition());
|
|
} else
|
|
{
|
|
return simModule.getPosition();
|
|
}
|
|
return new SwerveModulePosition(position, azimuth);
|
|
}
|
|
|
|
/**
|
|
* 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 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()
|
|
{
|
|
/* During simulation, when no absolute encoders are available, we return the state from the simulation module instead. */
|
|
if (SwerveDriveTelemetry.isSimulation)
|
|
{
|
|
Rotation2d absolutePosition = simModule.getState().angle;
|
|
return absolutePosition.getDegrees();
|
|
}
|
|
|
|
double angle;
|
|
if (absoluteEncoder != null)
|
|
{
|
|
angle = absoluteEncoder.getAbsolutePosition() - angleOffset;
|
|
if (absoluteEncoder.readingError)
|
|
{
|
|
angle = getRelativePosition();
|
|
}
|
|
} else
|
|
{
|
|
angle = getRelativePosition();
|
|
}
|
|
if (optimizeSwerveModuleState)
|
|
{
|
|
angle %= 360;
|
|
if (angle < 0.0)
|
|
{
|
|
angle += 360;
|
|
}
|
|
}
|
|
|
|
return angle;
|
|
}
|
|
|
|
/**
|
|
* Get the relative angle in degrees.
|
|
*
|
|
* @return Angle in degrees.
|
|
*/
|
|
public double getRelativePosition()
|
|
{
|
|
return angleMotor.getPosition();
|
|
}
|
|
|
|
/**
|
|
* Set the brake mode.
|
|
*
|
|
* @param brake Set the brake mode.
|
|
*/
|
|
public void setMotorBrake(boolean brake)
|
|
{
|
|
driveMotor.setMotorBrake(brake);
|
|
}
|
|
|
|
/**
|
|
* Set the conversion factor for the angle/azimuth motor controller.
|
|
*
|
|
* @param conversionFactor Angle motor conversion factor for PID, should be generated from
|
|
* {@link SwerveMath#calculateDegreesPerSteeringRotation(double, double)} or calculated.
|
|
*/
|
|
public void setAngleMotorConversionFactor(double conversionFactor)
|
|
{
|
|
angleMotor.configureIntegratedEncoder(conversionFactor);
|
|
}
|
|
|
|
/**
|
|
* Set the conversion factor for the drive motor controller.
|
|
*
|
|
* @param conversionFactor Drive motor conversion factor for PID, should be generated from
|
|
* {@link SwerveMath#calculateMetersPerRotation(double, double, double)} or calculated.
|
|
*/
|
|
public void setDriveMotorConversionFactor(double conversionFactor)
|
|
{
|
|
driveMotor.configureIntegratedEncoder(conversionFactor);
|
|
}
|
|
|
|
/**
|
|
* Get the angle {@link SwerveMotor} for the {@link SwerveModule}.
|
|
*
|
|
* @return {@link SwerveMotor} for the angle/steering motor of the module.
|
|
*/
|
|
public SwerveMotor getAngleMotor()
|
|
{
|
|
return angleMotor;
|
|
}
|
|
|
|
/**
|
|
* Get the drive {@link SwerveMotor} for the {@link SwerveModule}.
|
|
*
|
|
* @return {@link SwerveMotor} for the drive motor of the module.
|
|
*/
|
|
public SwerveMotor getDriveMotor()
|
|
{
|
|
return driveMotor;
|
|
}
|
|
|
|
/**
|
|
* Get the {@link SwerveAbsoluteEncoder} for the {@link SwerveModule}.
|
|
*
|
|
* @return {@link SwerveAbsoluteEncoder} for the swerve module.
|
|
*/
|
|
public SwerveAbsoluteEncoder getAbsoluteEncoder()
|
|
{
|
|
return absoluteEncoder;
|
|
}
|
|
|
|
/**
|
|
* Fetch the {@link SwerveModuleConfiguration} for the {@link SwerveModule} with the parsed configurations.
|
|
*
|
|
* @return {@link SwerveModuleConfiguration} for the {@link SwerveModule}.
|
|
*/
|
|
public SwerveModuleConfiguration getConfiguration()
|
|
{
|
|
return configuration;
|
|
}
|
|
|
|
|
|
/**
|
|
* Use external sensors for the feedback of the angle/azimuth/steer controller.
|
|
*/
|
|
public void useExternalFeedbackSensor()
|
|
{
|
|
if (absoluteEncoder == null)
|
|
{
|
|
externalSensorIsNull.set(true);
|
|
return;
|
|
}
|
|
if (angleOffset == 0)
|
|
{
|
|
internalOffsetIsZero.set(true);
|
|
}
|
|
if (absoluteEncoder.setAbsoluteEncoderOffset(configuration.angleOffset))
|
|
{
|
|
angleMotor.setAbsoluteEncoder(absoluteEncoder);
|
|
if (angleMotor.usingExternalFeedbackSensor())
|
|
{
|
|
angleOffset = 0;
|
|
} else
|
|
{
|
|
externalFeedbackIncompatible.set(true);
|
|
angleMotor.setAbsoluteEncoder(null);
|
|
absoluteEncoder.setAbsoluteEncoderOffset(0);
|
|
}
|
|
|
|
} else
|
|
{
|
|
externalOffsetIncompatible.set(true);
|
|
absoluteEncoder.setAbsoluteEncoderOffset(0);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Use external sensors for the feedback of the angle/azimuth/steer controller.
|
|
*/
|
|
public void useInternalFeedbackSensor()
|
|
{
|
|
if (absoluteEncoder == null)
|
|
{
|
|
externalSensorIsNull.set(true);
|
|
return;
|
|
}
|
|
if (angleOffset == 0)
|
|
{
|
|
internalOffsetIsZero.set(true);
|
|
}
|
|
angleMotor.setAbsoluteEncoder(null);
|
|
absoluteEncoder.setAbsoluteEncoderOffset(0);
|
|
angleOffset = configuration.angleOffset;
|
|
}
|
|
|
|
/**
|
|
* Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset.
|
|
*/
|
|
@Deprecated
|
|
public void pushOffsetsToEncoders()
|
|
{
|
|
if (absoluteEncoder != null && angleOffset == configuration.angleOffset)
|
|
{
|
|
// If the absolute encoder is attached.
|
|
if (angleMotor instanceof SparkMaxSwerve || angleMotor instanceof SparkMaxBrushedMotorSwerve)
|
|
{
|
|
if (absoluteEncoder instanceof SparkMaxEncoderSwerve)
|
|
{
|
|
angleMotor.setAbsoluteEncoder(absoluteEncoder);
|
|
if (absoluteEncoder.setAbsoluteEncoderOffset(angleOffset))
|
|
{
|
|
angleOffset = 0;
|
|
} else
|
|
{
|
|
angleMotor.setAbsoluteEncoder(null);
|
|
encoderOffsetWarning.set(true);
|
|
}
|
|
}
|
|
}
|
|
|
|
} else
|
|
{
|
|
noEncoderWarning.set(true);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Restore internal offset in YAGSL and either sets absolute encoder offset to 0 or restores old value.
|
|
*/
|
|
public void restoreInternalOffset()
|
|
{
|
|
angleMotor.setAbsoluteEncoder(null);
|
|
absoluteEncoder.setAbsoluteEncoderOffset(0);
|
|
angleOffset = configuration.angleOffset;
|
|
}
|
|
|
|
/**
|
|
* Get if the last Absolute Encoder had a read issue, such as it does not exist.
|
|
*
|
|
* @return If the last Absolute Encoder had a read issue, or absolute encoder does not exist.
|
|
*/
|
|
public boolean getAbsoluteEncoderReadIssue()
|
|
{
|
|
if (absoluteEncoder == null)
|
|
{
|
|
return true;
|
|
} else
|
|
{
|
|
return absoluteEncoder.readingError;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Get the maximum module velocity as a {@link LinearVelocity} based on the RPM and gear ratio.
|
|
*
|
|
* @return {@link LinearVelocity} max velocity of the drive wheel.
|
|
*/
|
|
public LinearVelocity getMaxVelocity()
|
|
{
|
|
getMaxDriveVelocityMetersPerSecond();
|
|
return maxDriveVelocity;
|
|
}
|
|
|
|
/**
|
|
* Get the maximum drive velocity of the module in Meters Per Second.
|
|
*
|
|
* @return Maximum drive motor velocity in Meters Per Second.
|
|
*/
|
|
public double getMaxDriveVelocityMetersPerSecond()
|
|
{
|
|
if (maxDriveVelocity == null)
|
|
{
|
|
maxDriveVelocity = InchesPerSecond.of(
|
|
(driveMotor.getSimMotor().freeSpeedRadPerSec /
|
|
configuration.conversionFactors.drive.gearRatio) *
|
|
configuration.conversionFactors.drive.diameter / 2.0);
|
|
maxDriveVelocityMetersPerSecond = maxDriveVelocity.in(MetersPerSecond);
|
|
}
|
|
return maxDriveVelocityMetersPerSecond;
|
|
}
|
|
|
|
/**
|
|
* Get the maximum module angular velocity as a {@link AngularVelocity} based on the RPM and gear ratio.
|
|
*
|
|
* @return {@link AngularVelocity} max velocity of the angle/azimuth.
|
|
*/
|
|
public AngularVelocity getMaxAngularVelocity()
|
|
{
|
|
if (maxAngularVelocity == null)
|
|
{
|
|
maxAngularVelocity = RotationsPerSecond.of(
|
|
RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) /
|
|
configuration.conversionFactors.angle.gearRatio);
|
|
}
|
|
return maxAngularVelocity;
|
|
}
|
|
|
|
/**
|
|
* Update data sent to {@link SmartDashboard}.
|
|
*/
|
|
public void updateTelemetry()
|
|
{
|
|
if (absoluteEncoder != null)
|
|
{
|
|
rawAbsoluteAnglePublisher.set(absoluteEncoder.getAbsolutePosition());
|
|
}
|
|
if (SwerveDriveTelemetry.isSimulation && SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
|
|
{
|
|
SwerveModulePosition pos = simModule.getPosition();
|
|
SwerveModuleState state = simModule.getState();
|
|
rawAnglePublisher.set(pos.angle.getDegrees());
|
|
rawDriveEncoderPublisher.set(pos.distanceMeters);
|
|
rawDriveVelocityPublisher.set(state.speedMetersPerSecond);
|
|
// For code coverage
|
|
angleMotor.getPosition();
|
|
drivePositionCache.getValue();
|
|
driveVelocityCache.getValue();
|
|
} else
|
|
{
|
|
rawAnglePublisher.set(angleMotor.getPosition());
|
|
rawDriveEncoderPublisher.set(drivePositionCache.getValue());
|
|
rawDriveVelocityPublisher.set(driveVelocityCache.getValue());
|
|
}
|
|
adjAbsoluteAnglePublisher.set(getAbsolutePosition());
|
|
absoluteEncoderIssuePublisher.set(getAbsoluteEncoderReadIssue());
|
|
|
|
}
|
|
|
|
/**
|
|
* Invalidate the {@link Cache} objects used by {@link SwerveModule}.
|
|
*/
|
|
public void invalidateCache()
|
|
{
|
|
absolutePositionCache.update();
|
|
drivePositionCache.update();
|
|
driveVelocityCache.update();
|
|
}
|
|
|
|
/**
|
|
* Obtains the {@link SwerveModuleSimulation} used in simulation.
|
|
*
|
|
* @return the module simulation, <b>null</b> if this method is called on a real robot
|
|
*/
|
|
public SwerveModuleSimulation getSimModule()
|
|
{
|
|
return simModule;
|
|
}
|
|
|
|
/**
|
|
* Configure the {@link SwerveModule#simModule} with the MapleSim
|
|
* {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation}
|
|
*
|
|
* @param swerveModuleSimulation MapleSim {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} to
|
|
* configure with.
|
|
* @param physicalCharacteristics {@link SwerveModulePhysicalCharacteristics} that represent the swerve drive.
|
|
*/
|
|
public void configureModuleSimulation(
|
|
org.ironmaple.simulation.drivesims.SwerveModuleSimulation swerveModuleSimulation,
|
|
SwerveModulePhysicalCharacteristics physicalCharacteristics)
|
|
{
|
|
this.simModule.configureSimModule(swerveModuleSimulation, physicalCharacteristics);
|
|
}
|
|
}
|