From 2d992a453a0689f6dd9a7fd24de81d08dab20ff7 Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Wed, 12 Jun 2024 15:10:29 -0500 Subject: [PATCH] Update --- swervelib/SwerveDrive.java | 55 ++++++++++++++----- swervelib/SwerveModule.java | 13 +++-- .../encoders/PWMDutyCycleEncoderSwerve.java | 10 ---- swervelib/encoders/SwerveAbsoluteEncoder.java | 2 +- swervelib/motors/SparkMaxSwerve.java | 10 +--- swervelib/motors/TalonSRXSwerve.java | 1 - swervelib/telemetry/SwerveDriveTelemetry.java | 8 +++ 7 files changed, 60 insertions(+), 39 deletions(-) diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index 98c2f5c..1745f2c 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -101,6 +101,10 @@ public class SwerveDrive * Whether to correct heading when driving translationally. Set to true to enable. */ public boolean headingCorrection = false; + /** + * Amount of seconds the duration of the timestep the speeds should be applied for. + */ + private double discretizationdtSeconds = 0.02; /** * Deadband for speeds in heading correction. */ @@ -185,12 +189,12 @@ public class SwerveDrive setMaximumSpeed(maxSpeedMPS); // Initialize Telemetry - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) { SmartDashboard.putData("Field", field); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.maxSpeed = maxSpeedMPS; SwerveDriveTelemetry.maxAngularVelocity = swerveController.config.maxAngularVelocity; @@ -342,6 +346,19 @@ public class SwerveDrive HEADING_CORRECTION_DEADBAND = deadband; } + /** + * Tertiary method of controlling the drive base given velocity in both field oriented and robot oriented at the same time. + * The inputs are added together so this is not intneded to be used to give the driver both methods of control. + * + * @param fieldOrientedVelocity The field oriented velocties to use + * @param robotOrientedVelocity The robot oriented velocties to use + */ + public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity, ChassisSpeeds robotOrientedVelocity) + { + ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading()).plus(robotOrientedVelocity); + drive(TotalVelocties); + } + /** * Secondary method of controlling the drive base given velocity and adjusting it for field oriented use. * @@ -464,7 +481,7 @@ public class SwerveDrive // https://www.chiefdelphi.com/t/whitepaper-swerve-drive-skew-and-second-order-kinematics/416964/5 if (chassisVelocityCorrection) { - velocity = ChassisSpeeds.discretize(velocity, 0.02); + velocity = ChassisSpeeds.discretize(velocity, discretizationdtSeconds); } // Heading Angular Velocity Deadband, might make a configuration option later. @@ -485,11 +502,11 @@ public class SwerveDrive } // Display commanded speed for testing - if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) + if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.INFO) { SmartDashboard.putString("RobotVelocity", velocity.toString()); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) { SwerveDriveTelemetry.desiredChassisSpeeds[1] = velocity.vyMetersPerSecond; SwerveDriveTelemetry.desiredChassisSpeeds[0] = velocity.vxMetersPerSecond; @@ -502,7 +519,6 @@ public class SwerveDrive setRawModuleStates(swerveModuleStates, isOpenLoop); } - /** * Set the maximum speeds for desaturation. * @@ -660,7 +676,7 @@ public class SwerveDrive */ public void postTrajectory(Trajectory trajectory) { - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) { field.getObject("Trajectory").setTrajectory(trajectory); } @@ -871,7 +887,7 @@ public class SwerveDrive { SwerveModuleState desiredState = new SwerveModuleState(0, swerveModule.configuration.moduleLocation.getAngle()); - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.desiredStates[swerveModule.moduleNumber * 2] = desiredState.angle.getDegrees(); @@ -932,7 +948,7 @@ public class SwerveDrive swerveDrivePoseEstimator.update(getYaw(), getModulePositions()); // Update angle accumulator if the robot is simulated - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { Pose2d[] modulePoses = getSwerveModulePoses(swerveDrivePoseEstimator.getEstimatedPosition()); if (SwerveDriveTelemetry.isSimulation) @@ -951,7 +967,7 @@ public class SwerveDrive SwerveDriveTelemetry.robotRotation = getOdometryHeading().getDegrees(); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.LOW.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.POSE.ordinal()) { field.setRobotPose(swerveDrivePoseEstimator.getEstimatedPosition()); } @@ -967,7 +983,7 @@ public class SwerveDrive SmartDashboard.putNumber("Raw IMU Yaw", getYaw().getDegrees()); SmartDashboard.putNumber("Adjusted IMU Yaw", getOdometryHeading().getDegrees()); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.measuredStates[module.moduleNumber * 2] = moduleState.angle.getDegrees(); SwerveDriveTelemetry.measuredStates[(module.moduleNumber * 2) + 1] = moduleState.speedMetersPerSecond; @@ -983,7 +999,7 @@ public class SwerveDrive moduleSynchronizationCounter = 0; } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.updateData(); } @@ -1122,11 +1138,11 @@ public class SwerveDrive * Pushes the Absolute Encoder offsets to the Encoder or Motor Controller, depending on type. Also removes the * internal offsets to prevent double offsetting. */ - public void pushOffsetsToControllers() + public void pushOffsetsToEncoders() { for (SwerveModule module : swerveModules) { - module.pushOffsetsToControllers(); + module.pushOffsetsToEncoders(); } } @@ -1156,4 +1172,15 @@ public class SwerveDrive } } + /** + * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction + * + * @param enable + * @param dtSeconds + */ + public void setChassisDiscretization(boolean enable, double dtSeconds){ + chassisVelocityCorrection = enable; + discretizationdtSeconds = dtSeconds; + } + } diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index 61bb591..8f71e61 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -74,6 +74,10 @@ public class SwerveModule * NT3 Raw drive motor. */ private final String rawDriveName; + /** + * NT3 Raw drive motor. + */ + private final String rawDriveVelName; /** * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. */ @@ -207,6 +211,7 @@ public class SwerveModule absoluteEncoderIssueName = "Module[" + configuration.name + "] Absolute Encoder Read Issue"; rawAngleName = "Module[" + configuration.name + "] Raw Angle Encoder"; rawDriveName = "Module[" + configuration.name + "] Raw Drive Encoder"; + rawDriveVelName = "Module[" + configuration.name + "] Raw Drive Velocity"; } /** @@ -252,7 +257,7 @@ public class SwerveModule this.antiJitterEnabled = antiJitter; if (antiJitter) { - pushOffsetsToControllers(); + pushOffsetsToEncoders(); } else { restoreInternalOffset(); @@ -366,7 +371,7 @@ public class SwerveModule simModule.updateStateAndPosition(desiredState); } - if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.HIGH.ordinal()) + if (SwerveDriveTelemetry.verbosity.ordinal() >= TelemetryVerbosity.INFO.ordinal()) { SwerveDriveTelemetry.desiredStates[moduleNumber * 2] = desiredState.angle.getDegrees(); SwerveDriveTelemetry.desiredStates[(moduleNumber * 2) + 1] = velocity; @@ -580,7 +585,7 @@ public class SwerveModule /** * Push absolute encoder offset in the memory of the encoder or controller. Also removes the internal angle offset. */ - public void pushOffsetsToControllers() + public void pushOffsetsToEncoders() { if (absoluteEncoder != null && angleOffset == configuration.angleOffset) { @@ -633,7 +638,7 @@ public class SwerveModule } SmartDashboard.putNumber(rawAngleName, angleMotor.getPosition()); SmartDashboard.putNumber(rawDriveName, driveMotor.getPosition()); - SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition()); + SmartDashboard.putNumber(rawDriveVelName, driveMotor.getVelocity()); SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition()); SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0); } } diff --git a/swervelib/encoders/PWMDutyCycleEncoderSwerve.java b/swervelib/encoders/PWMDutyCycleEncoderSwerve.java index 9b52c5d..24e15d6 100644 --- a/swervelib/encoders/PWMDutyCycleEncoderSwerve.java +++ b/swervelib/encoders/PWMDutyCycleEncoderSwerve.java @@ -26,10 +26,6 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder * An {@link Alert} for if the encoder cannot report accurate velocities. */ private Alert inaccurateVelocities; - /** - * An {@link Alert} for if the encoder is disconnected. - */ - private Alert disconnected; /** * Constructor for the PWM duty cycle encoder. @@ -43,10 +39,6 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder "Encoders", "The PWM Duty Cycle encoder may not report accurate velocities!", Alert.AlertType.WARNING_TRACE); - inaccurateVelocities = new Alert( - "Encoders", - "The swerve encoder on port " + pin + "is disconnected!", - Alert.AlertType.ERROR_TRACE); } @@ -69,7 +61,6 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder @Override public double getAbsolutePosition() { - disconnected.set(!encoder.isConnected()); return (isInverted ? -1.0 : 1.0) * encoder.getAbsolutePosition() * 360; } @@ -92,7 +83,6 @@ public class PWMDutyCycleEncoderSwerve extends SwerveAbsoluteEncoder @Override public double getVelocity() { - disconnected.set(!encoder.isConnected()); inaccurateVelocities.set(true); return encoder.get(); } diff --git a/swervelib/encoders/SwerveAbsoluteEncoder.java b/swervelib/encoders/SwerveAbsoluteEncoder.java index c992b81..c398c74 100644 --- a/swervelib/encoders/SwerveAbsoluteEncoder.java +++ b/swervelib/encoders/SwerveAbsoluteEncoder.java @@ -49,7 +49,7 @@ public abstract class SwerveAbsoluteEncoder /** * Sets the Absolute Encoder offset at the Encoder Level. * - * @param offset the offset the Absolute Encoder uses as the zero point. + * @param offset the offset the Absolute Encoder uses as the zero point in degrees. * @return if setting Absolute Encoder Offset was successful or not. */ public abstract boolean setAbsoluteEncoderOffset(double offset); diff --git a/swervelib/motors/SparkMaxSwerve.java b/swervelib/motors/SparkMaxSwerve.java index ced8a6e..aa19a0c 100644 --- a/swervelib/motors/SparkMaxSwerve.java +++ b/swervelib/motors/SparkMaxSwerve.java @@ -52,11 +52,6 @@ public class SparkMaxSwerve extends SwerveMotor */ private Supplier position; - /** - * An {@link Alert} for if there is an error configuring the motor. - */ - private Alert failureConfiguringAlert; - /** * Initialize the swerve motor. * @@ -79,9 +74,6 @@ public class SparkMaxSwerve extends SwerveMotor position = encoder::getPosition; // Spin off configurations in a different thread. // configureSparkMax(() -> motor.setCANTimeout(0)); // Commented out because it prevents feedback. - failureConfiguringAlert = new Alert("Motors", - "Failure configuring motor " + motor.getDeviceId(), - Alert.AlertType.WARNING_TRACE); } /** @@ -109,7 +101,7 @@ public class SparkMaxSwerve extends SwerveMotor return; } } - failureConfiguringAlert.set(true); + DriverStation.reportWarning("Failure configuring motor " + motor.getDeviceId(), true); } /** diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java index 4c42ce7..5c5849a 100644 --- a/swervelib/motors/TalonSRXSwerve.java +++ b/swervelib/motors/TalonSRXSwerve.java @@ -228,7 +228,6 @@ public class TalonSRXSwerve extends SwerveMotor @Override public void setInverted(boolean inverted) { - Timer.delay(1); motor.setInverted(inverted); } diff --git a/swervelib/telemetry/SwerveDriveTelemetry.java b/swervelib/telemetry/SwerveDriveTelemetry.java index 8a300d9..26094d3 100644 --- a/swervelib/telemetry/SwerveDriveTelemetry.java +++ b/swervelib/telemetry/SwerveDriveTelemetry.java @@ -125,6 +125,14 @@ public class SwerveDriveTelemetry * Low telemetry data, only post the robot position on the field. */ LOW, + /** + * Medium telemetry data, swerve directory + */ + INFO, + /** + * Info level + field info + */ + POSE, /** * Full swerve drive data is sent back in both human and machine readable forms. */