def driving(self):
while True:
self.pathFollower.updateRobotPosition()
if self.superDrive.gear != self.driveGear:
self.driveGear.applyGear(self.superDrive)
if self.app is not None:
self.app.gearGroup.highlight(self.driveMode)
self.app.speedGroup.highlight(self.driveSpeed)
if self.piston1.get() == SOLENOID_REVERSE and self.driveSpeed != "slow":
self.piston1.set(SOLENOID_FORWARD)
self.piston2.set(SOLENOID_FORWARD)
elif self.piston1.get() == SOLENOID_FORWARD and self.driveSpeed == "slow":
self.piston1.set(SOLENOID_REVERSE)
self.piston2.set(SOLENOID_REVERSE)
turn = sea.deadZone(self.controller.getX(0), deadZone=0.05)
turn *= self.driveGear.turnScale
mag = -sea.deadZone(self.controller.getY(1), deadZone=0.05)
mag *= self.driveGear.moveScale
self.superDrive.drive(turn, math.pi/2, mag)
self.ledStrip.setSpeed(self.ledInput)
yield
def teleop(self):
# reset joystick buttons
self.joystick.getRawButtonPressed(3)
self.joystick.getRawButtonPressed(4)
self.joystick.getRawButtonPressed(5)
while True:
mag = sea.deadZone(self.joystick.getY())
mag *= 5 # maximum feet per second
turn = sea.deadZone(self.joystick.getX())
turn *= math.radians(300) # maximum radians per second
self.superDrive.drive(mag, math.pi / 2, turn)
if self.joystick.getRawButtonPressed(4):
print("PercentOutput mode")
for wheel in self.superDrive.wheels:
if isinstance(wheel, sea.AngledWheel):
wheel.driveMode = ctre.ControlMode.PercentOutput
if self.joystick.getRawButtonPressed(3):
print("Velocity mode")
for wheel in self.superDrive.wheels:
if isinstance(wheel, sea.AngledWheel):
wheel.driveMode = ctre.ControlMode.Velocity
if self.joystick.getRawButtonPressed(5):
print("Position mode")
for wheel in self.superDrive.wheels:
if isinstance(wheel, sea.AngledWheel):
wheel.driveMode = ctre.ControlMode.Position
yield
def teleop(self):
self.resetPositions()
if self.app is not None:
self.app.clearEvents()
# reset joystick buttons
self.joystick.getRawButtonPressed(3)
self.joystick.getRawButtonPressed(4)
self.joystick.getRawButtonPressed(5)
while True:
if self.app is not None:
self.app.doEvents()
mag = sea.deadZone(self.joystick.getMagnitude())
mag *= 3 # maximum feet per second
direction = -self.joystick.getDirectionRadians() + math.pi / 2
turn = -sea.deadZone(self.joystick.getRawAxis(3))
turn *= math.radians(120) # maximum radians per second
self.superDrive.drive(mag, direction, turn)
if self.app is not None:
moveMag, moveDir, moveTurn, self.robotOrigin = \
self.superDrive.getRobotPositionOffset(self.robotOrigin)
self.app.moveRobot(moveMag, moveDir, moveTurn)
if self.joystick.getRawButtonPressed(4):
self.setDriveMode(ctre.ControlMode.PercentOutput)
if self.joystick.getRawButtonPressed(3):
self.setDriveMode(ctre.ControlMode.Velocity)
if self.joystick.getRawButtonPressed(5):
self.setDriveMode(ctre.ControlMode.Position)
yield
def teleop(self):
while True:
mag = sea.deadZone(self.joystick.getY())
mag *= 5 # maximum feet per second
turn = sea.deadZone(self.joystick.getX())
turn *= math.radians(300) # maximum radians per second
self.drivetrain.drive(mag, math.pi / 2, turn)
yield
def getThrottlePos(self):
throttle = sea.deadZone(-self.joystick.getRawAxis(sea.TFlightHotasX.AXIS_THROTTLE))
if throttle > 0.5:
return 3
elif throttle < -0.5:
return 1
else:
return 2
def teleop(self):
if self.app is not None:
self.app.clearEvents()
# reset joystick buttons
self.joystick.getRawButtonPressed(3)
self.joystick.getRawButtonPressed(4)
self.joystick.getRawButtonPressed(5)
while True:
if self.app is not None:
self.app.doEvents()
mag = sea.deadZone(self.joystick.getMagnitude())
mag *= 3 # maximum feet per second
direction = -self.joystick.getDirectionRadians() - math.pi / 2
turn = sea.deadZone(self.joystick.getRawAxis(3))
turn *= math.radians(120) # maximum radians per second
self.superDrive.drive(mag, direction, turn)
if self.app is not None:
moveMag, moveDir, moveTurn, self.robotOrigin = \
self.superDrive.getRobotPositionOffset(self.robotOrigin)
self.app.moveRobot(moveMag, moveDir, moveTurn)
if self.joystick.getRawButtonPressed(4):
print("PercentOutput mode")
for wheel in self.superDrive.wheels:
if isinstance(wheel, sea.SwerveWheel):
wheel.angledWheel.driveMode = ctre.ControlMode.PercentOutput
if self.joystick.getRawButtonPressed(3):
print("Velocity mode")
for wheel in self.superDrive.wheels:
if isinstance(wheel, sea.SwerveWheel):
wheel.angledWheel.driveMode = ctre.ControlMode.Velocity
if self.joystick.getRawButtonPressed(5):
print("Position mode")
for wheel in self.superDrive.wheels:
if isinstance(wheel, sea.SwerveWheel):
wheel.angledWheel.driveMode = ctre.ControlMode.Position
yield
def teleop(self):
angularOffset = 0
self.resetPositions()
if self.app is not None:
self.app.clearEvents()
# reset joystick buttons
self.joystick.getRawButtonPressed(3)
self.joystick.getRawButtonPressed(4)
self.joystick.getRawButtonPressed(5)
while True:
if self.app is not None:
self.app.doEvents()
self.pathFollower.updateRobotPosition()
angularOffset = 0
if not (sea.deadZone(-self.joystick.getRawAxis(4), 0.3) == 0 and \
sea.deadZone(-self.joystick.getRawAxis(3), 0.3) == 0):
angularOffset = sea.circleDistance(self.pathFollower.robotAngle, \
math.atan2(sea.deadZone(-self.joystick.getRawAxis(4), 0.3), \
sea.deadZone(-self.joystick.getRawAxis(3), 0.3)))
mag = sea.deadZone(self.joystick.getMagnitude())
mag *= 7 # maximum feet per second
direction = -self.joystick.getDirectionRadians() + math.pi / 2
direction -= self.pathFollower.robotAngle # field oriented mode
turn = angularOffset * math.radians(
270) # maximum radians per second
self.superDrive.drive(mag, direction, turn)
if self.app is not None:
moveMag, moveDir, moveTurn, self.robotOrigin = \
self.superDrive.getRobotPositionOffset(self.robotOrigin)
self.app.moveRobot(moveMag, moveDir, moveTurn)
if self.joystick.getRawButtonPressed(4):
self.setDriveMode(ctre.ControlMode.PercentOutput)
if self.joystick.getRawButtonPressed(3):
self.setDriveMode(ctre.ControlMode.Velocity)
if self.joystick.getRawButtonPressed(5):
self.setDriveMode(ctre.ControlMode.Position)
yield
def manualDriving(self):
self.manualMediumGear()
self.resetPositions()
alignAngle = None
# clear joystick events
for i in range(1,13):
self.joystick.getRawButtonPressed(i)
self.joystick.getRawButtonReleased(i)
while True:
# DRIVING
self.pathFollower.updateRobotPosition()
if self.joystick.getRawButtonPressed(4):
yield sea.AddParallelSignal(self.manualVisionAlign())
x = sea.deadZone(self.joystick.getX())
y = sea.deadZone(self.joystick.getY())
mag = math.hypot(x * (1 - 0.5*y**2) ** 0.5,y * (1 - 0.5*x**2) ** 0.5)
mag *= self.manualGear.moveScale
direction = -self.joystick.getDirectionRadians() + math.pi/2
if self.fieldOriented and not self.joystick.getRawButton(4):
direction -= self.pathFollower.robotAngle + math.pi/2
turn = -sea.deadZone(self.joystick.getRawAxis(sea.TFlightHotasX.AXIS_TWIST)) \
- 0.5 * sea.deadZone(self.joystick.getRawAxis(sea.TFlightHotasX.AXIS_LEVER))
if turn != 0:
alignAngle = None
turn *= self.manualGear.turnScale # maximum radians per second
if not self.joystick.getPOV() == -1:
pov = self.joystick.getPOV()
if pov == 45:
pov = 30
elif pov == 135:
pov = 150
elif pov == 225:
pov = 210
elif pov == 315:
pov = 330
alignAngle = -math.radians(pov) - math.pi / 2
if alignAngle is not None:
aDiff = sea.circleDistance(alignAngle, self.pathFollower.robotAngle)
turn = sea.feedbackLoopScale(-aDiff, 25, 2, drivetrain.mediumPositionGear.turnScale)
if not self.holdGear:
self.manualGear.applyGear(self.superDrive)
if self.buttonBoard.getRawButton(1):
alignAngle = None
self.superDrive.disable()
else:
self.multiDrive.drive(mag, direction, turn)
self.multiDrive.update()
yield
def getTurn(self):
return sea.deadZone(self.driverController.getX(self.CONTROLLER_LEFT),
deadZone=0.05)
def getMagnitude(self) -> float:
return -sea.deadZone(self.driverController.getY(self.CONTROLLER_LEFT),
deadZone=0.05)
def getTurn(self) -> float:
return -sea.deadZone(self.driverController.getX(self.CONTROLLER_RIGHT),
deadZone=0.05)
