move logic

webots/controllers/C1_pClient/C1_pClient.py

@@ -2,6 +2,7 @@
 
 import numpy
 import xml.etree.ElementTree as ET
+import atexit
 
 
 # You may need to import some classes of the controller module. Ex:
@@ -13,6 +14,8 @@
 CELL_SIZE = 0.15
 KP = 10.0
 MAX_SPEED = 6.27
+WHEEL_RADIUS = 0.025
+AXLE_LENGTH = 0.045
 
 class Map():
     def __init__(self, filename):
@@ -68,6 +71,11 @@ def __init__(self):
         # Set the initial velocity of the left and right wheel motors.
         self.driveMotors(0.0, 0.0)
 
+        #adding helpers!
+
+
+
+
         self.camera = self.robot.getDevice("camera")
         self.camera.enable(self.timeStep)
 
@@ -83,6 +91,13 @@ def __init__(self):
         self.abs_ref_pos = numpy.array(self.gps.getValues())
         print("Initial position:", self.abs_ref_pos)
 
+        # settle-and-log configuration
+        self.settle_time_s = 0.2
+        self.step_count = 0
+        self.loc_log = open("localization.csv", "w")
+        self.loc_log.write("step,command," + ",".join([f"p{i}" for i in range(CELLROWS*CELLCOLS)]) + "\n")
+        atexit.register(self.loc_log.close)
+
     def step(self):
         self.robot.step(self.timeStep)
         self.cur_pos = numpy.array(self.gps.getValues())
@@ -93,6 +108,73 @@ def driveMotors(self, leftSpeed, rightSpeed):
         self.leftMotor.setVelocity(max(min(leftSpeed,MAX_SPEED),-MAX_SPEED))
         self.rightMotor.setVelocity(max(min(rightSpeed,MAX_SPEED),-MAX_SPEED))
 
+    def _wrap_pi(self, a):
+        return (a + numpy.pi) % (2 * numpy.pi) - numpy.pi
+
+    def send_motor_command(self, left_speed, right_speed, duration_s):
+        self.driveMotors(left_speed, right_speed)
+        elapsed = 0.0
+        dt = self.timeStep / 1000.0
+        while elapsed < duration_s:
+            self.step()
+            elapsed += dt
+        self.driveMotors(0.0, 0.0)
+
+    def execute_command(self, command):
+        target_dirs = {'N': numpy.pi/2, 'E': 0.0, 'S': -numpy.pi/2, 'W': numpy.pi}
+        if command not in target_dirs:
+            return
+
+        # 1) Rotate to target heading
+        target_dir = target_dirs[command]
+        delta = self._wrap_pi(target_dir - self.cur_dir)
+
+        w = min(self.cruiseVelocity, MAX_SPEED)   # wheel angular speed (rad/s)
+        v_w = w * WHEEL_RADIUS                    # wheel linear rim speed (m/s)
+
+        if v_w > 1e-6 and abs(delta) > 1e-3:
+            t_rot = abs(delta) * AXLE_LENGTH / (2.0 * v_w)
+            sgn = 1.0 if delta > 0.0 else -1.0
+            self.send_motor_command(-sgn * w, sgn * w, t_rot)
+            self.step()
+
+        # 2) Drive forward one cell
+        d = CELL_SIZE
+        if v_w > 1e-6:
+            t_fwd = d / v_w
+        self.send_motor_command(w, w, t_fwd)  
+        self.step()
+
+        # Stop and wait for sensors to stabilize
+        self.driveMotors(0.0, 0.0)
+        self.wait_for(self.settle_time_s)
+
+        # Log localization probabilities
+        probs = self.localization_probabilities()
+        self.log_localization(command, probs)
+
+    def wait_for(self, seconds):
+        elapsed = 0.0
+        dt = self.timeStep / 1000.0
+        while elapsed < seconds:
+            self.step()
+            elapsed += dt
+
+    def localization_probabilities(self):
+        gx = int(round((self.cur_pos[0] - self.abs_ref_pos[0]) / CELL_SIZE))
+        gy = int(round((self.cur_pos[1] - self.abs_ref_pos[1]) / CELL_SIZE))
+        probs = [0.0] * (CELLROWS * CELLCOLS)
+        if 0 <= gx < CELLCOLS and 0 <= gy < CELLROWS:
+            idx = gy * CELLCOLS + gx
+            probs[idx] = 1.0
+        return probs
+
+    def log_localization(self, command, probs):
+        self.step_count += 1
+        line = f"{self.step_count},{command}," + ",".join(f"{p:.6f}" for p in probs) + "\n"
+        self.loc_log.write(line)
+        self.loc_log.flush()
+
 
     def rotate(self, target_dir):
         #print("Rotating to direction:", target_dir)
@@ -163,18 +245,9 @@ def printMap(self):
     target_pos = myrob.abs_ref_pos.copy()
 
     while myrob.step() != -1:
-        # Read next movement from file
         command = commands_file.readline().strip()
-        if command == "N":
-            print("Moving North")
-            target_pos[1] += CELL_SIZE
-        elif command == "S":
-            print("Moving South")
-            target_pos[1] -= CELL_SIZE
-        elif command == "E":
-            print("Moving East")
-            target_pos[0] += CELL_SIZE
-        elif command == "W":
-            print("Moving West")
-            target_pos[0] -= CELL_SIZE
-        myrob.move_to(target_pos)
+        if not command:
+            continue
+        print ("Executing command:", command)
+        myrob.execute_command(command)
+

webots/worlds/.pc_challenge_1.wbproj

@@ -0,0 +1,10 @@
+Webots Project File version R2025a
+perspectives: 000000ff00000000fd00000002000000010000011c0000024dfc0200000001fb0000001400540065007800740045006400690074006f007201000000170000024d0000003f00ffffff000000030000073e000000d9fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000073e0000006900ffffff000006200000024d00000001000000020000000100000008fc00000000
+simulationViewPerspectives: 000000ff000000010000000200000100000005200100000002010000000100
+sceneTreePerspectives: 000000ff000000010000000300000144000000cd000000000100000002010000000200
+maximizedDockId: -1
+centralWidgetVisible: 1
+orthographicViewHeight: 1
+textFiles: -1
+consoles: Console:All:All
+renderingDevicePerspectives: e-puck:camera;1;1;0;0