mirror of
https://github.com/BroncBotz3481/YAGSL
synced 2026-06-19 06:21:40 +00:00
255 lines
13 KiB
Java
255 lines
13 KiB
Java
package swervelib.telemetry;
|
|
|
|
import edu.wpi.first.math.geometry.Rotation2d;
|
|
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
|
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
|
import edu.wpi.first.networktables.NetworkTableInstance;
|
|
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.RobotBase;
|
|
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
|
|
|
/**
|
|
* Telemetry to describe the {@link swervelib.SwerveDrive} following frc-web-components. (Which follows AdvantageKit)
|
|
*/
|
|
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);
|
|
/**
|
|
* 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);
|
|
/**
|
|
* 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);
|
|
/**
|
|
* Measured swerve module states object.
|
|
*/
|
|
public static SwerveModuleState[] measuredStatesObj = new SwerveModuleState[4];
|
|
/**
|
|
* Desired swerve module states object
|
|
*/
|
|
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();
|
|
/**
|
|
* Describes the desired forward, sideways and angular velocity of the robot.
|
|
*/
|
|
public static ChassisSpeeds desiredChassisSpeedsObj = new ChassisSpeeds();
|
|
/**
|
|
* The robot's current rotation based on odometry or gyro readings
|
|
*/
|
|
public static Rotation2d robotRotationObj = new Rotation2d();
|
|
/**
|
|
* The current telemetry verbosity level.
|
|
*/
|
|
public static TelemetryVerbosity verbosity = TelemetryVerbosity.MACHINE;
|
|
/**
|
|
* State of simulation of the Robot, used to optimize retrieval.
|
|
*/
|
|
public static boolean isSimulation = RobotBase.isSimulation();
|
|
/**
|
|
* The number of swerve modules
|
|
*/
|
|
public static int moduleCount;
|
|
/**
|
|
* The Locations of the swerve drive wheels.
|
|
*/
|
|
public static double[] wheelLocations;
|
|
/**
|
|
* An array of rotation and velocity values describing the measured state of each swerve module
|
|
*/
|
|
public static double[] measuredStates;
|
|
/**
|
|
* An array of rotation and velocity values describing the desired state of each swerve module
|
|
*/
|
|
public static double[] desiredStates;
|
|
/**
|
|
* The robot's current rotation based on odometry or gyro readings
|
|
*/
|
|
public static double robotRotation = 0;
|
|
/**
|
|
* The maximum achievable speed of the modules, used to adjust the size of the vectors.
|
|
*/
|
|
public static double maxSpeed;
|
|
/**
|
|
* The units of the module rotations and robot rotation
|
|
*/
|
|
public static String rotationUnit = "degrees";
|
|
/**
|
|
* The distance between the left and right modules.
|
|
*/
|
|
public static double sizeLeftRight;
|
|
/**
|
|
* The distance between the front and back modules.
|
|
*/
|
|
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";
|
|
/**
|
|
* 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;
|
|
/**
|
|
* 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];
|
|
/**
|
|
* Describes the desired forward, sideways and angular velocity of the robot.
|
|
*/
|
|
public static double[] desiredChassisSpeeds = new double[3];
|
|
/**
|
|
* Struct publisher for AdvantageScope swerve widgets.
|
|
*/
|
|
private static StructArrayPublisher<SwerveModuleState> measuredStatesStruct
|
|
= NetworkTableInstance.getDefault()
|
|
.getStructArrayTopic(
|
|
"swerve/advantagescope/currentStates",
|
|
SwerveModuleState.struct)
|
|
.publish();
|
|
/**
|
|
* Struct publisher for AdvantageScope swerve widgets.
|
|
*/
|
|
private static StructArrayPublisher<SwerveModuleState> desiredStatesStruct
|
|
= NetworkTableInstance.getDefault()
|
|
.getStructArrayTopic(
|
|
"swerve/advantagescope/desiredStates",
|
|
SwerveModuleState.struct)
|
|
.publish();
|
|
/**
|
|
* Measured {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
|
|
*/
|
|
private static StructPublisher<ChassisSpeeds> measuredChassisSpeedsStruct
|
|
= NetworkTableInstance.getDefault()
|
|
.getStructTopic(
|
|
"swerve/advantagescope/measuredChassisSpeeds",
|
|
ChassisSpeeds.struct)
|
|
.publish();
|
|
/**
|
|
* Desired {@link ChassisSpeeds} for NT4 AdvantageScope swerve widgets.
|
|
*/
|
|
private static StructPublisher<ChassisSpeeds> desiredChassisSpeedsStruct
|
|
= NetworkTableInstance.getDefault()
|
|
.getStructTopic(
|
|
"swerve/advantagescope/desiredChassisSpeeds",
|
|
ChassisSpeeds.struct)
|
|
.publish();
|
|
/**
|
|
* Robot {@link Rotation2d} for AdvantageScope swerve widgets.
|
|
*/
|
|
private static StructPublisher<Rotation2d> robotRotationStruct
|
|
= NetworkTableInstance.getDefault()
|
|
.getStructTopic(
|
|
"swerve/advantagescope/robotRotation",
|
|
Rotation2d.struct)
|
|
.publish();
|
|
|
|
/**
|
|
* Upload data to smartdashboard
|
|
*/
|
|
public static void updateData()
|
|
{
|
|
measuredChassisSpeeds[0] = measuredChassisSpeedsObj.vxMetersPerSecond;
|
|
measuredChassisSpeeds[1] = measuredChassisSpeedsObj.vxMetersPerSecond;
|
|
measuredChassisSpeeds[2] = Math.toDegrees(measuredChassisSpeedsObj.omegaRadiansPerSecond);
|
|
|
|
desiredChassisSpeeds[0] = desiredChassisSpeedsObj.vxMetersPerSecond;
|
|
desiredChassisSpeeds[1] = desiredChassisSpeedsObj.vyMetersPerSecond;
|
|
desiredChassisSpeeds[2] = Math.toDegrees(desiredChassisSpeedsObj.omegaRadiansPerSecond);
|
|
|
|
robotRotation = robotRotationObj.getDegrees();
|
|
|
|
for (int i = 0; i < measuredStatesObj.length; i++)
|
|
{
|
|
SwerveModuleState state = measuredStatesObj[i];
|
|
if (state != null)
|
|
{
|
|
measuredStates[i * 2] = state.angle.getDegrees();
|
|
measuredStates[i * 2 + 1] = state.speedMetersPerSecond;
|
|
}
|
|
}
|
|
|
|
for (int i = 0; i < desiredStatesObj.length; i++)
|
|
{
|
|
SwerveModuleState state = desiredStatesObj[i];
|
|
if (state != null)
|
|
{
|
|
desiredStates[i * 2] = state.angle.getDegrees();
|
|
desiredStates[i * 2 + 1] = state.speedMetersPerSecond;
|
|
}
|
|
}
|
|
|
|
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);
|
|
|
|
desiredStatesStruct.set(desiredStatesObj);
|
|
measuredStatesStruct.set(measuredStatesObj);
|
|
desiredChassisSpeedsStruct.set(desiredChassisSpeedsObj);
|
|
measuredChassisSpeedsStruct.set(measuredChassisSpeedsObj);
|
|
robotRotationStruct.set(robotRotationObj);
|
|
}
|
|
|
|
/**
|
|
* Verbosity of telemetry data sent back.
|
|
*/
|
|
public enum TelemetryVerbosity
|
|
{
|
|
/**
|
|
* No telemetry data is sent back.
|
|
*/
|
|
NONE,
|
|
/**
|
|
* 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.
|
|
*/
|
|
HIGH,
|
|
/**
|
|
* Only send the machine readable data related to swerve drive.
|
|
*/
|
|
MACHINE
|
|
}
|
|
}
|