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)
