robot

  1#!/usr/bin/env python3
  2
  3import typing
  4import inspect
  5import commands2
  6
  7from robotswerve import RobotSwerve
  8from utils.match_monitor import MatchMonitorConnection
  9from utils.datalog_bridge import setup_logging
 10from utils.loop_timing import LoopTimer
 11import wpilib
 12import logging
 13
 14
 15class MyRobot(commands2.TimedCommandRobot):
 16    """
 17    Our default robot class, pass it to wpilib.run
 18    Command v2 robots are encouraged to inherit from TimedCommandRobot, which
 19    has an implementation of (self) -> None:
 20    which runs the scheduler for you
 21    """
 22    # 20 ms default period (50 Hz)
 23    kDefaultPeriod: typing.ClassVar[float] = 20.0
 24    autonomousCommand: typing.Optional[commands2.Command] = None
 25
 26    def __init__(self) -> None:
 27
 28        self.__errorLogged = False
 29        self.__lastError = None
 30        self.__errorCaughtCount = 0
 31
 32        self.__initFrameTiming()
 33
 34        # Bridge Python logging -> wpilog + NT-controlled log level
 35        setup_logging()
 36
 37        # setup our scheduling period. Defaulting to 20 Hz (50 ms)
 38        super().__init__(period=MyRobot.kDefaultPeriod / 1000)
 39        
 40        #Init telem files
 41        self.telemInit()
 42
 43        self.match_monitor = MatchMonitorConnection()
 44
 45        # Instantiate our RobotContainer. This will perform all our button bindings, and put our
 46        # autonomous chooser on the dashboard.
 47        if not hasattr(self, "container"):
 48            # to work around sim creating motors during tests, assign to class and if already created keep using created robot class for tests.
 49            # during robot running, this is only every called once
 50            MyRobot.container = RobotSwerve(lambda: self.isDisabled)
 51
 52    def robotInit(self) -> None:
 53        """
 54        This function is run when the robot is first started up and should be used for any
 55        initialization code.
 56        """
 57        
 58
 59    def telemInit(self) -> None:
 60        """Initialize data logging: NT logging, console, DS, and vendor loggers.
 61
 62        WPILib DataLogManager auto-detects USB and falls back to
 63        /home/lvuser/logs.  Phoenix 6 and REV also auto-detect USB.
 64        Logging is disabled in simulation to avoid junk files.
 65        """
 66        if self.isSimulation():
 67            return
 68
 69        # WPILib data log (.wpilog) — auto-uses USB if present
 70        wpilib.DataLogManager.start()
 71        wpilib.DataLogManager.logNetworkTables(True)
 72        wpilib.DataLogManager.logConsoleOutput(True)
 73        wpilib.DriverStation.startDataLog(wpilib.DataLogManager.getLog())
 74
 75        # Phoenix 6 signal logging (.hoot files)
 76        # Disabled: SignalLogger spams errors about full disks
 77        # from phoenix6 import SignalLogger
 78        # SignalLogger.enable_auto_logging(True)
 79        # SignalLogger.start()
 80
 81        # REV status logging (.revlog files)
 82        from rev import StatusLogger
 83        StatusLogger.start()
 84
 85    def robotPeriodic(self) -> None:
 86        self.__callAndCatch(self.container.robotPeriodic)
 87
 88        wpilib.SmartDashboard.putNumber("Code Crash Count", self.__errorCaughtCount)
 89        self.__frameTimingPeriodic()
 90
 91    def disabledInit(self) -> None:
 92        """This function is called once each time the robot enters Disabled mode."""
 93        self.__timing.reset_all()
 94        self.container.disabledInit()
 95        self.match_monitor.start_upload()
 96
 97    def disabledPeriodic(self) -> None:
 98        """This function is called periodically when disabled"""
 99        self.__timing.start("userCode")
100        self.container.disabledPeriodic()
101
102    def autonomousInit(self) -> None:
103        """This autonomous runs the autonomous command selected by your RobotContainer class."""
104        self.match_monitor.stop_upload()
105        self.__timing.reset_all()
106        self.container.autonomousInit()
107
108    def autonomousPeriodic(self) -> None:
109        """This function is called periodically during autonomous"""
110        self.__timing.start("userCode")
111        self.__callAndCatch(self.container.autonomousPeriodic)
112
113    def teleopInit(self) -> None:
114        self.match_monitor.stop_upload()
115        self.__timing.reset_all()
116        self.container.teleopInit()
117
118    def teleopPeriodic(self) -> None:
119        """This function is called periodically during operator control"""
120        self.__timing.start("userCode")
121        self.__callAndCatch(self.container.teleopPeriodic)
122
123    def testInit(self) -> None:
124        self.match_monitor.stop_upload()
125        self.__timing.reset_all()
126        self.container.testInit()
127
128    def testPeriodic(self) -> None:
129        self.__timing.start("userCode")
130        self.container.testPeriodic()
131
132    def getRobot(self) -> RobotSwerve:
133        return self.container
134
135    def __initFrameTiming(self):
136        """Set up loop timing instrumentation.
137
138        Wraps CommandScheduler.run() BEFORE super().__init__() captures it
139        so the scheduler channel measures the real execution cost.
140        """
141        self.__timing = LoopTimer(budget_sec=MyRobot.kDefaultPeriod / 1000)
142        self.__timing.add_channel("userCode")
143        self.__timing.add_channel("scheduler")
144
145        scheduler = commands2.CommandScheduler.getInstance()
146        original_run = scheduler.run
147        timing = self.__timing
148
149        def _timed_run():
150            timing.start("scheduler")
151            original_run()
152            timing.stop("scheduler")
153
154        scheduler.run = _timed_run
155
156    def __frameTimingPeriodic(self):
157        """Stop the userCode channel and publish all timing stats."""
158        self.__timing.stop("userCode")
159        self.__timing.publish()
160
161    def __callAndCatch(self, func: typing.Callable[[], None]) -> None:
162        try:
163            #Invoke the function
164            func()
165
166            #if we returned, it didnt crash so clear the last error if it was set
167            if self.__errorLogged and self.__lastError is not None:
168                logging.info(f"Logged error cleared for: {str(self.__lastError)}")
169                self.__errorLogged = False
170                self.__lastError = None
171        except Exception as e:
172            self.__lastError = e
173            name = inspect.currentframe().f_back.f_code.co_name
174            if self.isSimulation():
175                raise e
176
177            self.__errorCaughtCount = self.__errorCaughtCount + 1
178
179            if not self.__errorLogged:
180                logging.exception(f"(CRASH CATCH) {name} error: ")
181                self.__errorLogged = True
182
183
184if __name__ == "__main__":
185    print("Please run python -m robotpy <args>")
186    exit(1)