class XboxJoystick(Sensor):
"""
Sensor wrapper for the Xbox Controller.
Has boolean attributes for buttons: a/b/x/y/back/start_button,
l/r_shoulder
Attributes l/r_x/y_axis for thumbstick positions
trigger_pos and keypad_pos for trigger and keypad position
"""
l_x_axis = l_y_axis = r_x_axis = r_y_axis = 0
trigger_pos = keypad_pos = 0
a_button = b_button = x_button = y_button = False
l_shoulder = r_shoulder = back_button = start_button = False
def __init__(self, port):
"""
Initializes the joystick with some USB port.
"""
super().__init__()
self.j = Joystick(port)
def poll(self):
for i, state_id in enumerate(BUTTON_TABLE, 1):
self.update_state(state_id,
self.j.getRawButton(i))
# button index is offset by 1 due to wpilib 1-indexing
self.l_x_axis = self.j.getX()
self.l_y_axis = self.j.getY()
self.r_x_axis = self.j.getRawAxis(4)
self.r_y_axis = self.j.getRawAxis(5)
self.trigger_pos = self.j.getZ()
self.keypad_pos = self.j.getRawAxis(6)
class XboxController(object):
"""
Allows usage of an Xbox controller, with sensible names for xbox
specific buttons and axes.
"""
def __init__(self, port):
"""
:param port: The port on the driver station that the controller is
plugged into.
:type port: int
"""
self.joy = Joystick(port)
self.debounce = DpadDebouncer()
def getLeftX(self):
"""Get the left stick X axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(0)
def getLeftY(self):
"""Get the left stick Y axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(1)
#getX = self.getLeftX()
#getY = self.getLeftY()
def getLeftPressed(self):
"""Determines if the left stick is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(9)
def getPOV(self):
"""Get the state of the D-Pad
:returns: The angle of the D-Pad in degrees, or -1 if the D-Pad is not pressed.
:rtype: float
"""
return self.debounce.get(self.joy.getPOV())
def getRightX(self):
"""Get the right stick X axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(4)
def getRightY(self):
"""Get the right stick Y axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(5)
def getRightPressed(self):
"""Determines if the right stick is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(10)
def getButtonA(self):
"""Gets whether the A button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(1)
def getButtonB(self):
"""Gets whether the B button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(2)
def getButtonX(self):
"""Gets whether the X button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(3)
def getButtonY(self):
"""Gets whether the X button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(4)
def getStart(self):
"""Gets whether the Start button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(8)
def getBack(self):
"""Gets whether the Back button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(7)
def getLeftBumper(self):
"""Gets whether the left bumper is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(5)
def getRightBumper(self):
"""Gets whether the right bumper is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(6)
def getLeftTrigger(self):
"""Gets whether the left trigger is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawAxis(2) > 0
def getRightTrigger(self):
"""Gets whether the right trigger is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawAxis(3) > 0
def getTriggers(self):
return self.joy.getRawAxis(2)
def getRightTriggerRaw(self):
return self.joy.getRawAxis(3)
def getLeftTriggerRaw(self):
return self.joy.getRawAxis(2)
def rumble(self, left=None, right=None):
"""Sets the rumble amount on one/both side(s) of the controller"""
if left is not None:
self.joy.setRumble(Joystick.RumbleType.kLeftRumble_val, left)
if right is not None:
self.joy.setRumble(Joystick.RumbleType.kRightRumble_val, right)
class Jessica(AsyncRobot):
def __init__(self):
super().__init__()
# Create motors and stuff here
def robotInit(self):
self.controller = Joystick(0)
self.joystick = Joystick(1)
CameraServer.launch('vision.py:main')
self.autoChooser = SendableChooser()
self.autoChooser.addDefault("switch_scale", switch_scale)
# self.autoChooser.addObject("drive_forward", drive_straight)
SmartDashboard.putData("Autonomous Mode Chooser", self.autoChooser)
self.autoSideChooser = SendableChooser()
self.autoSideChooser.addDefault("left", "L")
self.autoSideChooser.addObject("right", "R")
self.autoSideChooser.addObject("middle", "M")
SmartDashboard.putData("Side Chooser", self.autoSideChooser)
RobotMap.driver_component.set_low_gear()
def robotPeriodic(self):
SmartDashboard.putNumber("driver/current_distance", RobotMap.driver_component.current_distance)
SmartDashboard.putNumber("driver/left_encoder",
RobotMap.driver_component.left_encoder_motor.getSelectedSensorPosition(0))
SmartDashboard.putNumber("driver/right_encoder",
RobotMap.driver_component.right_encoder_motor.getSelectedSensorPosition(0))
SmartDashboard.putNumber("driver/gyro", RobotMap.driver_component.driver_gyro.getAngle())
SmartDashboard.putNumber("lifter/current_position", RobotMap.lifter_component.current_position)
SmartDashboard.putNumber("lifter/current_elevator_position", RobotMap.lifter_component.current_elevator_position)
SmartDashboard.putNumber("lifter/current_carriage_position", RobotMap.lifter_component.current_carriage_position)
SmartDashboard.putBoolean("lifter/carriage_top_switch", RobotMap.lifter_component.carriage_top_switch.get())
SmartDashboard.putBoolean("lifter/carriage_bottom_switch", RobotMap.lifter_component.carriage_bottom_switch.get())
SmartDashboard.putBoolean("lifter/elevator_bottom_switch", RobotMap.lifter_component.elevator_bottom_switch.get())
SmartDashboard.putNumber("gripper/gripper_pot", RobotMap.gripper_component.pot.get())
def autonomousInit(self):
# Insert decision tree logic here.
game_data = self.ds.getGameSpecificMessage()
switch_position = game_data[0]
scale_position = game_data[1]
start_position = self.autoSideChooser.getSelected()
aut = self.autoChooser.getSelected()
self.start_command(aut(scale_position, switch_position, start_position))
# self.start_command(Turn(45, 1))
def disabledInit(self):
RobotMap.driver_component.moving_angular.clear()
RobotMap.driver_component.moving_linear.clear()
def autonomousPeriodic(self):
pass
def teleopInit(self):
self.man_mode = False
self.climb_mode = False
# self.start_command(Reset())
def teleopPeriodic(self):
# if self.joystick.getRawButtonPressed(1):
# p1
left_y = -self.controller.getRawAxis(1)
right_x = self.controller.getRawAxis(2)
self.start_command(curve_drive(left_y, right_x))
if self.controller.getRawButtonPressed(3):
self.start_command(Toggle())
if self.controller.getRawButtonPressed(14):
RobotMap.driver_component.toggle_gear()
# self.start_command(toggle_gear())
if self.controller.getRawButtonPressed(5):
RobotMap.driver_component.set_low_gear()
if self.controller.getRawButtonPressed(6):
RobotMap.driver_component.set_high_gear()
# vision = SmartDashboard.getNumber("vision", 0)
# vision_min = SmartDashboard.getNumber("vision_min", 0)
# vision_max = SmartDashboard.getNumber("vision_max", 0)
# clamped_vision = normalize_range(vision, vision_min, vision_max, -1, 1)
# left_vision = max(1 - abs(min(clamped_vision, 0)), 0)
# right_vision = abs(max(clamped_vision, 0))
# clamped_vision = min(max(vision, -1), 1)
# left_vision = 1 - abs(clamped_vision)
# right_vision = abs(clamped_vision)
# if not (vision_min == 0 and vision_max == 0):
# self.controller.setRumble(Joystick.RumbleType.kLeftRumble, clamped_vision)
# self.controller.setRumble(Joystick.RumbleType.kRightRumble, clamped_vision)
# p2
# l2 = -normalize_range(self.joystick.getRawAxis(3), -1, 1, 0, 1)
# r2 = normalize_range(self.joystick.getRawAxis(4), -1, 1, 0, 1)
# speed = r2 + l2
# if self.man_mode:
# self.start_command(move_lifter(speed))
right_y = -self.joystick.getRawAxis(5)
#up
if not self.climb_mode:
if self.joystick.getPOV() == 0:
self.start_command(move_lifter(1))
self.man_mode = True
elif self.joystick.getPOV() == 180:
self.start_command(move_lifter(-1))
self.man_mode = True
elif self.man_mode:
self.start_command(move_lifter(0))
else:
if self.joystick.getPOV() == 0:
self.start_command(lock_carriage_move_elevator(1))
self.man_mode = True
elif self.joystick.getPOV() == 180:
self.start_command(lock_carriage_move_elevator(right_y))
self.man_mode = True
elif self.man_mode:
self.start_command(lock_carriage_move_elevator(right_y))
l1 = 5
r1 = 6
square = 1
x = 2
circle = 3
triangle = 4
touchpad = 14
if self.joystick.getRawButtonPressed(touchpad):
self.climb_mode = not self.climb_mode
if self.climb_mode:
self.man_mode = True
self.start_command(LiftTo("up"))
if self.joystick.getRawButton(7) and self.climb_mode:
self.start_command(climb())
if not self.joystick.getRawButton(7) and self.climb_mode:
self.start_command(stop())
g_speed = 0.0
if self.joystick.getRawButton(square):
g_speed = -1.0
if self.joystick.getRawButton(x):
g_speed = 1.0
if self.joystick.getRawButton(circle):
g_speed = -0.50
self.start_command(move_left_right(g_speed))
if self.joystick.getRawButtonPressed(triangle):
self.start_command(toggle_spread())
if self.joystick.getRawButtonPressed(r1) and not self.climb_mode:
self.start_command(move_to_position_instant("scale_high"))
self.man_mode = False
if self.joystick.getRawButtonPressed(l1) and not self.climb_mode:
self.start_command(move_to_position_instant("floor"))
self.man_mode = False
# if self.joystick.getRawButtonPressed(touchpad):
# self.man_mode = not self.man_mode
options = 10
if self.joystick.getRawButtonPressed(options):
RobotMap.driver_component.reset_drive_sensors()
share = 9
if self.joystick.getRawButtonPressed(share):
self.start_command(Reset())
if self.joystick.getRawButton(8):
self.start_command(LiftTo("up"))
class Controller:
def __init__(self, channel):
self.channel = channel
self.joystick = Joystick(channel)
self.button_mapping = {}
self.invert_mapping = {}
def set_mapping(self, name, joystick_button_number):
self.button_mapping[name] = joystick_button_number
def get_mapping(self, name):
return self.button_mapping.get(name, None)
def set_invert_mapping(self, name, value):
self.invert_mapping[name] = value
def get_joystick_mapping(self, name):
if name in self.invert_mapping.keys():
if self.invert_mapping[name]:
n = self.get_mapping(name)
if n:
return -1 * self.joystick.getRawAxis(n)
n = self.get_mapping(name)
if n:
return self.joystick.getRawAxis(n)
return 0
def get_button_mapping(self, name):
n = self.get_mapping(name)
if n:
return self.joystick.getRawButton(n)
else:
return False
def set_drive_forward(self, value):
self.set_mapping("drive_forward", value)
def get_drive_forward(self):
return self.get_joystick_mapping("drive_forward")
def invert_drive_forward(self, value=True):
return self.set_invert_mapping("drive_forward", value)
def set_drive_turn(self, value):
self.set_mapping("drive_turn", value)
def get_drive_turn(self):
return self.get_joystick_mapping("drive_turn")
def invert_drive_turn(self, value=True):
return self.set_invert_mapping("drive_turn", value)
def set_elevator_low(self, value):
self.set_mapping("elevator_low", value)
def get_elevator_low(self):
return self.get_button_mapping("elevator_low")
def set_elevator_mid(self, value):
self.set_mapping("elevator_mid", value)
def get_elevator_mid(self):
return self.get_button_mapping("elevator_mid")
def set_elevator_high(self, value):
self.set_mapping("elevator_high", value)
def get_elevator_high(self):
return self.get_button_mapping("elevator_high")
def set_elevator_cargo_ship(self, value):
self.set_mapping("elevator_cargo_ship", value)
def get_elevator_cargo_ship(self):
return self.get_button_mapping("elevator_cargo_ship")
def set_shift_gears(self, value):
self.set_mapping("shift_gears", value)
def get_shift_gears(self):
return self.get_button_mapping("shift_gears")
def set_intake_hatch(self, value):
self.set_mapping("intake_hatch", value)
def get_intake_hatch(self):
return self.get_button_mapping("intake_hatch")
def set_grab_hatch(self, value):
self.set_mapping("grab_hatch", value)
def get_grab_hatch(self):
return self.get_button_mapping("grab_hatch")
def set_drop_hatch(self, value):
self.set_mapping("drop_hatch", value)
def get_drop_hatch(self):
return self.get_button_mapping("drop_hatch")
def set_hold_hatch(self, value):
self.set_mapping("hold_hatch", value)
def get_hold_hatch(self):
return self.get_button_mapping("hold_hatch")
def set_close_hatch(self, value):
self.set_mapping("close_hatch", value)
def get_close_hatch(self):
return self.get_button_mapping("close_hatch")
def set_rotate_hatch_down(self, value):
self.set_mapping("rotate_hatch_down", value)
def get_rotate_hatch_down(self):
return self.get_button_mapping("rotate_hatch_down")
def set_rotate_hatch_up(self, value):
self.set_mapping("rotate_hatch_up", value)
def get_rotate_hatch_up(self):
return self.get_button_mapping("rotate_hatch_up")
def set_rotate_cargo_down(self, value):
self.set_mapping("rotate_cargo_down", value)
def get_rotate_cargo_down(self):
return self.get_button_mapping("rotate_cargo_down")
def set_rotate_cargo_up(self, value):
self.set_mapping("rotate_cargo_up", value)
def get_rotate_cargo_up(self):
return self.get_button_mapping("rotate_cargo_up")
def set_outtake_hatch(self, value):
self.set_mapping("outtake_hatch", value)
def get_outtake_hatch(self):
return self.get_button_mapping("outtake_hatch")
def set_shoot(self, value):
self.set_mapping("shoot", value)
def get_shoot(self):
return self.get_button_mapping("shoot")
def set_intake(self, value):
self.set_mapping("intake", value)
def get_intake(self):
return self.get_button_mapping("intake")
def set_reset_auto_dock(self, value):
self.set_mapping("reset_auto_dock", value)
def get_reset_auto_dock(self):
return self.get_button_mapping("reset_auto_dock")
def set_auto_dock(self, value):
self.set_mapping("auto_dock", value)
def get_auto_dock(self):
return self.get_button_mapping("auto_dock")
def get_trigger(self, stick: wpilib.Joystick):
first_trigger = stick.getRawAxis(3)
second_trigger = stick.getRawAxis(2)
return (first_trigger * 0.5) + (second_trigger * 0.5)
class XboxController(object):
def __init__(self, port):
"""
:param port: The port on the driver station that the controller is
plugged into.
:type port: int
"""
self.joy = Joystick(port)
self.debounce = DpadDebouncer()
def getLeftX(self):
"""Get the left stick X axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(0)
def getLeftY(self):
"""Get the left stick Y axis
:returns: -1 to 1
:rtype: float
"""
return float(self.joy.getRawAxis(1))
#getX = self.getLeftX()
#getY = self.getLeftY()
def getLeftPressed(self):
"""Determines if the left stick is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(9)
def getPOV(self):
"""Get the state of the D-Pad
:returns: The angle of the D-Pad in degrees, or -1 if the D-Pad is not pressed.
:rtype: float
"""
return self.debounce.get(self.joy.getPOV())
def getRightX(self):
"""Get the right stick X axis
:returns: -1 to 1
:rtype: float
"""
return float(self.joy.getRawAxis(4))
def getRightY(self):
"""Get the right stick Y axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(5)
def getRightPressed(self):
"""Determines if the right stick is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(10)
def getButtonA(self):
"""Gets whether the A button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(1)
def getButtonB(self):
"""Gets whether the B button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(2)
def getButtonX(self):
"""Gets whether the X button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(3)
def getButtonY(self):
"""Gets whether the X button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(4)
def getStart(self):
"""Gets whether the Start button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(8)
def getBack(self):
"""Gets whether the Back button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(7)
def getLeftBumper(self):
"""Gets whether the left bumper is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(5)
def getRightBumper(self):
"""Gets whether the right bumper is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(6)
def getLeftTrigger(self):
"""Gets whether the left trigger is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawAxis(2) > 0
def getRightTrigger(self):
"""Gets whether the right trigger is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawAxis(3) > 0
def getTriggers(self):
return self.joy.getRawAxis(2)
def getRightTriggerRaw(self):
return self.joy.getRawAxis(3)
def getLeftTriggerRaw(self):
return self.joy.getRawAxis(2)
def rumble(self, left=None, right=None):
"""Sets the rumble amount on one/both side(s) of the controller"""
if left is not None:
self.joy.setRumble(Joystick.RumbleType.kLeftRumble_val, left)
if right is not None:
self.joy.setRumble(Joystick.RumbleType.kRightRumble_val, right)
def BoxLift(self, stick: wpilib.Joystick):
trigger = stick.getRawAxis(3)
stick = stick.getRawAxis(5)
self.box_lift_motor.set(ctre.ControlMode.Velocity,
(stick * trigger) * 6000)
class XboxController(object):
"""
Allows usage of an Xbox controller, with sensible names for xbox
specific buttons and axes.
"""
def __init__(self, port):
"""
:param port: The port on the driver station that the controller is
plugged into.
:type port: int
"""
self.joy = Joystick(port)
self.debounce = DpadDebouncer()
def left_x(self):
"""Get the left stick X axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(0)
def left_y(self):
"""Get the left stick Y axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(1)
getX = left_x
getY = left_y
def left_pressed(self):
"""Determines if the left stick is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(9)
def dpad(self):
"""Get the state of the D-Pad
:returns: The angle of the D-Pad in degrees, or -1 if the D-Pad is not pressed.
:rtype: float
"""
return self.debounce.get(self.joy.getPOV())
def right_x(self):
"""Get the right stick X axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(4)
def right_y(self):
"""Get the right stick Y axis
:returns: -1 to 1
:rtype: float
"""
return self.joy.getRawAxis(5)
def right_pressed(self):
"""Determines if the right stick is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(10)
def a(self):
"""Gets whether the A button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(1)
def b(self):
"""Gets whether the B button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(2)
def x(self):
"""Gets whether the X button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(3)
def y(self):
"""Gets whether the X button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(4)
def start(self):
"""Gets whether the Start button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(8)
def back(self):
"""Gets whether the Back button is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(7)
def left_bumper(self):
"""Gets whether the left bumper is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(5)
def right_bumper(self):
"""Gets whether the right bumper is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawButton(6)
def left_trigger(self):
"""Gets whether the left trigger is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawAxis(2) > 0
def right_trigger(self):
"""Gets whether the right trigger is pressed
:returns: True if pressed, False otherwise
:rtype: bool
"""
return self.joy.getRawAxis(3) > 0
def rumble(self, left=None, right=None):
"""Sets the rumble amount on one/both side(s) of the controller"""
if left is not None:
self.joy.setRumble(Joystick.RumbleType.kLeftRumble_val, left)
if right is not None:
self.joy.setRumble(Joystick.RumbleType.kRightRumble_val, right)
