def shoot(self, stick: wpilib.Joystick): if stick.getPOV() == 180: self.left_shooter.set(ctre.ControlMode.PercentOutput, -0.6) #rocket bajo self.right_shooter.set(ctre.ControlMode.PercentOutput, -0.6) self.intake_motor.set(-0.6) elif stick.getPOV() == 0: self.left_shooter.set(ctre.ControlMode.PercentOutput, -0.5) #rocket bajo self.right_shooter.set(ctre.ControlMode.PercentOutput, -0.5) self.intake_motor.set(-0.6) elif stick.getRawAxis(2) > 0.50: #intake main self.intake_motor.set(-0.8) self.left_shooter.set(ctre.ControlMode.PercentOutput, -0.3) self.right_shooter.set(ctre.ControlMode.PercentOutput, -0.3) elif stick.getRawButton(5) or stick.getRawButton(6): #regresar pelota self.intake_motor.set(0.4) self.left_shooter.set(ctre.ControlMode.PercentOutput, 0.4) self.right_shooter.set(ctre.ControlMode.PercentOutput, 0.4) else: #nada presionado self.left_shooter.set(ctre.ControlMode.PercentOutput, 0) self.right_shooter.set(ctre.ControlMode.PercentOutput, 0) self.intake_motor.set(0)
def drive(self, stick: wpilib.Joystick): if self.isRBPressed(stick) and not self.changeFrontButtonPressed: self.frontStatus = not self.frontStatus self.changeFront() self.changeFrontButtonPressed = True elif not self.isRBPressed(stick) and self.changeFrontButtonPressed: self.changeFrontButtonPressed = False """ if self.get_joystick_button(stick) in ("X", "B"): if not self.rotation_started: self.reset_angle_pid() self.rotation_started = True if self.get_joystick_button(stick) == "X": self.target_angle = self.navx.getAngle() - 90 else: self.target_angle = self.navx.getAngle() + 90 self.rotate_to_angle(stick) self.reset_distance_pid() """ if self.get_joystick_button(stick) in ("Y", "A"): if not self.distance_pid_started: self.distance_pid_started = True self.right_motor_offset = self.right_motor.getQuadraturePosition() self.left_motor_offset = self.left_motor.getQuadraturePosition() if self.get_joystick_button(stick) == "Y": right_target_encoder_ticks = self.right_motor_offset - ((63-21) * 85.551) left_target_encoder_ticks = self.left_motor_offset + ((63-21) * 85.551) else: right_target_encoder_ticks = self.right_motor_offset - ((130-21) * 85.551) left_target_encoder_ticks = self.left_motor_offset + ((130-21) * 85.551) if self.right_motor.getQuadraturePosition() < right_target_encoder_ticks: self.right_motor.set(ctre.ControlMode.PercentOutput, 0) self.left_motor.set(ctre.ControlMode.PercentOutput, 0) return else: self.right_motor.set(ctre.ControlMode.Velocity, -2400) if self.left_motor.getQuadraturePosition() > left_target_encoder_ticks: self.right_motor.set(ctre.ControlMode.PercentOutput, 0) self.left_motor.set(ctre.ControlMode.PercentOutput, 0) return else: self.left_motor.set(ctre.ControlMode.Velocity, -2400) elif stick.getPOV() != -1: self.drive_with_pad(stick) self.reset_angle_pid() self.reset_distance_pid() else: self.drive_with_joystick(stick) self.reset_angle_pid() self.reset_distance_pid()
def drive_with_pad(self, stick: wpilib.Joystick): trigger = self.get_trigger(stick) dpad = stick.getPOV() wpilib.SmartDashboard.putNumber("dpad", dpad) left_power = self.get_left_motor(dpad, trigger) right_motor = self.get_right_motor(dpad, trigger) self.set_motors(left_power, -right_motor)
def shoot(self, stick: wpilib.Joystick): if stick.getRawAxis(3) > 0.50: self.left_shooter.set(ctre.ControlMode.Velocity, 6000) #rocket bajo self.right_shooter.set(ctre.ControlMode.Velocity, 6000) if self.left_shooter.getQuadratureVelocity( ) >= 5700 and self.right_shooter.getQuadratureVelocity() <= -5700: self.intake_motor.set(-1) elif stick.getPOV() == 0: self.left_shooter.set(ctre.ControlMode.Velocity, 6500) #rocket bajo self.right_shooter.set(ctre.ControlMode.Velocity, 6500) if self.left_shooter.getQuadratureVelocity( ) >= 6200 and self.right_shooter.getQuadratureVelocity() <= -6200: self.intake_motor.set(-1) elif stick.getPOV() == 180: self.left_shooter.set(ctre.ControlMode.Velocity, 5500) #rocket bajo self.right_shooter.set(ctre.ControlMode.Velocity, 5500) if self.left_shooter.getQuadratureVelocity( ) >= 5200 and self.right_shooter.getQuadratureVelocity() <= -5200: self.intake_motor.set(-1) elif stick.getRawAxis(2) > 0.50: #intake main self.intake_motor.set(-1) self.left_shooter.set(ctre.ControlMode.PercentOutput, -0.3) self.right_shooter.set(ctre.ControlMode.PercentOutput, -0.3) elif stick.getRawButton(5) or stick.getRawButton(6): #regresar pelota self.intake_motor.set(0.5) self.left_shooter.set(ctre.ControlMode.PercentOutput, -0.3) self.right_shooter.set(ctre.ControlMode.PercentOutput, -0.3) else: #nada presionado self.left_shooter.set(ctre.ControlMode.PercentOutput, 0) self.right_shooter.set(ctre.ControlMode.PercentOutput, 0) self.intake_motor.set(0)
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() self.hid = GenericHID(port) self.hid.setOutputs(port) 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""" self.hid.setRumble(self.hid.RumbleType.kLeftRumble, left) self.hid.setRumble(self.hid.RumbleType.kRightRumble, right)
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 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)