Add block height-based detection

block.py

@@ -82,6 +82,7 @@ def _block_square_update(self):
 
     def append_trail(self):
         '''Appends current position information to the block's trail'''
+        # Not used
 
         self.trail.append({
             'position': self.position,
@@ -126,6 +127,7 @@ def draw(self, canvas):
 
     def move_manual(self, keypress, walls):
         '''Determine the direction to move & rotate the block based on keypresses.'''
+        # Not used
 
         move_vector = pm.Vector2(0, 0)
         rotation = 0
@@ -155,6 +157,8 @@ def move_manual(self, keypress, walls):
 
     def move(self, velocity, rotation, walls):
         '''Moves the robot, checking for collisions.'''
+        # Not used
+
         # Update robot position
         self.position += pm.Vector2.rotate(velocity, self.rotation)
         self.rotation += rotation
@@ -172,6 +176,7 @@ def check_collision_walls(self, walls: list):
         Checks for a collision between the robot's perimeter segments
         and a set of wall line segments.
         '''
+        # Not used
 
         # Loop through all the robot outline line segments, checking for collisions
         for segment_bot in self.outline_global_segments:

config/config.py

@@ -56,7 +56,7 @@
 robot_height = 6            # Robot height in inches
 block_position = [66, 5]   # Block starting location
 block_rotation = 0          # Block rotation (deg)
-block_size = 2              # Block side length in inches
+block_size = 3              # Block side length in inches
 
 # Drive information
 num_segments = 10           # Number of movement segments
@@ -154,6 +154,23 @@
 u0_info = {
     'id': 'u0',
     'position': [0, 3],
+    'height': 2,
+    'rotation': 0,
+    'error': 0.02,
+    'outline': [
+        pm.Vector2(-1, -0.5),
+        pm.Vector2(-1, 0.5),
+        pm.Vector2(1, 0.5),
+        pm.Vector2(1, -0.5)
+    ],
+    'visible': True,
+    'visible_measurement': False
+}
+
+u1_info = {
+    'id': 'u1',
+    'position': [0, 1],
+    'height': 6,
     'rotation': 0,
     'error': 0.02,
     'outline': [
@@ -194,11 +211,13 @@
     'error': 0.05,
     'bias': 0.1,
     'color': (127, 127, 127),
-    'visible': True
+    'visible': True,
+    'visible_measurement': True
 }
 
 sensors = {
     'u0': Ultrasonic(u0_info),
+    'u1': Ultrasonic(u1_info),
     'g0': Gyroscope(g0_info),
     'c0': Compass(c0_info),
     'i0': Infrared(i0_info)

devices/device.py

@@ -55,6 +55,7 @@ def __init__(self, d_id: str, position: list, rotation: float, visible: bool):
         self.outline_thickness = 0.2
         self.active_color = (255, 0, 0)
         self.visible = visible
+        self.visible_measurement = False
 
 
     def pos_update(self, bot_pos: pygame.math.Vector2, bot_rot: float):

devices/ultrasonic.py

@@ -47,6 +47,9 @@ def __init__(self, info: dict):
             pm.Vector2(1, -0.5)
         ])
 
+        # Device height
+        self.height = info.get('height', CONFIG.block_size)
+
         # Display color
         self.color = info.get('color', (0, 0, 255))
 
@@ -113,7 +116,13 @@ def simulate(self, value: float, environment: dict):
         ray_lengths = [self.max_range for item in rays]
 
         for ct, ray in enumerate(rays):
-            for square in [BLOCK.block_square, *MAZE.wall_squares]:
+            # Check if the sensor is at a height where the block would be seen
+            if self._block_visible(BLOCK):
+                to_check = [BLOCK.block_square, *MAZE.wall_squares]
+            else:
+                to_check = MAZE.wall_squares
+
+            for square in to_check:
                 for segment_wall in square:
                     collision_points = utilities.collision(ray, segment_wall)
                     if not collision_points:
@@ -129,3 +138,21 @@ def simulate(self, value: float, environment: dict):
         output = min(self.ray_lengths)
 
         return utilities.add_error(output, self.error_pct, self.reading_bounds)
+
+    def _block_visible(self, BLOCK):
+        '''Determines whether the block is visibile to an ultrasonic sensor based on its height.'''
+
+        # Get some geometric parameters
+        view_angle_z = self.beamwidth/2
+        h = self.height - BLOCK.height
+        vec = pm.Vector2(BLOCK.position.x - self.position_global.x, BLOCK.position.y - self.position_global.y)
+
+        # Calculate distance and angle to block
+        d = vec.magnitude()
+        th = abs(math.atan(h/d)) * 180/math.pi
+
+        # If angle to block is less than sensor view angle, it's visible
+        if th <= view_angle_z:
+            return True
+        else:
+            return False

robot.py

@@ -139,7 +139,7 @@ def draw_devices(self, canvas):
         for device in self.devices.values():
             if device.visible:
                 device.draw(canvas)
-                if (device.name == 'ultrasonic' or device.name == 'infrared'):
+                if device.visible_measurement:
                     device.draw_measurement(canvas)
 
     def move_manual(self, keypress, walls):

simmer.py

@@ -120,7 +120,7 @@
         ROBOT.update_device_positions()
 
         # Manually simulate a specific sensor or sensors
-        utilities.simulate_sensors(environment, ['u0', 'i0'])
+        utilities.simulate_sensors(environment, ['u0', 'u1', 'i0'])
 
         # Update the sensors that need to be updated every frame
         for sensor in ROBOT.sensors.values():