robot

  1#!/usr/bin/env python3
  2
  3import typing
  4import inspect
  5import commands2
  6
  7from robotswerve import RobotSwerve
  8from utils.datalog_bridge import setup_logging
  9import wpilib
 10import logging
 11
 12
 13class MyRobot(commands2.TimedCommandRobot):
 14    """
 15    Our default robot class, pass it to wpilib.run
 16    Command v2 robots are encouraged to inherit from TimedCommandRobot, which
 17    has an implementation of (self) -> None:
 18    which runs the scheduler for you
 19    """
 20    # 50 ms default period
 21    kDefaultPeriod: typing.ClassVar[float] = 50.0
 22    autonomousCommand: typing.Optional[commands2.Command] = None
 23
 24    def __init__(self) -> None:
 25
 26        self.__errorLogged = False
 27        self.__lastError = None
 28        self.__errorCaughtCount = 0
 29
 30        # Bridge Python logging -> wpilog + NT-controlled log level
 31        setup_logging()
 32
 33        # setup our scheduling period. Defaulting to 20 Hz (50 ms)
 34        super().__init__(period=MyRobot.kDefaultPeriod / 1000)
 35        # Instantiate our RobotContainer. This will perform all our button bindings, and put our
 36        # autonomous chooser on the dashboard.
 37        if not hasattr(self, "container"):
 38            # to work around sim creating motors during tests, assign to class and if already created keep using created robot class for tests.
 39            # during robot running, this is only every called once
 40            MyRobot.container = RobotSwerve(lambda: self.isDisabled)
 41
 42    def robotInit(self) -> None:
 43        """
 44        This function is run when the robot is first started up and should be used for any
 45        initialization code.
 46        """
 47
 48    def robotPeriodic(self) -> None:
 49        self.__callAndCatch(self.container.robotPeriodic)
 50
 51        wpilib.SmartDashboard.putNumber("Code Crash Count", self.__errorCaughtCount)
 52
 53    def disabledInit(self) -> None:
 54        """This function is called once each time the robot enters Disabled mode."""
 55        self.container.disabledInit()
 56
 57    def disabledPeriodic(self) -> None:
 58        """This function is called periodically when disabled"""
 59        self.container.disabledPeriodic()
 60
 61    def autonomousInit(self) -> None:
 62        """This autonomous runs the autonomous command selected by your RobotContainer class."""
 63        self.container.autonomousInit()
 64
 65    def autonomousPeriodic(self) -> None:
 66        """This function is called periodically during autonomous"""
 67        self.__callAndCatch(self.container.autonomousPeriodic)
 68
 69    def teleopInit(self) -> None:
 70        self.container.teleopInit()
 71
 72    def teleopPeriodic(self) -> None:
 73        """This function is called periodically during operator control"""
 74        self.__callAndCatch(self.container.teleopPeriodic)
 75
 76    def testInit(self) -> None:
 77        self.container.testInit()
 78
 79    def testPeriodic(self) -> None:
 80        self.container.testPeriodic()
 81
 82    def getRobot(self) -> RobotSwerve:
 83        return self.container
 84
 85    def __callAndCatch(self, func: typing.Callable[[], None]) -> None:
 86        try:
 87            #Invoke the function
 88            func()
 89
 90            #if we returned, it didnt crash so clear the last error if it was set
 91            if self.__errorLogged and self.__lastError is not None:
 92                logging.info(f"Logged error cleared for: {str(self.__lastError)}")
 93                self.__errorLogged = False
 94                self.__lastError = None
 95        except Exception as e:
 96            self.__lastError = e
 97            name = inspect.currentframe().f_back.f_code.co_name
 98            if self.isSimulation():
 99                raise e
100
101            self.__errorCaughtCount = self.__errorCaughtCount + 1
102
103            if not self.__errorLogged:
104                logging.exception(f"(CRASH CATCH) {name} error: ")
105                self.__errorLogged = True
106
107
108if __name__ == "__main__":
109    print("Please run python -m robotpy <args>")
110    exit(1)