def execute(self):
reset = True
for i in range(4, 8):
if not oi.OI().drive_buttons[i].get():
reset = False
break
if reset:
self.odemetry.reset()
x_speed = math.pow(oi.OI().driver.getY(),
Constants.TANK_DRIVE_EXPONENT)
y_speed = math.pow(oi.OI().driver.getX(),
Constants.TANK_DRIVE_EXPONENT)
rotation = math.pow(oi.OI().driver.getZ(),
Constants.TANK_DRIVE_EXPONENT)
if self.allocentric:
speed = vector2d.Vector2D(
x_speed, y_speed).getRotated(-self.odemetry.getAngle())
x_speed, y_speed = speed.getValues()
# self.drive.setDirectionOutput(x_speed, y_speed, rotation)
epsilon = 1e-3
if Constants.TANK_PERCENT_OUTPUT:
self.drive.setDirectionOutput(x_speed, y_speed, rotation)
else:
if abs(x_speed) <= epsilon and abs(y_speed) <= epsilon and abs(
rotation) <= epsilon:
self.drive.setPercentOutput(0, 0, 0, 0)
else:
self.drive.setDirectionVelocity(x_speed * self.multiplier,
y_speed * self.multiplier,
rotation * self.multiplier)
def __init__(self, path):
self.path = path
self.pursuit_points = [
pursuitpoint.PursuitPoint(p, c)
for p, c in zip(self.path.getPoints(), self.path.getCurvatures())
]
self.last_lookahead_index = 0
self.cur_curvature = 0
self.target_velocities = vector2d.Vector2D()
self.closest_point_index = 0
def execute(self):
x_speed = math.pow(oi.OI().driver.getY(),
Constants.TANK_DRIVE_EXPONENT)
y_speed = math.pow(oi.OI().driver.getX(),
Constants.TANK_DRIVE_EXPONENT)
rotation = math.pow(oi.OI().driver.getZ(),
Constants.TANK_DRIVE_EXPONENT)
if self.allocentric:
speed = vector2d.Vector2D(
x_speed, y_speed).getRotated(-self.odemetry.getAngle())
x_speed, y_speed = speed.getValues()
self.drive.setDirectionOutput(x_speed, y_speed, rotation)
def updateTargetVelocities(self, state):
"""Update the target velocities of the left and right wheels."""
robot_velocity = self.pursuit_points[self.closest_point_index].velocity
# Use kinematics (http://robotsforroboticists.com/drive-kinematics/) and algebra to find wheel target velocties
l_velocity = robot_velocity * \
(2 + self.cur_curvature * Constants.TRACK_WIDTH) / \
2 / Constants.PURE_PURSUIT_KV
r_velocity = robot_velocity * \
(2 - self.cur_curvature * Constants.TRACK_WIDTH) / \
2 / Constants.PURE_PURSUIT_KV
scale = max(abs(l_velocity), abs(r_velocity))
if scale > 1:
l_velocity /= scale
r_velocity /= scale
self.target_velocities = vector2d.Vector2D(l_velocity, r_velocity)
def execute(self):
x_speed = math.pow(oi.OI().driver.getY(),
Constants.TANK_DRIVE_EXPONENT)
y_speed = math.pow(oi.OI().driver.getX(),
Constants.TANK_DRIVE_EXPONENT)
rotation = math.pow(oi.OI().driver.getZ(),
Constants.TANK_DRIVE_ROTATION_EXPONENT)
if self.allocentric:
speed = vector2d.Vector2D(
x_speed, y_speed).getRotated(-self.odemetry.getAngle())
x_speed, y_speed = speed.getValues()
if Constants.TANK_PERCENT_OUTPUT:
self.drive.setDirectionOutput(x_speed, y_speed, rotation)
else:
self.drive.setDirectionVelocity(
x_speed*Constants.TANK_VELOCITY_MAX, y_speed*Constants.TANK_VELOCITY_MAX, rotation*Constants.TANK_VELOCITY_MAX)
def __init__(self, x=0.0, y=0.0, angle=0.0, pos=None):
if pos == None:
self.pos = vector2d.Vector2D(x, y)
else:
self.pos = pos
self.angle = angle
def interpolate2ndDerivative(self, t):
"""Interpolate a 2nd derivative along the generated curve where 0 <= t <= 1."""
ddx = (6 * self.ax * t) + (2 * self.bx)
ddy = (6 * self.ay * t) + (2 * self.by)
return vector2d.Vector2D(ddx, ddy)
def interpolateDerivative(self, t):
"""Interpolate a derivative along the generated curve where 0 <= t <= 1."""
dy = (3 * self.ay * t**2) + (2 * self.by * t) + (self.cy)
dx = (3 * self.ax * t**2) + (2 * self.bx * t) + (self.cx)
return vector2d.Vector2D(dx, dy)
def interpolatePoint(self, t):
"""Interpolate a point along the generated curve where 0 <= t <= 1."""
y = (self.ay * t**3) + (self.by * t**2) + (self.cy * t) + (self.dy)
x = (self.ax * t**3) + (self.bx * t**2) + (self.cx * t) + (self.dx)
return vector2d.Vector2D(x, y)