ecki/Robotics-at-Home-with-Raspberry-Pi-Pico
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Improve serial handling - make similar to other examples.

Browse Source
This commit is contained in:
Danny Staple 2022-11-13 19:11:04 +00:00
parent d3e380ba5f
commit be73bc3dc3
2 changed files with 29 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ch-13/2-displaying-a-pose/computer/display_from_robot.py
View File

@ -20,26 +20,24 @@ class RobotDisplay:
def handle_data(self, data): def handle_data(self, data):
self.line += data.decode("utf-8") self.line += data.decode("utf-8")
if not self.line.endswith("\n"): while "\n" in self.line:
return line, self.line = self.line.split("\n", 1)
print(f"Received data: {self.line}") print(f"Received data: ```{line}```")
try: try:
message = json.loads(self.line) message = json.loads(line)
except ValueError: except ValueError:
print("Error parsing JSON") print("Error parsing JSON")
return return
self.line = "" if "arena" in message:
if "arena" in message: self.arena = message
self.arena = message if "poses" in message:
if "poses" in message: # the robot poses are an array of [x, y, theta] arrays.
# the robot poses are an array of [x, y, theta] arrays. # matplotlib quiver plots wants an [x] ,[y] and [angle] arrays
# matplotlib quiver plots wants an array of [x,y] arrays, and a separate array of angles poses = np.array(message["poses"]).T
poses = np.array(message["poses"]).T self.pose_coords = poses[:2]
self.pose_coords = poses[:2] angle_rads = np.deg2rad(poses[2])
angle_rads = np.deg2rad(poses[2]) self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)])
self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)])
def draw(self): def draw(self):
plt.gca().clear() plt.gca().clear()
if self.arena: if self.arena:
@ -53,13 +51,11 @@ class RobotDisplay:
) )
if len(self.pose_coords) > 0: if len(self.pose_coords) > 0:
plt.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue") plt.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue")
plt.draw()
async def main(self): async def main(self):
plt.ion() plt.ion()
await self.ble_connection.connect()
try: try:
await self.ble_connection.connect()
request = json.dumps({"command": "arena"}).encode() request = json.dumps({"command": "arena"}).encode()
print(f"Sending request for arena: {request}") print(f"Sending request for arena: {request}")
await self.ble_connection.send_uart_data(request) await self.ble_connection.send_uart_data(request)
@ -67,10 +63,9 @@ class RobotDisplay:
while not self.display_closed: while not self.display_closed:
self.draw() self.draw()
plt.draw()
plt.pause(0.05) plt.pause(0.05)
await asyncio.sleep(0.01) await asyncio.sleep(0.01)
plt.show()
finally: finally:
await self.ble_connection.close() await self.ble_connection.close()

21
ch-13/3-moving-poses-with-sensors/computer/display_from_robot.py
View File

@ -13,8 +13,8 @@ class RobotDisplay:
self.line = "" self.line = ""
self.arena = {} self.arena = {}
self.display_closed = False self.display_closed = False
self.pose_coords = np.array([(0,), (0,)]) self.pose_coords = []
self.pose_uv = np.array([(1,), (1,)]) self.pose_uv = []
def handle_close(self, _): def handle_close(self, _):
self.display_closed = True self.display_closed = True
@ -29,30 +29,29 @@ class RobotDisplay:
except ValueError: except ValueError:
print("Error parsing JSON") print("Error parsing JSON")
return return
if "arena" in message: if "arena" in message:
self.arena = message self.arena = message
if "poses" in message: if "poses" in message:
# the robot poses are an array of [x, y, theta] arrays. # the robot poses are an array of [x, y, theta] arrays.
# matplotlib quiver plots wants an array of [x,y] arrays, and a separate array of angles # matplotlib quiver plots wants an [x] ,[y] and [angle] arrays
poses = np.array(message["poses"]).T poses = np.array(message["poses"]).T
self.pose_coords = poses[:2] self.pose_coords = poses[:2]
angle_rads = np.deg2rad(poses[2]) angle_rads = np.deg2rad(poses[2])
self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)]) self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)])
def draw(self): def draw(self):
self.ax.clear() plt.gca().clear()
if self.arena: if self.arena:
for line in self.arena["arena"]: for line in self.arena["arena"]:
self.ax.plot( plt.gca().plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black" [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
) )
for line in self.arena["target_zone"]: for line in self.arena["target_zone"]:
self.ax.plot( plt.gca().plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red" [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red"
) )
if len(self.pose_coords) > 0: if len(self.pose_coords) > 0:
self.ax.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue") plt.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue")
async def send_command(self, command): async def send_command(self, command):
request = json.dumps({"command": command}).encode() request = json.dumps({"command": command}).encode()
@ -69,9 +68,8 @@ class RobotDisplay:
plt.ion() plt.ion()
await self.ble_connection.connect() await self.ble_connection.connect()
try: try:
# await self.send_command("arena") await self.send_command("arena")
self.fig, self.ax = plt.subplots() self.gcf().canvas.mpl_connect("close_event", self.handle_close)
self.fig.canvas.mpl_connect("close_event", self.handle_close)
start_button = Button(plt.axes([0.7, 0.05, 0.1, 0.075]), "Start") start_button = Button(plt.axes([0.7, 0.05, 0.1, 0.075]), "Start")
start_button.on_clicked(self.start) start_button.on_clicked(self.start)
stop_button = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Stop") stop_button = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Stop")
@ -81,7 +79,6 @@ class RobotDisplay:
self.draw() self.draw()
plt.pause(0.05) plt.pause(0.05)
await asyncio.sleep(0.01) await asyncio.sleep(0.01)
plt.draw() plt.draw()
finally: finally:
await self.ble_connection.close() await self.ble_connection.close()