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)