def nearestBoxCoord(coord):
x = coord.x
y = coord.y
insideX = False
if x > WAYPOINT_BOX_X:
x = WAYPOINT_BOX_X
elif x < -WAYPOINT_BOX_X:
x = -WAYPOINT_BOX_X
else:
insideX = True
insideY = False
if y > WAYPOINT_BOX_Y:
y = WAYPOINT_BOX_Y
elif y < -WAYPOINT_BOX_Y:
y = -WAYPOINT_BOX_Y
else:
insideY = True
if insideX and insideY:
if x > 0:
xClosest = WAYPOINT_BOX_X
else:
xClosest = -WAYPOINT_BOX_X
if y > 0:
yClosest = WAYPOINT_BOX_Y
else:
yClosest = -WAYPOINT_BOX_Y
xDist = abs(x - xClosest)
yDist = abs(y - yClosest)
if xDist < yDist:
x = xClosest
else:
y = yClosest
nearest90DegreeAngle = coord.orientation + sea.circleDistance(
coord.orientation, 0, math.pi / 2)
return DriveCoordinate("Box point", x, y, nearest90DegreeAngle)
def driveToPoint(pathFollower, vision, coord, speed):
drivetrain.autoPositionGear.applyGear(pathFollower.drive)
visionIntermediateCoord = coordinates.getVisionAlignIntermediatePoint(
coord, pathFollower.robotX, pathFollower.robotY)
if visionIntermediateCoord is not None:
finalCoord = coord
coord = visionIntermediateCoord
angle = sea.circleDistance(pathFollower.robotAngle,
coord.orientation) + pathFollower.robotAngle
dist = math.hypot(coord.x - pathFollower.robotX,
coord.y - pathFollower.robotY)
if dist < 0.1:
time = 1
else:
time = dist / speed
yield from sea.ensureTrue(
pathFollower.driveToPointGenerator(coord.x, coord.y, angle, time, 0.2,
math.radians(2)), 25)
# approach vision target
if visionIntermediateCoord is not None:
distanceToTarget = math.hypot(coord.x - finalCoord.x,
coord.y - finalCoord.y)
yield from auto_vision.driveIntoVisionTargetOrGiveUpAndDriveForward(
pathFollower.drive, vision, pathFollower.drive,
distanceToTarget + VISION_APPROACH_MARGIN)
pathFollower.setPosition(finalCoord.x, finalCoord.y, None)
def _drive(self, magnitude, direction):
self._updateMotorPosition()
currentAngle = self._simulatedCurrentDirection
# steering should never rotate more than 90 degrees from any position
angleDiff = sea.circleDistance(currentAngle, direction, math.pi)
target = currentAngle + angleDiff
#print(math.degrees(currentAngle), math.degrees(target))
self._setSteering(target)
self.angledWheel.angle = currentAngle
self.angledWheel.x = self.x + math.cos(currentAngle) * self.offsetX - math.sin(currentAngle) * self.offsetY
self.angledWheel.y = self.y + math.sin(currentAngle) * self.offsetX + math.cos(currentAngle) * self.offsetY
self.angledWheel.drive(magnitude, direction)
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 clockwise(a, b):
return sea.circleDistance(a, b) < 0