|
| 1 | +#!/usr/bin/env python3 |
| 2 | +""" |
| 3 | + This is a sample program demonstrating how to communicate to a light controller from the robot |
| 4 | + code using the roboRIO's I2C port. |
| 5 | +""" |
| 6 | + |
| 7 | +import wpilib |
| 8 | + |
| 9 | + |
| 10 | +class MyRobot(wpilib.TimedRobot): |
| 11 | + PORT = wpilib.I2C.Port.kOnboard |
| 12 | + DEVICE_ADDRESS = 4 |
| 13 | + |
| 14 | + def robotInit(self): |
| 15 | + """Robot initialization function""" |
| 16 | + |
| 17 | + self.arduino = wpilib.I2C(self.PORT, self.DEVICE_ADDRESS) |
| 18 | + |
| 19 | + def writeString(self, s: str): |
| 20 | + # Creates a char array from the input string |
| 21 | + chars = s.encode("ascii") |
| 22 | + |
| 23 | + # Writes bytes over I2C |
| 24 | + self.arduino.writeBulk(chars) |
| 25 | + |
| 26 | + def robotPeriodic(self): |
| 27 | + # Creates a string to hold current robot state information, including |
| 28 | + # alliance, enabled state, operation mode, and match time. The message |
| 29 | + # is sent in format "AEM###" where A is the alliance color, (R)ed or |
| 30 | + # (B)lue, E is the enabled state, (E)nabled or (D)isabled, M is the |
| 31 | + # operation mode, (A)utonomous or (T)eleop, and ### is the zero-padded |
| 32 | + # time remaining in the match. |
| 33 | + # |
| 34 | + # For example, "RET043" would indicate that the robot is on the red |
| 35 | + # alliance, enabled in teleop mode, with 43 seconds left in the match. |
| 36 | + allianceString = "U" |
| 37 | + alliance = wpilib.DriverStation.getAlliance() |
| 38 | + if alliance is not None: |
| 39 | + allianceString = ( |
| 40 | + "R" if alliance == wpilib.DriverStation.Alliance.kRed else "B" |
| 41 | + ) |
| 42 | + |
| 43 | + enabledString = "E" if wpilib.DriverStation.isEnabled() else "D" |
| 44 | + autoString = "A" if wpilib.DriverStation.isAutonomous() else "T" |
| 45 | + matchTime = wpilib.DriverStation.getMatchTime() |
| 46 | + |
| 47 | + stateMessage = f"{allianceString}{enabledString}{autoString}{matchTime:03f}" |
| 48 | + |
| 49 | + self.writeString(stateMessage) |
| 50 | + |
| 51 | + |
| 52 | +if __name__ == "__main__": |
| 53 | + wpilib.run(MyRobot) |
0 commit comments