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

Using GCA and GCF causes weird stuff to happen when buttons are added.

Browse Source
This commit is contained in:
Danny Staple 2022-11-15 22:34:17 +00:00
parent 1cfd93887b
commit 2c91a42160
9 changed files with 81 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ch-13/1-modelling-space/computer/display_from_robot.py
View File

@ -29,20 +29,21 @@ class RobotDisplay:
self.arena = message
def draw(self):
plt.gca().clear()
self.ax.clear()
if self.arena:
for line in self.arena["arena"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
)
for line in self.arena["target_zone"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red"
)
async def main(self):
plt.ion()
await self.ble_connection.connect()
self.fig, self.ax = plt.subplots()
try:
request = json.dumps({"command": "arena"}).encode()
print(f"Sending request for arena: {request}")

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

@ -39,22 +39,23 @@ class RobotDisplay:
self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)])
def draw(self):
plt.gca().clear()
self.ax.clear()
if self.arena:
for line in self.arena["arena"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
)
for line in self.arena["target_zone"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red"
)
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")
self.ax.quiver(self.pose_coords[0], self.pose_coords[1], self.pose_uv[0], self.pose_uv[1], color="blue")
async def main(self):
plt.ion()
await self.ble_connection.connect()
self.fig, self.ax = plt.subplots()
try:
request = json.dumps({"command": "arena"}).encode()
print(f"Sending request for arena: {request}")

21
ch-13/2-displaying-a-pose/robot/code.py
View File

@ -13,6 +13,8 @@ class Simulation:
for n in range(population_size):
self.poses[n] = random.uniform(0, arena.width), random.uniform(0, arena.height), random.uniform(0, 360)
def send_json(data):
robot.uart.write((json.dumps(data)+"\n").encode())
async def command_handler(simulation):
while True:
@ -20,27 +22,20 @@ async def command_handler(simulation):
print("Receiving data...")
try:
data = robot.uart.readline().decode()
except UnicodeError:
print("Invalid data")
continue
try:
request = json.loads(data)
print(f"Received command: {request}")
except ValueError:
print("Invalid JSON")
except (UnicodeError, ValueError):
print("Invalid data")
continue
# {"command": "arena"}
if request["command"] == "arena":
response = {
send_json({
"arena": arena.boundary_lines,
"target_zone": arena.target_zone,
}
robot.uart.write((json.dumps(response)+"\n").encode())
})
else:
response = {
send_json({
"poses": simulation.poses.tolist(),
}
robot.uart.write((json.dumps(response)+"\n").encode())
})
await asyncio.sleep(0.1)
simulation= Simulation()

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

@ -40,18 +40,18 @@ class RobotDisplay:
self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)])
def draw(self):
plt.gca().clear()
self.ax.clear()
if self.arena:
for line in self.arena["arena"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
)
for line in self.arena["target_zone"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red"
)
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")
self.ax.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):
request = json.dumps({"command": command}).encode()
@ -67,9 +67,10 @@ class RobotDisplay:
async def main(self):
plt.ion()
await self.ble_connection.connect()
self.fig, self.ax = plt.subplots()
try:
await self.send_command("arena")
plt.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.on_clicked(self.start)
stop_button = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Stop")

22
ch-13/3-moving-poses-with-sensors/robot/code.py
View File

@ -30,6 +30,9 @@ class Simulation:
finally:
robot.stop()
def send_json(data):
robot.uart.write((json.dumps(data)+"\n").encode())
async def command_handler(simulation):
simulation_task = None
while True:
@ -37,22 +40,16 @@ async def command_handler(simulation):
print("Receiving data...")
try:
data = robot.uart.readline().decode()
except UnicodeError:
print("Invalid data")
continue
try:
request = json.loads(data)
print(f"Received command: {request}")
except ValueError:
print("Invalid JSON")
except (UnicodeError, ValueError):
print("Invalid data")
continue
# {"command": "arena"}
if request["command"] == "arena":
response = {
send_json({
"arena": arena.boundary_lines,
"target_zone": arena.target_zone,
}
robot.uart.write((json.dumps(response)+"\n").encode())
})
elif request["command"] == "start":
print("Starting simulation")
if simulation_task is None or simulation_task.done():
@ -63,10 +60,9 @@ async def command_handler(simulation):
simulation_task.cancel()
simulation_task = None
else:
response = {
send_json({
"poses": simulation.poses.tolist(),
}
robot.uart.write((json.dumps(response)+"\n").encode())
})
await asyncio.sleep(0.1)
simulation= Simulation()

11
ch-13/4-moving-poses-with-sensors-turning/computer/display_from_robot.py
View File

@ -40,18 +40,18 @@ class RobotDisplay:
self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)])
def draw(self):
plt.gca().clear()
self.ax.clear()
if self.arena:
for line in self.arena["arena"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
)
for line in self.arena["target_zone"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red"
)
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")
self.ax.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):
request = json.dumps({"command": command}).encode()
@ -67,9 +67,10 @@ class RobotDisplay:
async def main(self):
plt.ion()
await self.ble_connection.connect()
self.fig, self.ax = plt.subplots()
try:
await self.send_command("arena")
plt.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.on_clicked(self.start)
stop_button = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Stop")

24
ch-13/4-moving-poses-with-sensors-turning/robot/code.py
View File

@ -38,6 +38,9 @@ class Simulation:
finally:
robot.stop()
def send_json(data):
robot.uart.write((json.dumps(data)+"\n").encode())
async def command_handler(simulation):
simulation_task = None
while True:
@ -51,11 +54,10 @@ async def command_handler(simulation):
continue
# {"command": "arena"}
if request["command"] == "arena":
response = {
send_json({
"arena": arena.boundary_lines,
"target_zone": arena.target_zone,
}
robot.uart.write((json.dumps(response)+"\n").encode())
})
elif request["command"] == "start":
print("Starting simulation")
if simulation_task is None or simulation_task.done():
@ -66,10 +68,18 @@ async def command_handler(simulation):
simulation_task.cancel()
simulation_task = None
else:
response = {
"poses": simulation.poses.tolist(),
}
robot.uart.write((json.dumps(response)+"\n").encode())
sys_status, gyro, accel, mag = robot.imu.calibration_status
if sys_status != 3:
send_json({"imu_calibration": {
"gyro": gyro,
"accel": accel,
"mag": mag,
"sys": sys_status,
}})
else:
send_json({
"poses": simulation.poses.tolist(),
})
await asyncio.sleep(0.1)
simulation= Simulation()

11
ch-13/5-eliminating-poses/computer/display_from_robot.py
View File

@ -40,18 +40,18 @@ class RobotDisplay:
self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)])
def draw(self):
plt.gca().clear()
self.ax.clear()
if self.arena:
for line in self.arena["arena"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="black"
)
for line in self.arena["target_zone"]:
plt.gca().plot(
self.ax.plot(
[line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red"
)
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")
self.ax.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):
request = json.dumps({"command": command}).encode()
@ -67,9 +67,10 @@ class RobotDisplay:
async def main(self):
plt.ion()
await self.ble_connection.connect()
self.fig, self.ax = plt.subplots()
try:
await self.send_command("arena")
plt.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.on_clicked(self.start)
stop_button = Button(plt.axes([0.81, 0.05, 0.1, 0.075]), "Stop")

30
ch-13/5-eliminating-poses/robot/code.py
View File

@ -22,7 +22,7 @@ class Simulation:
pose[1] += speed_in_mm * np.sin(np.radians(pose[2]))
def eliminate_impossible_poses(self, left_distance, right_distance):
poses_to_keep = np.ones(len(self.poses), dtype=bool)
poses_to_keep = np.ones(len(self.poses), dtype=np.uint8)
# todo: would reorganising these pose arrays let us better use ulab tools?
for position, pose in enumerate(self.poses):
# first those outside the arena
@ -31,9 +31,9 @@ class Simulation:
left_distance_point = pose[0] + robot.distance_sensor_from_middle + left_distance * np.cos(np.radians(pose[2])), pose[1] + left_distance * np.sin(np.radians(pose[2]))
right_distance_point = pose[0] + robot.distance_sensor_from_middle + right_distance * np.cos(np.radians(pose[2])), pose[1] + right_distance * np.sin(np.radians(pose[2]))
keep = arena.point_is_inside_arena(left_distance_point) and arena.point_is_inside_arena(right_distance_point)
poses_to_keep[position] = keep
poses_to_keep[position] = int(keep)
# apply the keep as a mask to the poses using ulab. - doesn't work in ulab.
self.poses = [item for item, keep in zip(self.poses, poses_to_keep) if keep]
self.poses = np.array([item for item, keep in zip(self.poses, poses_to_keep) if keep])
async def run(self):
try:
@ -62,6 +62,9 @@ class Simulation:
finally:
robot.stop()
def send_json(data):
robot.uart.write((json.dumps(data)+"\n").encode())
async def command_handler(simulation):
simulation_task = None
while True:
@ -75,11 +78,10 @@ async def command_handler(simulation):
continue
# {"command": "arena"}
if request["command"] == "arena":
response = {
send_json({
"arena": arena.boundary_lines,
"target_zone": arena.target_zone,
}
robot.uart.write((json.dumps(response)+"\n").encode())
})
elif request["command"] == "start":
print("Starting simulation")
if simulation_task is None or simulation_task.done():
@ -90,10 +92,18 @@ async def command_handler(simulation):
simulation_task.cancel()
simulation_task = None
else:
response = {
"poses": simulation.poses.tolist(),
}
robot.uart.write((json.dumps(response)+"\n").encode())
sys_status, gyro, accel, mag = robot.imu.calibration_status
if sys_status != 3:
send_json({"imu_calibration": {
"gyro": gyro,
"accel": accel,
"mag": mag,
"sys": sys_status,
}})
else:
send_json({
"poses": simulation.poses.tolist(),
})
await asyncio.sleep(0.1)
simulation= Simulation()