Checkpoint 7

controllers/sheep/sheep.py

@@ -1,14 +1,13 @@
 """Sheep flocking controller (Webots).
 
-Each sheep broadcasts its GPS position every 3 steps on channel 1 and
-listens for the dog and peer sheep positions. The behavioural step is
-delegated to ``herding.flocking_sim.compute_heading_speed`` so the
-training environment and Webots run identical sheep dynamics.
+Each sheep emits its GPS position every 3 steps and listens for the
+dog's position and peer-sheep positions. The behavioural step is
+delegated to :func:`herding.world.flocking_sim.compute_heading_speed`
+so the env and Webots use identical sheep dynamics.
 
-Pen behaviour: a sheep latches to ``penned`` the first time it crosses
-the south-wall gate plane into the gate corridor. Once latched it turns
-pink (via the exposed ``woolColor`` PROTO field) and the force stack
-switches to in-pen containment.
+A sheep latches penned the first time it crosses the gate plane south;
+the wool turns pink (via the exposed ``woolColor`` PROTO field) and
+the dynamics switch to in-pen containment.
 """
 
 import math
@@ -32,10 +31,7 @@ from herding.world.geometry import (
 )
 
 
-# ---------------------------------------------------------------------------
-# Device setup
-# ---------------------------------------------------------------------------
-
+# --- Devices ---
 robot = Supervisor()
 timestep = int(robot.getBasicTimeStep())
 name = robot.getName()
@@ -55,14 +51,10 @@ receiver = robot.getDevice("receiver"); receiver.enable(timestep)
 emitter = robot.getDevice("emitter")
 
 
-# ---------------------------------------------------------------------------
-# Helpers
-# ---------------------------------------------------------------------------
+# --- Helpers ---
 
 def bearing():
-    # Compass returns north direction in sensor frame; for this Z-up world
-    # with north = +Y, atan2(n[0], n[1]) gives the standard math angle
-    # (0 = east, π/2 = north) matching atan2(fy, fx) used for headings.
+    """World-frame heading (0 = east, π/2 = north)."""
     n = compass.getValues()
     return math.atan2(n[0], n[1])
 
@@ -76,45 +68,36 @@ def drive(heading, speed_motor):
 
 
 def paint_pink():
-    # woolColor is declared as a PROTO field with IS binding to the DEF WOOL
-    # PBRAppearance baseColor; setting it propagates to every USE WOOL shape.
+    """Switch the sheep's wool to pink via the exposed PROTO field."""
     self_node.getField("woolColor").setSFColor([1.0, 0.55, 0.72])
 
 
-# ---------------------------------------------------------------------------
-# State
-# ---------------------------------------------------------------------------
-
+# --- State ---
 wander_angle = random.uniform(-math.pi, math.pi)
 step_count = 0
 dog_x, dog_y = None, None
-peers = {}        # name → (x, y), one entry per neighbour, cleared every 30 steps
+peers = {}                       # name → (x, y); periodically pruned
 penned = False
 
-# Stuck detection: differential-drive sheep can pin against a wall and need
-# a forced reverse-and-rotate to escape. If displacement < STUCK_DIST for
-# STUCK_STEPS consecutive steps, drive toward field centre.
+# Safety net for differential-drive sheep pinned against a wall.
 _prev_x, _prev_y = None, None
 _stuck_count = 0
 STUCK_STEPS = 20
 STUCK_DIST = 0.05
 
 
-# ---------------------------------------------------------------------------
-# Main loop
-# ---------------------------------------------------------------------------
-
+# --- Main loop ---
 while robot.step(timestep) != -1:
     step_count += 1
     pos = gps.getValues()
     x, y = pos[0], pos[1]
 
-    # Pen entry: one-way latch. Penned sheep get pink wool and switch behaviour.
     if not penned and is_penned_position(x, y):
         penned = True
         paint_pink()
 
-    # Refresh peer table — clear before receiving so fresh data is never lost.
+    # Stale peers get dropped periodically so a peer that's gone silent
+    # doesn't permanently distort the local CoM.
     if step_count % 30 == 0:
         peers.clear()
     while receiver.getQueueLength() > 0:
@@ -132,12 +115,12 @@ while robot.step(timestep) != -1:
         wander_angle=wander_angle,
     )
 
-    # Stuck detection — safety net for differential-drive wall pinning.
+    # Stuck-against-wall recovery: drive toward the field centre.
     if _prev_x is not None:
         moved = math.hypot(x - _prev_x, y - _prev_y)
         _stuck_count = _stuck_count + 1 if moved < STUCK_DIST else 0
     if _stuck_count >= STUCK_STEPS:
-        heading = math.atan2(-y, -x)   # always points away from the boundary
+        heading = math.atan2(-y, -x)
         speed = MAX_SPEED
         _stuck_count = 0
     _prev_x, _prev_y = x, y