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 Robot(wpilib.IterativeRobot):
def robotInit(self):
"""
This function is called upon program startup and
should be used for any initialization code.
"""
self.pneumaticControlModuleCANID = robotmap.PCM
self.kDriveTrain = robotmap.DriveTrain
self.kCubeGrabber = robotmap.CubeGrabber
self.kElevator = robotmap.Elevator
self.kSticks = robotmap.Sticks
self.kClimber = robotmap.Climber
self.dStick = Joystick(self.kSticks['drive'])
self.cStick = Joystick(self.kSticks['control'])
self.drive = Drive(self)
self.cubeGrabber = cubeGrabber(self)
self.elevator = Elevator(self)
self.climber = Climber(self)
self.sendableChooser()
def robotPeriodic(self):
pass
def disabledInit(self):
pass
def disabledPeriodic(self):
self.drive.stop()
def autonomousInit(self):
"""This function is run once each time the robot enters autonomous mode."""
self.autonomous = Autonomous(self)
self.autonomous.reset()
self.drive.autoInit()
def autonomousPeriodic(self):
"""This function is called periodically during autonomous."""
#self.autonomous.testMove(self.autonomous.WALL_TO_SCALE, -1, False)
#self.autonomous.testAngle(-90, -1)
#self.elevator.setElevatorPosition(self.elevator.kScale)
#self.autonomous.start()
self.autonomous.run()
#self.elevator.setElevatorPosition(-20000)
#self.autonomous.telemetry()
def teleopInit(self):
self.drive.teleInit()
def teleopPeriodic(self):
"""This function is called periodically during operator control."""
speed = (self.dStick.getY() * -1)**3
rotation = self.dStick.getTwist()/(1.1+self.dStick.getRawAxis(3))
# self.drive.moveSpeed(speed, speed)
self.drive.arcadeWithRPM(speed, rotation, 2800)
self.cubeGrabber.grabberFunction()
#
self.elevator.elevatorFunction()
#self.elevator.telemetry()
self.climber.climberFunction()
def testInit(self):
pass
def testPeriodic(self):
wpilib.LiveWindow.setEnabled(True)
pass
def sendableChooser(self):
self.startingChooser = SendableChooser()
self.startingChooser.addDefault('Move Forward Only', '!')
self.startingChooser.addObject('Starting Left', 'L')
self.startingChooser.addObject('Starting Middle', 'M')
self.startingChooser.addObject('Starting Right', 'R')
wpilib.SmartDashboard.putData('Starting Side', self.startingChooser)
self.startingDelayChooser = SendableChooser()
self.startingDelayChooser.addDefault('0', 0)
self.startingDelayChooser.addObject('1', 1)
self.startingDelayChooser.addObject('2', 2)
self.startingDelayChooser.addObject('3', 3)
self.startingDelayChooser.addObject('4', 4)
self.startingDelayChooser.addObject('5', 5)
self.startingDelayChooser.addObject('6', 6)
self.startingDelayChooser.addObject('7', 7)
self.startingDelayChooser.addObject('8', 8)
self.startingDelayChooser.addObject('9', 9)
self.startingDelayChooser.addObject('10', 10)
self.startingDelayChooser.addObject('11', 11)
self.startingDelayChooser.addObject('12', 12)
self.startingDelayChooser.addObject('13', 13)
self.startingDelayChooser.addObject('14', 14)
self.startingDelayChooser.addObject('15', 15)
wpilib.SmartDashboard.putData('Delay Time(sec)', self.startingDelayChooser)
self.switchOrScale = SendableChooser()
self.switchOrScale.addDefault('Switch', 'Switch')
self.switchOrScale.addObject('Scale', 'Scale')
wpilib.SmartDashboard.putData('Switch or Scale', self.switchOrScale)
class OI:
"""
Operator Input - This class ties together controls and commands
"""
def __init__(self, robot):
self.robot = robot
# Controllers
# Xbox
self.xbox_controller_1 = controls.xbox_controller.Xbox_Controller(0)
self.xbox_controller_2 = controls.xbox_controller.Xbox_Controller(1)
## COMMANDS ##
##Controller 1 Commands
# DRIVE COMMANDS
self.drive_command = commands.drivetrain.Drive_With_Tank_Values(
self.robot,
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_RIGHT_X,
inverted=INVERT_XBOX_RIGHT_X),
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_LEFT_Y,
inverted=INVERT_XBOX_LEFT_Y),
self._get_axis(self.xbox_controller_1,
controls.xbox_controller.XBOX_AXIS_LEFT_X,
inverted=INVERT_XBOX_LEFT_X),
)
self.robot.drive.setDefaultCommand(self.drive_command)
self.auto_choose = SendableChooser()
self.auto_choose.addObject('Do Nothing',
commands.autonomous.Do_Nothing(self.robot))
self.auto_choose.addDefault(
'Cross Baseline', commands.autonomous.Cross_Baseline(self.robot))
self.auto_choose.addObject(
'Center Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_CENTER_GEAR))
self.auto_choose.addDefault(
'Center Gear No Camera',
commands.autonomous.Center_Gear_Without_Camera(self.robot))
self.auto_choose.addObject(
'Right Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_RIGHT_GEAR))
self.auto_choose.addObject(
'Right Gear And Shoot Red',
commands.autonomous.Move_To_Gear_And_Shoot(self.robot,
const.ID_AUTO_RED_SIDE))
self.auto_choose.addObject(
'Left Gear',
commands.autonomous.Move_To_Gear(self.robot,
const.ID_AUTO_LEFT_GEAR))
self.auto_choose.addObject(
'Left Gear And Shoot Blue',
commands.autonomous.Move_To_Gear_And_Shoot(
self.robot, const.ID_AUTO_BLUE_SIDE))
self.auto_choose.addObject(
'Shoot From Hopper Red',
commands.autonomous.Shoot_From_Hopper(self.robot,
const.ID_AUTO_RED_SIDE))
self.auto_choose.addObject(
'Shoot From Hopper Blue',
commands.autonomous.Shoot_From_Hopper(self.robot,
const.ID_AUTO_BLUE_SIDE))
self.auto_choose.addObject(
'Shoot From Start',
commands.autonomous.Shoot_From_Start(self.robot))
SmartDashboard.putData('Autonomous Mode', self.auto_choose)
# Toggle Drive State
self.button_drive_shift = Xbox_Trigger(self.xbox_controller_1,
XBOX_BTN_LEFT_TRIGGER)
self.button_drive_shift.whenPressed(
commands.drivetrain.Toggle_Drive_State(self.robot))
# Toggle drivetrain gear
if not const.DEMO_MODE:
self.button_gear_shift = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_LEFT_BUMPER)
self.button_gear_shift.whenPressed(
commands.drivetrain.Toggle_Gear_State(self.robot))
# Slow speed for driving in Y direction so that we slow down before the gear peg
self.button_slow_driving = Xbox_Trigger(self.xbox_controller_1,
XBOX_BTN_RIGHT_TRIGGER)
self.button_slow_driving.whenPressed(
commands.drivetrain.Set_Sensitivity_Low(self.robot))
self.button_slow_driving.whenReleased(
commands.drivetrain.Set_Sensitivity_High(self.robot))
# Drives backwards small distance, to align better for gear feeding
self.button_drive_gear_feeder_reverse = Xbox_Button(
self.xbox_controller_1, XBOX_BTN_X)
self.button_drive_gear_feeder_reverse.whenPressed(
commands.drivetrain.Back_Up(self.robot))
if not const.DEMO_MODE:
# For testing
self.button_rotate_to_boiler = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_Y)
self.button_rotate_to_boiler.whenPressed(
commands.drivetrain.Rotate_To_Boiler(self.robot))
# For testing
self.rotate = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_RIGHT_BUMPER)
self.rotate.whenPressed(
commands.drivetrain.Rotate_To_Angle(self.robot, -52))
self.rotate_to_gear = Xbox_Button(self.xbox_controller_1,
XBOX_BTN_A)
self.rotate_to_gear.whenPressed(
commands.drivetrain.Rotate_To_Gear(self.robot))
## Controller 2 Commands
# Run Shooter
if const.DEMO_MODE:
# Lower top speed on shooter when in demo mode
self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_RIGHT_TRIGGER)
self.button_run_shooter.whenPressed(
commands.shooter.Run_Shooter_Wheel(self.robot))
self.button_run_shooter.whenReleased(
commands.shooter.Stop_Shooter_Wheel(self.robot))
else:
self.button_run_shooter = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_RIGHT_TRIGGER)
self.button_run_shooter.whenPressed(
commands.shooter.Run_Shooter_Wheel_Full_Speed(self.robot))
self.button_run_shooter.whenReleased(
commands.shooter.Stop_Shooter_Wheel(self.robot))
# Run Agitator
self.button_run_agitator = Xbox_Trigger(self.xbox_controller_2,
XBOX_BTN_LEFT_TRIGGER)
self.button_run_agitator.whenPressed(
commands.agitator.Run_Agitator(self.robot, 0.8))
self.button_run_agitator.whenReleased(
commands.agitator.Stop_Agitator(self.robot))
# FEEDER COMMANDS
# Toggles feeder on/off when pressed
self.button_run_feeder_forward = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_RIGHT_BUMPER)
self.button_run_feeder_forward.whenPressed(
commands.feeder.Run_Feeder(self.robot, 1.0))
self.button_run_feeder_forward.whenReleased(
commands.feeder.Stop_Feeder(self.robot))
self.button_run_feeder_backward = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_LEFT_BUMPER)
self.button_run_feeder_backward.whenPressed(
commands.feeder.Run_Feeder(self.robot, -1.0))
self.button_run_feeder_backward.whenReleased(
commands.feeder.Stop_Feeder(self.robot))
if not const.DEMO_MODE:
# GILLOTUINNEE
self.button_toggle_gear_lexan = Xbox_Button(
self.xbox_controller_2, XBOX_BTN_Y)
self.button_toggle_gear_lexan.whenPressed(
commands.gear_lexan.Toggle_Gear_Lexan_State(self.robot))
#self.button_toggle_gear_lexan.whenPressed( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_OPEN ) )
#self.button_toggle_gear_lexan.whenReleased( commands.gear_lexan.Set_Gear_Lexan_State( self.robot, const.ID_GEAR_LEXAN_CLOSED) )
# Release gear
self.button_toggle_gear_release = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_A)
self.button_toggle_gear_release.whenPressed(
commands.gear_claw.Open_Container(self.robot))
self.button_toggle_gear_release.whenReleased(
commands.gear_claw.Close_Container(self.robot))
# Toggle funnel
self.button_toggle_gear_funnel = Xbox_Button(self.xbox_controller_2,
XBOX_BTN_B)
self.button_toggle_gear_funnel.whenPressed(
commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot))
self.button_toggle_gear_funnel.whenPressed(
commands.gear_lexan.Set_Gear_Lexan_State(
self.robot, const.ID_GEAR_LEXAN_CLOSED))
self.button_toggle_gear_funnel.whenReleased(
commands.gear_funnel.Toggle_Gear_Funnel_State(self.robot))
# CLIMBER COMMANDS
self.button_run_climber_up = Joystick_POV(
self.xbox_controller_2, controls.joystick_pov.JOY_POV_UP)
self.button_run_climber_up.whenPressed(
commands.climber.Run_Climber(self.robot, 1.0))
self.button_run_climber_up.whenReleased(
commands.climber.Stop_Climber(self.robot))
#self.button_run_climber_down = Joystick_POV( self.xbox_controller_1, controls.joystick_pov.JOY_POV_DOWN )
#self.button_run_climber_down.whenPressed( commands.climber.Run_Climber( self.robot, -0.2 ) )
#self.button_run_climber_down.whenReleased( commands.climber.Stop_Climber( self.robot ) )
#self.button_change_gear_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_BACK )
#self.button_change_gear_camera_type.whenPressed( commands.gear_camera.Toggle_Gear_Camera_Mode( self.robot ) )
#self.button_change_shooter_camera_type = Xbox_Button( self.xbox_controller_2, XBOX_BTN_START )
#self.button_change_shooter_camera_type.whenPressed( commands.shooter_camera.Toggle_Shooter_Camera_Mode( self.robot ) )
#smart dashboard changing PID values
dp_trigger = SmartDashboard_Update_Trigger('Drive P: ',
self.robot.drive.default_kP)
dp_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(p=dp_trigger.
get_key_value())))
di_trigger = SmartDashboard_Update_Trigger('Drive I: ',
self.robot.drive.default_kI)
di_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(i=di_trigger.
get_key_value())))
dd_trigger = SmartDashboard_Update_Trigger('Drive D: ',
self.robot.drive.default_kD)
dd_trigger.whenActive(
Command_Call(lambda: self.robot.drive.update_pid(d=dd_trigger.
get_key_value())))
#smart dashboard changing Shooter PID values
sp_trigger = SmartDashboard_Update_Trigger(
'Shooter P: ', self.robot.shooter.default_kP)
sp_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
p=sp_trigger.get_key_value())))
si_trigger = SmartDashboard_Update_Trigger(
'Shooter I: ', self.robot.shooter.default_kI)
si_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
i=si_trigger.get_key_value())))
sd_trigger = SmartDashboard_Update_Trigger(
'Shooter D: ', self.robot.shooter.default_kD)
sd_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
d=sd_trigger.get_key_value())))
sf_trigger = SmartDashboard_Update_Trigger(
'Shooter F: ', self.robot.shooter.default_kF)
sf_trigger.whenActive(
Command_Call(lambda: self.robot.shooter.update_pid(
f=sf_trigger.get_key_value())))
enc_trigger = SmartDashboard_Update_Trigger(
'ENC Autocorrect constant ',
const.DRIVE_CORRECTION_PROPORTION_FORWARD_ENC)
enc_trigger.whenActive(
Command_Call(
lambda: const.update_enc_const(enc_trigger.get_key_value())))
drive_corr_trigger = SmartDashboard_Update_Trigger(
'Drive Correction Enbaled ', const.DRIVE_CORRECTION_ENABLED)
drive_corr_trigger.whenActive(
Command_Call(lambda: const.update_auto_correct(drive_corr_trigger.
get_key_value())))
shooter_trigger = SmartDashboard_Update_Trigger(
'Shooter Speed ', const.SHOOTER_SPEED)
shooter_trigger.whenActive(
Command_Call(lambda: const.update_shooter_speed(shooter_trigger.
get_key_value())))
# Trigger for changing cameras exposure
cam_exp_trigger = SmartDashboard_Update_Trigger(
'Cam Exposure ', const.DEFAULT_CAMERA_EXPOSURE)
cam_exp_trigger.whenActive(
Command_Call(lambda: self._update_cameras_exposure(
cam_exp_trigger.get_key_value())))
def _update_cameras_exposure(self, exposure):
# Do nothing, because RPi reads the value right from the SD trigger
pass
#self.robot.gear_camera.update_exposure( exposure )
#self.robot.shooter_camera.update_exposure( exposure )
def _get_axis(self, joystick, axis, inverted=False):
"""
Handles inverted joy axes and dead band
"""
def axis_func():
val = joystick.getAxis(axis)
if abs(val) < JOY_DEAD_BAND:
val = 0
if inverted:
val *= -1
return val
return axis_func
class OI:
def __init__(self, robot):
self.robot = robot
# update OI with logs
Event_Manager.add_listeners({
'auto.periodic' : self.log,
'teleop.periodic' : self.log
})
# notify listeners when SmartDashboard is updated
sd = NetworkTable.getTable('SmartDashboard')
sd.addTableListener(self.dashboard_listener, True)
# add autonomous mode chooser to smartdashboard
self.auto_choose = SendableChooser()
self.auto_choose.addObject('Auto Do Nothing', Autonomous.Modes.DO_NOTHING)
self.auto_choose.addObject('Auto Move Forward', Autonomous.Modes.MOVE_FORWARD)
self.auto_choose.addObject('Auto One Object', Autonomous.Modes.ONE_OBJECT)
SmartDashboard.putData('Autonomous Mode', self.auto_choose)
# joystick events
self.joystick = Joystick(0)
l_bumper = Joystick_Button(self.joystick, lc.L_BUMPER)
l_trigger = Joystick_Button(self.joystick, lc.L_TRIGGER)
r_bumper = Joystick_Button(self.joystick, lc.R_BUMPER)
r_trigger = Joystick_Button(self.joystick, lc.R_TRIGGER)
btn_one = Joystick_Button(self.joystick, 1)
btn_two = Joystick_Button(self.joystick, 2)
# update the joysick axis periodically
Event_Manager.add_listener('teleop.periodic', self.update_axis)
l_bumper.while_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.while_pressed'))
l_trigger.while_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.while_pressed'))
l_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.l_bumper.when_pressed'))
l_trigger.when_pressed(lambda: Event_Manager.trigger('joystick.l_trigger.when_pressed'))
l_bumper.when_released(lambda: Event_Manager.trigger('joystick.l_bumper.when_released'))
l_trigger.when_released(lambda: Event_Manager.trigger('joystick.l_trigger.when_released'))
r_bumper.when_pressed(lambda: Event_Manager.trigger('joystick.r_bumper.when_pressed'))
btn_two.when_pressed(lambda: Event_Manager.trigger('joystick.btn_two.when_pressed'))
btn_one.when_pressed(lambda: Event_Manager.trigger('joystick.btn_one.when_pressed'))
def log(self, data):
self.robot.drive.log()
self.robot.grabber_lift.log()
def dashboard_listener(self, source, key, value, is_new):
Event_Manager.trigger('dashboard.updated', {
'source' : source,
'key' : key,
'value' : value,
'is_new' : is_new
})
def update_axis(self, data):
Event_Manager.trigger('joystick.axis.updated', {
'x_left' : self._get_axis(self.joystick, lc.L_AXIS_X)(),
'y_left' : self._get_axis(self.joystick, lc.L_AXIS_Y)(),
'x_right' : self._get_axis(self.joystick, lc.R_AXIS_X)(),
'y_right' : self._get_axis(self.joystick, lc.R_AXIS_Y)()
})
def _get_axis(self, joystick, axis):
def axis_func():
val = joystick.getAxis(axis)
if abs(val) >= .1:
return val
else:
return 0
return axis_func
