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

Chapter 13 reduction

Browse Source
This commit is contained in:
Danny Staple
2023-01-08 23:11:35 +00:00
parent 68cfd39a19
commit 2db7a0fe3c
10 changed files with 28 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ch-13/4.3-monte-carlo/robot/arena.py
+1 -3
View File
@@ -29,6 +29,7 @@ def contains(x, y):
return False
return True
grid_cell_size = 50
overscan = 10 # 10 each way
@@ -65,9 +66,6 @@ def get_distance_likelihood(x, y):
return 1.0 / (1 + min_distance/250) ** 2
grid_cell_size = 50
overscan = 10 # 10 each way
# beam endpoint model
def make_distance_grid():
"""Take the boundary lines. With and overscan of 10 cells, and grid cell size of 5cm (50mm),
ch-13/4.3-monte-carlo/robot/code.py
+12 -14
View File
@@ -166,27 +166,25 @@ class Simulation:
def observe_distance_sensors(self, weights):
# modify the current weights based on the distance sensors
distance_sensor_left = np.zeros(
(self.poses.shape[0], 2), dtype=np.float)
distance_sensor_right = np.zeros(
(self.poses.shape[0], 2), dtype=np.float)
left_sensor = np.zeros((self.poses.shape[0], 2), dtype=np.float)
right_sensor = np.zeros((self.poses.shape[0], 2), dtype=np.float)
# left sensor
poses_left_90 = np.radians(self.poses[:, 2] + 90)
distance_sensor_left[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.distance_sensor_side_mm
distance_sensor_left[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.distance_sensor_side_mm
distance_sensor_left[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.left + robot.distance_sensor_forward_mm)
distance_sensor_left[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.left + robot.distance_sensor_forward_mm)
left_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_left_90) * robot.dist_side_mm
left_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_left_90) * robot.dist_side_mm
left_sensor[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.left + robot.dist_forward_mm)
left_sensor[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.left + robot.dist_forward_mm)
# right sensor
poses_right_90 = np.radians(self.poses[:, 2] - 90)
distance_sensor_right[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.distance_sensor_side_mm
distance_sensor_right[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.distance_sensor_side_mm
distance_sensor_right[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.right + robot.distance_sensor_forward_mm)
distance_sensor_right[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.right + robot.distance_sensor_forward_mm)
right_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_right_90) * robot.dist_side_mm
right_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_right_90) * robot.dist_side_mm
right_sensor[:, 0] += np.cos(self.poses[:, 2]) * (self.distance_sensors.right + robot.dist_forward_mm)
right_sensor[:, 1] += np.sin(self.poses[:, 2]) * (self.distance_sensors.right + robot.dist_forward_mm)
# Look up the distance in the arena
for index in range(self.poses.shape[0]):
sensor_weight = arena.get_distance_grid_at_point(distance_sensor_left[index,0], distance_sensor_left[index,1])
sensor_weight += arena.get_distance_grid_at_point(distance_sensor_right[index,0], distance_sensor_right[index,1])
sensor_weight = arena.get_distance_grid_at_point(left_sensor[index,0], left_sensor[index,1])
sensor_weight += arena.get_distance_grid_at_point(right_sensor[index,0], right_sensor[index,1])
weights[index] *= sensor_weight
return weights
ch-13/4.3-monte-carlo/robot/robot.py
+2 -2
View File
@@ -17,8 +17,8 @@ ticks_to_mm = wheel_circumference_mm / ticks_per_revolution
ticks_to_m = ticks_to_mm / 1000
m_to_ticks = 1 / ticks_to_m
wheelbase_mm = 170
distance_sensor_side_mm = 37 # approx mm
distance_sensor_forward_mm = 66 # approx mm
dist_side_mm = 37 # approx mm
dist_forward_mm = 66 # approx mm
motor_A2 = pwmio.PWMOut(board.GP17, frequency=100)
motor_A1 = pwmio.PWMOut(board.GP16, frequency=100)