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.1.0.1

Browse Source
This commit is contained in:
thenetworkgrinch
2024-12-17 18:49:55 +00:00
parent 4bc6978a20
commit 8050f43fa5
19 changed files with 1189 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
swervelib/SwerveDrive.java
View File

@@ -8,6 +8,7 @@ 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;
import static edu.wpi.first.units.Units.Volts;
@@ -31,6 +32,8 @@ import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.Force;
import edu.wpi.first.units.measure.LinearVelocity;
@@ -55,7 +58,6 @@ import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
import org.ironmaple.simulation.drivesims.SwerveModuleSimulation;
import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig;
import swervelib.encoders.CANCoderSwerve;
import swervelib.imu.IMUVelocity;
import swervelib.imu.Pigeon2Swerve;
import swervelib.imu.SwerveIMU;
import swervelib.math.SwerveMath;
@@ -100,18 +102,34 @@ public class SwerveDrive
/**
* Odometry lock to ensure thread safety.
*/
private final Lock odometryLock = new ReentrantLock();
private final Lock odometryLock = new ReentrantLock();
/**
* Alert to recommend Tuner X if the configuration is compatible.
*/
private final Alert tunerXRecommendation = new Alert("Swerve Drive",
"Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
"https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
AlertType.kWarning);
private final Alert tunerXRecommendation = new Alert("Swerve Drive",
"Your Swerve Drive is compatible with Tuner X swerve generator, please consider using that instead of YAGSL. More information here!\n" +
"https://pro.docs.ctr-electronics.com/en/latest/docs/tuner/tuner-swerve/index.html",
AlertType.kWarning);
/**
* NT4 Publisher for the IMU reading.
*/
private final DoublePublisher rawIMUPublisher
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/Raw IMU Yaw")
.publish();
/**
* NT4 Publisher for the IMU reading adjusted by offset and inversion.
*/
private final DoublePublisher adjustedIMUPublisher
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/Adjusted IMU Yaw")
.publish();
/**
* Field object.
*/
public Field2d field = new Field2d();
public Field2d field = new Field2d();
/**
* Swerve controller for controlling heading of the robot.
*/
@@ -120,30 +138,30 @@ public class SwerveDrive
* Correct chassis velocity in {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)} using 254's
* correction.
*/
public boolean chassisVelocityCorrection = true;
public boolean chassisVelocityCorrection = true;
/**
* Correct chassis velocity in {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} (auto) using 254's
* correction during auto.
*/
public boolean autonomousChassisVelocityCorrection = false;
public boolean autonomousChassisVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}
*/
public boolean angularVelocityCorrection = false;
public boolean angularVelocityCorrection = false;
/**
* Correct for skew that scales with angular velocity in
* {@link SwerveDrive#setChassisSpeeds(ChassisSpeeds chassisSpeeds)} during auto.
*/
public boolean autonomousAngularVelocityCorrection = false;
public boolean autonomousAngularVelocityCorrection = false;
/**
* Angular Velocity Correction Coefficent (expected values between -0.15 and 0.15).
*/
public double angularVelocityCoefficient = 0;
public double angularVelocityCoefficient = 0;
/**
* Whether to correct heading when driving translationally. Set to true to enable.
*/
public boolean headingCorrection = false;
public boolean headingCorrection = false;
/**
* MapleSim SwerveDrive.
*/
@@ -151,44 +169,39 @@ public class SwerveDrive
/**
* Amount of seconds the duration of the timestep the speeds should be applied for.
*/
private double discretizationdtSeconds = 0.02;
private double discretizationdtSeconds = 0.02;
/**
* Deadband for speeds in heading correction.
*/
private double HEADING_CORRECTION_DEADBAND = 0.01;
private double HEADING_CORRECTION_DEADBAND = 0.01;
/**
* Swerve IMU device for sensing the heading of the robot.
*/
private SwerveIMU imu;
/**
* Class that calculates robot's yaw velocity using IMU measurements. Used for angularVelocityCorrection in
* {@link SwerveDrive#drive(Translation2d, double, boolean, boolean)}.
*/
private IMUVelocity imuVelocity;
/**
* Simulation of the swerve drive.
*/
private SwerveIMUSimulation simIMU;
private SwerveIMUSimulation simIMU;
/**
* Counter to synchronize the modules relative encoder with absolute encoder when not moving.
*/
private int moduleSynchronizationCounter = 0;
private int moduleSynchronizationCounter = 0;
/**
* The last heading set in radians.
*/
private double lastHeadingRadians = 0;
private double lastHeadingRadians = 0;
/**
* The absolute max speed that your robot can reach while translating in meters per second.
*/
private double attainableMaxTranslationalSpeedMetersPerSecond = 0;
private double attainableMaxTranslationalSpeedMetersPerSecond = 0;
/**
* The absolute max speed the robot can reach while rotating radians per second.
*/
private double attainableMaxRotationalVelocityRadiansPerSecond = 0;
private double attainableMaxRotationalVelocityRadiansPerSecond = 0;
/**
* Maximum speed of the robot in meters per second.
*/
private double maxChassisSpeedMPS;
private double maxChassisSpeedMPS;
/**
* Creates a new swerve drivebase subsystem. Robot is controlled via the {@link SwerveDrive#drive} method, or via the
@@ -567,7 +580,7 @@ public class SwerveDrive
*/
public void drive(ChassisSpeeds robotRelativeVelocity, boolean isOpenLoop, Translation2d centerOfRotationMeters)
{
SwerveDriveTelemetry.startCtrlCycle();
robotRelativeVelocity = movementOptimizations(robotRelativeVelocity,
chassisVelocityCorrection,
angularVelocityCorrection);
@@ -702,6 +715,7 @@ public class SwerveDrive
*/
public void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoop)
{
SwerveDriveTelemetry.startCtrlCycle();
double maxModuleSpeedMPS = getMaximumModuleDriveVelocity().in(MetersPerSecond);
desiredStates = kinematics.toSwerveModuleStates(kinematics.toChassisSpeeds(desiredStates));
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, maxModuleSpeedMPS);
@@ -725,6 +739,7 @@ public class SwerveDrive
*/
public void drive(ChassisSpeeds robotRelativeVelocity, SwerveModuleState[] states, Force[] feedforwardForces)
{
SwerveDriveTelemetry.startCtrlCycle();
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal())
{
SwerveDriveTelemetry.desiredChassisSpeedsObj = robotRelativeVelocity;
@@ -763,7 +778,7 @@ public class SwerveDrive
*/
public void setChassisSpeeds(ChassisSpeeds robotRelativeSpeeds)
{
SwerveDriveTelemetry.startCtrlCycle();
robotRelativeSpeeds = movementOptimizations(robotRelativeSpeeds,
autonomousChassisVelocityCorrection,
autonomousAngularVelocityCorrection);
@@ -1108,6 +1123,7 @@ public class SwerveDrive
*/
public void updateOdometry()
{
SwerveDriveTelemetry.startOdomCycle();
odometryLock.lock();
invalidateCache();
try
@@ -1155,8 +1171,8 @@ public class SwerveDrive
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
module.updateTelemetry();
SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees());
SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees());
rawIMUPublisher.set(getYaw().getDegrees());
adjustedIMUPublisher.set(getOdometryHeading().getDegrees());
}
if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal())
{
@@ -1183,6 +1199,7 @@ public class SwerveDrive
throw e;
}
odometryLock.unlock();
SwerveDriveTelemetry.endOdomCycle();
}
/**
@@ -1442,7 +1459,6 @@ public class SwerveDrive
{
if (!SwerveDriveTelemetry.isSimulation)
{
imuVelocity = IMUVelocity.createIMUVelocity(imu);
angularVelocityCorrection = useInTeleop;
autonomousAngularVelocityCorrection = useInAuto;
angularVelocityCoefficient = angularVelocityCoeff;
@@ -1457,7 +1473,7 @@ public class SwerveDrive
*/
public ChassisSpeeds angularVelocitySkewCorrection(ChassisSpeeds robotRelativeVelocity)
{
var angularVelocity = new Rotation2d(imuVelocity.getVelocity() * angularVelocityCoefficient);
var angularVelocity = new Rotation2d(imu.getYawAngularVelocity().in(RadiansPerSecond) * angularVelocityCoefficient);
if (angularVelocity.getRadians() != 0.0)
{
robotRelativeVelocity.toFieldRelativeSpeeds(getOdometryHeading());

576
swervelib/SwerveInputStream.java Normal file
View File

@@ -0,0 +1,576 @@
package swervelib;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.Optional;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
import java.util.function.Supplier;
import swervelib.math.SwerveMath;
/**
* Helper class to easily transform Controller inputs into workable Chassis speeds. <br /> Inspired by SciBorgs.
* https://github.com/SciBorgs/Crescendo-2024/blob/main/src/main/java/org/sciborgs1155/lib/InputStream.java
* <p>
* Intended to easily create an interface that generates {@link ChassisSpeeds} from
* {@link edu.wpi.first.wpilibj.XboxController}
*/
public class SwerveInputStream implements Supplier<ChassisSpeeds>
{
/**
* Translation suppliers.
*/
private final DoubleSupplier controllerTranslationX;
/**
* Translational supplier.
*/
private final DoubleSupplier controllerTranslationY;
/**
* {@link SwerveDrive} object for transformations.
*/
private final SwerveDrive swerveDrive;
/**
* Rotation supplier as angular velocity.
*/
private Optional<DoubleSupplier> controllerOmega = Optional.empty();
/**
* Controller supplier as heading.
*/
private Optional<DoubleSupplier> controllerHeadingX = Optional.empty();
/**
* Controller supplier as heading.
*/
private Optional<DoubleSupplier> controllerHeadingY = Optional.empty();
/**
* Axis deadband for the controller.
*/
private Optional<Double> axisDeadband = Optional.empty();
/**
* Translational axis scalar value, should be between (0, 1].
*/
private Optional<Double> translationAxisScale = Optional.empty();
/**
* Angular velocity axis scalar value, should be between (0, 1]
*/
private Optional<Double> omegaAxisScale = Optional.empty();
/**
* Target to aim at.
*/
private Optional<Pose2d> aimTarget = Optional.empty();
/**
* Output {@link ChassisSpeeds} based on heading while this is True.
*/
private Optional<BooleanSupplier> headingEnabled = Optional.empty();
/**
* Locked heading for {@link SwerveInputMode#TRANSLATION_ONLY}
*/
private Optional<Rotation2d> lockedHeading = Optional.empty();
/**
* Output {@link ChassisSpeeds} based on aim while this is True.
*/
private Optional<BooleanSupplier> aimEnabled = Optional.empty();
/**
* Maintain current heading and drive without rotating, ideally.
*/
private Optional<BooleanSupplier> translationOnlyEnabled = Optional.empty();
/**
* {@link SwerveController} for simple control over heading.
*/
private SwerveController swerveController = null;
/**
* Current {@link SwerveInputMode} to use.
*/
private SwerveInputMode currentMode = SwerveInputMode.ANGULAR_VELOCITY;
/**
* Drive modes to keep track of.
*/
enum SwerveInputMode
{
/**
* Translation only mode, does not allow for rotation and maintains current heading.
*/
TRANSLATION_ONLY,
/**
* Output based off angular velocity
*/
ANGULAR_VELOCITY,
/**
* Output based off of heading.
*/
HEADING,
/**
* Output based off of targeting.
*/
AIM
}
/**
* Copy the {@link SwerveInputStream} object.
*
* @return Clone of current {@link SwerveInputStream}
*/
public SwerveInputStream copy()
{
SwerveInputStream newStream = new SwerveInputStream(swerveDrive, controllerTranslationX, controllerTranslationY);
newStream.controllerOmega = controllerOmega;
newStream.controllerHeadingX = controllerHeadingX;
newStream.controllerHeadingY = controllerHeadingY;
newStream.axisDeadband = axisDeadband;
newStream.translationAxisScale = translationAxisScale;
newStream.omegaAxisScale = omegaAxisScale;
newStream.aimTarget = aimTarget;
newStream.headingEnabled = headingEnabled;
newStream.aimEnabled = aimEnabled;
newStream.currentMode = currentMode;
newStream.translationOnlyEnabled = translationOnlyEnabled;
newStream.lockedHeading = lockedHeading;
newStream.swerveController = swerveController;
return newStream;
}
/**
* Create a {@link SwerveInputStream} for an easy way to generate {@link ChassisSpeeds} from a driver controller.
*
* @param drive {@link SwerveDrive} object for transformation.
* @param x Translation X input in range of [-1, 1]
* @param y Translation Y input in range of [-1, 1]
*/
private SwerveInputStream(SwerveDrive drive, DoubleSupplier x, DoubleSupplier y)
{
controllerTranslationX = x;
controllerTranslationY = y;
swerveDrive = drive;
}
/**
* Create a {@link SwerveInputStream} for an easy way to generate {@link ChassisSpeeds} from a driver controller.
*
* @param drive {@link SwerveDrive} object for transformation.
* @param x Translation X input in range of [-1, 1]
* @param y Translation Y input in range of [-1, 1]
* @param rot Rotation input in range of [-1, 1]
*/
public SwerveInputStream(SwerveDrive drive, DoubleSupplier x, DoubleSupplier y, DoubleSupplier rot)
{
this(drive, x, y);
controllerOmega = Optional.of(rot);
}
/**
* Create a {@link SwerveInputStream} for an easy way to generate {@link ChassisSpeeds} from a driver controller.
*
* @param drive {@link SwerveDrive} object for transformation.
* @param x Translation X input in range of [-1, 1]
* @param y Translation Y input in range of [-1, 1]
* @param headingX Heading X input in range of [-1, 1]
* @param headingY Heading Y input in range of [-1, 1]
*/
public SwerveInputStream(SwerveDrive drive, DoubleSupplier x, DoubleSupplier y, DoubleSupplier headingX,
DoubleSupplier headingY)
{
this(drive, x, y);
controllerHeadingX = Optional.of(headingX);
controllerHeadingY = Optional.of(headingY);
}
/**
* Create basic {@link SwerveInputStream} without any rotation components.
*
* @param drive {@link SwerveDrive} object for transformation.
* @param x {@link DoubleSupplier} of the translation X axis of the controller joystick to use.
* @param y {@link DoubleSupplier} of the translation X axis of the controller joystick to use.
* @return {@link SwerveInputStream} to use as you see fit.
*/
public static SwerveInputStream of(SwerveDrive drive, DoubleSupplier x, DoubleSupplier y)
{
return new SwerveInputStream(drive, x, y);
}
/**
* Add a rotation axis for Angular Velocity control
*
* @param rot Rotation axis with values from [-1, 1]
* @return self
*/
public SwerveInputStream withControllerRotationAxis(DoubleSupplier rot)
{
controllerOmega = Optional.of(rot);
return this;
}
/**
* Add heading axis for Heading based control.
*
* @param headingX Heading X axis with values from [-1, 1]
* @param headingY Heading Y axis with values from [-1, 1]
* @return self
*/
public SwerveInputStream withControllerHeadingAxis(DoubleSupplier headingX, DoubleSupplier headingY)
{
controllerHeadingX = Optional.of(headingX);
controllerHeadingY = Optional.of(headingY);
return this;
}
/**
* Set a deadband for all controller axis.
*
* @param deadband Deadband to set, should be between [0, 1)
* @return self
*/
public SwerveInputStream deadband(double deadband)
{
axisDeadband = deadband == 0 ? Optional.empty() : Optional.of(deadband);
return this;
}
/**
* Scale the translation axis for {@link SwerveInputStream} by a constant scalar value.
*
* @param scaleTranslation Translation axis scalar value. (0, 1]
* @return this
*/
public SwerveInputStream scaleTranslation(double scaleTranslation)
{
translationAxisScale = scaleTranslation == 0 ? Optional.empty() : Optional.of(scaleTranslation);
return this;
}
/**
* Scale the rotation axis input for {@link SwerveInputStream} to reduce the range in which they operate.
*
* @param scaleRotation Angular velocity axis scalar value. (0, 1]
* @return this
*/
public SwerveInputStream scaleRotation(double scaleRotation)
{
omegaAxisScale = scaleRotation == 0 ? Optional.empty() : Optional.of(scaleRotation);
return this;
}
/**
* Output {@link ChassisSpeeds} based on heading while the supplier is True.
*
* @param trigger Supplier to use.
* @return this.
*/
public SwerveInputStream headingWhile(BooleanSupplier trigger)
{
headingEnabled = Optional.of(trigger);
return this;
}
/**
* Set the heading enable state.
*
* @param headingState Heading enabled state.
* @return this
*/
public SwerveInputStream headingWhile(boolean headingState)
{
if (headingState)
{
headingEnabled = Optional.of(() -> true);
} else
{
headingEnabled = Optional.empty();
}
return this;
}
/**
* Aim the {@link SwerveDrive} at this pose while driving.
*
* @param aimTarget {@link Pose2d} to point at.
* @return this
*/
public SwerveInputStream aim(Pose2d aimTarget)
{
this.aimTarget = aimTarget.equals(Pose2d.kZero) ? Optional.empty() : Optional.of(aimTarget);
return this;
}
/**
* Enable aiming while the trigger is true.
*
* @param trigger When True will enable aiming at the current target.
* @return this.
*/
public SwerveInputStream aimWhile(BooleanSupplier trigger)
{
aimEnabled = Optional.of(trigger);
return this;
}
/**
* Enable aiming while the trigger is true.
*
* @param trigger When True will enable aiming at the current target.
* @return this.
*/
public SwerveInputStream aimWhile(boolean trigger)
{
if (trigger)
{
aimEnabled = Optional.of(() -> true);
} else
{
aimEnabled = Optional.empty();
}
return this;
}
/**
* Enable locking of rotation and only translating, overrides everything.
*
* @param trigger Translation only while returns true.
* @return this
*/
public SwerveInputStream translationOnlyWhile(BooleanSupplier trigger)
{
translationOnlyEnabled = Optional.of(trigger);
return this;
}
/**
* Enable locking of rotation and only translating, overrides everything.
*
* @param translationState Translation only if true.
* @return this
*/
public SwerveInputStream translationOnlyWhile(boolean translationState)
{
if (translationState)
{
translationOnlyEnabled = Optional.of(() -> true);
} else
{
translationOnlyEnabled = Optional.empty();
}
return this;
}
/**
* Find {@link SwerveInputMode} based off existing parameters of the {@link SwerveInputStream}
*
* @return The calculated {@link SwerveInputMode}, defaults to {@link SwerveInputMode#ANGULAR_VELOCITY}.
*/
private SwerveInputMode findMode()
{
if (translationOnlyEnabled.isPresent() && translationOnlyEnabled.get().getAsBoolean())
{
return SwerveInputMode.TRANSLATION_ONLY;
} else if (aimEnabled.isPresent() && aimEnabled.get().getAsBoolean())
{
if (aimTarget.isPresent())
{
return SwerveInputMode.AIM;
} else
{
DriverStation.reportError(
"Attempting to enter AIM mode without target, please use SwerveInputStream.aim() to select a target first!",
false);
}
} else if (headingEnabled.isPresent() && headingEnabled.get().getAsBoolean())
{
if (controllerHeadingX.isPresent() && controllerHeadingY.isPresent())
{
return SwerveInputMode.HEADING;
} else
{
DriverStation.reportError(
"Attempting to enter HEADING mode without heading axis, please use SwerveInputStream.withControllerHeadingAxis to add heading axis!",
false);
}
} else if (controllerOmega.isEmpty())
{
DriverStation.reportError(
"Attempting to enter ANGULAR_VELOCITY mode without a rotation axis, please use SwerveInputStream.withControllerRotationAxis to add angular velocity axis!",
false);
return SwerveInputMode.TRANSLATION_ONLY;
}
return SwerveInputMode.ANGULAR_VELOCITY;
}
/**
* Transition smoothly from one mode to another.
*
* @param newMode New mode to transition too.
*/
private void transitionMode(SwerveInputMode newMode)
{
// Handle removing of current mode.
switch (currentMode)
{
case TRANSLATION_ONLY ->
{
lockedHeading = Optional.empty();
break;
}
case ANGULAR_VELOCITY ->
{
// Do nothing
break;
}
case HEADING ->
{
// Do nothing
break;
}
case AIM ->
{
// Do nothing
break;
}
}
// Transitioning to new mode
switch (newMode)
{
case TRANSLATION_ONLY ->
{
lockedHeading = Optional.of(swerveDrive.getOdometryHeading());
break;
}
case ANGULAR_VELOCITY ->
{
if (swerveDrive.headingCorrection)
{
swerveDrive.setHeadingCorrection(false);
}
break;
}
case HEADING ->
{
// Do nothing
break;
}
case AIM ->
{
// Do nothing
break;
}
}
}
/**
* Apply the deadband if it exists.
*
* @param axisValue Axis value to apply the deadband too.
* @return axis value with deadband, else axis value straight.
*/
private double applyDeadband(double axisValue)
{
if (axisDeadband.isPresent())
{
return MathUtil.applyDeadband(axisValue, axisDeadband.get());
}
return axisValue;
}
/**
* Apply the scalar value if it exists.
*
* @param axisValue Axis value to apply teh scalar too.
* @return Axis value scaled by scalar value.
*/
private double applyRotationalScalar(double axisValue)
{
if (omegaAxisScale.isPresent())
{
return axisValue * omegaAxisScale.get();
}
return axisValue;
}
/**
* Scale the translational axis by the {@link SwerveInputStream#translationAxisScale} if it exists.
*
* @param xAxis X axis to scale.
* @param yAxis Y axis to scale.
* @return Scaled {@link Translation2d}
*/
private Translation2d applyTranslationScalar(double xAxis, double yAxis)
{
if (translationAxisScale.isPresent())
{
return SwerveMath.scaleTranslation(new Translation2d(xAxis, yAxis),
translationAxisScale.get());
}
return new Translation2d(xAxis, yAxis);
}
/**
* Gets a field-oriented {@link ChassisSpeeds}
*
* @return field-oriented {@link ChassisSpeeds}
*/
@Override
public ChassisSpeeds get()
{
double maximumChassisVelocity = swerveDrive.getMaximumChassisVelocity();
Translation2d scaledTranslation = applyTranslationScalar(applyDeadband(controllerTranslationX.getAsDouble()),
applyDeadband(controllerTranslationY.getAsDouble()));
double vxMetersPerSecond = scaledTranslation.getX() * maximumChassisVelocity;
double vyMetersPerSecond = scaledTranslation.getY() * maximumChassisVelocity;
double omegaRadiansPerSecond = 0;
SwerveInputMode newMode = findMode();
// Handle transitions here.
if (currentMode != newMode)
{
transitionMode(newMode);
}
if (swerveController == null)
{
swerveController = swerveDrive.getSwerveController();
}
switch (newMode)
{
case TRANSLATION_ONLY ->
{
omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
lockedHeading.get().getRadians());
break;
}
case ANGULAR_VELOCITY ->
{
omegaRadiansPerSecond = applyRotationalScalar(applyDeadband(controllerOmega.get().getAsDouble())) *
swerveDrive.getMaximumChassisAngularVelocity();
break;
}
case HEADING ->
{
omegaRadiansPerSecond = swerveController.headingCalculate(swerveDrive.getOdometryHeading().getRadians(),
swerveController.getJoystickAngle(controllerHeadingX.get()
.getAsDouble(),
controllerHeadingY.get()
.getAsDouble()));
break;
}
case AIM ->
{
Rotation2d currentHeading = swerveDrive.getOdometryHeading();
Translation2d relativeTrl = aimTarget.get().relativeTo(swerveDrive.getPose()).getTranslation();
Rotation2d target = new Rotation2d(relativeTrl.getX(), relativeTrl.getY()).plus(currentHeading);
omegaRadiansPerSecond = swerveController.headingCalculate(currentHeading.getRadians(), target.getRadians());
break;
}
}
currentMode = newMode;
return new ChassisSpeeds(vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
}
}

82
swervelib/SwerveModule.java
View File

@@ -8,6 +8,9 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.networktables.BooleanPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.units.measure.LinearVelocity;
import edu.wpi.first.wpilibj.Alert;
@@ -70,29 +73,37 @@ public class SwerveModule
*/
private final Alert noEncoderWarning;
/**
* NT3 Raw Absolute Angle publisher for the absolute encoder.
* NT4 Raw Absolute Angle publisher for the absolute encoder.
*/
private final String rawAbsoluteAngleName;
private final DoublePublisher rawAbsoluteAnglePublisher;
/**
* NT3 Adjusted Absolute angle publisher for the absolute encoder.
* NT4 Adjusted Absolute angle publisher for the absolute encoder.
*/
private final String adjAbsoluteAngleName;
private final DoublePublisher adjAbsoluteAnglePublisher;
/**
* NT3 Absolute encoder read issue.
* NT4 Absolute encoder read issue.
*/
private final String absoluteEncoderIssueName;
private final BooleanPublisher absoluteEncoderIssuePublisher;
/**
* NT3 raw angle motor.
* NT4 raw angle motor.
*/
private final String rawAngleName;
private final DoublePublisher rawAnglePublisher;
/**
* NT3 Raw drive motor.
* NT4 Raw drive motor.
*/
private final String rawDriveName;
private final DoublePublisher rawDriveEncoderPublisher;
/**
* NT3 Raw drive motor.
* NT4 Raw drive motor.
*/
private final String rawDriveVelName;
private final DoublePublisher rawDriveVelocityPublisher;
/**
* Speed setpoint publisher for the module motor-controller PID.
*/
private final DoublePublisher speedSetpointPublisher;
/**
* Angle setpoint publisher for the module motor-controller PID.
*/
private final DoublePublisher angleSetpointPublisher;
/**
* Maximum {@link LinearVelocity} for the drive motor of the swerve module.
*/
@@ -226,12 +237,22 @@ public class SwerveModule
moduleNumber,
AlertType.kWarning);
rawAbsoluteAngleName = "swerve/modules/" + configuration.name + "/Raw Absolute Encoder";
adjAbsoluteAngleName = "swerve/modules/" + configuration.name + "/Adjusted Absolute Encoder";
absoluteEncoderIssueName = "swerve/modules/" + configuration.name + "/Absolute Encoder Read Issue";
rawAngleName = "swerve/modules/" + configuration.name + "/Raw Angle Encoder";
rawDriveName = "swerve/modules/" + configuration.name + "/Raw Drive Encoder";
rawDriveVelName = "swerve/modules/" + configuration.name + "/Raw Drive Velocity";
rawAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getDoubleTopic(
"swerve/modules/" + configuration.name + "/Raw Absolute Encoder").publish();
adjAbsoluteAnglePublisher = NetworkTableInstance.getDefault().getDoubleTopic(
"swerve/modules/" + configuration.name + "/Adjusted Absolute Encoder").publish();
absoluteEncoderIssuePublisher = NetworkTableInstance.getDefault().getBooleanTopic(
"swerve/modules/" + configuration.name + "/Absolute Encoder Read Issue").publish();
rawAnglePublisher = NetworkTableInstance.getDefault().getDoubleTopic(
"swerve/modules/" + configuration.name + "/Raw Angle Encoder").publish();
rawDriveEncoderPublisher = NetworkTableInstance.getDefault().getDoubleTopic(
"swerve/modules/" + configuration.name + "/Raw Drive Encoder").publish();
rawDriveVelocityPublisher = NetworkTableInstance.getDefault().getDoubleTopic(
"swerve/modules/" + configuration.name + "/Raw Drive Velocity").publish();
speedSetpointPublisher = NetworkTableInstance.getDefault().getDoubleTopic(
"swerve/modules/" + configuration.name + "/Speed Setpoint").publish();
angleSetpointPublisher = NetworkTableInstance.getDefault().getDoubleTopic(
"swerve/modules/" + configuration.name + "/Angle Setpoint").publish();
}
/**
@@ -402,7 +423,7 @@ public class SwerveModule
LinearVelocity curVelocity = MetersPerSecond.of(lastState.speedMetersPerSecond);
desiredState.speedMetersPerSecond = nextVelocity.magnitude();
setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(curVelocity, nextVelocity).magnitude());
setDesiredState(desiredState, isOpenLoop, driveMotorFeedforward.calculate(nextVelocity).magnitude());
}
/**
@@ -457,10 +478,13 @@ public class SwerveModule
if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH)
{
SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Speed Setpoint",
desiredState.speedMetersPerSecond);
SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Angle Setpoint",
desiredState.angle.getDegrees());
speedSetpointPublisher.set(desiredState.speedMetersPerSecond);
angleSetpointPublisher.set(desiredState.angle.getDegrees());
}
if (moduleNumber == SwerveDriveTelemetry.moduleCount - 1)
{
SwerveDriveTelemetry.endCtrlCycle();
}
}
@@ -766,13 +790,13 @@ public class SwerveModule
{
if (absoluteEncoder != null)
{
SmartDashboard.putNumber(rawAbsoluteAngleName, absoluteEncoder.getAbsolutePosition());
rawAbsoluteAnglePublisher.set(absoluteEncoder.getAbsolutePosition());
}
SmartDashboard.putNumber(rawAngleName, angleMotor.getPosition());
SmartDashboard.putNumber(rawDriveName, drivePositionCache.getValue());
SmartDashboard.putNumber(rawDriveVelName, driveVelocityCache.getValue());
SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition());
SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0);
rawAnglePublisher.set(angleMotor.getPosition());
rawDriveEncoderPublisher.set(drivePositionCache.getValue());
rawDriveVelocityPublisher.set(driveVelocityCache.getValue());
adjAbsoluteAnglePublisher.set(getAbsolutePosition());
absoluteEncoderIssuePublisher.set(getAbsoluteEncoderReadIssue());
}
/**

74
swervelib/encoders/CANCoderSwerve.java
View File

@@ -2,17 +2,19 @@ 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;
import static edu.wpi.first.units.Units.Seconds;
import com.ctre.phoenix6.StatusCode;
import com.ctre.phoenix6.StatusSignal;
import com.ctre.phoenix6.configs.CANcoderConfiguration;
import com.ctre.phoenix6.configs.CANcoderConfigurator;
import com.ctre.phoenix6.configs.MagnetSensorConfigs;
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;
import edu.wpi.first.wpilibj.Alert.AlertType;
@@ -25,27 +27,47 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
/**
* Wait time for status frames to show up.
*/
public static double STATUS_TIMEOUT_SECONDS = 0.02;
public static double STATUS_TIMEOUT_SECONDS = Milliseconds.of(10).in(Seconds);
/**
* CANCoder with WPILib sendable and support.
*/
public CANcoder encoder;
public CANcoder encoder;
/**
* An {@link Alert} for if the CANCoder magnet field is less than ideal.
*/
private Alert magnetFieldLessThanIdeal;
private final Alert magnetFieldLessThanIdeal;
/**
* An {@link Alert} for if the CANCoder reading is faulty.
*/
private Alert readingFaulty;
private final Alert readingFaulty;
/**
* An {@link Alert} for if the CANCoder reading is faulty and the reading is ignored.
*/
private Alert readingIgnored;
private final Alert readingIgnored;
/**
* An {@link Alert} for if the absolute encoder offset cannot be set.
*/
private Alert cannotSetOffset;
private final Alert cannotSetOffset;
/**
* Magnet Health status signal for the CANCoder.
*/
private final StatusSignal<MagnetHealthValue> magnetHealth;
/**
* CANCoder reading cache.
*/
private final StatusSignal<Angle> angle;
/**
* Angular velocity of the {@link CANcoder}.
*/
private final StatusSignal<AngularVelocity> velocity;
/**
* {@link CANcoder} Configurator objet for this class.
*/
private CANcoderConfigurator config;
/**
* {@link CANcoderConfiguration} object for the CANcoder.
*/
private CANcoderConfiguration cfg = new CANcoderConfiguration();
/**
* Initialize the CANCoder on the standard CANBus.
@@ -61,12 +83,16 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
/**
* Initialize the CANCoder on the CANivore.
*
* @param id CAN ID.
* @param canbus CAN bus to initialize it on.
* @param id CAN ID of the {@link CANcoder}.
* @param canbus CAN bus to initialize it on. Should be "rio" or "" if the RIO CANbus, else is the CANivore name.
*/
public CANCoderSwerve(int id, String canbus)
{
encoder = new CANcoder(id, canbus);
config = encoder.getConfigurator();
magnetHealth = encoder.getMagnetHealth();
angle = encoder.getAbsolutePosition();
velocity = encoder.getVelocity();
magnetFieldLessThanIdeal = new Alert(
"Encoders",
"CANCoder " + encoder.getDeviceID() + " magnetic field is less than ideal.",
@@ -93,7 +119,8 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public void factoryDefault()
{
encoder.getConfigurator().apply(new CANcoderConfiguration());
cfg = new CANcoderConfiguration();
config.apply(cfg);
}
/**
@@ -113,15 +140,13 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public void configure(boolean inverted)
{
CANcoderConfigurator cfg = encoder.getConfigurator();
MagnetSensorConfigs magnetSensorConfiguration = new MagnetSensorConfigs();
cfg.refresh(magnetSensorConfiguration);
cfg.apply(magnetSensorConfiguration
.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1))
.withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive
: SensorDirectionValue.CounterClockwise_Positive));
config.refresh(cfg.MagnetSensor);
config.apply(cfg.MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1))
.withSensorDirection(inverted ? SensorDirectionValue.Clockwise_Positive
: SensorDirectionValue.CounterClockwise_Positive));
}
/**
* Get the absolute position of the encoder. Sets {@link SwerveAbsoluteEncoder#readingError} on erroneous readings.
*
@@ -131,7 +156,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
public double getAbsolutePosition()
{
readingError = false;
MagnetHealthValue strength = encoder.getMagnetHealth().getValue();
MagnetHealthValue strength = magnetHealth.refresh().getValue();
magnetFieldLessThanIdeal.set(strength != MagnetHealthValue.Magnet_Green);
if (strength == MagnetHealthValue.Magnet_Invalid || strength == MagnetHealthValue.Magnet_Red)
@@ -144,7 +169,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
readingFaulty.set(false);
}
StatusSignal<Angle> angle = encoder.getAbsolutePosition();
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
@@ -154,7 +179,7 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
{
break;
}
angle = angle.waitForUpdate(STATUS_TIMEOUT_SECONDS);
angle.waitForUpdate(STATUS_TIMEOUT_SECONDS);
}
if (angle.getStatus() != StatusCode.OK)
{
@@ -188,14 +213,13 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public boolean setAbsoluteEncoderOffset(double offset)
{
CANcoderConfigurator cfg = encoder.getConfigurator();
MagnetSensorConfigs magCfg = new MagnetSensorConfigs();
StatusCode error = cfg.refresh(magCfg);
StatusCode error = config.refresh(cfg.MagnetSensor);
if (error != StatusCode.OK)
{
return false;
}
error = cfg.apply(magCfg.withMagnetOffset(offset / 360));
error = config.apply(cfg.MagnetSensor.withMagnetOffset(offset / 360));
cannotSetOffset.setText(
"Failure to set CANCoder "
+ encoder.getDeviceID()
@@ -218,6 +242,6 @@ public class CANCoderSwerve extends SwerveAbsoluteEncoder
@Override
public double getVelocity()
{
return encoder.getVelocity().getValue().in(DegreesPerSecond);
return velocity.refresh().getValue().in(DegreesPerSecond);
}
}

25
swervelib/imu/ADIS16448Swerve.java
View File

@@ -1,7 +1,11 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.DegreesPerSecond;
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.ADIS16448_IMU;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
@@ -15,15 +19,19 @@ public class ADIS16448Swerve extends SwerveIMU
/**
* {@link ADIS16448_IMU} device to read the current headings from.
*/
private final ADIS16448_IMU imu;
private final ADIS16448_IMU imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the ADIS16448.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* Construct the ADIS16448 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -110,14 +118,11 @@ public class ADIS16448Swerve extends SwerveIMU
return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()));
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();
return yawVel.mut_setMagnitude(imu.getRate());
}
/**

24
swervelib/imu/ADIS16470Swerve.java
View File

@@ -1,7 +1,11 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.DegreesPerSecond;
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.ADIS16470_IMU;
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -16,15 +20,19 @@ public class ADIS16470Swerve extends SwerveIMU
/**
* {@link ADIS16470_IMU} device to read the current headings from.
*/
private final ADIS16470_IMU imu;
private final ADIS16470_IMU imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the ADIS16470.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* Construct the ADIS16470 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -110,14 +118,10 @@ public class ADIS16470Swerve extends SwerveIMU
return Optional.of(new Translation3d(imu.getAccelX(), imu.getAccelY(), imu.getAccelZ()));
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();
return yawVel.mut_setMagnitude(imu.getRate());
}
/**

24
swervelib/imu/ADXRS450Swerve.java
View File

@@ -1,7 +1,11 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.DegreesPerSecond;
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.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
@@ -15,15 +19,19 @@ public class ADXRS450Swerve extends SwerveIMU
/**
* {@link ADXRS450_Gyro} device to read the current headings from.
*/
private final ADXRS450_Gyro imu;
private final ADXRS450_Gyro imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the ADXRS450.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* Construct the ADXRS450 imu and reset default configurations. Publish the gyro to the SmartDashboard.
@@ -108,14 +116,10 @@ public class ADXRS450Swerve extends SwerveIMU
return Optional.empty();
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();
return yawVel.mut_setMagnitude(imu.getRate());
}
/**

24
swervelib/imu/AnalogGyroSwerve.java
View File

@@ -1,7 +1,11 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.DegreesPerSecond;
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.AnalogGyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
@@ -15,15 +19,19 @@ public class AnalogGyroSwerve extends SwerveIMU
/**
* Gyroscope object.
*/
private final AnalogGyro imu;
private final AnalogGyro imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the analog gyro.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* Analog port in which the gyroscope is connected. Can only be attached to analog ports 0 or 1.
@@ -115,14 +123,10 @@ public class AnalogGyroSwerve extends SwerveIMU
return Optional.empty();
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();
return yawVel.mut_setMagnitude(imu.getRate());
}
/**

26
swervelib/imu/CanandgyroSwerve.java
View File

@@ -1,8 +1,12 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.RotationsPerSecond;
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.AngularVelocity;
import edu.wpi.first.units.measure.MutAngularVelocity;
import java.util.Optional;
/**
@@ -14,19 +18,23 @@ public class CanandgyroSwerve 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;
/**
* Boron {@link Canandgyro} by Redux Robotics.
*/
private final Canandgyro imu;
private final Canandgyro imu;
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, RotationsPerSecond);
/**
* Offset for the Boron {@link Canandgyro}.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* Generate the SwerveIMU for {@link Canandgyro}.
@@ -112,14 +120,10 @@ public class CanandgyroSwerve extends SwerveIMU
return Optional.of(new Translation3d(imu.getAccelerationFrame().getValue()).times(9.81 / 16384.0));
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getAngularVelocityYaw();
return yawVel.mut_setMagnitude(imu.getAngularVelocityYaw());
}
/**

26
swervelib/imu/NavXSwerve.java
View File

@@ -1,9 +1,13 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.DegreesPerSecond;
import com.studica.frc.AHRS;
import com.studica.frc.AHRS.NavXComType;
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.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import java.util.Optional;
@@ -14,22 +18,26 @@ import java.util.Optional;
public class NavXSwerve extends SwerveIMU
{
/**
* Mutable {@link AngularVelocity} for readings.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* NavX IMU.
*/
private AHRS imu;
private AHRS imu;
/**
* Offset for the NavX.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* An {@link Alert} for if there is an error instantiating the NavX.
*/
private Alert navXError;
private Alert navXError;
/**
* Constructor for the NavX({@link AHRS}) swerve.
@@ -133,14 +141,10 @@ public class NavXSwerve extends SwerveIMU
.times(9.81));
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();
return yawVel.mut_setMagnitude(imu.getRate());
}
/**

27
swervelib/imu/Pigeon2Swerve.java
View File

@@ -8,7 +8,9 @@ import com.ctre.phoenix6.configs.Pigeon2Configurator;
import com.ctre.phoenix6.hardware.Pigeon2;
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.LinearAcceleration;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Optional;
import java.util.function.Supplier;
@@ -22,23 +24,28 @@ 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.
*/
private final MutAngularVelocity yawVel = new MutAngularVelocity(0, 0, DegreesPerSecond);
/**
* Offset for the {@link Pigeon2}.
*/
private Rotation3d offset = new Rotation3d();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* {@link Pigeon2} configurator.
*/
private Pigeon2Configurator cfg;
private Pigeon2Configurator cfg;
/**
* X Acceleration supplier
@@ -158,14 +165,10 @@ public class Pigeon2Swerve extends SwerveIMU
return Optional.empty();
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return Rotation rate in DegreesPerSecond.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue().in(DegreesPerSecond);
return yawVel.mut_replace(imu.getAngularVelocityZWorld().waitForUpdate(STATUS_TIMEOUT_SECONDS).getValue());
}
/**

24
swervelib/imu/PigeonSwerve.java
View File

@@ -1,9 +1,13 @@
package swervelib.imu;
import static edu.wpi.first.units.Units.DegreesPerSecond;
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;
@@ -16,15 +20,19 @@ public class PigeonSwerve extends SwerveIMU
/**
* {@link WPI_PigeonIMU} IMU device.
*/
private final WPI_PigeonIMU imu;
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();
private Rotation3d offset = new Rotation3d();
/**
* Inversion for the gyro
*/
private boolean invertedIMU = false;
private boolean invertedIMU = false;
/**
* Generate the SwerveIMU for {@link WPI_PigeonIMU}.
@@ -115,14 +123,10 @@ public class PigeonSwerve extends SwerveIMU
return Optional.of(new Translation3d(initial[0], initial[1], initial[2]).times(9.81 / 16384.0));
}
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
*/
public double getRate()
@Override
public MutAngularVelocity getYawAngularVelocity()
{
return imu.getRate();
return yawVel.mut_setMagnitude(imu.getRate());
}
/**

8
swervelib/imu/SwerveIMU.java
View File

@@ -2,6 +2,8 @@ package swervelib.imu;
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 java.util.Optional;
/**
@@ -57,11 +59,11 @@ public abstract class SwerveIMU
public abstract Optional<Translation3d> getAccel();
/**
* Fetch the rotation rate from the IMU in degrees per second. If rotation rate isn't supported returns empty.
* Fetch the rotation rate from the IMU as {@link AngularVelocity}
*
* @return {@link Double} of the rotation rate as an {@link Optional}.
* @return {@link AngularVelocity} of the rotation rate.
*/
public abstract double getRate();
public abstract MutAngularVelocity getYawAngularVelocity();
/**
* Get the instantiated IMU object.

20
swervelib/motors/SparkFlexSwerve.java
View File

@@ -19,6 +19,7 @@ import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.units.Units;
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.function.Supplier;
import swervelib.encoders.SwerveAbsoluteEncoder;
@@ -75,6 +76,10 @@ public class SparkFlexSwerve extends SwerveMotor
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* After the first post-module config update there will be an error thrown to alert to a possible issue.
*/
private boolean startupInitialized = false;
/**
@@ -135,7 +140,7 @@ public class SparkFlexSwerve extends SwerveMotor
{
return;
}
Timer.delay(Units.Milliseconds.of(10).in(Seconds));
Timer.delay(Units.Milliseconds.of(5).in(Seconds));
}
failureConfiguring.set(true);
}
@@ -420,7 +425,9 @@ public class SparkFlexSwerve extends SwerveMotor
@Override
public void burnFlash()
{
motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
configureSparkFlex(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
});
cfgUpdated = false;
}
@@ -449,7 +456,14 @@ public class SparkFlexSwerve extends SwerveMotor
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.1); // Give 100ms to apply changes
Timer.delay(0.01); // Give 10ms to apply changes
if (startupInitialized)
{
DriverStation.reportWarning("Applying changes mid-execution not recommended.", true);
} else
{
startupInitialized = true;
}
}
if (isDriveMotor)

19
swervelib/motors/SparkMaxBrushedMotorSwerve.java
View File

@@ -82,6 +82,10 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* After the first post-module config update there will be an error thrown to alert to a possible issue.
*/
private boolean startupInitialized = false;
/**
* Initialize the swerve motor.
@@ -191,7 +195,7 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
{
return;
}
Timer.delay(Units.Milliseconds.of(10).in(Seconds));
Timer.delay(Units.Milliseconds.of(5).in(Seconds));
}
failureConfiguringAlert.set(true);
}
@@ -499,7 +503,9 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
@Override
public void burnFlash()
{
motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
configureSparkMax(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
});
cfgUpdated = false;
}
@@ -528,7 +534,14 @@ public class SparkMaxBrushedMotorSwerve extends SwerveMotor
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.1); // Give 100ms to apply changes
Timer.delay(0.01); // Give 10ms to apply changes
if (startupInitialized)
{
DriverStation.reportWarning("Applying changes mid-execution not recommended.", true);
} else
{
startupInitialized = true;
}
}
if (isDriveMotor)

19
swervelib/motors/SparkMaxSwerve.java
View File

@@ -67,6 +67,10 @@ public class SparkMaxSwerve extends SwerveMotor
* Tracker for changes that need to be pushed.
*/
private boolean cfgUpdated = false;
/**
* After the first post-module config update there will be an error thrown to alert to a possible issue.
*/
private boolean startupInitialized = false;
/**
@@ -122,7 +126,7 @@ public class SparkMaxSwerve extends SwerveMotor
{
return;
}
Timer.delay(Units.Milliseconds.of(10).in(Seconds));
Timer.delay(Units.Milliseconds.of(5).in(Seconds));
}
DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true);
}
@@ -427,7 +431,9 @@ public class SparkMaxSwerve extends SwerveMotor
@Override
public void burnFlash()
{
motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
configureSparkMax(() -> {
return motor.configure(cfg, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters);
});
cfgUpdated = false;
}
@@ -456,7 +462,14 @@ public class SparkMaxSwerve extends SwerveMotor
if (cfgUpdated)
{
burnFlash();
Timer.delay(0.1); // Give 100ms to apply changes
Timer.delay(0.01); // Give 10ms to apply changes
if (startupInitialized)
{
DriverStation.reportWarning("Applying changes mid-execution not recommended.", true);
} else
{
startupInitialized = true;
}
}
if (isDriveMotor)

7
swervelib/parser/SwerveDriveConfiguration.java
View File

@@ -3,6 +3,7 @@ package swervelib.parser;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.system.plant.DCMotor;
import java.util.function.Supplier;
import org.ironmaple.simulation.drivesims.COTS;
import org.ironmaple.simulation.drivesims.GyroSimulation;
import swervelib.SwerveModule;
import swervelib.imu.NavXSwerve;
@@ -153,12 +154,12 @@ public class SwerveDriveConfiguration
{
if (imu instanceof Pigeon2Swerve)
{
return GyroSimulation.getPigeon2();
return COTS.ofPigeon2();
} else if (imu instanceof NavXSwerve)
{
return GyroSimulation.getNav2X();
return COTS.ofNav2X();
}
return GyroSimulation.getGeneric();
return COTS.ofGenericGyro();
}
}

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

@@ -195,6 +195,10 @@ public class DeviceJson
case "talonsrx":
return new TalonSRXSwerve(id, isDriveMotor, DCMotor.getCIM(1));
case "sparkmax_brushed":
if (canbus == null)
{
canbus = "";
}
switch (canbus)
{
case "greyhill_63r256":

363
swervelib/telemetry/SwerveDriveTelemetry.java
View File

@@ -3,13 +3,18 @@ package swervelib.telemetry;
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.networktables.DoubleArrayPublisher;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.networktables.StructArrayPublisher;
import edu.wpi.first.networktables.StructPublisher;
import edu.wpi.first.wpilibj.Alert;
import edu.wpi.first.wpilibj.Alert.AlertType;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Timer;
import swervelib.SwerveDrive;
/**
* Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which follows AdvantageKit)
@@ -20,156 +25,363 @@ public class SwerveDriveTelemetry
/**
* An {@link Alert} for if the CAN ID is greater than 40.
*/
public static final Alert canIdWarning = new Alert("JSON",
"CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!",
AlertType.kWarning);
public static final Alert canIdWarning = new Alert("JSON",
"CAN IDs greater than 40 can cause undefined behaviour, please use a CAN ID below 40!",
AlertType.kWarning);
/**
* An {@link Alert} for if there is an I2C lockup issue on the roboRIO.
*/
public static final Alert i2cLockupWarning = new Alert("IMU",
"I2C lockup issue detected on roboRIO. Check console for more information.",
AlertType.kWarning);
public static final Alert i2cLockupWarning = new Alert("IMU",
"I2C lockup issue detected on roboRIO. Check console for more information.",
AlertType.kWarning);
/**
* NavX serial comm issue.
*/
public static final Alert serialCommsIssueWarning = new Alert("IMU",
"Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.",
AlertType.kWarning);
public static final Alert serialCommsIssueWarning = new Alert("IMU",
"Serial comms is interrupted with USB and other serial traffic and causes intermittent connected/disconnection issues. Please consider another protocol or be mindful of this.",
AlertType.kWarning);
/**
* Module counter publisher for NT4
*/
private static final DoublePublisher moduleCountPublisher
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/moduleCount")
.publish();
/**
* Module measured states for Nt4
*/
private static final DoubleArrayPublisher measuredStatesArrayPublisher
= NetworkTableInstance.getDefault()
.getDoubleArrayTopic(
"swerve/measuredStates")
.publish();
/**
* Desired states for NT4
*/
private static final DoubleArrayPublisher desiredStatesArrayPublisher
= NetworkTableInstance.getDefault()
.getDoubleArrayTopic(
"swerve/desiredStates")
.publish();
/**
* Measured chassis speeds array publisher.
*/
private static final DoubleArrayPublisher measuredChassisSpeedsArrayPublisher
= NetworkTableInstance.getDefault()
.getDoubleArrayTopic(
"swerve/measuredChassisSpeeds")
.publish();
/**
* Desired chassis speeds array publisher.
*/
private static final DoubleArrayPublisher desiredChassisSpeedsArrayPublisher
= NetworkTableInstance.getDefault()
.getDoubleArrayTopic(
"swerve/desiredChassisSpeeds")
.publish();
/**
* Robot rotation publisher.
*/
private static final DoublePublisher robotRotationPublisher
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/robotRotation")
.publish();
/**
* Max angular velocity publisher.
*/
private static final DoublePublisher maxAngularVelocityPublisher
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/maxAngularVelocity")
.publish();
/**
* Struct publisher for AdvantageScope swerve widgets.
*/
private static final StructArrayPublisher<SwerveModuleState> measuredStatesStruct
= NetworkTableInstance.getDefault()
.getStructArrayTopic(
"swerve/advantagescope/currentStates",
SwerveModuleState.struct)
.publish();
/**
* Struct publisher for AdvantageScope swerve widgets.
*/
private static final StructArrayPublisher<SwerveModuleState> desiredStatesStruct
= NetworkTableInstance.getDefault()
.getStructArrayTopic(
"swerve/advantagescope/desiredStates",
SwerveModuleState.struct)
.publish();
/**
* Measured {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
*/
private static final StructPublisher<ChassisSpeeds> measuredChassisSpeedsStruct
= NetworkTableInstance.getDefault()
.getStructTopic(
"swerve/advantagescope/measuredChassisSpeeds",
ChassisSpeeds.struct)
.publish();
/**
* Desired {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
*/
private static final StructPublisher<ChassisSpeeds> desiredChassisSpeedsStruct
= NetworkTableInstance.getDefault()
.getStructTopic(
"swerve/advantagescope/desiredChassisSpeeds",
ChassisSpeeds.struct)
.publish();
/**
* Robot {@link Rotation2d} for AdvantageScope swerve widgets.
*/
private static final StructPublisher<Rotation2d> robotRotationStruct
= NetworkTableInstance.getDefault()
.getStructTopic(
"swerve/advantagescope/robotRotation",
Rotation2d.struct)
.publish();
/**
* Wheel locations array publisher for NT4.
*/
private static final DoubleArrayPublisher wheelLocationsArrayPublisher = NetworkTableInstance.getDefault()
.getDoubleArrayTopic(
"swerve/wheelLocation")
.publish();
/**
* Max speed publisher for NT4.
*/
private static final DoublePublisher maxSpeedPublisher = NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/maxSpeed")
.publish();
/**
* Rotation unit for NT4.
*/
private static final StringPublisher rotationUnitPublisher = NetworkTableInstance.getDefault()
.getStringTopic(
"swerve/rotationUnit")
.publish();
/**
* Chassis width publisher
*/
private static final DoublePublisher sizeLeftRightPublisher = NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/sizeLeftRight")
.publish();
/**
* Chassis Length publisher.
*/
private static final DoublePublisher sizeFrontBackPublisher = NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/sizeFrontBack")
.publish();
/**
* Chassis direction widget publisher.
*/
private static final StringPublisher forwardDirectionPublisher = NetworkTableInstance.getDefault()
.getStringTopic(
"swerve/forwardDirection")
.publish();
/**
* Odometry cycle time, updated whenever {@link SwerveDrive#updateOdometry()} is called.
*/
private static final DoublePublisher odomCycleTime
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/odomCycleMS")
.publish();
/**
* Control cycle time, updated whenever
* {@link swervelib.SwerveModule#setDesiredState(SwerveModuleState, boolean, double)} is called for the last module.
*/
private static final DoublePublisher ctrlCycleTime
= NetworkTableInstance.getDefault()
.getDoubleTopic(
"swerve/controlCycleMS")
.publish();
/**
* Odometry timer to track cycle times.
*/
private static final Timer odomTimer = new Timer();
/**
* Control timer to track cycle times.
*/
private static final Timer ctrlTimer = new Timer();
/**
* Measured swerve module states object.
*/
public static SwerveModuleState[] measuredStatesObj = new SwerveModuleState[4];
public static SwerveModuleState[] measuredStatesObj
= new SwerveModuleState[4];
/**
* Desired swerve module states object
*/
public static SwerveModuleState[] desiredStatesObj = new SwerveModuleState[4];
public static SwerveModuleState[] desiredStatesObj
= new SwerveModuleState[4];
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds();
public static ChassisSpeeds measuredChassisSpeedsObj = new ChassisSpeeds();
/**
* Describes the desired forward, sideways and angular velocity of the robot.
*/
public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds();
public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds();
/**
* The robot's current rotation based on odometry or gyro readings
*/
public static Rotation2d robotRotationObj = new Rotation2d();
public static Rotation2d robotRotationObj = new Rotation2d();
/**
* The current telemetry verbosity level.
*/
public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE;
public static TelemetryVerbosity verbosity
= TelemetryVerbosity.MACHINE;
/**
* State of simulation of the Robot, used to optimize retrieval.
*/
public static boolean isSimulation = RobotBase.isSimulation();
public static boolean isSimulation
= RobotBase.isSimulation();
/**
* The number of swerve modules
*/
public static int moduleCount;
public static int moduleCount;
/**
* The Locations of the swerve drive wheels.
*/
public static double[] wheelLocations;
public static double[] wheelLocations;
/**
* An array of rotation and velocity values describing the measured state of each swerve module
*/
public static double[] measuredStates;
public static double[] measuredStates;
/**
* An array of rotation and velocity values describing the desired state of each swerve module
*/
public static double[] desiredStates;
public static double[] desiredStates;
/**
* The robot's current rotation based on odometry or gyro readings
*/
public static double robotRotation = 0;
public static double robotRotation = 0;
/**
* The maximum achievable speed of the modules, used to adjust the size of the vectors.
*/
public static double maxSpeed;
public static double maxSpeed;
/**
* The units of the module rotations and robot rotation
*/
public static String rotationUnit = "degrees";
public static String rotationUnit = "degrees";
/**
* The distance between the left and right modules.
*/
public static double sizeLeftRight;
public static double sizeLeftRight;
/**
* The distance between the front and back modules.
*/
public static double sizeFrontBack;
public static double sizeFrontBack;
/**
* The direction the robot should be facing when the "Robot Rotation" is zero or blank. This option is often useful to
* align with odometry data or match videos. 'up', 'right', 'down' or 'left'
*/
public static String forwardDirection = "up";
public static String forwardDirection = "up";
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
public static double maxAngularVelocity;
public static double maxAngularVelocity;
/**
* The maximum achievable angular velocity of the robot. This is used to visualize the angular velocity from the
* chassis speeds properties.
*/
public static double[] measuredChassisSpeeds = new double[3];
public static double[] measuredChassisSpeeds = new double[3];
/**
* Describes the desired forward, sideways and angular velocity of the robot.
*/
public static double[] desiredChassisSpeeds = new double[3];
public static double[] desiredChassisSpeeds = new double[3];
/**
* Struct publisher for AdvantageScope swerve widgets.
* Update the telemetry settings that infrequently change.
*/
private static StructArrayPublisher<SwerveModuleState> measuredStatesStruct
= NetworkTableInstance.getDefault()
.getStructArrayTopic(
"swerve/advantagescope/currentStates",
SwerveModuleState.struct)
.publish();
public static boolean updateSettings = true;
/**
* Struct publisher for AdvantageScope swerve widgets.
* Start the ctrl timer to measure cycle time, independent of periodic loops.
*/
private static StructArrayPublisher<SwerveModuleState> desiredStatesStruct
= NetworkTableInstance.getDefault()
.getStructArrayTopic(
"swerve/advantagescope/desiredStates",
SwerveModuleState.struct)
.publish();
public static void startCtrlCycle()
{
if (ctrlTimer.isRunning())
{
ctrlTimer.reset();
} else
{
ctrlTimer.start();
}
}
/**
* Measured {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
* Update the Control cycle time.
*/
private static StructPublisher<ChassisSpeeds> measuredChassisSpeedsStruct
= NetworkTableInstance.getDefault()
.getStructTopic(
"swerve/advantagescope/measuredChassisSpeeds",
ChassisSpeeds.struct)
.publish();
public static void endCtrlCycle()
{
if (DriverStation.isTeleopEnabled() || DriverStation.isAutonomousEnabled() || DriverStation.isTestEnabled())
{
// 100ms per module on initialization is normal
ctrlCycleTime.set(ctrlTimer.get() * 1000);
}
ctrlTimer.reset();
}
/**
* Desired {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
* Start the odom cycle timer to calculate how long each odom took. Independent of periodic loops.
*/
private static StructPublisher<ChassisSpeeds> desiredChassisSpeedsStruct
= NetworkTableInstance.getDefault()
.getStructTopic(
"swerve/advantagescope/desiredChassisSpeeds",
ChassisSpeeds.struct)
.publish();
public static void startOdomCycle()
{
if (odomTimer.isRunning())
{
odomTimer.reset();
} else
{
odomTimer.start();
}
}
/**
* Robot {@link Rotation2d} for AdvantageScope swerve widgets.
* Update the odom cycle time.
*/
private static StructPublisher<Rotation2d> robotRotationStruct
= NetworkTableInstance.getDefault()
.getStructTopic(
"swerve/advantagescope/robotRotation",
Rotation2d.struct)
.publish();
public static void endOdomCycle()
{
if (DriverStation.isTeleopEnabled() || DriverStation.isAutonomousEnabled() || DriverStation.isTestEnabled())
{
odomCycleTime.set(odomTimer.get() * 1000);
}
odomTimer.reset();
}
/**
* Update only the settings that infrequently or never change.
*/
public static void updateSwerveTelemetrySettings()
{
if (updateSettings)
{
updateSettings = false;
wheelLocationsArrayPublisher.set(wheelLocations);
maxSpeedPublisher.set(maxSpeed);
rotationUnitPublisher.set(rotationUnit);
sizeLeftRightPublisher.set(sizeLeftRight);
sizeFrontBackPublisher.set(sizeFrontBack);
forwardDirectionPublisher.set(forwardDirection);
}
}
/**
* Upload data to smartdashboard
*/
public static void updateData()
{
if (updateSettings)
{
updateSwerveTelemetrySettings();
}
measuredChassisSpeeds[0] = measuredChassisSpeedsObj.vxMetersPerSecond;
measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vxMetersPerSecond;
measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeedsObj.omegaRadiansPerSecond);
@@ -200,19 +412,14 @@ public class SwerveDriveTelemetry
}
}
SmartDashboard.putNumber("swerve/moduleCount", moduleCount);
SmartDashboard.putNumberArray("swerve/wheelLocations", wheelLocations);
SmartDashboard.putNumberArray("swerve/measuredStates", measuredStates);
SmartDashboard.putNumberArray("swerve/desiredStates", desiredStates);
SmartDashboard.putNumber("swerve/robotRotation", robotRotation);
SmartDashboard.putNumber("swerve/maxSpeed", maxSpeed);
SmartDashboard.putString("swerve/rotationUnit", rotationUnit);
SmartDashboard.putNumber("swerve/sizeLeftRight", sizeLeftRight);
SmartDashboard.putNumber("swerve/sizeFrontBack", sizeFrontBack);
SmartDashboard.putString("swerve/forwardDirection", forwardDirection);
SmartDashboard.putNumber("swerve/maxAngularVelocity", maxAngularVelocity);
SmartDashboard.putNumberArray("swerve/measuredChassisSpeeds", measuredChassisSpeeds);
SmartDashboard.putNumberArray("swerve/desiredChassisSpeeds", desiredChassisSpeeds);
moduleCountPublisher.set(moduleCount);
measuredStatesArrayPublisher.set(measuredStates);
desiredStatesArrayPublisher.set(desiredStates);
robotRotationPublisher.set(robotRotation);
maxAngularVelocityPublisher.set(maxAngularVelocity);
measuredChassisSpeedsArrayPublisher.set(measuredChassisSpeeds);
desiredChassisSpeedsArrayPublisher.set(desiredChassisSpeeds);
desiredStatesStruct.set(desiredStatesObj);
measuredStatesStruct.set(measuredStatesObj);