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)