data.telemetry

  1# Internal imports
  2from config import OperatorRobotConfig
  3from subsystem.drivetrain.swerve_drivetrain import SwerveDrivetrain
  4
  5# Third-party imports
  6import wpilib
  7from ntcore import NetworkTableInstance
  8from wpimath.geometry import Pose2d, Rotation2d
  9from wpimath.kinematics import ChassisSpeeds, SwerveModuleState
 10from wpiutil.log import FloatLogEntry, StringLogEntry, BooleanLogEntry
 11
 12telemetryOdometryEntries = [
 13    ["robotPose", "robotpose"],
 14    ["targetPose", "targetpose"],
 15]
 16
 17telemetryFullSwerveDriveTrainEntries = [
 18    ["moduleStates", SwerveModuleState, True, "swervemodeulestates"],
 19    ["drivetrainVelocity", ChassisSpeeds, False, "swervevelocity"],
 20    ["drivetrainRotation", Rotation2d, False, "swerverotation"],
 21]
 22
 23telemetryRawSwerveDriveTrainEntries = []
 24for i in range(len(OperatorRobotConfig.swerve_module_channels)):
 25    telemetryRawSwerveDriveTrainEntries.extend(
 26        [
 27            [f"steerDegree{i + 1}", FloatLogEntry, f"module{i + 1}/steerdegree"],
 28            [f"drivePercent{i + 1}", FloatLogEntry, f"module{i + 1}/drivepercent"],
 29            [f"moduleVelocity{i + 1}", FloatLogEntry, f"module{i + 1}/velocity"],
 30        ]
 31    )
 32
 33driverStationEntries = [
 34    ["alliance", StringLogEntry, "alliance"],
 35    ["autonomous", BooleanLogEntry, "autonomous"],
 36    ["teleop", BooleanLogEntry, "teleop"],
 37    ["test", BooleanLogEntry, "test"],
 38    ["enabled", BooleanLogEntry, "enabled"],
 39]
 40
 41class Telemetry:
 42
 43    def __init__(
 44        self,
 45        driveTrain: SwerveDrivetrain = None,
 46        driverStation: wpilib.DriverStation = None,
 47    ):
 48        self.odometryPosition = driveTrain.pose_estimator
 49        self.driveTrain = driveTrain
 50        self.swerveModules = driveTrain.swerve_modules
 51        self.driverStation = driverStation
 52
 53        self.networkTable = NetworkTableInstance.getDefault()
 54        for entryname, logname in telemetryOdometryEntries:
 55            setattr(
 56                self,
 57                entryname,
 58                self.networkTable.getStructTopic(
 59                    "odometry/" + logname, Pose2d
 60                ).publish(),
 61            )
 62        for (
 63            entryname,
 64            entrytype,
 65            isarraytype,
 66            logname,
 67        ) in telemetryFullSwerveDriveTrainEntries:
 68            if isarraytype:
 69                setattr(
 70                    self,
 71                    entryname,
 72                    self.networkTable.getStructArrayTopic(
 73                        "swervedrivetrain/" + logname, entrytype
 74                    ).publish(),
 75                )
 76            else:
 77                setattr(
 78                    self,
 79                    entryname,
 80                    self.networkTable.getStructTopic(
 81                        "swervedrivetrain/" + logname, entrytype
 82                    ).publish(),
 83                )
 84
 85        # DataLogManager is started in robot.py telemInit() — just get the log
 86        self.datalog = wpilib.DataLogManager.getLog()
 87        for entryname, entrytype, logname in telemetryRawSwerveDriveTrainEntries:
 88            setattr(
 89                self,
 90                entryname,
 91                entrytype(self.datalog, "rawswervedrivetrain/" + logname),
 92            )
 93
 94    def getOdometryInputs(self):
 95        """
 96        Records the data for the positions of the bot in a field,
 97        Gives the x position, y position and rotation
 98        """
 99        if self.odometryPosition is not None:
100            pose = self.odometryPosition.getEstimatedPosition()
101            self.robotPose.set(pose)
102
103    def getFullSwerveState(self):
104        """
105        Retrieves values reflecting the current state of the swerve drive
106        """
107        if self.driveTrain and self.swerveModules:
108            self.moduleStates.set(
109                [swerveModule.current_state() for swerveModule in self.swerveModules]
110            )
111            self.drivetrainVelocity.set(self.driveTrain.current_robot_relative_speed())
112            self.drivetrainRotation.set(self.driveTrain.current_yaw())
113
114    def getRawSwerveInputs(self):
115        """
116        Gets the inputs for some swerve drive train inputs
117        it get the steer angle, the drive percent and the velocity
118        """
119        if self.swerveModules is not None:
120            for i, swerveModule in enumerate(self.swerveModules):
121                getattr(self, f"steerDegree{i + 1}").append(
122                    swerveModule.current_raw_absolute_steer_position()
123                )
124                getattr(self, f"drivePercent{i + 1}").append(
125                    swerveModule.drive_motor.getAppliedOutput()
126                )
127                getattr(self, f"moduleVelocity{i + 1}").append(
128                    swerveModule.current_state().speed
129                )
130
131    def getDriverStationInputs(self):
132        """
133        Gets the inputs of some match/general robot things,
134        the things being: Alliance color and what mode it is in and
135        if it is enabled
136        """
137        if self.driverStation is not None:
138            alliance = "No Alliance"
139            if self.driverStation.getAlliance() == wpilib.DriverStation.Alliance.kBlue:
140                alliance = "Blue"
141            if self.driverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed:
142                alliance = "Red"
143            self.alliance.append(alliance)
144            self.autonomous.append(self.driverStation.isAutonomous())
145            self.teleop.append(self.driverStation.isTeleop())
146            self.test.append(self.driverStation.isTest())
147            self.enabled.append(self.driverStation.isEnabled())
148
149    def runDefaultDataCollections(self):
150        self.getOdometryInputs()
151        self.getFullSwerveState()
152        self.getRawSwerveInputs()
153        self.getDriverStationInputs()
154
155    def logAdditionalOdometry(
156        self, odometer_value: Pose2d, log_entry_name: str
157    ) -> None:
158        getattr(self, log_entry_name).set(odometer_value)