Team2890/YAGSL
4
0
Fork 0
mirror of https://github.com/BroncBotz3481/YAGSL synced 2026-06-19 06:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Upgrading to 2025.7.0

Browse Source
This commit is contained in:
thenetworkgrinch
2025-02-22 06:15:56 +00:00
parent 62f8236678
commit 4016ee2190
41 changed files with 2237 additions and 557 deletions
Download Patch File Download Diff File Expand all files Collapse all files

142
swervelib/SwerveDrive.java
View File

@@ -6,7 +6,6 @@ import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.KilogramSquareMeters;
import static edu.wpi.first.units.Units.Kilograms;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Newtons;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Seconds;
@@ -52,14 +51,11 @@ import java.util.Map;
import java.util.Optional;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
import org.ironmaple.simulation.SimulatedArena;
import org.ironmaple.simulation.drivesims.AbstractDriveTrainSimulation;
import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig;
import swervelib.encoders.CANCoderSwerve;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.SwerveIMU;
@@ -75,7 +71,7 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
/**
* Swerve Drive class representing and controlling the swerve drive.
*/
public class SwerveDrive
public class SwerveDrive implements AutoCloseable
{
/**
@@ -118,16 +114,20 @@ public class SwerveDrive
*/
private final DoublePublisher rawIMUPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/Raw IMU Yaw")
"swerve/imu/raw")
.publish();
/**
* NT4 Publisher for the IMU reading adjusted by offset and inversion.
*/
private final DoublePublisher adjustedIMUPublisher
= NetworkTableInstance.getDefault()
.getTable(
"SmartDashboard")
.getDoubleTopic(
"swerve/Adjusted IMU Yaw")
"swerve/imu/adjusted")
.publish();
/**
* Field object.
@@ -224,7 +224,9 @@ public class SwerveDrive
SwerveDriveConfiguration config, SwerveControllerConfiguration controllerConfig, double maxSpeedMPS,
Pose2d startingPose)
{
this.maxChassisSpeedMPS = maxSpeedMPS;
this.attainableMaxTranslationalSpeedMetersPerSecond = this.maxChassisSpeedMPS = maxSpeedMPS;
this.attainableMaxRotationalVelocityRadiansPerSecond = Math.PI *
2; // Defaulting to something reasonable for most robots
swerveDriveConfiguration = config;
swerveController = new SwerveController(controllerConfig);
// Create Kinematics from swerve module locations.
@@ -247,17 +249,17 @@ public class SwerveDrive
.withCustomModuleTranslations(config.moduleLocationsMeters)
.withGyro(config.getGyroSim())
.withSwerveModule(new SwerveModuleSimulationConfig(
config.getDriveMotorSim(),
config.getAngleMotorSim(),
config.physicalCharacteristics.conversionFactor.drive.gearRatio,
config.physicalCharacteristics.conversionFactor.angle.gearRatio,
Volts.of(config.physicalCharacteristics.driveFrictionVoltage),
Volts.of(config.physicalCharacteristics.angleFrictionVoltage),
Inches.of(
config.physicalCharacteristics.conversionFactor.drive.diameter /
2),
KilogramSquareMeters.of(0.02),
config.physicalCharacteristics.wheelGripCoefficientOfFriction)
config.getDriveMotorSim(),
config.getAngleMotorSim(),
config.physicalCharacteristics.conversionFactor.drive.gearRatio,
config.physicalCharacteristics.conversionFactor.angle.gearRatio,
Volts.of(config.physicalCharacteristics.driveFrictionVoltage),
Volts.of(config.physicalCharacteristics.angleFrictionVoltage),
Inches.of(
config.physicalCharacteristics.conversionFactor.drive.diameter /
2),
KilogramSquareMeters.of(0.02),
config.physicalCharacteristics.wheelGripCoefficientOfFriction)
);
mapleSimDrive = new SwerveDriveSimulation(simulationConfig, startingPose);
@@ -332,6 +334,16 @@ public class SwerveDrive
HAL.report(kResourceType_RobotDrive, kRobotDriveSwerve_YAGSL);
}
@Override
public void close() {
imu.close();
tunerXRecommendation.close();
for (var module : swerveModules) {
module.close();
}
}
/**
* Update the cache validity period for the robot.
*
@@ -567,7 +579,7 @@ public class SwerveDrive
if (fieldRelative)
{
ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
velocity = ChassisSpeeds.fromFieldRelativeSpeeds(velocity, getOdometryHeading());
}
drive(velocity, isOpenLoop, new Translation2d());
}
@@ -619,31 +631,46 @@ public class SwerveDrive
}
/**
* Set the maximum speeds for desaturation.
* Set the maximum attainable speeds for desaturation.
*
* @param attainableMaxTranslationalSpeedMetersPerSecond The absolute max speed that your robot can reach while
* translating in meters per second.
* @param attainableMaxRotationalVelocityRadiansPerSecond The absolute max speed the robot can reach while rotating in
* radians per second.
*/
public void setMaximumSpeeds(
public void setMaximumAttainableSpeeds(
double attainableMaxTranslationalSpeedMetersPerSecond,
double attainableMaxRotationalVelocityRadiansPerSecond)
{
this.attainableMaxTranslationalSpeedMetersPerSecond = attainableMaxTranslationalSpeedMetersPerSecond;
this.attainableMaxRotationalVelocityRadiansPerSecond = attainableMaxRotationalVelocityRadiansPerSecond;
this.swerveController.config.maxAngularVelocity = attainableMaxRotationalVelocityRadiansPerSecond;
}
/**
* Set the maximum allowable speeds for desaturation.
*
* @param maxTranslationalSpeedMetersPerSecond The allowable max speed that your robot should reach while translating
* in meters per second.
* @param maxRotationalVelocityRadiansPerSecond The allowable max speed the robot should reach while rotating in
* radians per second.
*/
public void setMaximumAllowableSpeeds(
double maxTranslationalSpeedMetersPerSecond,
double maxRotationalVelocityRadiansPerSecond)
{
this.maxChassisSpeedMPS = maxTranslationalSpeedMetersPerSecond;
this.swerveController.config.maxAngularVelocity = maxRotationalVelocityRadiansPerSecond;
}
/**
* Get the maximum velocity from {@link SwerveDrive#attainableMaxTranslationalSpeedMetersPerSecond} or
* {@link SwerveDrive#maxChassisSpeedMPS} whichever is higher.
* {@link SwerveDrive#maxChassisSpeedMPS} whichever is the lower limit on the robot's speed.
*
* @return Maximum speed in meters/second.
* @return Minimum speed in meters/second of physically attainable and user allowable limits.
*/
public double getMaximumChassisVelocity()
{
return Math.max(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS);
return Math.min(this.attainableMaxTranslationalSpeedMetersPerSecond, maxChassisSpeedMPS);
}
/**
@@ -651,9 +678,9 @@ public class SwerveDrive
*
* @return {@link LinearVelocity} representing the maximum drive speed of a module.
*/
public LinearVelocity getMaximumModuleDriveVelocity()
public double getMaximumModuleDriveVelocity()
{
return swerveModules[0].getMaxVelocity();
return swerveModules[0].getMaxDriveVelocityMetersPerSecond();
}
/**
@@ -668,13 +695,13 @@ public class SwerveDrive
/**
* Get the maximum angular velocity, either {@link SwerveDrive#attainableMaxRotationalVelocityRadiansPerSecond} or
* {@link SwerveControllerConfiguration#maxAngularVelocity}.
* {@link SwerveControllerConfiguration#maxAngularVelocity}, whichever is the lower limit on the robot's speed.
*
* @return Maximum angular velocity in radians per second.
* @return Minimum angular velocity in radians per second of physically attainable and user allowable limits.
*/
public double getMaximumChassisAngularVelocity()
{
return Math.max(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity);
return Math.min(this.attainableMaxRotationalVelocityRadiansPerSecond, swerveController.config.maxAngularVelocity);
}
/**
@@ -688,8 +715,9 @@ public class SwerveDrive
boolean isOpenLoop)
{
// Desaturates wheel speeds
double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond);
if (attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0)
double maxModuleSpeedMPS = getMaximumModuleDriveVelocity();
if ((attainableMaxTranslationalSpeedMetersPerSecond != 0 || attainableMaxRotationalVelocityRadiansPerSecond != 0) &&
attainableMaxTranslationalSpeedMetersPerSecond != maxChassisSpeedMPS)
{
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, desiredChassisSpeed,
maxModuleSpeedMPS,
@@ -719,7 +747,7 @@ public class SwerveDrive
public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
{
SwerveDriveTelemetry.startCtrlCycle();
double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond);
double maxModuleSpeedMPS = getMaximumModuleDriveVelocity();
desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates));
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxModuleSpeedMPS);
@@ -748,6 +776,9 @@ public class SwerveDrive
}
for (SwerveModule module : swerveModules)
{
module.applyStateOptimizations(states[module.moduleNumber]);
module.applyAntiJitter(states[module.moduleNumber], false);
// from the module configuration, obtain necessary information to calculate feed-forward
// Warning: Will not work well if motor is not what we are expecting.
// Warning: Should replace module.getDriveMotor().simMotor with expected motor type first.
@@ -1128,7 +1159,7 @@ public class SwerveDrive
{
SwerveDriveTelemetry.startOdomCycle();
odometryLock.lock();
invalidateCache();
// invalidateCache();
try
{
// Update odometry
@@ -1255,7 +1286,7 @@ public class SwerveDrive
* {@link Timer#getFPGATimestamp()} or similar sources.
* @param visionMeasurementStdDevs Vision measurement standard deviation that will be sent to the
* {@link SwerveDrivePoseEstimator}.The standard deviation of the vision measurement,
* for best accuracy calculate the standard deviation at 2 or more points and fit a
* for best accuracy calculate the standard deviation at 2 or more points and fit a
* line to it with the calculated optimal standard deviation. (Units should be meters
* per pixel). By optimizing this you can get * vision accurate to inches instead of
* feet.
@@ -1355,10 +1386,34 @@ public class SwerveDrive
}
}
/**
* Set the motor controller closed loop feedback device to the defined external absolute encoder, with the given
* offset from the supplied configuration, overwriting any native offset.
*/
public void useExternalFeedbackSensor()
{
for (SwerveModule module : swerveModules)
{
module.useExternalFeedbackSensor();
}
}
/**
* Set the motor controller closed loop feedback device to the internal encoder instead of the absolute encoder.
*/
public void useInternalFeedbackSensor()
{
for (SwerveModule module : swerveModules)
{
module.useInternalFeedbackSensor();
}
}
/**
* Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the
* internal offsets to prevent double offsetting.
*/
@Deprecated
public void pushOffsetsToEncoders()
{
for (SwerveModule module : swerveModules)
@@ -1370,6 +1425,7 @@ public class SwerveDrive
/**
* Restores Internal YAGSL Encoder offsets and sets the Encoder stored offset back to 0
*/
@Deprecated
public void restoreInternalOffset()
{
for (SwerveModule module : swerveModules)
@@ -1378,6 +1434,20 @@ public class SwerveDrive
}
}
/**
* Set module optimization to be utilized or not. Sometimes it is desirable to be enabled for debugging purposes
* only.
*
* @param enabled Optimization enabled state.
*/
public void setModuleStateOptimization(boolean enabled)
{
for (SwerveModule module : swerveModules)
{
module.setModuleStateOptimization(enabled);
}
}
/**
* Enable auto-centering module wheels. This has a side effect of causing some jitter to the robot when a PID is not
* tuned perfectly. This function is a wrapper for {@link SwerveModule#setAntiJitter(boolean)} to perform

99
swervelib/SwerveDriveTest.java
View File

@@ -9,6 +9,8 @@ import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.units.measure.MutAngle;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.units.measure.MutDistance;
@@ -27,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
* Class to perform tests on the swerve drive.
@@ -101,7 +104,7 @@ public class SwerveDriveTest
* Power the drive motors for the swerve drive to a set voltage.
*
* @param swerveDrive {@link SwerveDrive} to control.
* @param volts DutyCycle percentage of voltage to send to drive motors.
* @param volts Voltage to send to drive motors.
*/
public static void powerDriveMotorsVoltage(SwerveDrive swerveDrive, double volts)
{
@@ -135,6 +138,60 @@ public class SwerveDriveTest
angleModules(swerveDrive, Rotation2d.fromDegrees(0));
}
/**
* Set the modules to their rotary position to allow running sysid and spinning the robot
*
* @param swerveDrive Swerve Drive to control.
*/
public static void setModulesToRotaryPosition(SwerveDrive swerveDrive)
{
SwerveModuleState[] rotaryStates = swerveDrive.kinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, 1));
for (int i = 0; i < swerveDrive.getModules().length; i++)
{
swerveDrive.getModules()[i].getAngleMotor().setReference(rotaryStates[i].angle.getDegrees(), 0);
}
}
/**
* Set the sim modules to center to 0 and power them to drive in a voltage. Calling this function in sim is equivalent
* to calling {@link #centerModules(SwerveDrive)} and {@link #powerDriveMotorsVoltage(SwerveDrive, double)} on a real
* robot.
*
* @param swerveDrive {@link SwerveDrive} to control.
* @param volts Voltage to send to drive motors.
* @param testWithSpinning - Whether to make the robot spin in place instead of driving in a straight line, true to
* make the robot spin, false to make the robot drive in straight line
*/
public static void runDriveMotorsCharacterizationOnSimModules(SwerveDrive swerveDrive, double volts,
boolean testWithSpinning)
{
SwerveModuleState[] rotaryStates = swerveDrive.kinematics.toSwerveModuleStates(new ChassisSpeeds(0, 0, 1));
for (int i = 0; i < swerveDrive.getModules().length; i++)
{
swerveDrive.getModules()[i].getSimModule().runDriveMotorCharacterization(
testWithSpinning
? rotaryStates[i].angle
: Rotation2d.kZero,
volts);
}
}
/**
* Set the sim modules to center to 0 and power them to drive in a voltage. Calling this function in sim is equivalent
* to calling {@link #centerModules(SwerveDrive)} and {@link #powerDriveMotorsVoltage(SwerveDrive, double)} on a real
* robot.
*
* @param swerveDrive {@link SwerveDrive} to control.
* @param volts Voltage to send to angle motors.
*/
public static void runAngleMotorsCharacterizationOnSimModules(SwerveDrive swerveDrive, double volts)
{
for (SwerveModule module : swerveDrive.getModules())
{
module.getSimModule().runAngleMotorCharacterization(volts);
}
}
/**
* Find the minimum amount of power required to move the swerve drive motors.
*
@@ -299,19 +356,35 @@ public class SwerveDriveTest
/**
* Sets up the SysId runner and logger for the drive motors
*
* @param config - The SysIdRoutine.Config to use
* @param swerveSubsystem - the subsystem to add to requirements
* @param swerveDrive - the SwerveDrive from which to access motor info
* @param maxVolts - The maximum voltage that should be applied to the drive motors.
* @param config - The SysIdRoutine.Config to use
* @param swerveSubsystem - the subsystem to add to requirements
* @param swerveDrive - the SwerveDrive from which to access motor info
* @param maxVolts - The maximum voltage that should be applied to the drive motors.
* @param testWithSpinning - Whether to make the robot spin in place instead of driving in a straight line, true to
* make the robot spin, false to make the robot drive in straight line
* @return A SysIdRoutine runner
*/
public static SysIdRoutine setDriveSysIdRoutine(Config config, SubsystemBase swerveSubsystem,
SwerveDrive swerveDrive, double maxVolts)
SwerveDrive swerveDrive, double maxVolts, boolean testWithSpinning)
{
return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
(Voltage voltage) -> {
SwerveDriveTest.centerModules(swerveDrive);
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
if (!SwerveDriveTelemetry.isSimulation)
{
if (testWithSpinning)
{
SwerveDriveTest.setModulesToRotaryPosition(swerveDrive);
} else
{
SwerveDriveTest.centerModules(swerveDrive);
}
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, Math.min(voltage.in(Volts), maxVolts));
} else
{
SwerveDriveTest.runDriveMotorsCharacterizationOnSimModules(swerveDrive,
voltage.in(Volts),
testWithSpinning);
}
},
log -> {
for (SwerveModule module : swerveDrive.getModules())
@@ -382,8 +455,14 @@ public class SwerveDriveTest
{
return new SysIdRoutine(config, new SysIdRoutine.Mechanism(
(Voltage voltage) -> {
SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
if (!SwerveDriveTelemetry.isSimulation)
{
SwerveDriveTest.powerAngleMotorsVoltage(swerveDrive, voltage.in(Volts));
SwerveDriveTest.powerDriveMotorsVoltage(swerveDrive, 0);
} else
{
SwerveDriveTest.runAngleMotorsCharacterizationOnSimModules(swerveDrive, voltage.in(Volts));
}
},
log -> {
for (SwerveModule module : swerveDrive.getModules())

273
swervelib/SwerveInputStream.java
View File

@@ -1,6 +1,7 @@
package swervelib;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
@@ -44,83 +45,111 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
/**
* Translation suppliers.
*/
private final DoubleSupplier controllerTranslationX;
private final DoubleSupplier controllerTranslationX;
/**
* Translational supplier.
*/
private final DoubleSupplier controllerTranslationY;
private final DoubleSupplier controllerTranslationY;
/**
* {@link SwerveDrive} object for transformations.
*/
private final SwerveDrive swerveDrive;
private final SwerveDrive swerveDrive;
/**
* Rotation supplier as angular velocity.
*/
private Optional<DoubleSupplier> controllerOmega = Optional.empty();
private Optional<DoubleSupplier> controllerOmega = Optional.empty();
/**
* Controller supplier as heading.
*/
private Optional<DoubleSupplier> controllerHeadingX = Optional.empty();
private Optional<DoubleSupplier> controllerHeadingX = Optional.empty();
/**
* Controller supplier as heading.
*/
private Optional<DoubleSupplier> controllerHeadingY = Optional.empty();
private Optional<DoubleSupplier> controllerHeadingY = Optional.empty();
/**
* Axis deadband for the controller.
*/
private Optional<Double> axisDeadband = Optional.empty();
private Optional<Double> axisDeadband = Optional.empty();
/**
* Translational axis scalar value, should be between (0, 1].
*/
private Optional<Double> translationAxisScale = Optional.empty();
private Optional<Double> translationAxisScale = Optional.empty();
/**
* Angular velocity axis scalar value, should be between (0, 1]
*/
private Optional<Double> omegaAxisScale = Optional.empty();
private Optional<Double> omegaAxisScale = Optional.empty();
/**
* Target to aim at.
*/
private Optional<Pose2d> aimTarget = Optional.empty();
private Optional<Pose2d> aimTarget = Optional.empty();
/**
* Target {@link Supplier<Pose2d>} to drive towards when driveToPose is enabled.
*/
private Optional<Supplier<Pose2d>> driveToPose = Optional.empty();
/**
* {@link ProfiledPIDController} for the X translation while driving to a pose. Units are m/s
*/
private Optional<ProfiledPIDController> driveToPoseXPIDController = Optional.empty();
/**
* {@link ProfiledPIDController} for the Y translation while driving to a pose. Units are m/s
*/
private Optional<ProfiledPIDController> driveToPoseYPIDController = Optional.empty();
/**
* {@link ProfiledPIDController} for the Rotational axis while driving to a pose. Units are m/s
*/
private Optional<ProfiledPIDController> driveToPoseOmegaPIDController = Optional.empty();
/**
* Output {@link ChassisSpeeds} based on heading while this is True.
*/
private Optional<BooleanSupplier> headingEnabled = Optional.empty();
private Optional<BooleanSupplier> headingEnabled = Optional.empty();
/**
* Locked heading for {@link SwerveInputMode#TRANSLATION_ONLY}
*/
private Optional<Rotation2d> lockedHeading = Optional.empty();
private Optional<Rotation2d> lockedHeading = Optional.empty();
/**
* Output {@link ChassisSpeeds} based on aim while this is True.
*/
private Optional<BooleanSupplier> aimEnabled = Optional.empty();
private Optional<BooleanSupplier> aimEnabled = Optional.empty();
/**
* Output {@link ChassisSpeeds} to move to a specific {@link Pose2d}.
*/
private Optional<BooleanSupplier> driveToPoseEnabled = Optional.empty();
/**
* Maintain current heading and drive without rotating, ideally.
*/
private Optional<BooleanSupplier> translationOnlyEnabled = Optional.empty();
private Optional<BooleanSupplier> translationOnlyEnabled = Optional.empty();
/**
* Cube the translation magnitude from the controller.
*/
private Optional<BooleanSupplier> translationCube = Optional.empty();
private Optional<BooleanSupplier> translationCube = Optional.empty();
/**
* Cube the angular velocity axis from the controller.
*/
private Optional<BooleanSupplier> omegaCube = Optional.empty();
private Optional<BooleanSupplier> omegaCube = Optional.empty();
/**
* Robot relative oriented output expected.
*/
private Optional<BooleanSupplier> robotRelative = Optional.empty();
private Optional<BooleanSupplier> robotRelative = Optional.empty();
/**
* Field oriented chassis output is relative to your current alliance.
*/
private Optional<BooleanSupplier> allianceRelative = Optional.empty();
private Optional<BooleanSupplier> allianceRelative = Optional.empty();
/**
* Heading offset enable state.
*/
private Optional<BooleanSupplier> headingOffsetEnabled = Optional.empty();
/**
* Heading offset to apply during heading based control.
*/
private Optional<Rotation2d> headingOffset = Optional.empty();
/**
* {@link SwerveController} for simple control over heading.
*/
private SwerveController swerveController = null;
private SwerveController swerveController = null;
/**
* Current {@link SwerveInputMode} to use.
*/
private SwerveInputMode currentMode = SwerveInputMode.ANGULAR_VELOCITY;
private SwerveInputMode currentMode = SwerveInputMode.ANGULAR_VELOCITY;
/**
@@ -195,9 +224,14 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
newStream.axisDeadband = axisDeadband;
newStream.translationAxisScale = translationAxisScale;
newStream.omegaAxisScale = omegaAxisScale;
newStream.driveToPose = driveToPose;
newStream.driveToPoseXPIDController = driveToPoseXPIDController;
newStream.driveToPoseYPIDController = driveToPoseYPIDController;
newStream.driveToPoseOmegaPIDController = driveToPoseOmegaPIDController;
newStream.aimTarget = aimTarget;
newStream.headingEnabled = headingEnabled;
newStream.aimEnabled = aimEnabled;
newStream.driveToPoseEnabled = driveToPoseEnabled;
newStream.currentMode = currentMode;
newStream.translationOnlyEnabled = translationOnlyEnabled;
newStream.lockedHeading = lockedHeading;
@@ -206,6 +240,8 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
newStream.translationCube = translationCube;
newStream.robotRelative = robotRelative;
newStream.allianceRelative = allianceRelative;
newStream.headingOffsetEnabled = headingOffsetEnabled;
newStream.headingOffset = headingOffset;
return newStream;
}
@@ -233,6 +269,103 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
return this;
}
/**
* Drive to a given pose with the provided {@link ProfiledPIDController}s
*
* @param pose {@link Supplier<Pose2d>} for ease of use.
* @param xPIDController PID controller for the X axis, units are m/s.
* @param yPIDController PID controller for the Y axis, units are m/s.
* @param omegaPIDController PID Controller for rotational axis, units are rad/s.
* @return self
*/
public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController xPIDController,
ProfiledPIDController yPIDController, ProfiledPIDController omegaPIDController)
{
driveToPose = Optional.of(pose);
driveToPoseXPIDController = Optional.of(xPIDController);
driveToPoseYPIDController = Optional.of(yPIDController);
driveToPoseOmegaPIDController = Optional.of(omegaPIDController);
return this;
}
/**
* Drive to a given pose with the provided {@link ProfiledPIDController}s
*
* @param pose {@link Supplier<Pose2d>} for ease of use.
* @param translation PID controller for the X and Y axis, units are m/s.
* @param rotation PID Controller for rotational axis, units are rad/s.
* @return self
*/
public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDController translation,
ProfiledPIDController rotation)
{
return driveToPose(pose, translation, new ProfiledPIDController(translation.getP(),
translation.getI(),
translation.getD(),
translation.getConstraints()), rotation);
}
/**
* Enable driving to the target pose.
*
* @param enabled Enable state of drive to pose.
* @return self.
*/
public SwerveInputStream driveToPoseEnabled(BooleanSupplier enabled)
{
driveToPoseEnabled = Optional.of(enabled);
return this;
}
/**
* Enable driving to the target pose.
*
* @param enabled Enable state of drive to pose.
* @return self.
*/
public SwerveInputStream driveToPoseEnabled(boolean enabled)
{
driveToPoseEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
return this;
}
/**
* Heading offset enabled boolean supplier.
*
* @param enabled Enable state
* @return self
*/
public SwerveInputStream headingOffset(BooleanSupplier enabled)
{
headingOffsetEnabled = Optional.of(enabled);
return this;
}
/**
* Heading offset enable
*
* @param enabled Enable state
* @return self
*/
public SwerveInputStream headingOffset(boolean enabled)
{
headingOffsetEnabled = enabled ? Optional.of(() -> enabled) : Optional.empty();
return this;
}
/**
* Set the heading offset angle.
*
* @param angle {@link Rotation2d} offset to apply
* @return self
*/
public SwerveInputStream headingOffset(Rotation2d angle)
{
headingOffset = Optional.of(angle);
return this;
}
/**
* Modify the output {@link ChassisSpeeds} so that it is always relative to your alliance.
*
@@ -476,7 +609,19 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
*/
private SwerveInputMode findMode()
{
if (translationOnlyEnabled.isPresent() && translationOnlyEnabled.get().getAsBoolean())
if (driveToPoseEnabled.isPresent() && driveToPoseEnabled.get().getAsBoolean())
{
if (driveToPose.isPresent())
{
if (driveToPoseOmegaPIDController.isPresent() && driveToPoseXPIDController.isPresent() &&
driveToPoseYPIDController.isPresent())
{
return SwerveInputMode.DRIVE_TO_POSE;
}
DriverStation.reportError("Drive to pose not supplied with pid controllers.", false);
}
DriverStation.reportError("Drive to pose enabled without supplier present.", false);
} else if (translationOnlyEnabled.isPresent() && translationOnlyEnabled.get().getAsBoolean())
{
return SwerveInputMode.TRANSLATION_ONLY;
} else if (aimEnabled.isPresent() && aimEnabled.get().getAsBoolean())
@@ -526,17 +671,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
lockedHeading = Optional.empty();
break;
}
case ANGULAR_VELOCITY ->
{
// Do nothing
break;
}
case HEADING ->
{
// Do nothing
break;
}
case AIM ->
case ANGULAR_VELOCITY, HEADING, AIM, DRIVE_TO_POSE ->
{
// Do nothing
break;
@@ -551,7 +686,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
lockedHeading = Optional.of(swerveDrive.getOdometryHeading());
break;
}
case ANGULAR_VELOCITY ->
case ANGULAR_VELOCITY, DRIVE_TO_POSE ->
{
if (swerveDrive.headingCorrection)
{
@@ -559,12 +694,7 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
}
break;
}
case HEADING ->
{
// Do nothing
break;
}
case AIM ->
case HEADING, AIM ->
{
// Do nothing
break;
@@ -651,16 +781,16 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
}
/**
* Change {@link ChassisSpeeds} to robot relative.
* Change {@link ChassisSpeeds} from robot relative if enabled.
*
* @param fieldRelativeSpeeds Field relative speeds to translate into robot-relative speeds.
* @return Robot relative {@link ChassisSpeeds}.
* @param fieldRelativeSpeeds Field or robot relative speeds to translate into robot-relative speeds.
* @return Field relative {@link ChassisSpeeds}.
*/
private ChassisSpeeds applyRobotRelativeTranslation(ChassisSpeeds fieldRelativeSpeeds)
{
if (robotRelative.isPresent() && robotRelative.get().getAsBoolean())
{
return ChassisSpeeds.fromFieldRelativeSpeeds(fieldRelativeSpeeds, swerveDrive.getOdometryHeading());
return ChassisSpeeds.fromRobotRelativeSpeeds(fieldRelativeSpeeds, swerveDrive.getOdometryHeading());
}
return fieldRelativeSpeeds;
}
@@ -709,6 +839,24 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
return fieldRelativeRotation;
}
/**
* Adds offset to rotation if one is set.
*
* @param fieldRelativeRotation Field-relative {@link Rotation2d} to offset
* @return Offsetted {@link Rotation2d}
*/
private Rotation2d applyHeadingOffset(Rotation2d fieldRelativeRotation)
{
if (headingOffsetEnabled.isPresent() && headingOffsetEnabled.get().getAsBoolean())
{
if (headingOffset.isPresent())
{
return fieldRelativeRotation.rotateBy(headingOffset.get());
}
}
return fieldRelativeRotation;
}
/**
* Gets a {@link ChassisSpeeds}
*
@@ -759,12 +907,22 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
case HEADING ->
{
omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
applyAllianceAwareRotation(Rotation2d.fromRadians(
swerveController.getJoystickAngle(
controllerHeadingX.get()
.getAsDouble(),
controllerHeadingY.get()
.getAsDouble()))).getRadians());
applyHeadingOffset(
applyAllianceAwareRotation(
Rotation2d.fromRadians(
swerveController.getJoystickAngle(
controllerHeadingX.get()
.getAsDouble(),
controllerHeadingY.get()
.getAsDouble())))).getRadians());
// Prevent rotation if controller heading inputs are not past axisDeadband
if (Math.abs(controllerHeadingX.get().getAsDouble()) + Math.abs(controllerHeadingY.get().getAsDouble()) <
axisDeadband.get())
{
omegaRadiansPerSecond = 0;
}
speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
break;
}
@@ -777,6 +935,17 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
speeds = new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
break;
}
case DRIVE_TO_POSE ->
{
Pose2d target = driveToPose.get().get();
Pose2d pose = swerveDrive.getPose();
omegaRadiansPerSecond = driveToPoseOmegaPIDController.get().calculate(pose.getRotation()
.getRadians(),
target.getRotation().getRadians());
speeds = new ChassisSpeeds(driveToPoseXPIDController.get().calculate(pose.getX(), target.getX()),
driveToPoseYPIDController.get().calculate(pose.getY(), target.getY()),
omegaRadiansPerSecond);
}
}
currentMode = newMode;
@@ -804,6 +973,10 @@ public class SwerveInputStream implements Supplier<ChassisSpeeds>
/**
* Output based off of targeting.
*/
AIM
AIM,
/**
* Drive to a target pose.
*/
DRIVE_TO_POSE
}
}

249
swervelib/SwerveModule.java
View File

@@ -1,5 +1,6 @@
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;
@@ -33,7 +34,7 @@ import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity;
/**
* The Swerve Module class which represents and controls Swerve Modules for the swerve drive.
*/
public class SwerveModule
public class SwerveModule implements AutoCloseable
{
/**
@@ -72,6 +73,29 @@ public class SwerveModule
* 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.
*/
@@ -108,6 +132,10 @@ public class SwerveModule
* 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.
*/
@@ -119,7 +147,7 @@ public class SwerveModule
/**
* Anti-Jitter AKA auto-centering disabled.
*/
private boolean antiJitterEnabled = true;
private boolean antiJitterEnabled = true;
/**
* Last swerve module state applied.
*/
@@ -132,18 +160,22 @@ public class SwerveModule
* Simulated swerve module.
*/
private SwerveModuleSimulation simModule;
/**
* Enables utilization off {@link SwerveModuleState#optimize(Rotation2d)}
*/
private boolean optimizeSwerveModuleState = true;
/**
* Encoder synchronization queued.
*/
private boolean synchronizeEncoderQueued = false;
private boolean synchronizeEncoderQueued = false;
/**
* Encoder, Absolute encoder synchronization enabled.
*/
private boolean synchronizeEncoderEnabled = false;
private boolean synchronizeEncoderEnabled = false;
/**
* Encoder synchronization deadband in degrees.
*/
private double synchronizeEncoderDeadband = 3;
private double synchronizeEncoderDeadband = 3;
/**
@@ -196,7 +228,10 @@ public class SwerveModule
absolutePositionCache = new Cache<>(this::getRawAbsolutePosition, 20);
// Config angle motor/controller
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor);
if (!angleMotor.usingExternalFeedbackSensor())
{
angleMotor.configureIntegratedEncoder(moduleConfiguration.conversionFactors.angle.factor);
}
angleMotor.configurePIDF(moduleConfiguration.anglePIDF);
angleMotor.configurePIDWrapping(0, 360);
angleMotor.setInverted(moduleConfiguration.angleMotorInverted);
@@ -255,6 +290,14 @@ public class SwerveModule
"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.
*
@@ -269,6 +312,31 @@ public class SwerveModule
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.
*
@@ -406,27 +474,20 @@ public class SwerveModule
*/
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, boolean force)
{
desiredState.optimize(Rotation2d.fromDegrees(getAbsolutePosition()));
// If we are forcing the angle
if (!force && antiJitterEnabled)
{
// Prevents module rotation if speed is less than 1%
SwerveMath.antiJitter(desiredState, lastState, Math.min(maxDriveVelocity.in(MetersPerSecond), 4));
}
applyStateOptimizations(desiredState);
applyAntiJitter(desiredState, force);
// Cosine compensation.
LinearVelocity nextVelocity = configuration.useCosineCompensator
? getCosineCompensatedVelocity(desiredState)
: MetersPerSecond.of(desiredState.speedMetersPerSecond);
LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond);
desiredState.speedMetersPerSecond = nextVelocity.magnitude();
double nextVelocityMetersPerSecond = configuration.useCosineCompensator
? getCosineCompensatedVelocity(desiredState)
: desiredState.speedMetersPerSecond;
double curVelocityMetersPerSecond = lastState.speedMetersPerSecond;
desiredState.speedMetersPerSecond = nextVelocityMetersPerSecond;
setDesiredState(desiredState,
isOpenLoop,
driveMotorFeedforward.calculateWithVelocities(curVelocity.in(MetersPerSecond),
nextVelocity.in(MetersPerSecond)));
driveMotorFeedforward.calculateWithVelocities(curVelocityMetersPerSecond,
nextVelocityMetersPerSecond));
}
/**
@@ -440,7 +501,6 @@ public class SwerveModule
public void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop,
double driveFeedforwardVoltage)
{
if (isOpenLoop)
{
double percentOutput = desiredState.speedMetersPerSecond / maxDriveVelocity.in(MetersPerSecond);
@@ -497,7 +557,7 @@ public class SwerveModule
* @param desiredState Desired {@link SwerveModuleState} to use.
* @return Cosine compensated velocity in meters/second.
*/
private LinearVelocity getCosineCompensatedVelocity(SwerveModuleState desiredState)
private double getCosineCompensatedVelocity(SwerveModuleState desiredState)
{
double cosineScalar = 1.0;
// Taken from the CTRE SwerveModule class.
@@ -515,7 +575,39 @@ public class SwerveModule
cosineScalar = 1;
}
return MetersPerSecond.of(desiredState.speedMetersPerSecond).times(cosineScalar);
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));
}
}
/**
@@ -605,10 +697,13 @@ public class SwerveModule
{
angle = getRelativePosition();
}
angle %= 360;
if (angle < 0.0)
if (optimizeSwerveModuleState)
{
angle += 360;
angle %= 360;
if (angle < 0.0)
{
angle += 360;
}
}
return angle;
@@ -696,9 +791,64 @@ public class SwerveModule
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)
@@ -759,15 +909,27 @@ public class SwerveModule
* @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 = MetersPerSecond.of(
(RadiansPerSecond.of(driveMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) /
maxDriveVelocity = InchesPerSecond.of(
(driveMotor.getSimMotor().freeSpeedRadPerSec /
configuration.conversionFactors.drive.gearRatio) *
configuration.conversionFactors.drive.diameter);
configuration.conversionFactors.drive.diameter / 2.0);
maxDriveVelocityMetersPerSecond = maxDriveVelocity.in(MetersPerSecond);
}
return maxDriveVelocity;
return maxDriveVelocityMetersPerSecond;
}
/**
@@ -780,7 +942,7 @@ public class SwerveModule
if (maxAngularVelocity == null)
{
maxAngularVelocity = RotationsPerSecond.of(
RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) *
RadiansPerSecond.of(angleMotor.getSimMotor().freeSpeedRadPerSec).in(RotationsPerSecond) /
configuration.conversionFactors.angle.gearRatio);
}
return maxAngularVelocity;
@@ -795,11 +957,26 @@ public class SwerveModule
{
rawAbsoluteAnglePublisher.set(absoluteEncoder.getAbsolutePosition());
}
rawAnglePublisher.set(angleMotor.getPosition());
rawDriveEncoderPublisher.set(drivePositionCache.getValue());
rawDriveVelocityPublisher.set(driveVelocityCache.getValue());
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());
}
/**

6
swervelib/encoders/AnalogAbsoluteEncoderSwerve.java
View File

@@ -47,6 +47,12 @@ public class AnalogAbsoluteEncoderSwerve extends SwerveAbsoluteEncoder
AlertType.kWarning);
}
@Override
public void close()
{
encoder.close();
}
/**
* Construct the Encoder given the analog input channel.
*

17
swervelib/encoders/CANCoderSwerve.java
View File

@@ -1,6 +1,5 @@
package swervelib.encoders;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.DegreesPerSecond;
import static edu.wpi.first.units.Units.Milliseconds;
import static edu.wpi.first.units.Units.Rotations;
@@ -13,7 +12,6 @@ import com.ctre.phoenix6.configs.CANcoderConfigurator;
import com.ctre.phoenix6.hardware.CANcoder;
import com.ctre.phoenix6.signals.MagnetHealthValue;
import com.ctre.phoenix6.signals.SensorDirectionValue;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.Alert;
@@ -28,7 +26,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
/**
* Wait time for status frames to show up.
*/
public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(10).in(Seconds);
public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(1).in(Seconds);
/**
* An {@link Alert} for if the CANCoder magnet field is less than ideal.
*/
@@ -114,6 +112,12 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
AlertType.kWarning);
}
@Override
public void close()
{
encoder.close();
}
/**
* Reset the encoder to factory defaults.
*/
@@ -158,6 +162,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
{
readingError = false;
MagnetHealthValue strength = magnetHealth.refresh().getValue();
angle.refresh();
magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green);
if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red)
@@ -170,8 +175,6 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
readingFaulty.set(false);
}
angle.refresh();
// 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
for (int i = 0; i < maximumRetries; i++)
@@ -190,8 +193,8 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
{
readingIgnored.set(false);
}
return angle.getValue().in(Degrees);
// Convert from Rotations to Degrees.
return angle.getValueAsDouble() * 360;
}
/**

8
swervelib/encoders/CanAndMagSwerve.java
View File

@@ -26,7 +26,13 @@ public class CanAndMagSwerve extends SwerveAbsoluteEncoder
public CanAndMagSwerve(int canid)
{
encoder = new Canandmag(canid);
settings = encoder.getSettings();
settings = new CanandmagSettings();
}
@Override
public void close()
{
encoder.close();
}
/**

129
swervelib/encoders/DIODutyCycleEncoderSwerve.java Normal file
View File

@@ -0,0 +1,129 @@
package swervelib.encoders;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.Timer;
/**
* DutyCycle encoders such as "US Digital MA3 with DIO Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
* Encoder." attached via a DIO lane.
* <p>
* Credits to
* <a href="https://github.com/p2reneker25/2035-YAGSL/blob/main/swervelib/encoders/DIODutyCycleEncoderSwerve.java">
* p2reneker25</a> for building this.
*/
public class DIODutyCycleEncoderSwerve extends SwerveAbsoluteEncoder
{
/**
* Duty Cycle Encoder.
*/
private final DutyCycleEncoder encoder;
/**
* Inversion state.
*/
private boolean isInverted;
/**
* An {@link Alert} for if the encoder cannot report accurate velocities.
*/
private Alert inaccurateVelocities;
/**
* The Offset in degrees of the DIO absolute encoder.
*/
private double offset;
/**
* Constructor for the DIO duty cycle encoder.
*
* @param pin DIO lane for the encoder.
*/
public DIODutyCycleEncoderSwerve(int pin)
{
encoder = new DutyCycleEncoder(pin);
Timer.delay(2);
inaccurateVelocities = new Alert(
"Encoders",
"The DIO Duty Cycle encoder may not report accurate velocities!",
AlertType.kWarning);
}
@Override
public void close()
{
encoder.close();
}
/**
* Configure the inversion state of the encoder.
*
* @param inverted Whether the encoder is inverted.
*/
@Override
public void configure(boolean inverted)
{
isInverted = inverted;
}
/**
* Get the absolute position of the encoder.
*
* @return Absolute position in degrees from [0, 360).
*/
@Override
public double getAbsolutePosition()
{
return (isInverted ? -1.0 : 1.0) * ((encoder.get() * 360) - offset);
}
/**
* Get the encoder object.
*
* @return {@link DutyCycleEncoder} from the class.
*/
@Override
public Object getAbsoluteEncoder()
{
return encoder;
}
/**
* Get the velocity in degrees/sec.
*
* @return velocity in degrees/sec.
*/
@Override
public double getVelocity()
{
inaccurateVelocities.set(true);
return encoder.get();
}
/**
* Reset the encoder to factory defaults.
*/
@Override
public void factoryDefault()
{
// Do nothing
}
/**
* Clear sticky faults on the encoder.
*/
@Override
public void clearStickyFaults()
{
// Do nothing
}
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
this.offset = offset;
return true;
}
}

165
swervelib/encoders/SparkFlexEncoderSwerve.java Normal file
View File

@@ -0,0 +1,165 @@
package swervelib.encoders;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.spark.SparkAbsoluteEncoder;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.config.SparkFlexConfig;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import swervelib.motors.SparkFlexSwerve;
import swervelib.motors.SwerveMotor;
/**
* SparkFlex absolute encoder, attached through the data port.
*/
public class SparkFlexEncoderSwerve extends SwerveAbsoluteEncoder
{
/**
* The {@link AbsoluteEncoder} representing the duty cycle encoder attached to the SparkFlex.
*/
public SparkAbsoluteEncoder encoder;
/**
* An {@link Alert} for if there is a failure configuring the encoder.
*/
private Alert failureConfiguring;
/**
* {@link SparkFlexSwerve} instance.
*/
private SwerveMotor sparkFlex;
/**
* Create the {@link SparkFlexEncoderSwerve} object as a duty cycle from the {@link SparkFlex} motor.
*
* @param motor Motor to create the encoder from.
* @param conversionFactor The conversion factor to set if the output is not from 0 to 360.
*/
public SparkFlexEncoderSwerve(SwerveMotor motor, int conversionFactor)
{
failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkFlex Absolute Encoder",
AlertType.kWarning);
if (motor.getMotor() instanceof SparkFlex)
{
sparkFlex = motor;
encoder = ((SparkFlex) motor.getMotor()).getAbsoluteEncoder();
setConversionFactor(conversionFactor);
} else
{
throw new RuntimeException("Motor given to instantiate SparkFlexEncoder is not a CANSparkFlex");
}
}
@Override
public void close()
{
// SPARK Flex encoder gets closed with the motor
// I don't think an encoder getting closed should
// close the entire motor so i will keep this empty
// sparkFlex.close();
}
/**
* Reset the encoder to factory defaults.
*/
@Override
public void factoryDefault()
{
// Do nothing
}
/**
* Clear sticky faults on the encoder.
*/
@Override
public void clearStickyFaults()
{
// Do nothing
}
/**
* Configure the absolute encoder to read from [0, 360) per second.
*
* @param inverted Whether the encoder is inverted.
*/
@Override
public void configure(boolean inverted)
{
if (sparkFlex instanceof SparkFlexSwerve)
{
SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
cfg.absoluteEncoder.inverted(inverted);
((SparkFlexSwerve) sparkFlex).updateConfig(cfg);
}
}
/**
* Set the conversion factor of the {@link SparkFlexEncoderSwerve}.
*
* @param conversionFactor Position conversion factor from ticks to unit.
*/
public void setConversionFactor(double conversionFactor)
{
SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(conversionFactor)
.velocityConversionFactor(conversionFactor / 60);
((SparkFlexSwerve) sparkFlex).updateConfig(cfg);
}
/**
* Get the absolute position of the encoder.
*
* @return Absolute position in degrees from [0, 360).
*/
@Override
public double getAbsolutePosition()
{
return encoder.getPosition();
}
/**
* Get the instantiated absolute encoder Object.
*
* @return Absolute encoder object.
*/
@Override
public Object getAbsoluteEncoder()
{
return encoder;
}
/**
* Sets the Absolute Encoder Offset inside of the SparkFlex's Memory.
*
* @param offset the offset the Absolute Encoder uses as the zero point.
* @return if setting Absolute Encoder Offset was successful or not.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
if (sparkFlex instanceof SparkFlexSwerve)
{
SparkFlexConfig cfg = ((SparkFlexSwerve) sparkFlex).getConfig();
cfg.absoluteEncoder.zeroOffset(offset);
((SparkFlexSwerve) sparkFlex).updateConfig(cfg);
return true;
}
return false;
}
/**
* Get the velocity in degrees/sec.
*
* @return velocity in degrees/sec.
*/
@Override
public double getVelocity()
{
return encoder.getVelocity();
}
}

60
swervelib/encoders/SparkMaxAnalogEncoderSwerve.java
View File

@@ -3,6 +3,7 @@ package swervelib.encoders;
import com.revrobotics.REVLibError;
import com.revrobotics.spark.SparkAnalogSensor;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
@@ -47,8 +48,7 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
{
sparkMax = motor;
encoder = ((SparkMax) motor.getMotor()).getAnalog();
motor.setAbsoluteEncoder(this);
sparkMax.configureIntegratedEncoder(360 / maxVoltage);
setConversionFactor(360.0 / maxVoltage);
} else
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
@@ -64,6 +64,15 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
}
@Override
public void close()
{
// SPARK MAX Analog encoder gets closed with the motor
// I don't think an encoder getting closed should
// close the entire motor so i will keep this empty
// sparkMax.close();
}
/**
* Run the configuration until it succeeds or times out.
*
@@ -81,6 +90,49 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
failureConfiguring.set(true);
}
/**
* Set the conversion factor of the {@link SparkMaxAnalogEncoderSwerve}.
*
* @param conversionFactor Position conversion factor from ticks to unit.
*/
public void setConversionFactor(double conversionFactor)
{
SparkMaxConfig cfg = null;
if (sparkMax instanceof SparkMaxSwerve)
{
cfg = ((SparkMaxSwerve) sparkMax).getConfig();
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
}
if (cfg != null)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
cfg.signals
.analogVelocityAlwaysOn(true)
.analogVoltageAlwaysOn(true)
.analogPositionAlwaysOn(true)
.analogVoltagePeriodMs(20)
.analogPositionPeriodMs(20)
.analogVelocityPeriodMs(20);
cfg.analogSensor
.positionConversionFactor(conversionFactor)
.velocityConversionFactor(conversionFactor / 60);
}
if (sparkMax instanceof SparkMaxSwerve)
{
((SparkMaxSwerve) sparkMax).updateConfig(cfg);
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
}
}
/**
* Reset the encoder to factory defaults.
*/
@@ -110,12 +162,12 @@ public class SparkMaxAnalogEncoderSwerve extends SwerveAbsoluteEncoder
if (sparkMax instanceof SparkMaxSwerve)
{
SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
cfg.analogSensor.inverted(true);
cfg.analogSensor.inverted(inverted);
((SparkMaxSwerve) sparkMax).updateConfig(cfg);
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
cfg.analogSensor.inverted(true);
cfg.analogSensor.inverted(inverted);
((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
}
}

78
swervelib/encoders/SparkMaxEncoderSwerve.java
View File

@@ -4,6 +4,7 @@ import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.REVLibError;
import com.revrobotics.spark.SparkAbsoluteEncoder;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
@@ -46,7 +47,7 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
{
failureConfiguring = new Alert(
"Encoders",
"Failure configuring SparkMax Analog Encoder",
"Failure configuring SparkMax Absolute Encoder",
AlertType.kWarning);
offsetFailure = new Alert(
"Encoders",
@@ -56,14 +57,22 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
{
sparkMax = motor;
encoder = ((SparkMax) motor.getMotor()).getAbsoluteEncoder();
motor.setAbsoluteEncoder(this);
motor.configureIntegratedEncoder(conversionFactor);
setConversionFactor(conversionFactor);
} else
{
throw new RuntimeException("Motor given to instantiate SparkMaxEncoder is not a CANSparkMax");
}
}
@Override
public void close()
{
// SPARK MAX encoder gets closed with the motor
// I don't think an encoder getting closed should
// close the entire motor so i will keep this empty
// sparkFlex.close();
}
/**
* Run the configuration until it succeeds or times out.
*
@@ -110,16 +119,62 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
if (sparkMax instanceof SparkMaxSwerve)
{
SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
cfg.analogSensor.inverted(true);
cfg.absoluteEncoder.inverted(inverted);
((SparkMaxSwerve) sparkMax).updateConfig(cfg);
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
cfg.analogSensor.inverted(true);
cfg.absoluteEncoder.inverted(inverted);
((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
}
}
/**
* Set the conversion factor of the {@link SparkMaxEncoderSwerve}.
*
* @param conversionFactor Position conversion factor from ticks to unit.
*/
public void setConversionFactor(double conversionFactor)
{
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
// This needs to be set to 20ms or under to properly update the swerve module position for odometry
// Configuration taken from 3005, the team who helped develop the Max Swerve:
// https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
// Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
// From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
SparkMaxConfig cfg = null;
if (sparkMax instanceof SparkMaxSwerve)
{
cfg = ((SparkMaxSwerve) sparkMax).getConfig();
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
}
if (cfg != null)
{
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(conversionFactor)
.velocityConversionFactor(conversionFactor / 60);
}
if (sparkMax instanceof SparkMaxSwerve)
{
((SparkMaxSwerve) sparkMax).updateConfig(cfg);
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
}
}
/**
* Get the absolute position of the encoder.
*
@@ -151,19 +206,6 @@ public class SparkMaxEncoderSwerve extends SwerveAbsoluteEncoder
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
if (sparkMax instanceof SparkMaxSwerve)
{
SparkMaxConfig cfg = ((SparkMaxSwerve) sparkMax).getConfig();
cfg.absoluteEncoder.zeroOffset(offset);
((SparkMaxSwerve) sparkMax).updateConfig(cfg);
return true;
} else if (sparkMax instanceof SparkMaxBrushedMotorSwerve)
{
SparkMaxConfig cfg = ((SparkMaxBrushedMotorSwerve) sparkMax).getConfig();
cfg.absoluteEncoder.zeroOffset(offset);
((SparkMaxBrushedMotorSwerve) sparkMax).updateConfig(cfg);
return true;
}
return false;
}

8
swervelib/encoders/SwerveAbsoluteEncoder.java
View File

@@ -3,9 +3,15 @@ package swervelib.encoders;
/**
* Swerve abstraction class to define a standard interface with absolute encoders for swerve modules..
*/
public abstract class SwerveAbsoluteEncoder
public abstract class SwerveAbsoluteEncoder implements AutoCloseable
{
// This is a bit weird because some encoders are closable
// while some get closed with the motor controller
// so for some encoders this will be an empty function
@Override
public abstract void close();
/**
* The maximum amount of times the swerve encoder will attempt to configure itself if failures occur.
*/

9
swervelib/encoders/TalonSRXEncoderSwerve.java
View File

@@ -44,6 +44,15 @@ public class TalonSRXEncoderSwerve extends SwerveAbsoluteEncoder
}
}
@Override
public void close()
{
// TalonSRX encoder gets closed with the motor
// I don't think an encoder getting closed should
// close the entire motor so i will keep this empty
// sparkFlex.close();
}
@Override
public void factoryDefault()
{

178
swervelib/encoders/ThriftyNovaEncoderSwerve.java
View File

@@ -6,89 +6,111 @@ import swervelib.motors.ThriftyNovaSwerve;
/**
* Thrifty Nova absolute encoder, attached through the data port.
*/
public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder {
/** The absolute encoder is directly interfaced through the Thrifty Nova motor. */
protected ThriftyNovaSwerve motor;
/**
* Inversion state of the attached encoder.
*/
protected boolean inverted = false;
/**
* Offset of the absolute encoder.
*/
protected double offset = 0.0;
/**
* Create the {@link ThriftyNovaEncoderSwerve} object as an absolute encoder from the {@link ThriftyNovaSwerve} motor.
*
* @param motor {@link SwerveMotor} through which to interface with the attached encoder .
*/
public ThriftyNovaEncoderSwerve(SwerveMotor motor) {
this.motor = (ThriftyNovaSwerve)motor;
motor.setAbsoluteEncoder(null);
}
public class ThriftyNovaEncoderSwerve extends SwerveAbsoluteEncoder
{
/**
* Set factory default.
*/
@Override
public void factoryDefault() {
}
/**
* The absolute encoder is directly interfaced through the Thrifty Nova motor.
*/
protected ThriftyNovaSwerve motor;
/**
* Inversion state of the attached encoder.
*/
protected boolean inverted = false;
/**
* Offset of the absolute encoder.
*/
protected double offset = 0.0;
/**
* Clear sticky faults.
*/
@Override
public void clearStickyFaults() {
}
/**
* Create the {@link ThriftyNovaEncoderSwerve} object as an absolute encoder from the {@link ThriftyNovaSwerve}
* motor.
*
* @param motor {@link SwerveMotor} through which to interface with the attached encoder .
*/
public ThriftyNovaEncoderSwerve(SwerveMotor motor)
{
this.motor = (ThriftyNovaSwerve) motor;
motor.setAbsoluteEncoder(null);
}
/**
* Configure the absolute encoder.
*
* @param inverted Whether the encoder is inverted.
*/
@Override
public void configure(boolean inverted) {
this.inverted = inverted;
}
@Override
public void close()
{
// ThriftyNova encoder gets closed with the motor
// I don't think an encoder getting closed should
// close the entire motor so i will keep this empty
// sparkFlex.close();
}
/**
* Get the absolute position of the encoder.
*
* @return Absolute position in degrees from [0, 360).
*/
@Override
public double getAbsolutePosition() {
return (motor.getPosition() + offset) * (inverted ? -1.0 : 1.0);
}
/**
* Set factory default.
*/
@Override
public void factoryDefault()
{
}
/**
* Get the instantiated absolute encoder Object.
*/
@Override
public Object getAbsoluteEncoder() {
return null;
}
/**
* Clear sticky faults.
*/
@Override
public void clearStickyFaults()
{
}
/**
* Set the absolute encoder offset.
*
* @param offset offset in degrees from [0, 360).
* @return true if successful.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset) {
this.offset = offset;
return true;
}
/**
* Configure the absolute encoder.
*
* @param inverted Whether the encoder is inverted.
*/
@Override
public void configure(boolean inverted)
{
this.inverted = inverted;
}
/**
* Get the absolute encoder velocity.
* WARNING: Angular velocity is generally not measurable at high speeds.
* @return Velocity in degrees per second.
*/
@Override
public double getVelocity() {
return motor.getVelocity();
}
/**
* Get the absolute position of the encoder.
*
* @return Absolute position in degrees from [0, 360).
*/
@Override
public double getAbsolutePosition()
{
return (motor.getPosition() + offset) * (inverted ? -1.0 : 1.0);
}
/**
* Get the instantiated absolute encoder Object.
*/
@Override
public Object getAbsoluteEncoder()
{
return null;
}
/**
* Set the absolute encoder offset.
*
* @param offset offset in degrees from [0, 360).
* @return true if successful.
*/
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
this.offset = offset;
return true;
}
/**
* Get the absolute encoder velocity. WARNING: Angular velocity is generally not measurable at high speeds.
*
* @return Velocity in degrees per second.
*/
@Override
public double getVelocity()
{
return motor.getVelocity();
}
}

7
swervelib/imu/ADIS16448Swerve.java
View File

@@ -20,7 +20,7 @@ public class ADIS16448Swerve extends SwerveIMU
*/
private final ADIS16448_IMU imu;
/**
* Mutable {@link AngularVelocity} for readings.
* Mutable {@link MutAngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
@@ -42,6 +42,11 @@ public class ADIS16448Swerve extends SwerveIMU
SmartDashboard.putData(imu);
}
@Override
public void close() {
imu.close();
}
/**
* Reset IMU to factory default.
*/

7
swervelib/imu/ADIS16470Swerve.java
View File

@@ -21,7 +21,7 @@ public class ADIS16470Swerve extends SwerveIMU
*/
private final ADIS16470_IMU imu;
/**
* Mutable {@link AngularVelocity} for readings.
* Mutable {@link MutAngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
@@ -44,6 +44,11 @@ public class ADIS16470Swerve extends SwerveIMU
SmartDashboard.putData(imu);
}
@Override
public void close() {
imu.close();
}
/**
* Reset IMU to factory default.
*/

7
swervelib/imu/ADXRS450Swerve.java
View File

@@ -20,7 +20,7 @@ public class ADXRS450Swerve extends SwerveIMU
*/
private final ADXRS450_Gyro imu;
/**
* Mutable {@link AngularVelocity} for readings.
* Mutable {@link MutAngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
@@ -42,6 +42,11 @@ public class ADXRS450Swerve extends SwerveIMU
SmartDashboard.putData(imu);
}
@Override
public void close() {
imu.close();
}
/**
* Reset IMU to factory default.
*/

7
swervelib/imu/AnalogGyroSwerve.java
View File

@@ -20,7 +20,7 @@ public class AnalogGyroSwerve extends SwerveIMU
*/
private final AnalogGyro imu;
/**
* Mutable {@link AngularVelocity} for readings.
* Mutable {@link MutAngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
@@ -49,6 +49,11 @@ public class AnalogGyroSwerve extends SwerveIMU
SmartDashboard.putData(imu);
}
@Override
public void close() {
imu.close();
}
/**
* Reset IMU to factory default.
*/

9
swervelib/imu/CanandgyroSwerve.java
View File

@@ -6,7 +6,6 @@ import com.reduxrobotics.sensors.canandgyro.Canandgyro;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.MutAngularVelocity;
import java.util.Optional;
/**
@@ -24,7 +23,7 @@ public class CanandgyroSwerve extends SwerveIMU
*/
private final Canandgyro imu;
/**
* Mutable {@link AngularVelocity} for readings.
* Mutable {@link MutAngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, RotationsPerSecond);
/**
@@ -46,6 +45,12 @@ public class CanandgyroSwerve extends SwerveIMU
imu = new Canandgyro(canid);
}
@Override
public void close()
{
imu.close();
}
/**
* Reset {@link Canandgyro} to factory default.
*/

6
swervelib/imu/NavXSwerve.java
View File

@@ -18,7 +18,7 @@ public class NavXSwerve extends SwerveIMU
{
/**
* Mutable {@link AngularVelocity} for readings.
* Mutable {@link MutAngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
@@ -60,6 +60,10 @@ public class NavXSwerve extends SwerveIMU
}
}
@Override
public void close() {
imu.close();
}
/**
* Reset offset to current gyro reading. Does not call NavX({@link AHRS#reset()}) because it has been reported to be

56
swervelib/imu/Pigeon2Swerve.java
View File

@@ -23,41 +23,41 @@ public class Pigeon2Swerve extends SwerveIMU
/**
* Wait time for status frames to show up.
*/
public static double STATUS_TIMEOUT_SECONDS = 0.04;
public static double STATUS_TIMEOUT_SECONDS = 0.04;
/**
* {@link Pigeon2} IMU device.
*/
private final Pigeon2 imu;
private final Pigeon2 imu;
/**
* Mutable {@link AngularVelocity} for readings.
* Mutable {@link MutAngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the {@link Pigeon2}.
*/
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* {@link Pigeon2} configurator.
*/
private Pigeon2Configurator cfg;
private final MutAngularVelocity yawVel = new MutAngularVelocity(0,
0,
DegreesPerSecond);
/**
* X Acceleration supplier
*/
private Supplier<StatusSignal<LinearAcceleration>> xAcc;
private final Supplier<StatusSignal<LinearAcceleration>> xAcc;
/**
* Y Accelleration supplier.
*/
private Supplier<StatusSignal<LinearAcceleration>> yAcc;
private final Supplier<StatusSignal<LinearAcceleration>> yAcc;
/**
* Z Acceleration supplier.
*/
private Supplier<StatusSignal<LinearAcceleration>> zAcc;
private final Supplier<StatusSignal<LinearAcceleration>> zAcc;
/**
* Offset for the {@link Pigeon2}.
*/
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* {@link Pigeon2} configurator.
*/
private Pigeon2Configurator cfg;
/**
* Generate the SwerveIMU for {@link Pigeon2}.
@@ -85,6 +85,12 @@ public class Pigeon2Swerve extends SwerveIMU
this(canid, "");
}
@Override
public void close() {
imu.close();
}
/**
* Reset {@link Pigeon2} to factory default.
*/
@@ -159,15 +165,15 @@ public class Pigeon2Swerve extends SwerveIMU
@Override
public Optional<Translation3d> getAccel()
{
// TODO: Implement later.
return Optional.empty();
return Optional.of(new Translation3d(xAcc.get().getValueAsDouble(),
yAcc.get().getValueAsDouble(),
zAcc.get().getValueAsDouble()));
}
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return yawVel.mut_replace(imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue());
return yawVel.mut_replace(imu.getAngularVelocityZWorld().refresh().getValue());
}
/**

8
swervelib/imu/PigeonSwerve.java
View File

@@ -21,7 +21,7 @@ public class PigeonSwerve extends SwerveIMU
*/
private final WPI_PigeonIMU imu;
/**
* Mutable {@link AngularVelocity} for readings.
* Mutable {@link MutAngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
@@ -45,6 +45,12 @@ public class PigeonSwerve extends SwerveIMU
SmartDashboard.putData(imu);
}
@Override
public void close() {
imu.close();
}
/**
* Reset IMU to factory default.
*/

158
swervelib/imu/PigeonViaTalonSRXSwerve.java Normal file
View File

@@ -0,0 +1,158 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.DegreesPerSecond;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import edu.wpi.first.math.geometry.Quaternion;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
/**
* SwerveIMU interface for the {@link WPI_PigeonIMU}.
*/
public class PigeonViaTalonSRXSwerve extends SwerveIMU
{
/**
* {@link TalonSRX} TalonSRX the IMU is attached to.
*/
private final WPI_TalonSRX talon;
/**
* {@link WPI_PigeonIMU} IMU device.
*/
private final WPI_PigeonIMU imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the {@link WPI_PigeonIMU}.
*/
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
/**
* Generate the SwerveIMU for {@link WPI_PigeonIMU} attached to a {@link TalonSRX}.
*
* @param canid CAN ID for the {@link TalonSRX} the {@link WPI_PigeonIMU} is attached to, does not support CANBus.
*/
public PigeonViaTalonSRXSwerve(int canid)
{
talon = new WPI_TalonSRX(canid);
imu = new WPI_PigeonIMU(talon);
offset = new Rotation3d();
SmartDashboard.putData(imu);
}
@Override
public void close() {
imu.close();
talon.close();
}
/**
* Reset IMU to factory default.
*/
@Override
public void factoryDefault()
{
imu.configFactoryDefault();
}
/**
* Clear sticky faults on IMU.
*/
@Override
public void clearStickyFaults()
{
imu.clearStickyFaults();
}
/**
* Set the gyro offset.
*
* @param offset gyro offset as a {@link Rotation3d}.
*/
public void setOffset(Rotation3d offset)
{
this.offset = offset;
}
/**
* Set the gyro to invert its default direction
*
* @param invertIMU invert gyro direction
*/
public void setInverted(boolean invertIMU)
{
invertedIMU = invertIMU;
}
/**
* Fetch the {@link Rotation3d} from the IMU without any zeroing. Robot relative.
*
* @return {@link Rotation3d} from the IMU.
*/
@Override
public Rotation3d getRawRotation3d()
{
double[] wxyz = new double[4];
imu.get6dQuaternion(wxyz);
Rotation3d reading = new Rotation3d(new Quaternion(wxyz[0], wxyz[1], wxyz[2], wxyz[3]));
return invertedIMU ? reading.unaryMinus() : reading;
}
/**
* Fetch the {@link Rotation3d} from the IMU. Robot relative.
*
* @return {@link Rotation3d} from the IMU.
*/
@Override
public Rotation3d getRotation3d()
{
return getRawRotation3d().minus(offset);
}
/**
* Fetch the acceleration [x, y, z] from the IMU in meters per second squared. If acceleration isn't supported returns
* empty.
*
* @return {@link Translation3d} of the acceleration as an {@link Optional}.
*/
@Override
public Optional<Translation3d> getAccel()
{
short[] initial = new short[3];
imu.getBiasedAccelerometer(initial);
return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0));
}
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return yawVel.mut_setMagnitude(imu.getRate());
}
/**
* Get the instantiated {@link WPI_PigeonIMU} IMU object.
*
* @return IMU object.
*/
@Override
public Object getIMU()
{
return imu;
}
}

6
swervelib/imu/SwerveIMU.java
View File

@@ -3,15 +3,17 @@ package swervelib.imu;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Translation3d;
import edu.wpi.first.units.measure.MutAngularVelocity;
import java.util.Optional;
/**
* Swerve IMU abstraction to define a standard interface with a swerve drive.
*/
public abstract class SwerveIMU
public abstract class SwerveIMU implements AutoCloseable
{
@Override
public abstract void close();
/**
* Reset IMU to factory default.
*/

2
swervelib/math/SwerveMath.java
View File

@@ -95,7 +95,7 @@ public class SwerveMath
optimalVoltage
/ calculateMaxAcceleration(wheelGripCoefficientOfFriction);
/// ^ Volt-seconds^2 per meter (max voltage divided by max accel)
return new SimpleMotorFeedforward(0, kv, ka);
return new SimpleMotorFeedforward(0, kv, 0);
}
/**

146
swervelib/motors/SparkFlexSwerve.java
View File

@@ -6,22 +6,22 @@ import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkFlex;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import com.revrobotics.spark.config.SparkFlexConfig;
import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.Optional;
import java.util.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
@@ -33,46 +33,42 @@ import swervelib.telemetry.SwerveDriveTelemetry;
public class SparkFlexSwerve extends SwerveMotor
{
/**
* Config retry delay.
*/
private final double configDelay = Milliseconds.of(5).in(Seconds);
/**
* {@link SparkFlex} Instance.
*/
private final SparkFlex motor;
private final SparkFlex motor;
/**
* Integrated encoder.
*/
public RelativeEncoder encoder;
public RelativeEncoder encoder;
/**
* Absolute encoder attached to the SparkFlex (if exists)
*/
public SwerveAbsoluteEncoder absoluteEncoder;
public Optional<SwerveAbsoluteEncoder> absoluteEncoder = Optional.empty();
/**
* Closed-loop PID controller.
*/
public SparkClosedLoopController pid;
public SparkClosedLoopController pid;
/**
* Supplier for the velocity of the motor controller.
*/
private Supplier<Double> velocity;
private Supplier<Double> velocity;
/**
* Supplier for the position of the motor controller.
*/
private Supplier<Double> position;
/**
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
private Supplier<Double> position;
/**
* An {@link Alert} for if there is an error configuring the motor.
*/
private Alert failureConfiguring;
/**
* An {@link Alert} for if the absolute encoder's offset is set in the json instead of the hardware client.
*/
private Alert absoluteEncoderOffsetWarning;
private Alert failureConfiguring;
/**
* Configuration object for {@link SparkFlex} motor.
*/
private SparkFlexConfig cfg = new SparkFlexConfig();
private SparkFlexConfig cfg = new SparkFlexConfig();
/**
@@ -86,6 +82,10 @@ public class SparkFlexSwerve extends SwerveMotor
{
this.motor = motor;
this.isDriveMotor = isDriveMotor;
failureConfiguring = new Alert("Motors",
"Failure configuring motor " +
motor.getDeviceId(),
AlertType.kWarning);
factoryDefaults();
clearStickyFaults();
@@ -95,14 +95,7 @@ public class SparkFlexSwerve extends SwerveMotor
// Spin off configurations in a different thread.
// configureSparkFlex(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback.
failureConfiguring = new Alert("Motors",
"Failure configuring motor " +
motor.getDeviceId(),
AlertType.kWarning);
absoluteEncoderOffsetWarning = new Alert("Motors",
"IF possible configure the duty cycle encoder offset in the REV Hardware Client instead of using the " +
"absoluteEncoderOffset in the Swerve Module JSON!",
AlertType.kWarning);
velocity = encoder::getVelocity;
position = encoder::getPosition;
}
@@ -132,11 +125,17 @@ public class SparkFlexSwerve extends SwerveMotor
{
return;
}
Timer.delay(Milliseconds.of(5).in(Seconds));
Timer.delay(configDelay);
}
failureConfiguring.set(true);
}
@Override
public void close()
{
motor.close();
}
/**
* Get the current configuration of the {@link SparkFlex}
*
@@ -230,9 +229,9 @@ public class SparkFlexSwerve extends SwerveMotor
* @return connected absolute encoder state.
*/
@Override
public boolean isAttachedAbsoluteEncoder()
public boolean usingExternalFeedbackSensor()
{
return absoluteEncoder != null;
return absoluteEncoder.isPresent();
}
/**
@@ -264,7 +263,7 @@ public class SparkFlexSwerve extends SwerveMotor
{
if (encoder == null)
{
absoluteEncoder = null;
this.absoluteEncoder = Optional.empty();
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
velocity = this.encoder::getVelocity;
@@ -272,11 +271,10 @@ public class SparkFlexSwerve extends SwerveMotor
} else if (encoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
absoluteEncoderOffsetWarning.set(true);
absoluteEncoder = encoder;
this.absoluteEncoder = Optional.of(encoder);
velocity = absoluteEncoder::getVelocity;
position = absoluteEncoder::getAbsolutePosition;
velocity = this.absoluteEncoder.get()::getVelocity;
position = this.absoluteEncoder.get()::getAbsolutePosition;
}
return this;
}
@@ -301,56 +299,31 @@ public class SparkFlexSwerve extends SwerveMotor
.primaryEncoderVelocityAlwaysOn(false)
.iAccumulationAlwaysOn(false)
.appliedOutputPeriodMs(10)
.faultsPeriodMs(20)
;
if (absoluteEncoder == null)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
.faultsPeriodMs(20);
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
} else
{
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
// This needs to be set to 20ms or under to properly update the swerve module position for odometry
// Configuration taken from 3005, the team who helped develop the Max Swerve:
// https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
// Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
// From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
}
}
}
@@ -382,6 +355,15 @@ public class SparkFlexSwerve extends SwerveMotor
}
/**
* Disable PID Wrapping on the motor.
*/
@Override
public void disablePIDWrapping()
{
cfg.closedLoop.positionWrappingEnabled(false);
}
/**
* Set the idle mode.
*
@@ -539,7 +521,7 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void setPosition(double position)
{
if (absoluteEncoder == null)
if (absoluteEncoder.isEmpty())
{
configureSparkFlex(() -> encoder.setPosition(position));
}

150
swervelib/motors/SparkMaxBrushedMotorSwerve.java
View File

@@ -3,13 +3,12 @@ package swervelib.motors;
import static edu.wpi.first.units.Units.Milliseconds;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkBase.PersistMode;
import com.revrobotics.spark.SparkBase.ResetMode;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import com.revrobotics.spark.SparkMax;
@@ -35,50 +34,50 @@ import swervelib.telemetry.SwerveDriveTelemetry;
public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
/**
* Config retry delay.
*/
private final double configDelay = Milliseconds.of(5).in(Seconds);
/**
* SparkMAX Instance.
*/
private final SparkMax motor;
private final SparkMax motor;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
public SwerveAbsoluteEncoder absoluteEncoder;
public Optional<SwerveAbsoluteEncoder> absoluteEncoder;
/**
* Integrated encoder.
*/
public Optional<RelativeEncoder> encoder = Optional.empty();
public Optional<RelativeEncoder> encoder = Optional.empty();
/**
* Closed-loop PID controller.
*/
public SparkClosedLoopController pid;
public SparkClosedLoopController pid;
/**
* Supplier for the velocity of the motor controller.
*/
private Supplier<Double> velocity;
private Supplier<Double> velocity;
/**
* Supplier for the position of the motor controller.
*/
private Supplier<Double> position;
/**
* Factory default already occurred.
*/
private boolean factoryDefaultOccurred = false;
private Supplier<Double> position;
/**
* An {@link Alert} for if the motor has no encoder.
*/
private Alert noEncoderAlert;
private Alert noEncoderAlert;
/**
* An {@link Alert} for if there is an error configuring the motor.
*/
private Alert failureConfiguringAlert;
private Alert failureConfiguringAlert;
/**
* An {@link Alert} for if the motor has no encoder defined.
*/
private Alert noEncoderDefinedAlert;
private Alert noEncoderDefinedAlert;
/**
* Configuration object for {@link SparkMax} motor.
*/
private SparkMaxConfig cfg = new SparkMaxConfig();
private SparkMaxConfig cfg = new SparkMaxConfig();
/**
* Initialize the swerve motor.
@@ -187,11 +186,17 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
return;
}
Timer.delay(Milliseconds.of(5).in(Seconds));
Timer.delay(configDelay);
}
failureConfiguringAlert.set(true);
}
@Override
public void close()
{
motor.close();
}
/**
* Get the current configuration of the {@link SparkMax}
*
@@ -284,9 +289,9 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
* @return connected absolute encoder state.
*/
@Override
public boolean isAttachedAbsoluteEncoder()
public boolean usingExternalFeedbackSensor()
{
return absoluteEncoder != null;
return absoluteEncoder.isPresent();
}
/**
@@ -318,7 +323,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
if (encoder == null)
{
absoluteEncoder = null;
this.absoluteEncoder = Optional.empty();
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
this.encoder.ifPresentOrElse((RelativeEncoder enc) -> {
@@ -333,16 +338,12 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
DriverStation.reportWarning(
"IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
" absoluteEncoderOffset in the Swerve Module JSON!",
false);
absoluteEncoder = encoder;
velocity = absoluteEncoder::getVelocity;
position = absoluteEncoder::getAbsolutePosition;
this.absoluteEncoder = Optional.of(encoder);
velocity = this.absoluteEncoder.get()::getVelocity;
position = this.absoluteEncoder.get()::getAbsolutePosition;
noEncoderDefinedAlert.set(false);
}
if (absoluteEncoder == null && this.encoder.isEmpty())
if (absoluteEncoder.isEmpty() && this.encoder.isEmpty())
{
noEncoderDefinedAlert.set(true);
throw new RuntimeException("An encoder MUST be defined to work with a SparkMAX");
@@ -371,65 +372,29 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
.iAccumulationAlwaysOn(false)
.appliedOutputPeriodMs(10)
.faultsPeriodMs(20);
if (absoluteEncoder == null)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
} else
{
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
// This needs to be set to 20ms or under to properly update the swerve module position for odometry
// Configuration taken from 3005, the team who helped develop the Max Swerve:
// https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
// Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
// From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
} else
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
cfg.signals
.analogVoltageAlwaysOn(true)
.analogPositionAlwaysOn(true)
.analogVoltagePeriodMs(20)
.analogPositionPeriodMs(20);
cfg.analogSensor
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
}
}
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
}
/**
@@ -460,6 +425,15 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
}
/**
* Disable PID Wrapping on the motor.
*/
@Override
public void disablePIDWrapping()
{
cfg.closedLoop.positionWrappingEnabled(false);
}
/**
* Set the idle mode.
*
@@ -480,6 +454,10 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
public void setInverted(boolean inverted)
{
cfg.inverted(inverted);
if (isDriveMotor)
{
cfg.encoder.inverted(inverted);
}
}
/**
@@ -620,7 +598,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void setPosition(double position)
{
if (absoluteEncoder == null)
if (absoluteEncoder.isEmpty())
{
encoder.ifPresent((RelativeEncoder enc) -> {
configureSparkMax(() -> enc.setPosition(position));

141
swervelib/motors/SparkMaxSwerve.java
View File

@@ -3,7 +3,6 @@ package swervelib.motors;
import static edu.wpi.first.units.Units.Milliseconds;
import static edu.wpi.first.units.Units.Seconds;
import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.REVLibError;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.spark.ClosedLoopSlot;
@@ -19,6 +18,7 @@ import com.revrobotics.spark.config.SparkMaxConfig;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Timer;
import java.util.Optional;
import java.util.function.Supplier;
import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
import swervelib.encoders.SparkMaxEncoderSwerve;
@@ -32,38 +32,38 @@ import swervelib.telemetry.SwerveDriveTelemetry;
public class SparkMaxSwerve extends SwerveMotor
{
/**
* Config retry delay.
*/
private final double configDelay = Milliseconds.of(5).in(Seconds);
/**
* {@link SparkMax} Instance.
*/
private final SparkMax motor;
private final SparkMax motor;
/**
* Integrated encoder.
*/
public RelativeEncoder encoder;
/**
* Absolute encoder attached to the SparkMax (if exists)
*/
public SwerveAbsoluteEncoder absoluteEncoder;
public RelativeEncoder encoder;
/**
* Closed-loop PID controller.
*/
public SparkClosedLoopController pid;
public SparkClosedLoopController pid;
/**
* Factory default already occurred.
* Absolute encoder attached to the SparkMax (if exists)
*/
private boolean factoryDefaultOccurred = false;
private Optional<SwerveAbsoluteEncoder> absoluteEncoder = Optional.empty();
/**
* Supplier for the velocity of the motor controller.
*/
private Supplier<Double> velocity;
private Supplier<Double> velocity;
/**
* Supplier for the position of the motor controller.
*/
private Supplier<Double> position;
private Supplier<Double> position;
/**
* Configuration object for {@link SparkMax} motor.
*/
private SparkMaxConfig cfg = new SparkMaxConfig();
private SparkMaxConfig cfg = new SparkMaxConfig();
/**
@@ -118,11 +118,17 @@ public class SparkMaxSwerve extends SwerveMotor
{
return;
}
Timer.delay(Milliseconds.of(5).in(Seconds));
Timer.delay(configDelay);
}
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
}
@Override
public void close()
{
motor.close();
}
/**
* Get the current configuration of the {@link SparkMax}
*
@@ -217,9 +223,9 @@ public class SparkMaxSwerve extends SwerveMotor
* @return connected absolute encoder state.
*/
@Override
public boolean isAttachedAbsoluteEncoder()
public boolean usingExternalFeedbackSensor()
{
return absoluteEncoder != null;
return absoluteEncoder.isPresent();
}
/**
@@ -251,7 +257,7 @@ public class SparkMaxSwerve extends SwerveMotor
{
if (encoder == null)
{
absoluteEncoder = null;
this.absoluteEncoder = Optional.empty();
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
velocity = this.encoder::getVelocity;
@@ -262,13 +268,9 @@ public class SparkMaxSwerve extends SwerveMotor
cfg.closedLoop.feedbackSensor(encoder instanceof SparkMaxAnalogEncoderSwerve
? FeedbackSensor.kAnalogSensor : FeedbackSensor.kAbsoluteEncoder);
DriverStation.reportWarning(
"IF possible configure the encoder offset in the REV Hardware Client instead of using the" +
" absoluteEncoderOffset in the Swerve Module JSON!",
false);
absoluteEncoder = encoder;
velocity = absoluteEncoder::getVelocity;
position = absoluteEncoder::getAbsolutePosition;
this.absoluteEncoder = Optional.of(encoder);
velocity = this.absoluteEncoder.get()::getVelocity;
position = this.absoluteEncoder.get()::getAbsolutePosition;
}
return this;
}
@@ -295,66 +297,29 @@ public class SparkMaxSwerve extends SwerveMotor
.appliedOutputPeriodMs(10)
.faultsPeriodMs(20);
if (absoluteEncoder == null)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
cfg.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
cfg.encoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
// Changes the measurement period and number of samples used to calculate the velocity for the intergrated motor controller
// Notability this changes the returned velocity and the velocity used for the onboard velocity PID loop (TODO: triple check the PID portion of this statement)
// Default settings of 32ms and 8 taps introduce ~100ms of measurement lag
// https://www.chiefdelphi.com/t/shooter-encoder/400211/11
// This value was taken from:
// https://github.com/Mechanical-Advantage/RobotCode2023/blob/9884d13b2220b76d430e82248fd837adbc4a10bc/src/main/java/org/littletonrobotics/frc2023/subsystems/drive/ModuleIOSparkMax.java#L132-L133
// and tested on 9176 for YAGSL, notably 3005 uses 16ms instead of 10 but 10 is more common based on github searches
cfg.encoder
.quadratureMeasurementPeriod(10)
.quadratureAverageDepth(2);
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
// Taken from
// https://github.com/frc3512/SwerveBot-2022/blob/9d31afd05df6c630d5acb4ec2cf5d734c9093bf8/src/main/java/frc/lib/util/SparkMaxUtil.java#L67
// Unused frames can be set to 65535 to decrease CAN ultilization.
cfg.signals
.primaryEncoderVelocityAlwaysOn(isDriveMotor) // Disable velocity reporting for angle motors.
.primaryEncoderPositionAlwaysOn(true)
.primaryEncoderPositionPeriodMs(20);
} else
{
// By default the SparkMax relays the info from the duty cycle encoder to the roborio every 200ms on CAN frame 5
// This needs to be set to 20ms or under to properly update the swerve module position for odometry
// Configuration taken from 3005, the team who helped develop the Max Swerve:
// https://github.com/FRC3005/Charged-Up-2023-Public/blob/2b6a7c695e23edebafa27a76cf639a00f6e8a3a6/src/main/java/frc/robot/subsystems/drive/REVSwerveModule.java#L227-L244
// Some of the frames can probably be adjusted to decrease CAN utilization, with 65535 being the max.
// From testing, 20ms on frame 5 sometimes returns the same value while constantly powering the azimuth but 8ms may be overkill,
// with limited testing 19ms did not return the same value while the module was constatntly rotating.
if (absoluteEncoder.getAbsoluteEncoder() instanceof AbsoluteEncoder)
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAbsoluteEncoder);
cfg.signals
.absoluteEncoderPositionAlwaysOn(true)
.absoluteEncoderPositionPeriodMs(20);
cfg.absoluteEncoder
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
} else
{
cfg.closedLoop.feedbackSensor(FeedbackSensor.kAnalogSensor);
cfg.signals
.analogVoltageAlwaysOn(true)
.analogPositionAlwaysOn(true)
.analogVoltagePeriodMs(20)
.analogPositionPeriodMs(20);
cfg.analogSensor
.positionConversionFactor(positionConversionFactor)
.velocityConversionFactor(positionConversionFactor / 60);
}
}
}
@@ -387,6 +352,16 @@ public class SparkMaxSwerve extends SwerveMotor
}
/**
* Disable PID Wrapping on the motor.
*/
@Override
public void disablePIDWrapping()
{
cfg.closedLoop
.positionWrappingEnabled(false);
}
/**
* Set the idle mode.
*
@@ -546,7 +521,7 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void setPosition(double position)
{
if (absoluteEncoder == null)
if (absoluteEncoder.isEmpty())
{
configureSparkMax(() -> encoder.setPosition(position));
}

12
swervelib/motors/SwerveMotor.java
View File

@@ -7,9 +7,12 @@ import swervelib.parser.PIDFConfig;
/**
* Swerve motor abstraction which defines a standard interface for motors within a swerve module.
*/
public abstract class SwerveMotor
public abstract class SwerveMotor implements AutoCloseable
{
@Override
public abstract void close();
/**
* The maximum amount of times the swerve motor will attempt to configure a motor if failures occur.
*/
@@ -65,6 +68,11 @@ public abstract class SwerveMotor
*/
public abstract void configurePIDWrapping(double minInput, double maxInput);
/**
* Disable PID Wrapping on the motor.
*/
public abstract void disablePIDWrapping();
/**
* Set the idle mode.
*
@@ -191,5 +199,5 @@ public abstract class SwerveMotor
*
* @return connected absolute encoder state.
*/
public abstract boolean isAttachedAbsoluteEncoder();
public abstract boolean usingExternalFeedbackSensor();
}

467
swervelib/motors/TalonFXSSwerve.java Normal file
View File

@@ -0,0 +1,467 @@
package swervelib.motors;
import static edu.wpi.first.units.Units.Degrees;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.Volts;
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
import com.ctre.phoenix6.configs.TalonFXSConfigurator;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.controls.VelocityVoltage;
import com.ctre.phoenix6.hardware.TalonFXS;
import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.system.plant.DCMotor;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.parser.PIDFConfig;
import swervelib.telemetry.SwerveDriveTelemetry;
/**
* {@link TalonFXS} Swerve Motor. Made by Team 1466 WebbRobotics.
*/
public class TalonFXSSwerve extends SwerveMotor
{
/**
* Wait time for status frames to show up.
*/
public static double STATUS_TIMEOUT_SECONDS = 0.02;
/**
* Factory default already occurred.
*/
private final boolean factoryDefaultOccurred = false;
/**
* Whether the absolute encoder is integrated.
*/
private final boolean absoluteEncoder = false;
/**
* Motion magic angle voltage setter.
*/
private final MotionMagicVoltage m_angleVoltageSetter = new MotionMagicVoltage(0);
/**
* Velocity voltage setter for controlling drive motor.
*/
private final VelocityVoltage m_velocityVoltageSetter = new VelocityVoltage(0);
/**
* TalonFXS motor controller.
*/
private final TalonFXS motor;
/**
* Conversion factor for the motor.
*/
private double conversionFactor;
/**
* Current TalonFXS configuration.
*/
private TalonFXSConfiguration configuration = new TalonFXSConfiguration();
/**
* Current TalonFXS Configurator.
*/
private TalonFXSConfigurator cfg;
/**
* Constructor for TalonFXS swerve motor.
*
* @param motor Motor to use.
* @param isDriveMotor Whether this motor is a drive motor.
* @param motorType {@link DCMotor} which the {@link TalonFXS} is attached to.
*/
public TalonFXSSwerve(TalonFXS motor, boolean isDriveMotor, DCMotor motorType)
{
this.isDriveMotor = isDriveMotor;
this.motor = motor;
this.cfg = motor.getConfigurator();
this.simMotor = motorType;
factoryDefaults();
clearStickyFaults();
}
/**
* Construct the TalonFXS swerve motor given the ID and CANBus.
*
* @param id ID of the TalonFXS on the CANBus.
* @param canbus CANBus on which the TalonFXS is on.
* @param isDriveMotor Whether the motor is a drive or steering motor.
* @param motorType {@link DCMotor} which the {@link TalonFXS} is attached to.
*/
public TalonFXSSwerve(int id, String canbus, boolean isDriveMotor, DCMotor motorType)
{
this(new TalonFXS(id, canbus), isDriveMotor, motorType);
}
/**
* Construct the TalonFXS swerve motor given the ID.
*
* @param id ID of the TalonFXS on the canbus.
* @param isDriveMotor Whether the motor is a drive or steering motor.
* @param motorType {@link DCMotor} which the {@link TalonFXS} is attached to.
*/
public TalonFXSSwerve(int id, boolean isDriveMotor, DCMotor motorType)
{
this(new TalonFXS(id), isDriveMotor, motorType);
}
/**
* Configure the factory defaults.
*/
@Override
public void factoryDefaults()
{
if (!factoryDefaultOccurred)
{
configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake;
configuration.ClosedLoopGeneral.ContinuousWrap = true;
cfg.apply(configuration);
m_angleVoltageSetter.UpdateFreqHz = 0;
// m_angleVoltageExpoSetter.UpdateFreqHz = 0;
m_velocityVoltageSetter.UpdateFreqHz = 0;
// motor.configFactoryDefault();
// motor.setSensorPhase(true);
// motor.configSelectedFeedbackSensor(TalonFXSFeedbackDevice.IntegratedSensor, 0, 30);
// motor.configNeutralDeadband(0.001);
}
}
/**
* Clear the sticky faults on the motor controller.
*/
@Override
public void clearStickyFaults()
{
motor.clearStickyFaults();
}
/**
* Set the absolute encoder to be a compatible absolute encoder.
*
* @param encoder The encoder to use.
*/
@Override
public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
{
// Do not support.
return this;
}
/**
* Configure the integrated encoder for the swerve module. Sets the conversion factors for position and velocity.
*
* @param positionConversionFactor The conversion factor to apply for position.
* <p><br>
* Degrees: <br>
* <code>
* 360 / (angleGearRatio * encoderTicksPerRotation)
* </code><br>
* <p><br>
* Meters:<br>
* <code>
* (Math.PI * wheelDiameter) / (driveGearRatio * encoderTicksPerRotation)
* </code>
*/
@Override
public void configureIntegratedEncoder(double positionConversionFactor)
{
cfg.refresh(configuration);
positionConversionFactor = 1 / positionConversionFactor;
if (!isDriveMotor)
{
positionConversionFactor *= 360;
}
conversionFactor = positionConversionFactor;
configuration.MotionMagic =
configuration.MotionMagic.withMotionMagicCruiseVelocity(100.0 / positionConversionFactor)
.withMotionMagicAcceleration((100.0 / positionConversionFactor) / 0.100)
.withMotionMagicExpo_kV(0.12 * positionConversionFactor)
.withMotionMagicExpo_kA(0.1);
/*
configuration.Feedback.withFeedbackSensorSource(FeedbackSensorSourceValue.RotorSensor)
.withSensorToMechanismRatio(positionConversionFactor);
*/
cfg.apply(configuration);
}
/**
* Configure the PIDF values for the closed loop controller. 0 is disabled or off.
*
* @param config Configuration class holding the PIDF values.
*/
@Override
public void configurePIDF(PIDFConfig config)
{
cfg.refresh(configuration.Slot0);
cfg.apply(
configuration.Slot0.withKP(config.p).withKI(config.i).withKD(config.d).withKS(config.f));
// configuration.slot0.integralZone = config.iz;
// configuration.slot0.closedLoopPeakOutput = config.output.max;
}
/**
* Configure the PID wrapping for the position closed loop controller.
*
* @param minInput Minimum PID input.
* @param maxInput Maximum PID input.
*/
@Override
public void configurePIDWrapping(double minInput, double maxInput)
{
cfg.refresh(configuration.ClosedLoopGeneral);
configuration.ClosedLoopGeneral.ContinuousWrap = true;
cfg.apply(configuration.ClosedLoopGeneral);
}
/**
* Disable PID Wrapping on the motor.
*/
@Override
public void disablePIDWrapping()
{
cfg.refresh(configuration.ClosedLoopGeneral);
configuration.ClosedLoopGeneral.ContinuousWrap = false;
cfg.apply(configuration.ClosedLoopGeneral);
}
/**
* Set the idle mode.
*
* @param isBrakeMode Set the brake mode.
*/
@Override
public void setMotorBrake(boolean isBrakeMode)
{
motor.setNeutralMode(isBrakeMode ? NeutralModeValue.Brake : NeutralModeValue.Coast);
}
/**
* Set the motor to be inverted.
*
* @param inverted State of inversion.
*/
@Override
public void setInverted(boolean inverted)
{
// Timer.delay(1);
cfg.refresh(configuration.MotorOutput);
configuration.MotorOutput.withInverted(
inverted ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive);
cfg.apply(configuration.MotorOutput);
}
/**
* Save the configurations from flash to EEPROM.
*/
@Override
public void burnFlash()
{
// Do nothing
}
/**
* Set the percentage output.
*
* @param percentOutput percent out for the motor controller.
*/
@Override
public void set(double percentOutput)
{
motor.set(percentOutput);
}
/**
* Set the closed loop PID controller reference point.
*
* @param setpoint Setpoint in MPS or Angle in degrees.
* @param feedforward Feedforward in volt-meter-per-second or kV.
*/
@Override
public void setReference(double setpoint, double feedforward)
{
setReference(setpoint, feedforward, getPosition());
}
/**
* Set the closed loop PID controller reference point.
*
* @param setpoint Setpoint in meters per second or angle in degrees.
* @param feedforward Feedforward in volt-meter-per-second or kV.
* @param position Only used on the angle motor, the position of the motor in degrees.
*/
@Override
public void setReference(double setpoint, double feedforward, double position)
{
// if (SwerveDriveTelemetry.isSimulation)
// {
// PhysicsSim.getInstance().run();
// }
if (isDriveMotor)
{
motor.setControl(m_velocityVoltageSetter.withVelocity(setpoint).withFeedForward(feedforward));
} else
{
motor.setControl(m_angleVoltageSetter.withPosition(setpoint / 360.0));
}
}
/**
* Get the voltage output of the motor controller.
*
* @return Voltage output.
*/
@Override
public double getVoltage()
{
return motor.getMotorVoltage().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(Volts);
}
/**
* Set the voltage of the motor.
*
* @param voltage Voltage to set.
*/
@Override
public void setVoltage(double voltage)
{
motor.setVoltage(voltage);
}
/**
* Get the applied dutycycle output.
*
* @return Applied dutycycle output to the motor.
*/
@Override
public double getAppliedOutput()
{
return motor.getDutyCycle().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue();
}
/**
* Get the velocity of the integrated encoder.
*
* @return velocity in Meters Per Second, or Degrees per Second.
*/
@Override
public double getVelocity()
{
return motor.getVelocity().getValue().magnitude();
}
/**
* Get the position of the integrated encoder.
*
* @return Position in Meters or Degrees.
*/
@Override
public double getPosition()
{
return motor.getPosition().getValue().magnitude();
}
/**
* Set the integrated encoder position.
*
* @param position Integrated encoder position. Should be angle in degrees or meters.
*/
@Override
public void setPosition(double position)
{
if (!absoluteEncoder && !SwerveDriveTelemetry.isSimulation)
{
cfg.setPosition(Degrees.of(position).in(Rotations));
}
}
/**
* Set the voltage compensation for the swerve module motor.
*
* @param nominalVoltage Nominal voltage for operation to output to.
*/
@Override
public void setVoltageCompensation(double nominalVoltage)
{
// Do not implement
}
/**
* Set the current limit for the swerve drive motor, remember this may cause jumping if used in conjunction with
* voltage compensation. This is useful to protect the motor from current spikes.
*
* @param currentLimit Current limit in AMPS at free speed.
*/
@Override
public void setCurrentLimit(int currentLimit)
{
cfg.refresh(configuration.CurrentLimits);
cfg.apply(
configuration.CurrentLimits.withSupplyCurrentLimit(currentLimit)
.withSupplyCurrentLimitEnable(true));
}
/**
* Set the maximum rate the open/closed loop output can change by.
*
* @param rampRate Time in seconds to go from 0 to full throttle.
*/
@Override
public void setLoopRampRate(double rampRate)
{
cfg.refresh(configuration.ClosedLoopRamps);
cfg.apply(configuration.ClosedLoopRamps.withVoltageClosedLoopRampPeriod(rampRate));
}
/**
* Get the motor object from the module.
*
* @return Motor object.
*/
@Override
public Object getMotor()
{
return motor;
}
/**
* Get the {@link DCMotor} of the motor class.
*
* @return {@link DCMotor} of this type.
*/
@Override
public DCMotor getSimMotor()
{
if (simMotor == null)
{
simMotor = DCMotor.getKrakenX60(1);
}
return simMotor;
}
/**
* Queries whether the absolute encoder is directly attached to the motor controller.
*
* @return connected absolute encoder state.
*/
@Override
public boolean usingExternalFeedbackSensor()
{
return absoluteEncoder;
}
/**
* Closes handles for unit testing.
*/
@Override
public void close()
{
motor.close();
}
}

19
swervelib/motors/TalonFXSwerve.java
View File

@@ -131,6 +131,12 @@ public class TalonFXSwerve extends SwerveMotor
}
}
@Override
public void close() {
motor.close();
}
/**
* Clear the sticky faults on the motor controller.
*/
@@ -221,6 +227,17 @@ public class TalonFXSwerve extends SwerveMotor
cfg.apply(configuration.ClosedLoopGeneral);
}
/**
* Disable PID Wrapping on the motor.
*/
@Override
public void disablePIDWrapping()
{
cfg.refresh(configuration.ClosedLoopGeneral);
configuration.ClosedLoopGeneral.ContinuousWrap = false;
cfg.apply(configuration.ClosedLoopGeneral);
}
/**
* Set the idle mode.
*
@@ -442,7 +459,7 @@ public class TalonFXSwerve extends SwerveMotor
* @return connected absolute encoder state.
*/
@Override
public boolean isAttachedAbsoluteEncoder()
public boolean usingExternalFeedbackSensor()
{
return absoluteEncoder;
}

20
swervelib/motors/TalonSRXSwerve.java
View File

@@ -9,6 +9,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import edu.wpi.first.math.system.plant.DCMotor;
import swervelib.encoders.SwerveAbsoluteEncoder;
import swervelib.encoders.TalonSRXEncoderSwerve;
import swervelib.math.SwerveMath;
import swervelib.parser.PIDFConfig;
import swervelib.parser.json.modules.ConversionFactorsJson;
@@ -31,7 +32,7 @@ public class TalonSRXSwerve extends SwerveMotor
/**
* Whether the absolute encoder is integrated.
*/
private final boolean absoluteEncoder = false;
private boolean absoluteEncoder = false;
/**
* TalonSRX motor controller.
*/
@@ -84,6 +85,11 @@ public class TalonSRXSwerve extends SwerveMotor
this(new WPI_TalonSRX(id), isDriveMotor, motorType);
}
@Override
public void close() {
motor.close();
}
/**
* Configure the factory defaults.
*/
@@ -114,6 +120,7 @@ public class TalonSRXSwerve extends SwerveMotor
@Override
public SwerveMotor setAbsoluteEncoder(SwerveAbsoluteEncoder encoder)
{
absoluteEncoder = encoder instanceof TalonSRXEncoderSwerve;
// Do not support.
return this;
}
@@ -217,6 +224,15 @@ public class TalonSRXSwerve extends SwerveMotor
// Do nothing
}
/**
* Disable PID Wrapping on the motor.
*/
@Override
public void disablePIDWrapping()
{
// Do nothing
}
/**
* Set the idle mode.
*
@@ -468,7 +484,7 @@ public class TalonSRXSwerve extends SwerveMotor
* @return connected absolute encoder state.
*/
@Override
public boolean isAttachedAbsoluteEncoder()
public boolean usingExternalFeedbackSensor()
{
return absoluteEncoder;
}

37
swervelib/motors/ThriftyNovaSwerve.java
View File

@@ -26,6 +26,14 @@ import swervelib.parser.PIDFConfig;
public class ThriftyNovaSwerve extends SwerveMotor
{
/**
* {@link DCMotor} for simulation and calculations.
*/
private final DCMotor simMotor;
/**
* Closed-loop PID controller.
*/
public PIDController pid;
/**
* ThriftyNova Instance.
*/
@@ -34,10 +42,6 @@ public class ThriftyNovaSwerve extends SwerveMotor
* The Encoder type being used
*/
private EncoderType encoderType;
/**
* Closed-loop PID controller.
*/
public PIDController pid;
/**
* Factory default already occurred.
*/
@@ -58,10 +62,6 @@ public class ThriftyNovaSwerve extends SwerveMotor
* The position conversion factor for the encoder
*/
private double velocityConversionFactor = 1.0 / 60.0;
/**
* {@link DCMotor} for simulation and calculations.
*/
private final DCMotor simMotor;
/**
* Initialize the swerve motor.
@@ -105,6 +105,16 @@ public class ThriftyNovaSwerve extends SwerveMotor
this(new ThriftyNova(id), isDriveMotor, motor);
}
@Override
public void close() {
try {
motor.close();
} catch (Exception e) {
throw new RuntimeException(e);
}
}
/**
* Set factory defaults on the motor controller.
*/
@@ -226,6 +236,15 @@ public class ThriftyNovaSwerve extends SwerveMotor
// Do nothing
}
/**
* Disable PID Wrapping on the motor.
*/
@Override
public void disablePIDWrapping()
{
// Do nothing
}
/**
* Set the idle mode.
*
@@ -416,7 +435,7 @@ public class ThriftyNovaSwerve extends SwerveMotor
* @return connected absolute encoder state.
*/
@Override
public boolean isAttachedAbsoluteEncoder()
public boolean usingExternalFeedbackSensor()
{
return EncoderType.ABS == encoderType;
}

6
swervelib/parser/PIDFConfig.java
View File

@@ -103,6 +103,10 @@ public class PIDFConfig
*/
public PIDController createPIDController()
{
return new PIDController(p, i, d);
PIDController pidController = new PIDController(p, i, d);
if (iz != 0) {
pidController.setIZone(iz);
}
return pidController;
}
}

9
swervelib/parser/SwerveParser.java
View File

@@ -1,5 +1,6 @@
package swervelib.parser;
import com.fasterxml.jackson.databind.DeserializationFeature;
import com.fasterxml.jackson.databind.JsonNode;
import com.fasterxml.jackson.databind.ObjectMapper;
import edu.wpi.first.math.geometry.Pose2d;
@@ -57,17 +58,21 @@ public class SwerveParser
checkDirectory(directory);
swerveDriveJson =
new ObjectMapper()
.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
.readValue(new File(directory, "swervedrive.json"), SwerveDriveJson.class);
controllerPropertiesJson =
new ObjectMapper()
.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
.readValue(
new File(directory, "controllerproperties.json"), ControllerPropertiesJson.class);
pidfPropertiesJson =
new ObjectMapper()
.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
.readValue(
new File(directory, "modules/pidfproperties.json"), PIDFPropertiesJson.class);
physicalPropertiesJson =
new ObjectMapper()
.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
.readValue(
new File(directory, "modules/physicalproperties.json"),
PhysicalPropertiesJson.class);
@@ -77,7 +82,9 @@ public class SwerveParser
moduleConfigs.put(swerveDriveJson.modules[i], i);
File moduleFile = new File(directory, "modules/" + swerveDriveJson.modules[i]);
assert moduleFile.exists();
moduleJsons[i] = new ObjectMapper().readValue(moduleFile, ModuleJson.class);
moduleJsons[i] = new ObjectMapper()
.configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false)
.readValue(moduleFile, ModuleJson.class);
}
}

36
swervelib/parser/json/DeviceJson.java
View File

@@ -6,13 +6,13 @@ import static swervelib.telemetry.SwerveDriveTelemetry.serialCommsIssueWarning;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.studica.frc.AHRS.NavXComType;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.wpilibj.DriverStation;
import swervelib.encoders.AnalogAbsoluteEncoderSwerve;
import swervelib.encoders.CANCoderSwerve;
import swervelib.encoders.CanAndMagSwerve;
import swervelib.encoders.PWMDutyCycleEncoderSwerve;
import swervelib.encoders.DIODutyCycleEncoderSwerve;
import swervelib.encoders.SparkFlexEncoderSwerve;
import swervelib.encoders.SparkMaxAnalogEncoderSwerve;
import swervelib.encoders.SparkMaxEncoderSwerve;
import swervelib.encoders.SwerveAbsoluteEncoder;
@@ -26,12 +26,14 @@ import swervelib.imu.CanandgyroSwerve;
import swervelib.imu.NavXSwerve;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.PigeonSwerve;
import swervelib.imu.PigeonViaTalonSRXSwerve;
import swervelib.imu.SwerveIMU;
import swervelib.motors.SparkFlexSwerve;
import swervelib.motors.SparkMaxBrushedMotorSwerve;
import swervelib.motors.SparkMaxBrushedMotorSwerve.Type;
import swervelib.motors.SparkMaxSwerve;
import swervelib.motors.SwerveMotor;
import swervelib.motors.TalonFXSSwerve;
import swervelib.motors.TalonFXSwerve;
import swervelib.motors.TalonSRXSwerve;
import swervelib.motors.ThriftyNovaSwerve;
@@ -81,6 +83,11 @@ public class DeviceJson
return new SparkMaxAnalogEncoderSwerve(motor, 3.3);
case "sparkmax_analog5v":
return new SparkMaxAnalogEncoderSwerve(motor, 5);
case "sparkflex_integrated":
case "sparkflex_attached":
case "sparkflex_canandmag":
case "sparkflex_canandcoder":
return new SparkFlexEncoderSwerve(motor, 360);
case "canandcoder_can":
case "canandmag_can":
return new CanAndMagSwerve(id);
@@ -89,13 +96,16 @@ public class DeviceJson
case "throughbore":
case "am_mag":
case "dutycycle":
return new PWMDutyCycleEncoderSwerve(id);
return new DIODutyCycleEncoderSwerve(id);
case "thrifty":
case "ma3":
case "analog":
return new AnalogAbsoluteEncoderSwerve(id);
case "cancoder":
return new CANCoderSwerve(id, canbus != null ? canbus : "");
case "srxmag_standalone":
return new TalonSRXEncoderSwerve(new TalonSRXSwerve(id, false, DCMotor.getCIM(1)),
FeedbackDevice.PulseWidthEncodedPosition);
case "talonsrx_pwm":
return new TalonSRXEncoderSwerve(motor, FeedbackDevice.PulseWidthEncodedPosition);
case "talonsrx_analog":
@@ -151,6 +161,8 @@ public class DeviceJson
return new NavXSwerve(NavXComType.kMXP_UART);
case "pigeon":
return new PigeonSwerve(id);
case "pigeon_via_talonsrx":
return new PigeonViaTalonSRXSwerve(id);
case "pigeon2":
return new Pigeon2Swerve(id, canbus != null ? canbus : "");
default:
@@ -172,10 +184,22 @@ public class DeviceJson
}
switch (type)
{
case "talonfxs_neo":
return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNEO(1));
case "talonfxs_neo550":
return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeo550(1));
case "talonfxs_vortex":
return new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeoVortex(1));
case "talonfxs_minion":
throw new UnsupportedOperationException("Cannot create minion combination yet"); //new TalonFXSSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getNeoVortex(1));
case "sparkmax_neo":
case "neo":
case "sparkmax":
return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNEO(1));
case "sparkmax_vortex":
return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1));
case "sparkmax_minion":
throw new UnsupportedOperationException("Cannot create minion combination yet");
case "sparkmax_neo550":
case "neo550":
return new SparkMaxSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
@@ -187,6 +211,8 @@ public class DeviceJson
return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNEO(1));
case "sparkflex_neo550":
return new SparkFlexSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
case "sparkflex_minion":
throw new UnsupportedOperationException("Cannot create minion combination yet");
case "falcon500":
case "falcon":
return new TalonFXSwerve(id, canbus != null ? canbus : "", isDriveMotor, DCMotor.getFalcon500(1));
@@ -230,6 +256,10 @@ public class DeviceJson
return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNEO(1));
case "nova_neo550":
return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNeo550(1));
case "nova_vortex":
return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getNeoVortex(1));
case "nova_minion":
throw new UnsupportedOperationException("Cannot create minion combination");//return new ThriftyNovaSwerve(id, isDriveMotor, DCMotor.getMinion(1));
default:
throw new RuntimeException(type + " is not a recognized motor type.");
}

12
swervelib/parser/json/ModuleJson.java
View File

@@ -75,6 +75,14 @@ public class ModuleJson
SwerveMotor angleMotor = angle.createMotor(false);
SwerveAbsoluteEncoder absEncoder = encoder.createEncoder(angleMotor);
//Throw an error if module locations are improperly set
if (location.front == 0 && location.left == 0)
{
throw new RuntimeException("Improper Module Location Settings!\n" +
"Your module location is set to 0 for both 'front' and 'left' values.\n" +
"Set the distance from the center of the robot to the center of the wheel in your module JSON file!");
}
// Set the conversion factors to null if they are both 0.
if (!conversionFactors.works() && physicalCharacteristics.conversionFactor == null)
{
@@ -119,8 +127,8 @@ public class ModuleJson
conversionFactors,
absEncoder,
absoluteEncoderOffset,
Units.inchesToMeters(Math.round(location.x) == 0 ? location.front : location.x),
Units.inchesToMeters(Math.round(location.y) == 0 ? location.left : location.y),
Units.inchesToMeters(Math.round(location.front)),
Units.inchesToMeters(Math.round(location.left)),
anglePIDF,
velocityPIDF,
physicalCharacteristics,

5
swervelib/parser/json/PhysicalPropertiesJson.java
View File

@@ -63,12 +63,11 @@ public class PhysicalPropertiesJson
if (conversionFactor.drive != 0 && conversionFactor.angle != 0 && conversionFactors.isDriveEmpty() &&
conversionFactors.isAngleEmpty())
{
new Alert("Configuration",
throw new RuntimeException(
"\n'conversionFactor': {'drive': " + conversionFactor.drive + ", 'angle': " + conversionFactor.angle +
"} \nis deprecated, please use\n" +
"'conversionFactors': {'drive': {'factor': " + conversionFactor.drive + "}, 'angle': {'factor': " +
conversionFactor.angle + "} }",
AlertType.kError).set(true);
conversionFactor.angle + "} }");
}
return new SwerveModulePhysicalCharacteristics(

4
swervelib/parser/json/modules/LocationJson.java
View File

@@ -10,9 +10,9 @@ public class LocationJson
/**
* Location of the swerve module in inches from the center of the robot horizontally.
*/
public double front = 0, x = 0;
public double front = 0;
/**
* Location of the swerve module in inches from the center of the robot vertically.
*/
public double left = 0, y = 0;
public double left = 0;
}

34
swervelib/simulation/SwerveModuleSimulation.java
View File

@@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import org.ironmaple.simulation.drivesims.SelfControlledSwerveDriveSimulation;
import swervelib.SwerveDrive;
import swervelib.parser.SwerveModulePhysicalCharacteristics;
/**
@@ -14,12 +15,16 @@ import swervelib.parser.SwerveModulePhysicalCharacteristics;
public class SwerveModuleSimulation
{
private SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null;
/**
* MapleSim module.
*/
public SelfControlledSwerveDriveSimulation.SelfControlledModuleSimulation mapleSimModule = null;
/**
* Configure the maple sim module
*
* @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for simulation
* @param simModule the {@link org.ironmaple.simulation.drivesims.SwerveModuleSimulation} object for
* simulation
* @param physicalCharacteristics Physical characteristics of the swerve drive from the JSON or built.
*/
public void configureSimModule(org.ironmaple.simulation.drivesims.SwerveModuleSimulation simModule,
@@ -42,6 +47,31 @@ public class SwerveModuleSimulation
mapleSimModule.runModuleState(desiredState);
}
/**
* Runs a drive motor characterization on the sim module. This is called from
* {@link swervelib.SwerveDriveTest#runDriveMotorsCharacterizationOnSimModules(SwerveDrive, double, boolean)} to run
* sysId during simulation
*
* @param desiredFacing the desired facing of the module
* @param volts the voltage to run
*/
public void runDriveMotorCharacterization(Rotation2d desiredFacing, double volts)
{
mapleSimModule.runDriveMotorCharacterization(desiredFacing, volts);
}
/**
* Runs a drive motor characterization on the sim module. This method is called from
* {@link swervelib.SwerveDriveTest#runAngleMotorsCharacterizationOnSimModules(SwerveDrive, double)} to run sysId
* during simulation
*
* @param volts the voltage to run
*/
public void runAngleMotorCharacterization(double volts)
{
mapleSimModule.runSteerMotorCharacterization(volts);
}
/**
* Get the simulated swerve module position.
*

2
swervelib/telemetry/SwerveDriveTelemetry.java
View File

@@ -429,7 +429,7 @@ public class SwerveDriveTelemetry
updateSwerveTelemetrySettings();
}
measuredChassisSpeeds[0] = measuredChassisSpeedsObj.vxMetersPerSecond;
measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vxMetersPerSecond;
measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vyMetersPerSecond;
measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeedsObj.omegaRadiansPerSecond);
desiredChassisSpeeds[0] = desiredChassisSpeedsObj.vxMetersPerSecond;