class OI():
__instances = None
DEADZONE = 1 / 128
def __init__(self):
self.driverController = XboxController(0)
self.manipulatorController = XboxController(1)
self.aButtonDriver = JoystickButton(self.driverController, 0)
self.aButtonDriver.whenPressed(DriveStraightCommand(10))
@staticmethod
def getInstance() -> OI:
if (OI.__instances == None):
OI.__instances = OI()
return OI.__instances
def __deadzone(self, input, deadzone=DEADZONE) -> Number:
absValue = math.fabs(input)
# no signum ;-;
return 0 if abs < deadzone else ((absValue - deadzone) /
(1.0 - deadzone) *
math.copysign(1, input))
def getLeftY(self):
return self.__deadzone(
self.driverController.getY(GenericHID.Hand.kLeft))
def getRightY(self):
return self.__deadzone(
self.driverController.getY(GenericHID.Hand.kRight))
class MoveArmWithTriggers(Command):
'''Move the arm.'''
def __init__(self, robot):
'''Save the robot object, and pull in the arm subsystem.'''
super().__init__()
self.robot = robot
self.requires(self.robot.arm)
self.xbox0 = XboxController(0)
def initialize(self):
#Called just before this Command runs the first time
pass
def execute(self):
#Called repeatedly when this Command is scheduled to run
self.robot.arm.move(self.xbox0.getTriggerAxis(GenericHID.Hand.kLeft) +
self.xbox0.getTriggerAxis(GenericHID.Hand.kRight)*-1)
def isFinished(self):
#Make this return true when this Command no longer needs to run execute()
return self.isTimedOut()
def end(self):
#Called once after isFinished returns true
pass
def interrupted(self):
'''Called when another Command which requires one or more of the same
subsystems is scheduled to run
'''
pass
self.end()
def __init__(self, robot):
'''Save the robot object, and pull in the arm subsystem.'''
super().__init__()
self.robot = robot
self.requires(self.robot.arm)
self.xbox0 = XboxController(0)
def createControllers(self, ConSpec):
con = None
if ConSpec['jobType'] == 'main':
if ConSpec['Type'] == 'xbox':
con = XboxController(ConSpec['Id'])
elif ConSpec['Type'] == 'xtreme':
con = Joystick(ConSpec['Id'])
elif ConSpec['Type'] == 'gameCube':
con = Joystick(ConSpec['Id'])
else:
print("IDK your Controller")
elif ConSpec['jobType'] == 'side':
if ConSpec['Type'] == 'xbox':
con = XboxController(ConSpec['Id'])
elif ConSpec['Type'] == 'xtreme':
con = Joystick(ConSpec['Id'])
elif ConSpec['Type'] == 'custom':
con = Joystick(ConSpec['Id'])
else:
print("IDK your Controller")
else:
print("IDK your Controller")
return con
def __init__(self):
self.driverController = XboxController(0)
self.manipulatorController = XboxController(1)
self.aButtonDriver = JoystickButton(self.driverController, 0)
self.aButtonDriver.whenPressed(DriveStraightCommand(10))
def xboxController(self):
"""
Assign commands to button actions, and publish your joysticks so you
can read values from them later.
"""
self.xboxController = robot_map.xboxController
self.leftStickY = XboxController.getY(robot_map.xboxControllerLeftStickY)
self.rightStickY = XboxController.getY(robot_map.xboxControllerRightStickY)
def handle_hang_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
if gamepad.getStartButton() and gamepad.getBumper(Hand.kRightHand):
self.kraken_controller.engage()
if self.hang.fire_hook and (
gamepad.getTriggerAxis(Hand.kLeftHand) > 0.9
or gamepad.getTriggerAxis(Hand.kRightHand) > 0.9):
self.hang.winch()
def drive(self, joystick : wpilib.XboxController):
try:
# drive the robot using the oi object provided as well as the number of the controller to use
self.drivetrain.driveCartesian(self.robot.oi.handleNumber(joystick.getX(wpilib.XboxController.Hand.kLeft)),
-self.robot.oi.handleNumber(joystick.getY(wpilib.XboxController.Hand.kLeft)),
-joystick.getRawAxis(2) if abs(joystick.getRawAxis(2))>0.1 else 0)
except:
if not wpilib.DriverStation.getInstance().isFMSAttached():
raise
def createObjects(self):
'''Create motors and stuff here'''
# Objects that are created here are shared with all classes
# that declare them. For example, if I had:
# self.elevator_motor = wpilib.TalonSRX(2)
# here, then I could have
# class Elevator:
# elevator_motor = wpilib.TalonSRX
# and that variable would be available to both the MyRobot
# class and the Elevator class. This "variable injection"
# is especially useful if you want to certain objects with
# multiple different classes.
# create the imu object
self.bno055 = BNO055()
# the "logger" - allows you to print to the logging screen
# of the control computer
self.logger = logging.getLogger("robot")
# the SmartDashboard network table allows you to send
# information to a html dashboard. useful for data display
# for drivers, and also for plotting variables over time
# while debugging
self.sd = NetworkTable.getTable('SmartDashboard')
# boilerplate setup for the joystick
self.joystick = wpilib.Joystick(0)
self.gamepad = XboxController(1)
self.pressed_buttons_js = set()
self.pressed_buttons_gp = set()
self.drive_motor_a = CANTalon(2)
self.drive_motor_b = CANTalon(5)
self.drive_motor_c = CANTalon(4)
self.drive_motor_d = CANTalon(3)
self.gear_alignment_motor = CANTalon(14)
self.winch_motor = CANTalon(11)
self.winch_motor.setInverted(True)
self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=0,
reverseChannel=1)
# self.rope_lock_solenoid = wpilib.DoubleSolenoid(forwardChannel=3,
# reverseChannel=4)
self.gear_push_solenoid = wpilib.Solenoid(2)
self.gear_drop_solenoid = wpilib.Solenoid(3)
# self.gear_push_solenoid = wpilib.DoubleSolenoid(forwardChannel=1, reverseChannel=2)
# self.gear_drop_solenoid = wpilib.Solenoid(0)
self.test_trajectory = generate_trapezoidal_trajectory(
0, 0, 3, 0, Chassis.max_vel, 1, -1, Chassis.motion_profile_freq)
self.throttle = 1.0
self.direction = 1.0
self.led_dio = wpilib.DigitalOutput(1)
self.compressor = wpilib.Compressor()
def handle_shooter_inputs(
self, joystick: wpilib.Joystick, gamepad: wpilib.XboxController
) -> None:
if joystick.getTrigger():
self.shooter_controller.fire_input()
if gamepad.getBackButton() and gamepad.getRawButtonPressed(5):
# Disable turret in case of catastrophic malfunction
# Make this toggle to allow re-enabling turret in case it was accidentally disabled
self.shooter.disabled = not self.shooter.disabled
self.turret.disabled = self.shooter.disabled
def handle_hang_inputs(
self, joystick: wpilib.Joystick, gamepad: wpilib.XboxController
) -> None:
if gamepad.getStartButton() and gamepad.getBumper(GenericHID.Hand.kRightHand):
self.hang.raise_hook()
if self.hang.fire_hook and (
gamepad.getTriggerAxis(GenericHID.Hand.kLeftHand) > 0.9
or gamepad.getTriggerAxis(GenericHID.Hand.kRightHand) > 0.9
):
self.hang.winch()
def __init__(self):
self.drive_gamepad = XboxController(0)
self.op_gamepad = XboxController(1)
self._action_listeners = []
self._while_listeners = []
self.arm_toggle = LambdaDebouncer(
lambda: self.op_gamepad.getTriggerAxis(XboxController.Hand.kRight
) > 0.75)
def __init__(self, port): self._controller = XboxController(port) self._a = False self._b = False self._x = False self._y = False self._lb = False self._rb = False self._lt = False self._rt = False self._ls = False self._rs = False self._start = False self._back = False self._dpad = -1
def __init__(self):
# Create Joysticks
self._driveJoy = XboxController(0)
self._cyController = Joystick(1)
# Create Buttons
self._blastenTheHatches = JoystickButton(self._driveJoy, 2)
self._servoOpen = JoystickButton(self._driveJoy, 4)
self._servoHalf = JoystickButton(self._driveJoy, 3)
self._servoClose = JoystickButton(self._driveJoy, 1)
self._suplexButton = JoystickButton(self._driveJoy, 8)
self._backButton = JoystickButton(self._driveJoy, 7)
# Testing
self._moveMastUpButton = JoystickButton(self._cyController, 8)
self._moveMastDownButton = JoystickButton(self._cyController, 9)
# Connect Buttons to Commands
hatchEffector = Command.getRobot().hatchEffector
self._blastenTheHatches.whileHeld(hatchEffector.ParallelShoot(hatchEffector))
self._servoOpen.whileHeld(hatchEffector.ServoOpen(hatchEffector))
self._servoHalf.whileHeld(hatchEffector.ServoHalf(hatchEffector))
self._servoClose.whileHeld(hatchEffector.ServoClose(hatchEffector))
suplex = Command.getRobot().suplex
self._suplexButton.whileHeld(suplex.Smash(suplex, Flipper.FlipDirection.UP))
self._backButton.whileHeld(suplex.Smash(suplex, Flipper.FlipDirection.DOWN))
mastyBoi = Command.getRobot().mastyBoi
self._moveMastUpButton.whileHeld(mastyBoi.HoistTheColors(mastyBoi))
self._moveMastDownButton.whileHeld(mastyBoi.RetrieveTheColors(mastyBoi))
def handle_shooter_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
if joystick.getTrigger():
self.shooter_controller.fire_input()
if gamepad.getBackButton() and gamepad.getBumperPressed(
Hand.kLeftHand):
# Disable turret in case of catastrophic malfunction
# Make this toggle to allow re-enabling turret in case it was accidentally disabled
self.shooter.toggle()
self.turret.toggle()
# Hold to stay in manual aiming mode
if gamepad.getXButton():
self.shooter_controller.manual_slew(
rescale_js(gamepad.getX(Hand.kLeftHand), 0.1) *
-self.MANUAL_SLEW_SPEED)
def get_button_pressed(self, controller: wpilib.XboxController, button):
"""
Get button pressed on a given controller, but do it so that the config file can be used
param self
param controller: the wpilib.XboxController object to get button data from
param button: the desired button to check, determined from the config file
"""
return controller.getRawButtonPressed(button)
def robotInit(self):
"""Set up everything we need for a working robot."""
self.driverController = XboxController(1)
self.driveSubsystem = Drivetrain(self.driverController)
do = RunCommand(self.driveSubsystem.arcadeDrive, self.driveSubsystem)
self.driveSubsystem.setDefaultCommand(do)
def init():
"""
Initialize operator input (OI) objects.
"""
global joystick, controller, start_btn, divider
joystick = Joystick(robotmap.joystick.port)
controller = XboxController(robotmap.joystick.port)
start_btn = JoystickButton(joystick, 7)
divider = 1
def createObjects(self):
"""
Robot-wide initialization code should go here. Replaces robotInit
"""
self.map = RobotMap()
self.xboxMap = XboxMap(XboxController(1), XboxController(0))
ReadBufferValue = 18
self.MXPserial = SerialPort(115200, SerialPort.Port.kMXP, 8,
SerialPort.Parity.kParity_None, SerialPort.StopBits.kStopBits_One)
self.MXPserial.setReadBufferSize(ReadBufferValue)
self.MXPserial.setWriteBufferSize(2 * ReadBufferValue)
self.MXPserial.setWriteBufferMode(SerialPort.WriteBufferMode.kFlushOnAccess)
self.MXPserial.setTimeout(.1)
self.smartDashboardTable = NetworkTables.getTable('SmartDashboard')
self.instantiateSubsystemGroup("motors", createMotor)
self.instantiateSubsystemGroup("gyros", gyroFactory)
self.instantiateSubsystemGroup("digitalInput", breaksensorFactory)
self.instantiateSubsystemGroup("compressors", compressorFactory)
self.instantiateSubsystemGroup("solenoids", solenoidFactory)
# Check each component for compatibility
testComponentCompatibility(self, ShooterLogic)
testComponentCompatibility(self, ShooterMotorCreation)
testComponentCompatibility(self, DriveTrain)
testComponentCompatibility(self, Winch)
testComponentCompatibility(self, ButtonManager)
testComponentCompatibility(self, Pneumatics)
testComponentCompatibility(self, Elevator)
testComponentCompatibility(self, ScorpionLoader)
testComponentCompatibility(self, AutoAlign)
testComponentCompatibility(self, TestBoard)
testComponentCompatibility(self, AutoShoot)
testComponentCompatibility(self, FeederMap)
testComponentCompatibility(self, Lidar)
testComponentCompatibility(self, LoaderLogic)
testComponentCompatibility(self, GoToDist)
def createObjects(self):
"""
Robot-wide initialization code should go here. Replaces robotInit
"""
self.map = RobotMap()
self.xboxMap = XboxMap(XboxController(0), XboxController(1))
self.instantiateSubsystemGroup("motors", createMotor)
self.instantiateSubsystemGroup("gyros", gyroFactory)
self.instantiateSubsystemGroup("digitalInput", breaksensorFactory)
self.instantiateSubsystemGroup("compressors", compressorFactory)
self.instantiateSubsystemGroup("solenoids", solenoidFactory)
# Check each componet for compatibility
testComponentCompatibility(self, ShooterLogic)
testComponentCompatibility(self, ShooterMotorCreation)
testComponentCompatibility(self, DriveTrain)
testComponentCompatibility(self, Lifter)
testComponentCompatibility(self, ButtonManager)
testComponentCompatibility(self, Pneumatics)
testComponentCompatibility(self, Elevator)
testComponentCompatibility(self, ScorpionLoader)
def __init__(self):
# Create the driver's controller.
self.driverController = XboxController(constants.kDriverControllerPort)
# Create an instance of the drivetrain subsystem.
self.robotDrive = Drivetrain()
# Configure and set the button bindings for the driver's controller.
self.configureButtons()
# Set the default command for the drive subsystem. It's default command will allow
# the robot to drive with the controller.
self.robotDrive.setDefaultCommand(
RunCommand(
lambda: self.robotDrive.arcadeDrive(
-self.driverController.getRawAxis(1),
self.driverController.getRawAxis(2) * 0.65,
),
self.robotDrive,
))
def handle_intake_inputs(self, joystick: wpilib.Joystick,
gamepad: wpilib.XboxController) -> None:
if joystick.getRawButtonPressed(2):
if self.indexer.intaking:
self.indexer.disable_intaking()
self.indexer.raise_intake()
else:
self.indexer.enable_intaking()
self.indexer.lower_intake()
if gamepad.getAButton():
# Dump all balls out the intake to try to clear jam, etc
self.indexer.clearing = True
else:
# Normal operation
self.indexer.clearing = False
if gamepad.getBButton():
# Reverse only intake to clear an intake jam without losing other balls
self.indexer.intake_clearing = True
else:
self.indexer.intake_clearing = False
def scale_input_xbone_triggers(controller: wpilib.XboxController, factor: float) -> float:
"""
:param controller: The Xbox controller object.
Function to scale the input from the Xbox One controller's triggers. Right trigger returns a positive
value from 0 to 1, left trigger returns values from -1 to 0. Currently, the system works linearly.
:param factor: The factor to scale the power by.
:return: Returns a value from 0 to 1, from the inputs of the right and left bumpers.
"""
# Define the left and right hands.
left_hand = wpilib.interfaces.GenericHID.Hand.kLeftHand
right_hand = wpilib.interfaces.GenericHID.Hand.kRightHand
# The trigger value is the right bumpers value minus the left's.
trigger_value = controller.getTriggerAxis(right_hand) - (controller.getTriggerAxis(left_hand))
# If the triggers' total is less than zero, we need to take the square root of it,
# then multiply it by the factor, and then negate it.
if trigger_value < 0:
return_value = -abs(trigger_value) * factor
# Otherwise, just assign the sqrt times the factor.
else:
return_value = abs(trigger_value) * factor
return return_value
def __init__(self):
self.timer = Timer()
self.driver_station = DriverStation(controller=XboxController(0))
# self.gyroscope = AHRS.create_i2c()
# self.left_encoder = Encoder()
# self.right_encoder = Encoder()
self.drivetrain = TankDrivetrain(timer=self.timer, left_motors=[TalonSRX(10), TalonSRX(6)],
right_motors=[TalonSRX(12), TalonSRX(18)])
self.pneumatics = Pneumatics(compressor=Compressor(0), start_compressor=True, timer=self.timer)
self.beak = BeakMechanism(beak_solenoid=DoubleSolenoid(0, 4, 5), diag_solenoid=DoubleSolenoid(0, 0, 1),
driver_station=self.driver_station, timer=self.timer, cooldown=0.5)
# self.odometry = EncoderOdometry(left_encoder=self.left_encoder, right_encoder=self.right_encoder, gyro=self.gyroscope)
'''
def robotInit(self):
""" Initalizes all subsystems and user controls. """
if self.numSubsystems > 0:
# Create drive subsystems
pwmOfs: int = 0
if self.enableDrive:
leftMotors = SpeedControllerGroup(wpilib.VictorSP(0), wpilib.VictorSP(1))
rightMotors = SpeedControllerGroup(wpilib.VictorSP(2), wpilib.VictorSP(3))
gamePad: GenericHID = XboxController(0)
drive: Drive = Drive(99, leftMotors, rightMotors)
drive.setDefaultCommand(DriveArcade(drive, leftMotors, rightMotors, gamePad))
pwmOfs += 4
for i in range(0, self.numSubsystems):
pwm: int = pwmOfs + i * 2
leftMotor: SpeedController = wpilib.VictorSP(pwm)
rightMotor: SpeedController = wpilib.VictorSP(pwm + 1)
drive = Drive(i, leftMotor, rightMotor)
SmartDashboard.putData("Forward " + str(i), DrivePower(drive, 0.2, 0.2))
SmartDashboard.putData("Backward " + str(i), DrivePower(drive, -0.2, -0.2))
# Add command to dashboard to track time for one periodic pass
self.performance = Performance()
SmartDashboard.putData("Measure Performance", self.performance)
class _OI:
def __init__(self):
self.drive_gamepad = XboxController(0)
self.op_gamepad = XboxController(1)
self._action_listeners = []
self._while_listeners = []
self.arm_toggle = LambdaDebouncer(
lambda: self.op_gamepad.getTriggerAxis(XboxController.Hand.kRight
) > 0.75)
# Utility
def exec_while_condition(self, condition: Callable, cmd: Command):
self._while_listeners.append((condition, cmd))
def update(self):
for condition, action in self._action_listeners:
if condition():
action()
for condition, command in self._while_listeners:
command = typing.cast(Command, command)
cond_result = condition()
if cond_result:
if command.isRunning():
pass
else:
command.start()
else:
command.cancel()
def add_action_listener(self, condition, action):
self._action_listeners.append((condition, action))
# OpDrive
def get_left_power(self):
return -self.drive_gamepad.getY(XboxController.Hand.kLeft)
def get_right_power(self):
return -self.drive_gamepad.getY(XboxController.Hand.kRight)
def get_turn_command(self):
return -self.drive_gamepad.getX(XboxController.Hand.kRight)
def get_fine_left_turn(self):
return self.drive_gamepad.getXButton()
def get_fine_right_turn(self):
return self.drive_gamepad.getBButton()
def get_fine_forward(self):
return self.drive_gamepad.getPOV() == 0
def get_fine_backward(self):
return self.drive_gamepad.getPOV() == -180
def get_spot_turn(self):
return self.drive_gamepad.getBumper(XboxController.Hand.kLeft)
# Intake
def intake_is_active(self):
return self.op_gamepad.getBumper(XboxController.Hand.kRight)
def get_outtake_command(self):
return self.op_gamepad.getY(XboxController.Hand.kRight)
def toggle_arm(self):
return self.arm_toggle
def arm_is_open(self):
return self.op_gamepad.getBumper(XboxController.Hand.kLeft)
def rumble_op(self):
rumble_val = 1
self.op_gamepad.setRumble(XboxController.RumbleType.kLeftRumble,
rumble_val)
self.op_gamepad.setRumble(XboxController.RumbleType.kRightRumble,
rumble_val)
def unrumble_op(self):
self.op_gamepad.setRumble(XboxController.RumbleType.kLeftRumble, 0)
self.op_gamepad.setRumble(XboxController.RumbleType.kRightRumble, 0)
# Elevator
def get_elevator_manual_command(self):
return -self.op_gamepad.getY(XboxController.Hand.kLeft)
def elevator_is_manual_control(self):
return self.op_gamepad.getTriggerAxis(XboxController.Hand.kLeft) > 0.75
def elevator_move_to_top(self):
return self.op_gamepad.getPOV() == 0
def elevator_move_to_switch(self):
return self.op_gamepad.getPOV() == 90
def elevator_move_to_bottom(self):
return self.op_gamepad.getPOV() == 180
def elevator_zero(self):
return False
# Climber
def do_drop_forks(self):
return self.op_gamepad.getBackButton()
def do_climb(self):
return self.op_gamepad.getStartButton()
class XboxControllerPlus(): def __init__(self, port): self._controller = XboxController(port) self._a = False self._b = False self._x = False self._y = False self._lb = False self._rb = False self._lt = False self._rt = False self._ls = False self._rs = False self._start = False self._back = False self._dpad = -1 def getAButton(self): a = self._a b = self._controller.getAButton() self._a = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def getBButton(self): a = self._b b = self._controller.getBButton() self._b = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def getXButton(self): a = self._x b = self._controller.getXButton() self._x = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def getYButton(self): a = self._y b = self._controller.getYButton() self._y = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def getBumper(self, h): if h == kLeft: a = self._lb else: a = self._rb b = self._controller.getBumper(h) if h == kLeft: self._lb = b else: self._rb = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def getTrigger(self, h): if h == kLeft: a = self._lt else: a = self._rt b = self._controller.getTriggerAxis(h) >= 0.5 if h == kLeft: self._lt = b else: self._rt = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def getStickButton(self, h): if h == kLeft: a = self._ls else: a = self._rs b = self._controller.getStickButton(h) if h == kLeft: self._ls = b else: self._rs = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def getStartButton(self): a = self._start b = self._controller.getStartButton() self._start = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def getBackButton(self): a = self._back b = self._controller.getBackButton() self._back = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def getDpad(self, value): a = self._dpad b = self._controller.getPOV(0) self._dpad = b if b != a: if b: return PRESSED else: return RELEASED elif b: return DOWN else: return UP def setRumble(self, type1, d): self._controller.setRumble(type1, d) def getX(self, hand): return self._controller.getX(hand) def getY(self, hand): return self._controller.getY(hand)
def __init__(self, robot):
super().__init__()
self.robot = robot
self.requires(self.robot.climbmotors)
self.xbox1 = XboxController(1)
def get_button_config_held(self, controller: wpilib.XboxController,
button_name):
return controller.getRawButton(self.settings[button_name])
def get_button_pressed(self, controller: wpilib.XboxController, button):
return controller.getRawButtonPressed(button)
