def lift(self, stick: wpilib.Joystick):
if stick.getRawButton(4):
self.arm_pivot_motor.set(0.5)
elif stick.getRawButton(3):
self.arm_pivot_motor.set(-0.5)
else:
self.arm_pivot_motor.set(0)
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 Climb(self, stick: wpilib.Joystick):
if stick.getRawButton(9) == True:
self.main_lift.set(-.7)
elif stick.getRawButton(7) == True:
self.main_lift.set(.7)
else:
self.main_lift.set(0)
def BallIntake(self, stick: wpilib.Joystick):
if stick.getRawButton(1) == True:
self.intake_motor.set(.8)
elif stick.getRawButton(10) == True:
self.intake_motor.set(-.8)
else:
self.intake_motor.set(0)
class Attack3Joystick(Sensor):
"""
Sensor wrapper for the Attack 3 Joystick.
Has boolean attributes for buttons: trigger, button2-9
and double x_axis, y_axis for joystick position
"""
x_axis = y_axis = 0
trigger = button2 = button3 = \
button4 = button5 = button6 = \
button7 = button8 = button9 = \
button10 = button11 = 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))
self.x_axis = self.j.getX()
self.y_axis = self.j.getY()
class Attack3Joystick(Sensor):
"""
Sensor wrapper for the Attack 3 Joystick.
Has boolean attributes for buttons: trigger, button2-9
and double x_axis, y_axis for joystick position
"""
x_axis = y_axis = 0
trigger = button2 = button3 = \
button4 = button5 = button6 = \
button7 = button8 = button9 = \
button10 = button11 = 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))
self.x_axis = self.j.getX()
self.y_axis = self.j.getY()
def get_joystick_button(self, stick: wpilib.Joystick):
stateA = stick.getRawButton(1)
stateB = stick.getRawButton(2)
stateX = stick.getRawButton(3)
stateY = stick.getRawButton(4)
if stateA:
return "A"
elif stateB:
return "B"
elif stateX:
return "X"
elif stateY:
return "Y"
else:
return False
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 MyRobot(MagicRobot):
solenoids = Solenoids
def createObjects(self):
self.shiftBaseSolenoid = wpilib.DoubleSolenoid(
rMap.conf_shifterSolenoid1, rMap.conf_shifterSolenoid2)
self.rightFrontBaseMotor = CANTalon(rMap.conf_rightFrontBaseTalon)
self.rightRearBaseMotor = CANTalon(rMap.conf_rightRearBaseTalon)
self.leftFrontBaseMotor = CANTalon(rMap.conf_leftFrontBaseTalon)
self.leftRearBaseMotor = CANTalon(rMap.conf_leftRearBaseTalon)
self.rightFrontBaseMotor.enableControl()
self.rightRearBaseMotor.enableControl()
self.rightRearBaseMotor.setControlMode(CANTalon.ControlMode.Follower)
self.rightRearBaseMotor.set(rMap.conf_rightFrontBaseTalon)
self.leftFrontBaseMotor.enableControl()
self.leftRearBaseMotor.enableControl()
self.leftFrontBaseMotor.setInverted(True)
self.leftRearBaseMotor.setControlMode(CANTalon.ControlMode.Follower)
self.leftRearBaseMotor.set(rMap.conf_leftFrontBaseTalon)
self.leftJoy = Joystick(rMap.conf_left_joy)
self.rightJoy = Joystick(rMap.conf_right_joy)
self.xbox = Joystick(rMap.conf_xbox)
self.robotDrive = wpilib.RobotDrive(self.leftFrontBaseMotor,
self.rightFrontBaseMotor)
def autonomous(self):
"""Drive left and right motors for two seconds, then stop."""
MagicRobot.autonomous(self)
def teleopInit(self):
MagicRobot.teleopInit(self)
def teleopPeriodic(self):
if self.isSimulation():
# easier to control in simulation
self.robotDrive.arcadeDrive(self.leftJoy)
else:
self.robotDrive.tankDrive(self.leftJoy.getY(),
self.rightJoy.getY())
try:
if self.leftJoy.getRawButton(7):
self.solenoids.setShift()
print("RED " + str(self.solenoids.setShift()))
except:
self.onException()
def test(self):
'''Runs during test mode'''
wpilib.LiveWindow.run()
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 MyRobot(TimedRobot):
""" Used to test power levels to move cargo arms. """
def robotInit(self):
self.maxVolts = 10
self.joystick = Joystick(0)
self.motor: Talon = Talon(31)
self.motor.configClearPositionOnLimitF(True)
self.motor.configVoltageCompSaturation(self.maxVolts)
def getNumber(self, key: str, defVal) -> float:
val = SmartDashboard.getNumber(key, None)
if val == None:
val = defVal
SmartDashboard.putNumber(key, val)
return val
def teleopPeriodic(self):
power = 0
if self.joystick.getRawButton(4):
power = self.getNumber("Up Volts", 2.5) / self.maxVolts
elif self.joystick.getRawButton(1):
power = self.getNumber("Down Volts", -1.0) / self.maxVolts
else:
power = -self.joystick.getRawAxis(1)
if abs(power) < 0.1:
power = 0
self.motor.set(power)
def robotPeriodic(self):
SmartDashboard.putBoolean('Fwd closed',
self.motor.isFwdLimitSwitchClosed())
SmartDashboard.putBoolean('Rev closed',
self.motor.isRevLimitSwitchClosed())
SmartDashboard.putNumber('Motor position',
self.motor.getQuadraturePosition())
SmartDashboard.putNumber("Motor power",
self.motor.getMotorOutputPercent())
SmartDashboard.putNumber("Motor volts",
self.motor.getMotorOutputVoltage())
SmartDashboard.putNumber("Motor current",
self.motor.getOutputCurrent())
def ControlPosition(self, stick: wpilib.Joystick):
if stick.getRawButton(4) == True:
if self.GoalIsEvaluated == False:
self.goal = (self.arm.getSelectedSensorPosition() +
(4161 * 11))
self.GoalIsEvaluated = True
elif self.GoalIsEvaluated == True:
if self.arm.getSelectedSensorPosition() < (self.goal - 100):
self.arm.set(ctre.ControlMode.PercentOutput, .4)
if self.arm.getSelectedSensorPosition() > self.goal:
self.arm.set(ctre.ControlMode.PercentOutput, 0)
self.GoalIsEvaluated = False
wpilib.SmartDashboard.putNumber(keyName="goal", value=self.goal)
wpilib.SmartDashboard.putBoolean(keyName="GoalIsEvaluated",
value=self.GoalIsEvaluated)
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)
def isRBPressed(self, stick: wpilib.Joystick):
return stick.getRawButton(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)
def handle_hang_inputs(self, joystick: wpilib.Joystick) -> None:
if joystick.getRawButton(3) and joystick.getRawButton(4):
self.hang.raise_hook()
if self.hang.fire_hook and joystick.getRawButton(4):
self.hang.winch()
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)
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")
