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

Upgrading to 2025.1.0.1

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

82
swervelib/SwerveModule.java
View File

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