# Mohamed AlJedaie — Robot 1 (Webots Robo Soccer 2025) # Notes (read first): # - Paste this entire file into robot1.py for YOUR robot (yellow or blue). No external libraries are used. # - The code is built to work with common Webots soccer templates (e-puck / differential drive). # - If your course template already creates devices and variables, keep them and only merge the STRATEGY BLOCK # from below. The "ADAPTERS" section tries to auto-detect common device names so the file can run as-is. # - No classes. Only tiny helpers. Clear, fair play: no crashing, no field exits. # ----------------------------- # CONFIG (safe to tweak) # ----------------------------- MAX_SPEED = 6.28 # typical for e-puck motors TIMESTEP = 32 # will be overwritten by robot.getBasicTimeStep() if available BALL_LOCK_GAIN = 2.2 # how strongly we turn toward the ball APPROACH_GAIN = 4.0 # forward speed factor when ball is in front AVOID_GAIN = 3.5 # turn away from obstacles (sonar/prox) KICK_BOOST = 0.8 # extra forward boost when very close BALL_CLOSE = 0.18 # distance (m) we consider "kicking range" BALL_SEEN_MIN = 0.02 # minimal "confidence" to consider the ball seen SCAN_SPEED = 0.45 # base scan turn when ball not seen WALL_NEAR = 0.12 # obstacle distance (m) threshold GOAL_X_ABS = 0.65 # estimated x coordinate of goals (+/-), adjust if field differs SMOOTH_ALPHA = 0.25 # low-pass smoothing for bearings # ----------------------------- # ADAPTERS (devices & helpers) # ----------------------------- try: from controller import Robot robot = Robot() # timestep try: TIMESTEP = int(robot.getBasicTimeStep()) except Exception: pass # Try to attach motors using common names motor_names = [ ('left wheel motor', 'right wheel motor'), ('left wheel', 'right wheel'), ('left_motor', 'right_motor'), ('left_wheel_motor', 'right_wheel_motor'), ('motor_left', 'motor_right') ] left_motor = right_motor = None for ln, rn in motor_names: try: lm = robot.getDevice(ln) rm = robot.getDevice(rn) lm.setPosition(float('inf')); rm.setPosition(float('inf')) lm.setVelocity(0.0); rm.setVelocity(0.0) left_motor, right_motor = lm, rm break except Exception: continue assert left_motor and right_motor, "Motors not found. Rename in ADAPTERS section." # Compass (for absolute field heading) compass = None for name in ['compass', 'imu compass', 'my_compass']: try: c = robot.getDevice(name); c.enable(TIMESTEP); compass = c; break except Exception: pass # GPS (for side/goal selection & positioning) gps = None for name in ['gps', 'my_gps', 'global_gps']: try: g = robot.getDevice(name); g.enable(TIMESTEP); gps = g; break except Exception: pass # Sonar / proximity sensors (front L/R); fall back to e-puck ps0..ps7 front_left, front_right = None, None sonar_pairs = [ ('sonar_left', 'sonar_right'), ('ultrasonic_left', 'ultrasonic_right'), ('front_left', 'front_right') ] for ln, rn in sonar_pairs: try: fl = robot.getDevice(ln); fr = robot.getDevice(rn) fl.enable(TIMESTEP); fr.enable(TIMESTEP) front_left, front_right = fl, fr; break except Exception: pass if front_left is None or front_right is None: # Try e-puck style proximity array ps = [] for i in range(8): try: d = robot.getDevice(f'ps{i}'); d.enable(TIMESTEP); ps.append(d) except Exception: pass # Map rough front sensors if len(ps) >= 8: front_left, front_right = ps[7], ps[0] # (ps7 left-front, ps0 right-front) # "IR ball sensor" — many templates expose a bearing to ball via a Receiver or custom supervisor. # We try a few common options: receiver = None for name in ['ball_receiver', 'receiver', 'ir_receiver']: try: r = robot.getDevice(name); r.enable(TIMESTEP); receiver = r; break except Exception: pass # Tiny helpers def clamp(x, lo, hi): return lo if x < lo else hi if x > hi else x def heading_from_compass(comp): # returns yaw in radians (-pi..pi) where 0 faces +x import math if comp is None: return None v = comp.getValues() # [x, z, y] or [x, y, z] depending on robot; robust calc uses atan2 # try both conventions try: return math.atan2(v[0], v[2]) except Exception: return math.atan2(v[0], v[1]) def sonar_m_to_meters(ds): # If ultrasonic, many are already meters. For e-puck ps, values are ~[0..4k]; map to ~[0.02..0.2]m. if ds is None: return 1.0 val = ds.getValue() if val > 10.0: # likely e-puck IR proximity # simple reciprocal mapping (empirical) return clamp(0.02 + (3500.0 / max(1.0, val)) * 0.02, 0.02, 0.25) return float(val) def read_ball_bearing_and_strength(): # Returns (bearing_rad, strength 0..1) if ball is seen, else (None, 0.0). # Case A: the course template sends a 2-float packet [bearing, strength] if receiver and receiver.getQueueLength() > 0: data = receiver.getData() try: import struct, math # Try 2 doubles if len(data) == 16: bearing, power = struct.unpack('dd', data) else: # Assume bytes of floats bearing, power = struct.unpack('ff', data[:8]) receiver.nextPacket() # Normalize bearing = max(-math.pi, min(math.pi, bearing)) power = clamp(power, 0.0, 1.0) return bearing, power except Exception: receiver.nextPacket() # Case B: fall back to a naive "no ball" detection return None, 0.0 # Determine which goal to attack using GPS X (sign). If None, assume we start on the left. attack_sign = +1 # +1 means attack +X goal; -1 means attack -X goal if gps is not None: pos = gps.getValues() if pos and len(pos) >= 2: # If we spawn with x < 0, we usually attack +X (right goal) attack_sign = +1 if pos[0] < 0.0 else -1 # Smoothing memory ball_bearing_smooth = 0.0 have_bearing = False # ----------------------------- # MAIN LOOP (STRATEGY BLOCK) # ----------------------------- import math while robot.step(TIMESTEP) != -1: # Read sensors comp_yaw = heading_from_compass(compass) # may be None gps_pos = gps.getValues() if gps else [0.0, 0.0, 0.0] bl = sonar_m_to_meters(front_left) br = sonar_m_to_meters(front_right) ball_bearing, ball_power = read_ball_bearing_and_strength() # Smooth the bearing if available if ball_bearing is not None: if not have_bearing: ball_bearing_smooth = ball_bearing have_bearing = True else: # low-pass filter ball_bearing_smooth = (1.0 - SMOOTH_ALPHA) * ball_bearing_smooth + SMOOTH_ALPHA * ball_bearing else: have_bearing = False # -------------------- # 1) OBSTACLE AVOIDANCE (highest priority) # -------------------- avoid_turn = 0.0 if bl < WALL_NEAR or br < WALL_NEAR: # turn away from the closer wall avoid_turn = AVOID_GAIN * ( (WALL_NEAR - br) - (WALL_NEAR - bl) ) # -------------------- # 2) BALL TRACK & APPROACH # -------------------- turn_cmd = 0.0 fwd_cmd = 0.0 if have_bearing and ball_power >= BALL_SEEN_MIN: # Turn toward the ball (proportional) turn_cmd = BALL_LOCK_GAIN * ball_bearing_smooth # When roughly in front, go forward forward_factor = max(0.0, 1.0 - abs(ball_bearing_smooth) / (math.pi/2)) fwd_cmd = APPROACH_GAIN * forward_factor # Small kick when we are very close to the ball (approx using sonar average as proxy) avg_front = (bl + br) * 0.5 if avg_front < BALL_CLOSE: fwd_cmd += KICK_BOOST else: # Ball not seen: scan in place; add gentle randomization based on position bias = 0.0 try: bias = 0.15 if gps_pos[2] > 0 else -0.15 except Exception: pass turn_cmd = SCAN_SPEED + bias fwd_cmd = 0.02 # creep forward a little # -------------------- # 3) GOAL BIAS (line up toward opponent goal when ball is centered) # -------------------- goal_bias = 0.0 if comp_yaw is not None and have_bearing and abs(ball_bearing_smooth) < 0.25: # desired heading is toward opponent goal at x = attack_sign * GOAL_X_ABS try: gx = attack_sign * GOAL_X_ABS gy = 0.0 dx = gx - gps_pos[0] dy = gy - gps_pos[2] # z is forward in Webots (x,z plane) desired = math.atan2(dx, dy) # yaw facing +x uses atan2(x, z) # heading error err = (desired - comp_yaw + math.pi) % (2*math.pi) - math.pi goal_bias = 0.6 * err except Exception: pass # Combine commands (avoid > track > goal) omega = avoid_turn if abs(avoid_turn) > 0.01 else (turn_cmd + goal_bias) v = fwd_cmd * (1.0 if abs(avoid_turn) < 0.01 else 0.5) # slow down while avoiding # Convert (v, omega) to wheels WHEEL_BASE = 0.052 # ~e-puck axle (m) – only affects ratio left = v - 0.5 * omega * WHEEL_BASE right = v + 0.5 * omega * WHEEL_BASE # Scale to motor range scale = max(1.0, max(abs(left), abs(right)) / MAX_SPEED) left_speed = clamp(left / scale, -MAX_SPEED, MAX_SPEED) right_speed = clamp(right / scale, -MAX_SPEED, MAX_SPEED) # Safety: if both very close to walls, back out gently if bl < 0.05 and br < 0.05: left_speed, right_speed = -0.6, -0.6 # Send to motors left_motor.setVelocity(left_speed) right_motor.setVelocity(right_speed) except Exception as e: # If this file is imported for testing (not running under Webots), just print the strategy available. print("Robot 1 controller loaded (strategy only). Runtime adapter error:", e)