class OI: """ This class is the glue that binds the controls on the physical operator interface to the commands and command groups that allow control of the robot. """ _config = None _command_config = None _controllers = [] _auto_program_chooser = None _starting_chooser = None def __init__(self, robot, configfile='/home/lvuser/py/configs/joysticks.ini'): self.robot = robot self._config = configparser.ConfigParser() self._config.read(configfile) self._init_joystick_binding() for i in range(2): self._controllers.append(self._init_joystick(i)) self._create_smartdashboard_buttons() def setup_button_bindings(self): #release_gear_a_button = JoystickButton(self._controllers[UserController.SCORING], JoystickButtons.A) #release_gear_a_button.whileHeld(ReleaseGear(self.robot)) pass def get_axis(self, user, axis): """Read axis value for specified controller/axis. Args: user: Controller ID to read from axis: Axis ID to read from. Return: Current position for the specified axis. (Range [-1.0, 1.0]) """ controller = self._controllers[user] value = 0.0 if axis == JoystickAxis.DPADX: value = controller.getPOV() if value == 90: value = 1.0 elif value == 270: value = -1.0 else: value = 0.0 elif axis == JoystickAxis.DPADY: value = controller.getPOV() if value == 0: value = -1.0 elif value == 180: value = 1.0 else: value = 0.0 else: config_section = "JoyConfig" + str(user) dead_zone = self._config.getfloat(config_section, "DEAD_ZONE") value = controller.getRawAxis(axis) if abs(value) < dead_zone: value = 0.0 return value def get_button_state(self, user, button): return self._controllers[user].getRawButton(button) def _create_smartdashboard_buttons(self): self._auto_program_chooser = SendableChooser() #self._auto_program_chooser.addDefault("Cross Line", 1) #self._auto_program_chooser.addObject("Hang Gear", 2) self._auto_program_chooser.addObject("Do Nothing", 3) SmartDashboard.putData("Autonomous", self._auto_program_chooser) self._starting_chooser = SendableChooser() self._starting_chooser.addDefault("Left", 1) self._starting_chooser.addObject("Center", 2) self._starting_chooser.addObject("Right", 3) SmartDashboard.putData("Starting_Position", self._starting_chooser) def get_auto_choice(self): return self._auto_program_chooser.getSelected() def get_position(self): value = self._starting_chooser.getSelected() return value def _init_joystick(self, driver): config_section = "JoyConfig" + str(driver) stick = wpilib.Joystick(self._config.getint(config_section, "PORT"), self._config.getint(config_section, "AXES"), self._config.getint(config_section, "BUTTONS")) return stick def _init_joystick_binding(self): axis_binding_section = "AxisBindings" JoystickAxis.LEFTX = self._config.getint(axis_binding_section, "LEFTX") JoystickAxis.LEFTY = self._config.getint(axis_binding_section, "LEFTY") JoystickAxis.RIGHTX = self._config.getint(axis_binding_section, "RIGHTX") JoystickAxis.RIGHTY = self._config.getint(axis_binding_section, "RIGHTY") JoystickAxis.DPADX = self._config.getint(axis_binding_section, "DPADX") JoystickAxis.DPADY = self._config.getint(axis_binding_section, "DPADY") button_binding_section = "ButtonBindings" JoystickButtons.X = self._config.getint(button_binding_section, "X") JoystickButtons.A = self._config.getint(button_binding_section, "A") JoystickButtons.B = self._config.getint(button_binding_section, "B") JoystickButtons.Y = self._config.getint(button_binding_section, "Y") JoystickButtons.LEFTBUMPER = self._config.getint( button_binding_section, "LEFTBUMPER") JoystickButtons.RIGHTBUMPER = self._config.getint( button_binding_section, "RIGHTBUMPER") JoystickButtons.LEFTTRIGGER = self._config.getint( button_binding_section, "LEFTTRIGGER") JoystickButtons.RIGHTTRIGGER = self._config.getint( button_binding_section, "RIGHTTRIGGER") JoystickButtons.BACK = self._config.getint(button_binding_section, "BACK") JoystickButtons.START = self._config.getint(button_binding_section, "START")
class MyRobot(CommandBasedRobot): dashboard = True frequency = 20 period = 1 / frequency def __init__(self): super().__init__(self.period) def robotInit(self): ''' This is a good place to set up your subsystems and anything else that you will need to access later. ''' wpilib.CameraServer.launch() self.lastAuto = False self.lastAlign = False self.lastStraightAlign = False self.teleop = False Command.getRobot = lambda x=0: self # Load system preferences prior to constructing # subsystems map.loadPreferences() # Construct subsystems prior to constructing commands self.limelight = Limelight.Limelight(self) #not a subsystem self.hatch = HatchMech.HatchMech(self) self.hatch.initialize() #self.cargo = CargoMech0.CargoMech() self.cargo = CargoMech.CargoMech() #not a subsystem self.cargo.initialize() self.climber = Climber.Climber() #not a subsystem self.climber.initialize(self) self.drive = Drive.Drive(self) self.compressor = wpilib.Compressor(0) self.timer = wpilib.Timer() self.timer.start() self.watch = wpilib.Watchdog(150, None) ''' Since OI instantiates commands and commands need access to subsystems, OI must be initialized after subsystems. ''' [self.joystick0, self.joystick1, self.xbox] = oi.commands() self.updateDashboardInit() self.DriveStraight = DriveStraight() self.DriveStraightSide = DriveStraightSide() self.LeftCargo = LeftCargo() self.RightCargo = RightCargo() self.CenterCargo = CenterCargo() self.SetSpeedDT = SetSpeedDT() # Set up auton chooser self.autonChooser = SendableChooser() #self.autonChooser.setDefaultOption("Drive Straight", "DriveStraight") self.autonChooser.setDefaultOption("Level 1 Center", "Level1Center") self.autonChooser.addOption("Left Cargo", "LeftCargo") self.autonChooser.addOption("Right Cargo", "RightCargo") self.autonChooser.addOption("Do Nothing", "DoNothing") self.autonChooser.addOption("Level 1 Center", "Level1Center") self.autonChooser.addOption("Drive Straight Side", "DriveStraightSide") SmartDashboard.putData("Auto mode", self.autonChooser) def robotPeriodic(self): self.limelight.readLimelightData() SmartDashboard.putBoolean("teleop", self.teleop) if (self.dashboard): self.updateDashboardPeriodic() def autonomousInit(self): super().autonomousInit() self.drive.zero() self.timer.reset() self.timer.start() self.autoSelector(self.autonChooser.getSelected()) def autonomousPeriodic(self): super().autonomousPeriodic() #driver takes control of drivetrain deadband = 0.1 if (abs(self.joystick0.getRawAxis(map.drive)) > abs(deadband)): self.SetSpeedDT.start() if (abs(self.joystick1.getRawAxis(map.drive)) > abs(deadband)): self.SetSpeedDT.start() self.cargo.periodic() def teleopInit(self): super().teleopInit() self.disabledInit() self.teleop = True def teleopPeriodic(self): self.climber.periodic() self.cargo.periodic() self.hatch.periodic() super().teleopPeriodic() def updateDashboardInit(self): #self.drive.dashboardInit() #self.hatch.dashboardInit() self.cargo.dashboardInit() #self.climber.dashboardInit() #self.limelight.dashboardInit() #sequences.dashboardInit() #autonomous.dashboardInit() #pass def updateDashboardPeriodic(self): #SmartDashboard.putNumber("Timer", self.timer.get()) self.drive.dashboardPeriodic() #self.hatch.dashboardPeriodic() self.cargo.dashboardPeriodic() #self.climber.dashboardPeriodic() #self.limelight.dashboardPeriodic() #sequences.dashboardPeriodic() #autonomous.dashboardPeriodic() #pass def disabledInit(self): self.scheduler.removeAll() self.drive.disable() self.hatch.disable() self.cargo.disable() self.climber.disable() def disabledPeriodic(self): self.disabledInit() def driverLeftButton(self, id): """ Return a button off of the left driver joystick that we want to map a command to. """ return wpilib.buttons.JoystickButton(self.joystick0, id) def driverRightButton(self, id): """ Return a button off of the right driver joystick that we want to map a command to. """ return wpilib.buttons.JoystickButton(self.joystick1, id) def operatorButton(self, id): """ Return a button off of the operator gamepad that we want to map a command to. """ return wpilib.buttons.JoystickButton(self.xbox, id) def operatorAxis(self, id): """ Return a Joystick off of the operator gamepad that we want to map a command to. """ #id is axis channel for taking value of axis return self.xbox.getRawAxis(id) #wpilib.joystick.setAxisChannel(self.xbox, id) def readOperatorButton(self, id): """ Return button value """ return self.xbox.getRawButton(id) def readDriverRightButton(self, id): """ Return button value from right joystick """ return self.joystick1.getRawButton(id) def readDriverLeftButton(self, id): """ Return button value from left joystick """ return self.joystick0.getRawButton(id) def autoSelector(self, auto): if auto == "DriveStraight": self.DriveStraight.start() elif auto == "DoNothing": self.disabledInit() elif auto == "Level1Center": self.CenterCargo.start() elif auto == "DriverControl": self.driverControl() elif auto == "DriveStraightSide": self.DriveStraightSide.start() elif auto == "RightCargo": self.RightCargo.start() elif auto == "LeftCargo": self.LeftCargo.start() else: self.disabledInit() def driverControl(self): self.SetSpeedDT.start()