From 6aa91f25985754cf1637d12e2d5f70932dc1b32c Mon Sep 17 00:00:00 2001 From: thenetworkgrinch Date: Wed, 12 Jun 2024 15:58:37 -0500 Subject: [PATCH] Updated Telemetry, added javadocs --- swervelib/SwerveDrive.java | 24 +++++++----- swervelib/SwerveDriveTest.java | 12 +++--- swervelib/SwerveModule.java | 55 ++++++++++++++-------------- swervelib/motors/TalonSRXSwerve.java | 1 - 4 files changed, 48 insertions(+), 44 deletions(-) diff --git a/swervelib/SwerveDrive.java b/swervelib/SwerveDrive.java index 1745f2c..9f45b91 100644 --- a/swervelib/SwerveDrive.java +++ b/swervelib/SwerveDrive.java @@ -104,7 +104,7 @@ 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. */ @@ -347,15 +347,17 @@ public class SwerveDrive } /** - * 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. - * + * 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) + public void driveFieldOrientedandRobotOriented(ChassisSpeeds fieldOrientedVelocity, + ChassisSpeeds robotOrientedVelocity) { - ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading()).plus(robotOrientedVelocity); + ChassisSpeeds TotalVelocties = ChassisSpeeds.fromFieldRelativeSpeeds(fieldOrientedVelocity, getOdometryHeading()) + .plus(robotOrientedVelocity); drive(TotalVelocties); } @@ -1174,11 +1176,13 @@ public class SwerveDrive /** * Sets the Chassis discretization seconds as well as enableing/disabling the Chassis velocity correction - * - * @param enable - * @param dtSeconds + * + * @param enable Enable chassis velocity correction, which will use + * {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} with the following. + * @param dtSeconds The duration of the timestep the speeds should be applied for. */ - public void setChassisDiscretization(boolean enable, double dtSeconds){ + public void setChassisDiscretization(boolean enable, double dtSeconds) + { chassisVelocityCorrection = enable; discretizationdtSeconds = dtSeconds; } diff --git a/swervelib/SwerveDriveTest.java b/swervelib/SwerveDriveTest.java index 947601d..85d9a45 100644 --- a/swervelib/SwerveDriveTest.java +++ b/swervelib/SwerveDriveTest.java @@ -286,9 +286,9 @@ public class SwerveDriveTest double power = powerSupplied.get(); double distance = module.getPosition().distanceMeters; double velocity = module.getDriveMotor().getVelocity(); - SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Drive Power", power); - SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Drive Position", distance); - SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Drive Velocity", velocity); + SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Drive Power", power); + SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Drive Position", distance); + SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Drive Velocity", velocity); log.motor("drive-" + module.configuration.name) .voltage(m_appliedVoltage.mut_replace(power, Volts)) .linearPosition(m_distance.mut_replace(distance, Meters)) @@ -358,9 +358,9 @@ public class SwerveDriveTest double power = powerSupplied.get(); double angle = module.getAbsolutePosition(); double velocity = module.getAbsoluteEncoder().getVelocity(); - SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Angle Power", power); - SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Angle Position", angle); - SmartDashboard.putNumber("Module[" + module.configuration.name + "] SysId Absolute Encoder Velocity", velocity); + SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Power", power); + SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Angle Position", angle); + SmartDashboard.putNumber("swerve/modules/" + module.configuration.name + "/SysId Absolute Encoder Velocity", velocity); log.motor("angle-" + module.configuration.name) .voltage(m_appliedVoltage.mut_replace(power, Volts)) .angularPosition(m_anglePosition.mut_replace(angle, Degrees)) diff --git a/swervelib/SwerveModule.java b/swervelib/SwerveModule.java index 8f71e61..756ef79 100644 --- a/swervelib/SwerveModule.java +++ b/swervelib/SwerveModule.java @@ -82,14 +82,14 @@ public class SwerveModule * Module number for kinematics, usually 0 to 3. front left -> front right -> back left -> back right. */ public int moduleNumber; - /** - * Feedforward for the drive motor during closed loop control. - */ - private SimpleMotorFeedforward driveMotorFeedforward; /** * Maximum speed of the drive motors in meters per second. */ public double maxSpeed; + /** + * Feedforward for the drive motor during closed loop control. + */ + private SimpleMotorFeedforward driveMotorFeedforward; /** * Anti-Jitter AKA auto-centering disabled. */ @@ -206,12 +206,12 @@ public class SwerveModule moduleNumber, Alert.AlertType.WARNING); - rawAbsoluteAngleName = "Module[" + configuration.name + "] Raw Absolute Encoder"; - adjAbsoluteAngleName = "Module[" + configuration.name + "] Adjusted Absolute Encoder"; - 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"; + 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"; } /** @@ -274,6 +274,16 @@ public class SwerveModule this.driveMotorFeedforward = drive; } + /** + * Get the current drive motor PIDF values. + * + * @return {@link PIDFConfig} of the drive motor. + */ + public PIDFConfig getDrivePIDF() + { + return configuration.velocityPIDF; + } + /** * Set the drive PIDF values. * @@ -286,13 +296,13 @@ public class SwerveModule } /** - * Get the current drive motor PIDF values. + * Get the current angle/azimuth/steering motor PIDF values. * - * @return {@link PIDFConfig} of the drive motor. + * @return {@link PIDFConfig} of the angle motor. */ - public PIDFConfig getDrivePIDF() + public PIDFConfig getAnglePIDF() { - return configuration.velocityPIDF; + return configuration.anglePIDF; } /** @@ -306,16 +316,6 @@ public class SwerveModule angleMotor.configurePIDF(config); } - /** - * Get the current angle/azimuth/steering motor PIDF values. - * - * @return {@link PIDFConfig} of the angle motor. - */ - public PIDFConfig getAnglePIDF() - { - return configuration.anglePIDF; - } - /** * Set the desired state of the swerve module.
WARNING: If you are not using one of the functions from * {@link SwerveDrive} you may screw up {@link SwerveDrive#kinematics} @@ -379,8 +379,8 @@ public class SwerveModule if (SwerveDriveTelemetry.verbosity == TelemetryVerbosity.HIGH) { - SmartDashboard.putNumber("Module[" + configuration.name + "] Speed Setpoint", desiredState.speedMetersPerSecond); - SmartDashboard.putNumber("Module[" + configuration.name + "] Angle Setpoint", desiredState.angle.getDegrees()); + SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Speed Setpoint", desiredState.speedMetersPerSecond); + SmartDashboard.putNumber("swerve/modules/" + configuration.name + "/Angle Setpoint", desiredState.angle.getDegrees()); } } @@ -638,7 +638,8 @@ public class SwerveModule } SmartDashboard.putNumber(rawAngleName, angleMotor.getPosition()); SmartDashboard.putNumber(rawDriveName, driveMotor.getPosition()); - SmartDashboard.putNumber(rawDriveVelName, driveMotor.getVelocity()); SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition()); + SmartDashboard.putNumber(rawDriveVelName, driveMotor.getVelocity()); + SmartDashboard.putNumber(adjAbsoluteAngleName, getAbsolutePosition()); SmartDashboard.putNumber(absoluteEncoderIssueName, getAbsoluteEncoderReadIssue() ? 1 : 0); } } diff --git a/swervelib/motors/TalonSRXSwerve.java b/swervelib/motors/TalonSRXSwerve.java index 5c5849a..30db007 100644 --- a/swervelib/motors/TalonSRXSwerve.java +++ b/swervelib/motors/TalonSRXSwerve.java @@ -7,7 +7,6 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.Timer; import swervelib.encoders.SwerveAbsoluteEncoder; import swervelib.math.SwerveMath; import swervelib.parser.PIDFConfig;