def draw_2point_arc(self, point1, point2, radius, direction):
"""Draw an arc given two points and a radius."""
if point1 is None:
point1 = self.cart
Display.debug_circle(point1, radius)
Display.debug_circle(point2, radius)
if point1 == point2:
if direction == CW:
center_point = Cartesian(point1.x + radius, point1.y)
start_angle = Radians(math.pi)
else:
center_point = Cartesian(point1.x - radius, point1.y)
start_angle = Radians(0.0)
end_angle = Radians(start_angle + math.tau)
Display.debug_point(center_point, 'purple')
else:
center_point = arc_center(point1, point2, radius, direction)
Display.debug_point(center_point, 'blue')
start_angle = calculate_angle(center_point, point1) % math.tau
# TODO: This needs direct to choose end angle.
end_angle = calculate_angle(center_point, point2) % math.tau
self.draw_multi_point_arc(point1, point2, center_point, radius,
start_angle, end_angle)
def __init__(self, shape, translation=None, rotation=None):
self.shape = shape
if translation is None:
translation = Cartesian(0.0, 0.0)
self.translation = translation
if rotation is None:
rotation = Radians(0.0)
self.rotation = rotation
self.pivot = Cartesian(0.0, 0.0)
def __init__(self):
"""Set the HUD elements."""
if Pen.canvas is None:
Pen.canvas = turtle_global.getscreen().getcanvas()
self._transform = Cartesian(0.866666666, -0.866666666).pixels()
self.pen_down = False
self.color = 'black'
self._internal_position = Cartesian(0.0, 0.0)
def __init__(self, controller):
"""Init."""
self.c = controller
self._previous_cart = Cartesian(0.0, 0.0)
self._cart = Cartesian(0.0, 0.0)
self.cart_error = Cartesian(0.0, 0.0)
self.grid_size = CartesianResolution(Millimeters(0.01),
Millimeters(0.01))
self.offset_to_origin = Cartesian(0.0, 0.0)
def __init__(self, controller):
"""Init."""
self.display = Display()
self.c = controller
self.polar = Polar(0.0, 0.0)
self.polar_error = Polar(0.0, 0.0)
self.cart = Cartesian(0.0, 0.0)
self.cart_error = Cartesian(0.0, 0.0)
self.grid_size = self.c.grid_size
self.direction = CCW
def draw_polar_line(self, *args):
"""Draw line between absolute points using polor interpolation."""
if len(args) == 4:
start_point = Cartesian(*args[:2])
end_point = Cartesian(*args[2:])
elif len(args) == 2:
start_point, end_point = args
else:
raise RuntimeError('Incorrect arguments.')
print 'draw_polar_line {} => {}'.format(start_point, end_point)
# turtle.up()
self.set_controller_pos(start_point)
# turtle.down()
self.set_controller_pos(end_point)
def draw_polygon(self, center_point, radius, edge_count, start_angle=None):
"""Draw a polygon."""
if start_angle is None:
# Set the starting angle for the circle to be close to our current location.
if center_point == Cartesian(0.0, 0.0):
start_angle = 0.0
else:
base = center_point - self.cart
start_angle = Cartesian(1.0, 0.0).angle(base)
if self.cart.x < center_point.x or self.cart.y < center_point.y:
start_angle += math.pi
print 'draw_polygon centered on {} radius {} edges {}'.format(
center_point, radius, edge_count)
end_angle = Radians(start_angle + (math.tau))
direction = CCW
return self.draw_polygon_arc(center_point, radius, edge_count,
start_angle, end_angle, direction)
def draw_circle(self, center_point, radius):
"""Draw a circle with the given center and radius."""
print 'draw_circle centered on {} radius {}'.format(
center_point, radius)
circumference = float(radius) * math.tau
edges_per_mm = 0.5
edge_count = circumference * float(edges_per_mm)
edge_count = int(math.ceil(edge_count))
# Set the starting angle for the circle to be close to our current location.
if center_point == Cartesian(0.0, 0.0):
start_angle = 0.0
else:
base = center_point - self.cart
start_angle = Cartesian(1.0, 0.0).angle(base)
if self.cart.x < center_point.x or self.cart.y < center_point.y:
start_angle += math.pi
self.draw_polygon(center_point, radius, edge_count, start_angle)
def move_fast(self, delta):
"""Set our position using relative cartesian coordinates."""
if delta == Cartesian(0, 0):
return
current = self.cart_error + delta
whole = Steps(current // self.grid_size, cartesian=True)
current = current % self.grid_size
self.cart = self.cart + (self.grid_size * whole)
self.set_controller_pos(self.cart)
self.cart_error = current
def koru(self):
"""A constant spiral."""
turtle.up()
start = Polar(0.0, 0.0)
self.setpos(start)
print 'koru'
koru = Transform(Koru(), Cartesian(0.0, 150.0))
turtle.down()
for step in koru:
self.setpos(step)
turtle.up()
def arc_center(p1, p2, radius, direction):
"""Return arc center based on start and end points, radius and arc direction (2=CW, 3=CCW).
radius can be negative (for arcs over 180 degrees)
via https://math.stackexchange.com/a/482049
"""
angle = 0.0
additional_angle = 0.0
l1 = 0.0
l2 = 0.0
diff = 0.0
allowed_error = 0.002
t1 = None
t2 = None
if direction == CW:
t1 = p2
t2 = p1
else:
t1 = p1
t2 = p2
# find angle arc covers
angle = calculate_angle(t1, t2)
l1 = point_distance(t1, t2) / 2.0
diff = l1 - abs(radius)
if abs(radius) < l1 and diff > allowed_error:
raise RuntimeError('Wrong radius.')
l2 = math.sqrt(abs(radius ** 2 - l1 ** 2))
if l1 == 0:
additional_angle = math.pi / 2.0
else:
additional_angle = math.atan(l2 / l1) # atan2???
# Add or subtract from angle (depending of radius sign)
if radius < 0:
angle -= additional_angle
else:
angle += additional_angle
# calculate center (from t1)
x = float(t1.x + abs(radius) * math.cos(angle))
y = float(t1.y + abs(radius) * math.sin(angle))
return Cartesian(x, y)
def move(self, delta):
"""Set our position using relative cartesian coordinates."""
if delta == Cartesian(0, 0):
return
steps_number = delta.max_abs_value()
delta_step = delta / steps_number
current = self.cart_error
for r in range(int(steps_number) + 1):
current += delta_step
whole = Steps(current // self.grid_size, cartesian=True)
current = current % self.grid_size
# print '\t', r + 1, 'in', int(steps_number) + 1, whole
self.cart = self.cart + (self.grid_size * whole)
self.set_controller_pos(self.cart)
self.cart_error = current
def __init__(self, radius, speed=1.0):
"""Init.
radius of the cog
speed multiplies the turning rate relative to the parent.
"""
self.radius = Millimeters(radius)
self.offset = None
self.children = list()
self.track = Polar(0.0, 0.0)
self.theta = Radians(0.0)
self.speed_boost = speed
self.base_speed = 1.0
self.center = Cartesian(0.0, 0.0)
self.direction = 1.0
self.parent_radius = None
self.id = uuid.uuid4()
class Display(object):
"""Abstraction that converts our polar motor signals to pixels on the screen."""
c = DisplayPosition(0, 0)
p = Polar(0, 0)
c_pixels = DisplayPosition(0, 0)
c_pixels_whole = DisplayPosition(0, 0)
offset = Cartesian(0, 0)
hud = None
canvas = None
debug_enabled = True
debug_pen = None
class HUD(threading.Thread):
"""HUD manager."""
def __init__(self):
"""Setup the HUD elements."""
super(Display.HUD, self).__init__()
self.canvas = turtle.screen.getcanvas()
self.hud_lines = dict()
self.queues = dict()
self.next_pos = TkSpace(-102.0, -98.0)
self.stop = False
self.daemon = True
def __setitem__(self, queue_name, value):
"""Set text on HUD item.
Use a queue to make the value avaiable when we refresh the HUD.
Passing new keys into here will create new HUD lines.
"""
if queue_name not in self.queues:
self.queues[queue_name] = LifoQueue()
if queue_name not in self.hud_lines:
self.hud_lines[queue_name] = self.canvas.create_text(*self.next_pos, text='', fill='grey', anchor=tk.SW)
self.next_pos.y += 5.0
try:
while True:
self.queues[queue_name].get_nowait()
except Empty:
pass
self.queues[queue_name].put_nowait(value)
def run(self):
"""Update the HUD text."""
while not self.stop:
for queue_name, q in self.queues.iteritems():
try:
self.canvas.itemconfig(self.hud_lines[queue_name], text=q.get_nowait())
except Empty:
pass
time.sleep(0.5)
def __init__(self):
"""Set the HUD elements."""
if Display.hud is None:
Display.hud = Display.HUD()
Display.hud.start()
if Display.canvas is None:
Display.canvas = turtle.screen.getcanvas()
Display.debug_pen = Pen()
self.pen = Pen()
def setpos(self, polar):
"""Set the position of of the polar coordinates to pixels on the screen."""
self.p = polar
self.c = self.p.cartesian()
new_pixel_position = self.c.pixels().trunc(factor=10)
if self.c_pixels != new_pixel_position:
# a = new_pixel_position * 0.86666666666666666666666
# self.pen.down()
# self.pen.setpos(self.c)
# self.pen.up()
# TODO: What is going on here?!?
# Some how my converion from mm to pixels that works everywhere else doesn't here.
turtle.setpos(new_pixel_position * 0.86666666666666666 * 0.86666666666666666666666)
if not FAST:
turtle.setheading(polar.t.degrees())
self.c_pixels = new_pixel_position
# Post values to the HUD
self.hud['hud_polar'] = self.p
self.hud['hud_cartesian'] = self.c
@classmethod
def set_offset(cls, offset):
"""Apply an offset to align with the real drawings."""
cls.offset = offset
@classmethod
def debug_point(cls, point, color):
"""Draw a colored dot."""
if cls.debug_enabled:
pen = Display.debug_pen
pen.color = color
if cls.offset:
point = point + cls.offset
pen.setpos(point)
pen.down()
pen.dot(5, color)
pen.up()
@classmethod
def debug_line(cls, start_point, end_point, color='grey'):
"""Draw a line."""
if cls.debug_enabled:
pen = Display.debug_pen
if cls.offset:
start_point = start_point + cls.offset
end_point = end_point + cls.offset
pen.down()
pen.line(start_point, end_point)
pen.up()
@classmethod
def debug_circle(cls, center_point, radius, color='grey'):
"""Draw a line."""
if cls.debug_enabled:
pen = Display.debug_pen
if cls.offset:
center_point = center_point + cls.offset
pen.down()
pen.circle(center_point, radius, color)
pen.up()
@classmethod
def debug_text(cls, point, text):
"""Write some text."""
pass
# if cls.debug_enabled:
# with DebugPen('grey', 1) as pen:
# pen.setpos(point.pixels() * 0.86666666666666666 * 0.86666666666666666)
# pen.write(text)
@classmethod
def debug_clear(cls):
"""Clear all debug drawing."""
if cls.debug_enabled:
debug_pen.clear()