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 execute(self):
if self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.WPI_MECANUM:
self._spootnikDrives.driveCartesianWithJoy()
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.MORPHEUS:
self._spootnikDrives.morpheusDrive()
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.FIELD_ORIENTED:
self._spootnikDrives.fieldOrientedDrive()
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.NO_JOY:
self._spootnikDrives.noJoyDrive()
def _printDriveType(self):
if self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.WPI_MECANUM:
self.logger.info("WPI Mecanum Drive selected")
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.MORPHEUS:
self.logger.info("Morpheus Drive selected")
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.FIELD_ORIENTED:
self.logger.info("Field Oriented Drive selected")
elif self._driveSwitcherVal == Command.getRobot(
).DriveSwitcher.NO_JOY:
self.logger.info("No Joy Drive selected")
def execute(self):
left_trigger = Command.getRobot().oi.driveJoy.getTriggerAxis(
GenericHID.Hand.kLeft)
right_trigger = Command.getRobot().oi.driveJoy.getTriggerAxis(
GenericHID.Hand.kRight)
left_trigger = self.apply_deadband(left_trigger)
right_trigger = self.apply_deadband(right_trigger)
# self.logger.info("Left trigger: {}".format(left_trigger))
# self.logger.info("Right trigger: {}".format(right_trigger))
if left_trigger > 0:
self._cargo_effector.cargo_motor.set(left_trigger)
else:
self._cargo_effector.cargo_motor.set(-right_trigger)
def __init__(self, left, right, timeoutInSeconds):
super().__init__('Set Speed %d and rotation %d' % (left, right),
timeoutInSeconds)
self.left = left
self.right = right
self.requires(Command.getRobot().drivetrain)
print("Created timed command")
def __init__(self, spootnikDrives):
super().__init__("DriveWithJoy")
self.logger = logging.getLogger("DriveWithJoy")
self.requires(spootnikDrives)
self._spootnikDrives = spootnikDrives
self._driveSwitcherVal = Command.getRobot(
).driveSwitcher.getSelected()
self._printDriveType()
def __init__(self):
super().__init__(subsystem=Command.getRobot().drivetrain)
def execute(self):
joyX = Command.getRobot().oi.driveStick.getX()
joyY = Command.getRobot().oi.driveStick.getY()
Command.getRobot().drivetrain.customArcadeDrive(joyX,
-joyY,
squareInputs=True)
def initialize(self):
Command.getRobot().drivetrain.setPIDSlot(1)
def __init__(self, drivetrain: Drivetrain):
"""
x: (left position (m), left velocity (m/s), right position (m), right velocity (m/s).T
y: (left position (m), right position (m)).T
:param drivetrain:
"""
from numpy import array as matrix
derpymodel = True
if derpymodel:
self.u_offset = matrix([[1.29], [1.32]])
A = matrix([[1., 0.01954524, 0., 0.], [0., 0.95487116, 0., 0.],
[0., 0., 1., 0.01953105], [0., 0., 0., 0.95347481]])
B = matrix([[0.00016013, 0.], [0.01589044, 0.], [0., 0.00016005],
[0., 0.01587891]])
C = matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
D = matrix([[0., 0.], [0., 0.]])
K = matrix([[
8.70337746e+01, 1.54364232e+01, 1.64506224e-13, 6.89196592e-15
], [
-2.48568085e-14, 5.01719083e-15, 8.70882843e+01, 1.53690171e+01
]])
Kff = matrix([[778.96386513, 1.93253647, 0., 0.],
[0., 0., 778.71561076, 1.93145314]])
L = matrix([[1.28456071e+00, 4.48179733e-17],
[1.39021836e+01, 2.18957546e-15],
[-3.61345038e-17, 1.28327788e+00],
[-1.76400902e-15, 1.38293101e+01]])
Ainv = matrix([[1., -0.02046898, 0., 0.], [0., 1.04726171, 0., 0.],
[0., 0., 1., -0.02048408], [0., 0., 0.,
1.04879541]])
else:
A = matrix([[1., 0.01631643, 0., 0.00238268],
[0., 0.66737266, 0., 0.20542149],
[0., 0.00238268, 1., 0.01631643],
[0., 0.20542149, 0., 0.66737266]])
B = matrix([[0.00123499, -0.00079884], [0.11152021, -0.06887181],
[-0.00079884, 0.00123499], [-0.06887181, 0.11152021]])
C = matrix([[1, 0, 0, 0], [0, 0, 1, 0]])
D = matrix([[0, 0], [0, 0]])
K = matrix([[56.82612189, 6.03969859, 18.60045886, 3.51953103],
[18.60045886, 3.51953103, 56.82612189, 6.03969859]])
Kff = matrix([[731.26285269, 1.74253981, 334.3110478, 0.86234499],
[334.3110478, 0.86234499, 731.26285269, 1.74253981]])
L = matrix([[1.12809912, 0.08747579], [5.70458841, 4.3576656],
[0.08747579, 1.12809912], [4.3576656, 5.70458841]])
Ainv = matrix([[1., -0.02579361, 0., 0.0043692],
[0., 1.65523823, 0., -0.5094927],
[0., 0.0043692, 1., -0.02579361],
[0., -0.5094927, 0., 1.65523823]])
super().__init__(A=A, Ainv=Ainv, B=B, C=C, D=D, K=K, Kff=Kff, L=L)
self.drivetrain = drivetrain
self.period = Command.getRobot().getPeriod()
self._at_reference = False
def __init__(self, fnom):
_CsvTrajectoryCommand.__init__(self, fnom)
StateSpaceDriveController.__init__(self, Command.getRobot().drivetrain)
self.u_min = -8
self.u_max = 8
def isFinished(self):
return abs(Command.getRobot().liftElevator.getLiftPos() - self.targetPos) < 1
def initialize(self):
Command.getRobot().lift.setLiftReference(self.targetPos)
def driveCartesianWithJoy(self):
joy = Command.getRobot().oi.driveJoy
ySpeed = joy.getX(GenericHID.Hand.kLeft)
xSpeed = -joy.getY(GenericHID.Hand.kLeft)
zRotation = joy.getX(GenericHID.Hand.kRight)
self.drive.driveCartesian(ySpeed, xSpeed, zRotation)
def __init__(self):
super().__init__("Drive to BaseLine")
spootnikDrives = Command.getRobot().spootnikDrives
self.addSequential(spootnikDrives.DriveForTime(spootnikDrives, 2.0))
def initialize(self):
Command.getRobot().liftElevator.setElevatorReference(self.targetPos)
def end(self):
#print("done")
Command.getRobot().drivetrain.moveTank(0, 0)
def execute(self):
#it seems like rot / magnitude is reversed, is this a bug in the robot lib sim?
Command.getRobot().drivetrain.moveTank(self.left, -self.right)