diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index f0acfc6..ad09420 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -8,6 +8,7 @@ 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.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.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 void drive( */ public void drive(ChassisSpeeds robotRelativeVelocity, boolean isOpenLoop, Translation2d centerOfRotationMeters) { - + SwerveDriveTelemetry.startCtrlCycle(); robotRelativeVelocity = movementOptimizations(robotRelativeVelocity, chassisVelocityCorrection, angularVelocityCorrection); @@ -702,6 +715,7 @@ private void setRawModuleStates(SwerveModuleState[] desiredStates, ChassisSpeeds */ 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 void setModuleStates(SwerveModuleState[] desiredStates, boolean isOpenLoo */ 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 void drive(ChassisSpeeds robotRelativeVelocity, SwerveModuleState[] state */ public void setChassisSpeeds(ChassisSpeeds robotRelativeSpeeds) { - + SwerveDriveTelemetry.startCtrlCycle(); robotRelativeSpeeds = movementOptimizations(robotRelativeSpeeds, autonomousChassisVelocityCorrection, autonomousAngularVelocityCorrection); @@ -1108,6 +1123,7 @@ public void replaceSwerveModuleFeedforward(SimpleMotorFeedforward driveFeedforwa */ public void updateOdometry() { + SwerveDriveTelemetry.startOdomCycle(); odometryLock.lock(); invalidateCache(); try @@ -1155,8 +1171,8 @@ public void updateOdometry() 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 void updateOdometry() throw e; } odometryLock.unlock(); + SwerveDriveTelemetry.endOdomCycle(); } /** @@ -1442,7 +1459,6 @@ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAut { if (!SwerveDriveTelemetry.isSimulation) { - imuVelocity = IMUVelocity.createIMUVelocity(imu); angularVelocityCorrection = useInTeleop; autonomousAngularVelocityCorrection = useInAuto; angularVelocityCoefficient = angularVelocityCoeff; @@ -1457,7 +1473,7 @@ public void setAngularVelocityCompensation(boolean useInTeleop, boolean useInAut */ 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()); diff --git a/swervelib/SwerveInputStream.java b/swervelib/SwerveInputStream.java new file mode 100644 index 0000000..4a8748d --- /dev/null +++ b/swervelib/SwerveInputStream.java @@ -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.
Inspired by SciBorgs. + * https://github.com/SciBorgs/Crescendo-2024/blob/main/src/main/java/org/sciborgs1155/lib/InputStream.java + *

+ * Intended to easily create an interface that generates {@link ChassisSpeeds} from + * {@link edu.wpi.first.wpilibj.XboxController} + */ +public class SwerveInputStream implements Supplier +{ + + /** + * 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 controllerOmega = Optional.empty(); + /** + * Controller supplier as heading. + */ + private Optional controllerHeadingX = Optional.empty(); + /** + * Controller supplier as heading. + */ + private Optional controllerHeadingY = Optional.empty(); + /** + * Axis deadband for the controller. + */ + private Optional axisDeadband = Optional.empty(); + /** + * Translational axis scalar value, should be between (0, 1]. + */ + private Optional translationAxisScale = Optional.empty(); + /** + * Angular velocity axis scalar value, should be between (0, 1] + */ + private Optional omegaAxisScale = Optional.empty(); + /** + * Target to aim at. + */ + private Optional aimTarget = Optional.empty(); + /** + * Output {@link ChassisSpeeds} based on heading while this is True. + */ + private Optional headingEnabled = Optional.empty(); + /** + * Locked heading for {@link SwerveInputMode#TRANSLATION_ONLY} + */ + private Optional lockedHeading = Optional.empty(); + /** + * Output {@link ChassisSpeeds} based on aim while this is True. + */ + private Optional aimEnabled = Optional.empty(); + /** + * Maintain current heading and drive without rotating, ideally. + */ + private Optional 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); + } +} diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index 97967f2..ddd694d 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -8,6 +8,9 @@ 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 SwerveModule(int moduleNumber, SwerveModuleConfiguration moduleConfigurat 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 void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, 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 void setDesiredState(SwerveModuleState desiredState, boolean isOpenLoop, 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 void updateTelemetry() { 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()); } /** diff --git a/swervelib/encoders/CANCoderSwerve.java b/swervelib/encoders/CANCoderSwerve.java index 0477b24..3789dc3 100644 --- a/swervelib/encoders/CANCoderSwerve.java +++ b/swervelib/encoders/CANCoderSwerve.java @@ -2,17 +2,19 @@ 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 magnetHealth; + /** + * CANCoder reading cache. + */ + private final StatusSignal angle; + /** + * Angular velocity of the {@link CANcoder}. + */ + private final StatusSignal 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 CANCoderSwerve(int id) /** * 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 CANCoderSwerve(int id, String canbus) @Override public void factoryDefault() { - encoder.getConfigurator().apply(new CANcoderConfiguration()); + cfg = new CANcoderConfiguration(); + config.apply(cfg); } /** @@ -113,15 +140,13 @@ public void clearStickyFaults() @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 void configure(boolean inverted) 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 double getAbsolutePosition() readingFaulty.set(false); } - StatusSignal 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 double getAbsolutePosition() { break; } - angle = angle.waitForUpdate(STATUS_TIMEOUT_SECONDS); + angle.waitForUpdate(STATUS_TIMEOUT_SECONDS); } if (angle.getStatus() != StatusCode.OK) { @@ -188,14 +213,13 @@ public Object getAbsoluteEncoder() @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 boolean setAbsoluteEncoderOffset(double offset) @Override public double getVelocity() { - return encoder.getVelocity().getValue().in(DegreesPerSecond); + return velocity.refresh().getValue().in(DegreesPerSecond); } } diff --git a/swervelib/imu/ADIS16448Swerve.java b/swervelib/imu/ADIS16448Swerve.java index be6efa5..8447fc6 100644 --- a/swervelib/imu/ADIS16448Swerve.java +++ b/swervelib/imu/ADIS16448Swerve.java @@ -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 Optional getAccel() 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()); } /** diff --git a/swervelib/imu/ADIS16470Swerve.java b/swervelib/imu/ADIS16470Swerve.java index 81eb3b1..620710d 100644 --- a/swervelib/imu/ADIS16470Swerve.java +++ b/swervelib/imu/ADIS16470Swerve.java @@ -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 Optional getAccel() 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()); } /** diff --git a/swervelib/imu/ADXRS450Swerve.java b/swervelib/imu/ADXRS450Swerve.java index e0be2e8..033ad3e 100644 --- a/swervelib/imu/ADXRS450Swerve.java +++ b/swervelib/imu/ADXRS450Swerve.java @@ -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 Optional getAccel() 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()); } /** diff --git a/swervelib/imu/AnalogGyroSwerve.java b/swervelib/imu/AnalogGyroSwerve.java index 1f3d134..a3afaf1 100644 --- a/swervelib/imu/AnalogGyroSwerve.java +++ b/swervelib/imu/AnalogGyroSwerve.java @@ -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 Optional getAccel() 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()); } /** diff --git a/swervelib/imu/CanandgyroSwerve.java b/swervelib/imu/CanandgyroSwerve.java index aed3a14..5d5f67a 100644 --- a/swervelib/imu/CanandgyroSwerve.java +++ b/swervelib/imu/CanandgyroSwerve.java @@ -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 Optional getAccel() 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()); } /** diff --git a/swervelib/imu/NavXSwerve.java b/swervelib/imu/NavXSwerve.java index dd2797d..a99450e 100644 --- a/swervelib/imu/NavXSwerve.java +++ b/swervelib/imu/NavXSwerve.java @@ -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 @@ 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 Optional getAccel() .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()); } /** diff --git a/swervelib/imu/Pigeon2Swerve.java b/swervelib/imu/Pigeon2Swerve.java index fc0a113..c471776 100644 --- a/swervelib/imu/Pigeon2Swerve.java +++ b/swervelib/imu/Pigeon2Swerve.java @@ -8,7 +8,9 @@ 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 Optional getAccel() 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()); } /** diff --git a/swervelib/imu/PigeonSwerve.java b/swervelib/imu/PigeonSwerve.java index e2dfafe..12ccf78 100644 --- a/swervelib/imu/PigeonSwerve.java +++ b/swervelib/imu/PigeonSwerve.java @@ -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 Optional getAccel() 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()); } /** diff --git a/swervelib/imu/SwerveIMU.java b/swervelib/imu/SwerveIMU.java index 1b1e5ec..2b23036 100644 --- a/swervelib/imu/SwerveIMU.java +++ b/swervelib/imu/SwerveIMU.java @@ -2,6 +2,8 @@ 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 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. diff --git a/swervelib/motors/SparkFlexSwerve.java b/swervelib/motors/SparkFlexSwerve.java index e9f4b84..ecbcf26 100644 --- a/swervelib/motors/SparkFlexSwerve.java +++ b/swervelib/motors/SparkFlexSwerve.java @@ -19,6 +19,7 @@ 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 @@ private void configureSparkFlex(Supplier config) { 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 void setInverted(boolean inverted) @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 void setReference(double setpoint, double feedforward) 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) diff --git a/swervelib/motors/SparkMaxBrushedMotorSwerve.java b/swervelib/motors/SparkMaxBrushedMotorSwerve.java index 0502da4..26fb76c 100644 --- a/swervelib/motors/SparkMaxBrushedMotorSwerve.java +++ b/swervelib/motors/SparkMaxBrushedMotorSwerve.java @@ -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 @@ private void configureSparkMax(Supplier config) { 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 void setInverted(boolean inverted) @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 void setReference(double setpoint, double feedforward) 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) diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java index b36da81..4b2955b 100644 --- a/swervelib/motors/SparkMaxSwerve.java +++ b/swervelib/motors/SparkMaxSwerve.java @@ -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 @@ private void configureSparkMax(Supplier config) { 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 void setInverted(boolean inverted) @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 void setReference(double setpoint, double feedforward) 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) diff --git a/swervelib/parser/SwerveDriveConfiguration.java b/swervelib/parser/SwerveDriveConfiguration.java index 6c62581..573d574 100644 --- a/swervelib/parser/SwerveDriveConfiguration.java +++ b/swervelib/parser/SwerveDriveConfiguration.java @@ -3,6 +3,7 @@ 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 Supplier getGyroSim() { 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(); } } diff --git a/swervelib/parser/json/DeviceJson.java b/swervelib/parser/json/DeviceJson.java index 71a11a4..1d6f339 100644 --- a/swervelib/parser/json/DeviceJson.java +++ b/swervelib/parser/json/DeviceJson.java @@ -195,6 +195,10 @@ public SwerveMotor createMotor(boolean isDriveMotor) case "talonsrx": return new TalonSRXSwerve(id, isDriveMotor, DCMotor.getCIM(1)); case "sparkmax_brushed": + if (canbus == null) + { + canbus = ""; + } switch (canbus) { case "greyhill_63r256": diff --git a/swervelib/telemetry/SwerveDriveTelemetry.java b/swervelib/telemetry/SwerveDriveTelemetry.java index 07d984b..5a7c382 100644 --- a/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/swervelib/telemetry/SwerveDriveTelemetry.java @@ -3,13 +3,18 @@ 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 measuredStatesStruct + = NetworkTableInstance.getDefault() + .getStructArrayTopic( + "swerve/advantagescope/currentStates", + SwerveModuleState.struct) + .publish(); + /** + * Struct publisher for AdvantageScope swerve widgets. + */ + private static final StructArrayPublisher desiredStatesStruct + = NetworkTableInstance.getDefault() + .getStructArrayTopic( + "swerve/advantagescope/desiredStates", + SwerveModuleState.struct) + .publish(); + /** + * Measured {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets. + */ + private static final StructPublisher measuredChassisSpeedsStruct + = NetworkTableInstance.getDefault() + .getStructTopic( + "swerve/advantagescope/measuredChassisSpeeds", + ChassisSpeeds.struct) + .publish(); + /** + * Desired {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets. + */ + private static final StructPublisher desiredChassisSpeedsStruct + = NetworkTableInstance.getDefault() + .getStructTopic( + "swerve/advantagescope/desiredChassisSpeeds", + ChassisSpeeds.struct) + .publish(); + /** + * Robot {@link Rotation2d} for AdvantageScope swerve widgets. + */ + private static final StructPublisher 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 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 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 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. + */ + public static void startOdomCycle() + { + if (odomTimer.isRunning()) + { + + odomTimer.reset(); + } else + { + odomTimer.start(); + + } + } + + /** + * Update the odom cycle time. */ - private static StructPublisher desiredChassisSpeedsStruct - = NetworkTableInstance.getDefault() - .getStructTopic( - "swerve/advantagescope/desiredChassisSpeeds", - ChassisSpeeds.struct) - .publish(); + public static void endOdomCycle() + { + if (DriverStation.isTeleopEnabled() || DriverStation.isAutonomousEnabled() || DriverStation.isTestEnabled()) + { + odomCycleTime.set(odomTimer.get() * 1000); + } + odomTimer.reset(); + } + /** - * Robot {@link Rotation2d} for AdvantageScope swerve widgets. + * Update only the settings that infrequently or never change. */ - private static StructPublisher robotRotationStruct - = NetworkTableInstance.getDefault() - .getStructTopic( - "swerve/advantagescope/robotRotation", - Rotation2d.struct) - .publish(); + 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 static void updateData() } } - 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);