data.telemetry

  1# Internal imports
  2from config import OperatorRobotConfig
  3from subsystem.drivetrain.swerve_drivetrain import SwerveDrivetrain
  4from subsystem.intakeactions import IntakeSubsystem
  5
  6# Third-party imports
  7import wpilib
  8from ntcore import NetworkTableInstance
  9from wpimath.geometry import Pose2d, Rotation2d
 10from wpimath.kinematics import ChassisSpeeds, SwerveModuleState
 11from wpiutil.log import BooleanLogEntry, StringLogEntry, FloatLogEntry, IntegerLogEntry
 12
 13telemetryButtonEntries = [
 14    ["AButton", BooleanLogEntry, "/button/letter/a"],
 15    ["BButton", BooleanLogEntry, "/button/letter/b"],
 16    ["XButton", BooleanLogEntry, "/button/letter/x"],
 17    ["YButton", BooleanLogEntry, "/button/letter/y"],
 18    ["BackButton", BooleanLogEntry, "/button/front/back"],
 19    ["StartButton", BooleanLogEntry, "/button/front/start"],
 20    ["LeftBumper", BooleanLogEntry, "/button/bumper/left"],
 21    ["RightBumper", BooleanLogEntry, "/button/bumper/right"],
 22    ["LeftStickButton", BooleanLogEntry, "button/stickbutton/left"],
 23    ["RightStickButton", BooleanLogEntry, "button/stickbutton/right"],
 24    ["RightTrigger", FloatLogEntry, "button/trigger/right"],
 25    ["LeftTrigger", FloatLogEntry, "button/trigger/left"],
 26    ["JoystickLeftY", FloatLogEntry, "button/joystick/lefty"],
 27    ["JoystickLeftX", FloatLogEntry, "button/joystick/leftx"],
 28    ["JoystickRightY", FloatLogEntry, "button/joystick/righty"],
 29    ["JoystickRightX", FloatLogEntry, "button/joystick/rightx"],
 30    ["DPad", IntegerLogEntry, "button/dpad"],
 31]
 32
 33telemetryOdometryEntries = [
 34    ["robotPose", "robotpose"],
 35    ["targetPose", "targetpose"],
 36]
 37
 38telemetryFullSwerveDriveTrainEntries = [
 39    ["moduleStates", SwerveModuleState, True, "swervemodeulestates"],
 40    ["drivetrainVelocity", ChassisSpeeds, False, "swervevelocity"],
 41    ["drivetrainRotation", Rotation2d, False, "swerverotation"],
 42]
 43
 44telemetryRawSwerveDriveTrainEntries = []
 45for i in range(len(OperatorRobotConfig.swerve_module_channels)):
 46    telemetryRawSwerveDriveTrainEntries.extend(
 47        [
 48            [f"steerDegree{i + 1}", FloatLogEntry, f"module{i + 1}/steerdegree"],
 49            [f"drivePercent{i + 1}", FloatLogEntry, f"module{i + 1}/drivepercent"],
 50            [f"moduleVelocity{i + 1}", FloatLogEntry, f"module{i + 1}/velocity"],
 51        ]
 52    )
 53
 54driverStationEntries = [
 55    ["alliance", StringLogEntry, "alliance"],
 56    ["autonomous", BooleanLogEntry, "autonomous"],
 57    ["teleop", BooleanLogEntry, "teleop"],
 58    ["test", BooleanLogEntry, "test"],
 59    ["enabled", BooleanLogEntry, "enabled"],
 60]
 61
 62intakeEntries = [
 63    # ["intakeSpeed", "intakespeed"],
 64    ["rollerSpeed", "rollerspeed"],
 65]
 66
 67class Telemetry:
 68
 69    def __init__(
 70        self,
 71        driverController: wpilib.XboxController = None,
 72        mechController: wpilib.XboxController = None,
 73        driveTrain: SwerveDrivetrain = None,
 74        driverStation: wpilib.DriverStation = None,
 75        intake: IntakeSubsystem = None
 76    ):
 77        self.driverController = driverController
 78        self.mechController = mechController
 79        self.odometryPosition = driveTrain.pose_estimator
 80        self.driveTrain = driveTrain
 81        self.swerveModules = driveTrain.swerve_modules
 82        self.driverStation = driverStation
 83        self.intake = intake
 84
 85        self.networkTable = NetworkTableInstance.getDefault()
 86        for entryname, logname in telemetryOdometryEntries:
 87            setattr(
 88                self,
 89                entryname,
 90                self.networkTable.getStructTopic(
 91                    "odometry/" + logname, Pose2d
 92                ).publish(),
 93            )
 94        for (
 95            entryname,
 96            entrytype,
 97            isarraytype,
 98            logname,
 99        ) in telemetryFullSwerveDriveTrainEntries:
100            if isarraytype:
101                setattr(
102                    self,
103                    entryname,
104                    self.networkTable.getStructArrayTopic(
105                        "swervedrivetrain/" + logname, entrytype
106                    ).publish(),
107                )
108            else:
109                setattr(
110                    self,
111                    entryname,
112                    self.networkTable.getStructTopic(
113                        "swervedrivetrain/" + logname, entrytype
114                    ).publish(),
115                )
116        for entryname, logname in intakeEntries:
117            setattr(
118                self,
119                entryname,
120                self.networkTable.getStructTopic(
121                    "intake/" + logname, entrytype
122                ).publish(),
123            )
124
125        self.datalog = wpilib.DataLogManager.getLog()
126        for entryname, entrytype, logname in telemetryButtonEntries:
127            setattr(
128                self, "driver" + entryname, entrytype(self.datalog, "driver/" + logname)
129            )
130            setattr(
131                self, "mech" + entryname, entrytype(self.datalog, "mech/" + logname)
132            )
133        for entryname, entrytype, logname in telemetryRawSwerveDriveTrainEntries:
134            setattr(
135                self,
136                entryname,
137                entrytype(self.datalog, "rawswervedrivetrain/" + logname),
138            )
139        for entryname, entrytype, logname in driverStationEntries:
140            setattr(
141                self, entryname, entrytype(self.datalog, "driverstation/" + logname)
142            )
143
144        if self.driverStation:
145            self.driverStation.startDataLog(self.datalog)
146
147    def getDriverControllerInputs(self):
148        """
149        Records data for buttons and axis inputs for the first controller
150        """
151        if self.driverController is not None:
152            self.driverAButton.append(self.driverController.getAButton())  # bool
153            self.driverBButton.append(self.driverController.getBButton())  # bool
154            self.driverXButton.append(self.driverController.getXButton())  # bool
155            self.driverYButton.append(self.driverController.getYButton())  # bool
156            self.driverBackButton.append(
157                self.driverController.getBackButton()
158            )  # left side , bool
159            self.driverStartButton.append(
160                self.driverController.getStartButton()
161            )  # right side , bool
162            self.driverLeftBumper.append(self.driverController.getLeftBumper())  # bool
163            self.driverRightBumper.append(self.driverController.getRightBumper())  # bool
164            self.driverLeftStickButton.append(
165                self.driverController.getLeftStickButton()
166            )  # bool
167            self.driverRightStickButton.append(
168                self.driverController.getRightStickButton()
169            )  # bool
170            self.driverLeftTrigger.append(
171                self.driverController.getLeftTriggerAxis()
172            )  # float 0-1
173            self.driverRightTrigger.append(
174                self.driverController.getRightTriggerAxis()
175            )  # float 0-1
176            self.driverJoystickLeftY.append(self.driverController.getLeftY())  # float -1-1
177            self.driverJoystickLeftX.append(self.driverController.getLeftX())  # float -1-1
178            self.driverJoystickRightY.append(
179                self.driverController.getRightY()
180            )  # float -1-1
181            self.driverJoystickRightX.append(
182                self.driverController.getRightX()
183            )  # float -1-1
184            self.driverDPad.append(self.driverController.getPOV())  # ints
185
186    def getMechControllerInputs(self):
187        """
188        Records data for buttons and axis inputs for the second controller
189        """
190        if self.mechController is not None:
191            self.mechAButton.append(self.mechController.getAButton())  # bool
192            self.mechBButton.append(self.mechController.getBButton())  # bool
193            self.mechXButton.append(self.mechController.getXButton())  # bool
194            self.mechYButton.append(self.mechController.getYButton())  # bool
195            self.mechBackButton.append(
196                self.mechController.getBackButton()
197            )  # left side , bool
198            self.mechStartButton.append(
199                self.mechController.getStartButton()
200            )  # right side , bool
201            self.mechLeftBumper.append(self.mechController.getLeftBumper())  # bool
202            self.mechRightBumper.append(self.mechController.getRightBumper())  # bool
203            self.mechLeftStickButton.append(
204                self.mechController.getLeftStickButton()
205            )  # bool
206            self.mechRightStickButton.append(
207                self.mechController.getRightStickButton()
208            )  # bool
209            self.mechLeftTrigger.append(
210                self.mechController.getLeftTriggerAxis()
211            )  # float 0-1
212            self.mechRightTrigger.append(
213                self.mechController.getRightTriggerAxis()
214            )  # float 0-1
215            self.mechJoystickLeftY.append(self.mechController.getLeftY())  # float -1-1
216            self.mechJoystickLeftX.append(self.mechController.getLeftX())  # float -1-1
217            self.mechJoystickRightY.append(self.mechController.getRightY())  # float -1-1
218            self.mechJoystickRightX.append(self.mechController.getRightX())  # float -1-1
219            self.mechDPad.append(self.mechController.getPOV())  # ints
220
221    def getOdometryInputs(self):
222        """
223        Records the data for the positions of the bot in a field,
224        Gives the x position, y position and rotation
225        """
226        if self.odometryPosition is not None:
227            pose = self.odometryPosition.getEstimatedPosition()
228            self.robotPose.set(pose)
229
230    def getFullSwerveState(self):
231        """
232        Retrieves values reflecting the current state of the swerve drive
233        """
234        if self.driveTrain and self.swerveModules:
235            self.moduleStates.set(
236                [swerveModule.current_state() for swerveModule in self.swerveModules]
237            )
238            self.drivetrainVelocity.set(self.driveTrain.current_robot_relative_speed())
239            self.drivetrainRotation.set(self.driveTrain.current_yaw())
240
241    def getRawSwerveInputs(self):
242        """
243        Gets the inputs for some swerve drive train inputs
244        it get the steer angle, the drive percent and the velocity
245        """
246        if self.swerveModules is not None:
247            for i, swerveModule in enumerate(self.swerveModules):
248                getattr(self, f"steerDegree{i + 1}").append(
249                    swerveModule.current_raw_absolute_steer_position()
250                )
251                getattr(self, f"drivePercent{i + 1}").append(
252                    swerveModule.drive_motor.getAppliedOutput()
253                )
254                getattr(self, f"moduleVelocity{i + 1}").append(
255                    swerveModule.current_state().speed
256                )
257
258    def getDriverStationInputs(self):
259        """
260        Gets the inputs of some match/general robot things,
261        the things being: Alliance color and what mode it is in and
262        if it is enabled
263        """
264        if self.driverStation is not None:
265            alliance = "No Alliance"
266            if self.driverStation.getAlliance() == wpilib.DriverStation.Alliance.kBlue:
267                alliance = "Blue"
268            if self.driverStation.getAlliance() == wpilib.DriverStation.Alliance.kRed:
269                alliance = "Red"
270            self.alliance.append(alliance)
271            self.autonomous.append(self.driverStation.isAutonomous())
272            self.teleop.append(self.driverStation.isTeleop())
273            self.test.append(self.driverStation.isTest())
274            self.enabled.append(self.driverStation.isEnabled())
275
276    def getIntakeInputs(self):
277        if self.intake is not None:
278            # self.intake.intakeVelocity = self.intakeSpeed.getEntry(getattr(self, "intakeSpeed"))
279            self.intake.rollerVelocity = self.rollerSpeed.getEntry(getattr(self, "rollerSpeed"))
280
281
282    def runDefaultDataCollections(self):
283        self.getDriverControllerInputs()
284        self.getMechControllerInputs()
285        self.getOdometryInputs()
286        self.getFullSwerveState()
287        self.getRawSwerveInputs()
288        self.getIntakeInputs()
289        self.getDriverStationInputs()
290
291    def logAdditionalOdometry(
292        self, odometer_value: Pose2d, log_entry_name: str
293    ) -> None:
294        getattr(self, log_entry_name).set(odometer_value)