def right(self, angle):
"""
Turn right by \a angle degrees
"""
self.angle += math.pi / 180 * angle
self.angle %= 2 * math.pi
return rp.build_command('turn', angle * 10)
def goto(self, x, y):
bearing = self.pos.towards(Point(x, y))
distance = self.pos.distance(Point(x, y))
angle = (bearing - self.angle) % (2 * math.pi)
# turn left for forth quadrant, go backwards for second and third
if angle > 1.5 * math.pi:
angle -= 2 * math.pi
elif angle > 0.5 * math.pi:
angle -= math.pi
distance *= -1
print(self.pos, self.angle, bearing, distance, angle)
self.pos = Point(x, y)
return [ rp.build_command('turn', int(1800 / math.pi * angle)),
rp.build_command('drive', int(distance)),
rp.build_command('turn', int(-1800 / math.pi * angle)) ]
def services_resolved(self):
super().services_resolved()
print("[%s] Resolved services" % (self.mac_address))
self.uart_service = next(s for s in self.services
if s.uuid == uart_service_uuid)
self.tx_characteristic = next(
c for c in self.uart_service.characteristics
if c.uuid == tx_characteristic_uuid)
self.rx_characteristic = next(
c for c in self.uart_service.characteristics
if c.uuid == rx_characteristic_uuid)
self.rx_characteristic.enable_notifications() # listen to RX messages
# get root versions
# useful for populating the result_queue, so the controlling thread
# can wait on device connection
self.manager.send_command(
rp.build_command('versions', rp.board_ids['main']))
def disconnect(self):
self.send_command(rp.build_command('disconnect'))
self.stop()
self.robot.disconnect()
self.thread.join()
def direct_drive(self, left, right):
return rp.build_command('motors', left, right)
def reset(self):
angle = (self.angle - math.pi / 2)
if abs(angle) > math.pi:
angle = math.pi / 2 - self.angle
return (goto(0, 0) +
[rp.build_command('turn', 1800 / math.pi * angle)])
def say(self, phrase):
return rp.build_command('say', *rp.str_to_payload(phrase))
def pendown(self):
self.pen = True
self.eraser = False
return rp.build_command('setpen', 1)
def penup(self):
self.pen = False
self.eraser = False
return rp.build_command('setpen', 0)
def forward(self, dist):
self.pos += Point(dist * math.sin(self.angle),
dist * math.cos(self.angle))
return rp.build_command('drive', dist)
