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

Display working. Sensors starting to work.

Browse Source 
Clusters are forming.
This commit is contained in:
Danny Staple 2022-12-06 21:32:47 +00:00
parent d3e128e010
commit 389f2f768a
2 changed files with 44 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
ch-13/full-version/computer/display_from_robot.py
View File

@ -14,17 +14,18 @@ class RobotDisplay:
self.arena = {} self.arena = {}
self.display_closed = False self.display_closed = False
self.fig, self.ax = plt.subplots() self.fig, self.ax = plt.subplots()
self.pose_coords = [] self.poses = np.zeros([200, 3], dtype=np.float32)
self.pose_uv = []
def handle_close(self, _): def handle_close(self, _):
self.display_closed = True self.display_closed = True
def handle_data(self, data): def handle_data(self, data):
self.line += data.decode("utf-8") self.line += data.decode("utf-8")
# print(f"Received data: {data.decode('utf-8')}")
# print(f"Line is now: {self.line}")
while "\n" in self.line: while "\n" in self.line:
line, self.line = self.line.split("\n", 1) line, self.line = self.line.split("\n", 1)
# print(f"Received data: {line}") print(f"Received line: {line}")
try: try:
message = json.loads(line) message = json.loads(line)
except ValueError: except ValueError:
@ -33,12 +34,8 @@ class RobotDisplay:
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. print(message)
# matplotlib quiver plots wants an [x] ,[y] and [angle] arrays self.poses[message["offset"]: message["offset"] + len(message["poses"])] = message["poses"]
poses = np.array(message["poses"])
self.pose_coords = poses[:,0:1].T
angle_rads = np.deg2rad(poses[:,2])
self.pose_uv = np.array([np.cos(angle_rads), np.sin(angle_rads)])
def draw(self): def draw(self):
self.ax.clear() self.ax.clear()
@ -47,12 +44,7 @@ class RobotDisplay:
self.ax.plot( self.ax.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"]: self.ax.scatter(self.poses[:,0], self.poses[:,1], color="blue")
# self.ax.plot(
# [line[0][0], line[1][0]], [line[0][1], line[1][1]], color="red"
# )
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")
async def send_command(self, command): async def send_command(self, command):
#+ "\n" - why does adding this (which sounds right) cause the ble stack (on the robot or computer? ) not to work any more? #+ "\n" - why does adding this (which sounds right) cause the ble stack (on the robot or computer? ) not to work any more?

57
ch-13/full-version/robot/code.py
View File

@ -28,8 +28,8 @@ class Simulation:
# speed is proportional to distance from wall -> further we are from wall, faster we can go # speed is proportional to distance from wall -> further we are from wall, faster we can go
# turn is proportional to difference between left and right distance sensors. # turn is proportional to difference between left and right distance sensors.
self.forward_distance_pid = pid_controller.PIDController(0.1, 0.01, 0.01) self.forward_distance_pid = pid_controller.PIDController(0.01, 0.001, 0.001)
self.turn_pid = pid_controller.PIDController(0.1, 0.01, 0.01) self.turn_pid = pid_controller.PIDController(0.01, 0.001, 0.001)
self.distance_aim = 100 self.distance_aim = 100
def apply_sensor_model(self): def apply_sensor_model(self):
@ -88,7 +88,9 @@ class Simulation:
weights[index] = 1 / (left_error + right_error) weights[index] = 1 / (left_error + right_error)
#normalise the weights #normalise the weights
print("Weights sum before normalising:", np.sum(weights))
weights = weights / np.sum(weights) weights = weights / np.sum(weights)
print("Weights sum:", np.sum(weights))
return weights return weights
def resample(self, weights): def resample(self, weights):
@ -107,7 +109,7 @@ class Simulation:
cumulative_weights += weights[source_index] cumulative_weights += weights[source_index]
samples.append(source_index) samples.append(source_index)
# set poses to the resampled poses # set poses to the resampled poses
self.poses = self.poses[samples] self.poses = np.array([self.poses[n] for n in samples])
async def move_robot(self): async def move_robot(self):
"""move forward, apply the motion model""" """move forward, apply the motion model"""
@ -116,13 +118,14 @@ class Simulation:
encoder_right = robot.right_encoder.read() encoder_right = robot.right_encoder.read()
# move forward - use distance sensor to determine how far to go # move forward - use distance sensor to determine how far to go
distance_error = self.distance_aim - min(self.left_distance, self.right_distance) print("left_distance:", self.left_distance, "right_distance:", self.right_distance)
distance_error = min(self.left_distance, self.right_distance) - self.distance_aim
forward_speed = self.forward_distance_pid.calculate(distance_error, self.time_step) forward_speed = self.forward_distance_pid.calculate(distance_error, self.time_step)
turn_error = self.left_distance - self.right_distance turn_error = self.left_distance - self.right_distance
turn_speed = self.turn_pid.calculate(turn_error, self.time_step) turn_speed = self.turn_pid.calculate(turn_error, self.time_step)
print("forward_speed:", forward_speed, "turn_speed:", turn_speed)
robot.set_left(forward_speed + turn_speed) # robot.set_left(forward_speed + turn_speed)
robot.set_right(forward_speed - turn_speed) # robot.set_right(forward_speed - turn_speed)
await asyncio.sleep(self.time_step) await asyncio.sleep(self.time_step)
# record sensor changes # record sensor changes
@ -140,13 +143,14 @@ class Simulation:
heading_standard_dev = 2 # degrees heading_standard_dev = 2 # degrees
speed_standard_dev = 5 # mm speed_standard_dev = 5 # mm
radians = np.radians(self.poses[2]) radians = np.radians(self.poses[:,2])
heading_model = [get_gaussian_sample(0, heading_standard_dev) for _ in range(self.poses.shape[1])] heading_model = np.array([get_gaussian_sample(0, heading_standard_dev) for _ in range(self.poses.shape[0])])
speed_model = [get_gaussian_sample(speed_in_mm, speed_standard_dev) for _ in range(self.poses.shape[1])] speed_model = np.array([get_gaussian_sample(speed_in_mm, speed_standard_dev) for _ in range(self.poses.shape[0])])
# print("Radians shape:", radians.shape, "heading_model shape:", len(heading_model), "speed_model shape:", len(speed_model), "poses shape:", self.poses.shape)
self.poses[:,0] += speed_model * np.cos(radians) self.poses[:,0] += speed_model * np.cos(radians)
self.poses[:,1] += speed_model * np.sin(radians) self.poses[:,1] += speed_model * np.sin(radians)
self.poses[:,2] += np.full(self.poses[2].shape, heading_change + heading_model) self.poses[:,2] += heading_change + heading_model
self.poses[:,2] = np.vectorize(lambda n: n % 360)(self.poses[2]) self.poses[:,2] = np.vectorize(lambda n: float(n % 360))(self.poses[:,2])
async def distance_sensor_updater(self): async def distance_sensor_updater(self):
robot.left_distance.start_ranging() robot.left_distance.start_ranging()
@ -163,10 +167,14 @@ class Simulation:
async def run(self): async def run(self):
asyncio.create_task(self.distance_sensor_updater()) asyncio.create_task(self.distance_sensor_updater())
try: try:
for _ in range(15): while True:
print("Applying sensor model")
weights = self.apply_sensor_model() weights = self.apply_sensor_model()
print("Sensor model complete.\nResampling")
self.resample(weights) self.resample(weights)
print("Resampling complete.\nMoving robot")
await self.move_robot() await self.move_robot()
print("Robot move complete")
finally: finally:
robot.stop() robot.stop()
@ -207,18 +215,22 @@ async def updater(simulation):
} }
} }
) )
send_json( # The big time delay is in sending the poses.
{ print("Sending poses", simulation.poses.shape[0])
"poses": simulation.poses.tolist(), for n in range(0, simulation.poses.shape[0], 10):
} print("Sending poses from ", n, "to", n+10, "of", simulation.poses.shape[0], "poses")
) send_json({
"poses": simulation.poses[n:n+10].tolist(),
"offset": n,
})
await asyncio.sleep(0.01)
await asyncio.sleep(0.5) await asyncio.sleep(0.5)
async def command_handler(simulation): async def command_handler(simulation):
update_task = asyncio.create_task(updater(simulation))
simulation_task = asyncio.create_task(simulation.run())
print("Starting handler") print("Starting handler")
update_task = None
simulation_task = None
while True: while True:
if robot.uart.in_waiting: if robot.uart.in_waiting:
print("Receiving data...") print("Receiving data...")
@ -232,6 +244,11 @@ async def command_handler(simulation):
"arena": arena.boundary_lines "arena": arena.boundary_lines
} }
) )
if not update_task:
update_task = asyncio.create_task(updater(simulation))
elif request["command"] == "start":
simulation_task = asyncio.create_task(simulation.run())
await asyncio.sleep(0.1) await asyncio.sleep(0.1)