Resampling index error appears to be gone. Still need more dialing in.

ch-13/4.3-monte-carlo_perf/robot/arena.py

@@ -35,7 +35,7 @@ def contains(x, y):
 
 
 grid_cell_size = 50
-overscan = 10 # 10 each way
+overscan = 2 # 2 each way
 
 
 def get_distance_to_segment(x, y, segment):

ch-13/4.3-monte-carlo_perf/robot/code.py

@@ -31,7 +31,7 @@ class DistanceSensorTracker:
 
 class CollisionAvoid:
     def __init__(self, distance_sensors):
-        self.speed = 0.6
+        self.speed = 0.5
         self.distance_sensors = distance_sensors
 
     async def main(self):
@@ -185,12 +185,12 @@ class Simulation:
         # modify the current weights based on the distance sensors
         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_angle = np.radians(self.poses[:, 2]) + left_angle
         left_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_left_angle) * left_hypotenuse
         left_sensor[:, 1] = self.poses[:, 1] + np.sin(poses_left_angle) * left_hypotenuse
 
-
         # right sensor
         poses_right_angle = np.radians(self.poses[:, 2]) - right_angle
         right_sensor[:, 0] = self.poses[:, 0] + np.cos(poses_right_angle) * right_hypotenuse
@@ -229,14 +229,14 @@ class Simulation:
                 weight_index = shift + current_index * interval
                 while weight_index >= cumulative_weights:
                     source_index += 1
-                    source_index = min(len(weights), source_index)
+                    source_index = min(len(weights) - 1, source_index)
                     cumulative_weights += weights[source_index]
                 samples[current_index] = self.poses[source_index]
         except IndexError:
-            send_json({"error": "IndexError in resample.", "weights": [weights.tolist()]})
+            send_json({"error": "IndexError in resample.", "weights": weights.tolist()})
             raise
         if samples.shape[0] != sample_count:
-            send_json({"error": "Sample count mismatch in resample.", "samples": [samples.tolist()]})
+            send_json({"error": "Sample count mismatch in resample.", "samples": samples.tolist()})
             raise Exception("Sample count mismatch in resample.")
         self.pc_resample.stop()
         return samples

ch-13/4.3-monte-carlo_perf/robot/robot.py

@@ -8,7 +8,7 @@ import adafruit_bno055
 
 uart = busio.UART(board.GP12, board.GP13, baudrate=9600, timeout=0.1)
 
-wheel_diameter_mm = 69.5
+wheel_diameter_mm = 70
 wheel_circumference_mm = math.pi * wheel_diameter_mm
 gear_ratio = 298
 encoder_poles = 28