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()
