# Robot 1 — smarter 1v1 soccer controller (no external libs, simple Python) # Sensors used if present: IR ball sensor (or Receiver), compass, GPS, sonar (ps0..ps7) # Strategy: search -> approach ball -> align to opponent goal -> drive/kick # Safety: obstacle avoidance, wall escape, gentle when near opponent/walls # Works for Yellow or Blue (auto-detect attack direction from starting side) from controller import Robot import math import sys from collections import deque # ========================= # ---- CONFIG & LIMITS ---- # ========================= MAX_SPEED = 6.0 # conservative for e-puck (max ~6.28) CRUISE_SPEED = 4.0 TURN_SPEED = 3.2 SPIN_SPEED = 2.5 KICK_SPEED = 6.0 # Ball logic thresholds BALL_SEEN_TIMEOUT = 1.5 # seconds before we switch to search BALL_CLOSE_DIR_EPS = 0.15 # radians (~8.6 deg) considered "centered" BALL_VERY_CLOSE = 0.20 # meters: GPS distance to ball is unknown; use approach heuristics # Avoidance thresholds (tune to your sonar scale; e-puck IR ~0..~4000) NEAR_LEFT = 90 NEAR_RIGHT = 90 NEAR_FRONT = 120 # Field extents (approx; used to avoid ramming walls if GPS is available) FIELD_X = 0.75 FIELD_Y = 0.55 WALL_MARGIN = 0.06 # ========================= # ---- DEVICE NAME HINTS --- # ========================= NAME_OPTIONS = { "left_motor": ["left wheel motor", "left wheel", "left_motor", "left_wheel"], "right_motor": ["right wheel motor", "right wheel", "right_motor", "right_wheel"], # IR-style ball direction sensors (some worlds use a Receiver that gives direction vector) "ball_ir": ["ir", "ir_ball", "ball_sensor", "ballIR", "ball"], "receiver": ["receiver", "emitter_receiver", "radio", "rx"], # Proximity / sonar (e-puck style) "sonar": [f"ps{i}" for i in range(8)], "compass": ["compass", "inertial compass", "imu_compass"], "gps": ["gps", "GPS"], } # ========================= # ---- UTILITY HELPERS ---- # ========================= def try_get_device(robot, names): for n in names: dev = robot.getDevice(n) if robot.getDevice(n) is not None else None if dev: return dev return None def clamp(v, lo, hi): if v < lo: return lo if v > hi: return hi return v def set_speed(left, right): left_motor.setVelocity(clamp(left, -MAX_SPEED, MAX_SPEED)) right_motor.setVelocity(clamp(right, -MAX_SPEED, MAX_SPEED)) def heading_deg_from_compass(c3): rad = -math.atan2(c3[0], c3[2]) deg = (rad * 180.0 / math.pi) % 360.0 return deg def smallest_angle_rad(a): while a > math.pi: a -= 2*math.pi while a < -math.pi: a += 2*math.pi return a # ========================= # ---- CONTROLLER START ---- # ========================= robot = Robot() TIMESTEP = int(robot.getBasicTimeStep()) if TIMESTEP <= 0: TIMESTEP = 32 left_motor = try_get_device(robot, NAME_OPTIONS["left_motor"]) right_motor = try_get_device(robot, NAME_OPTIONS["right_motor"]) if left_motor is None or right_motor is None: print("ERROR: wheel motors not found. Adjust NAME_OPTIONS.", file=sys.stderr) for m in (left_motor, right_motor): if m: m.setPosition(float('inf')) m.setVelocity(0.0) ball_ir = try_get_device(robot, NAME_OPTIONS["ball_ir"]) receiver = try_get_device(robot, NAME_OPTIONS["receiver"]) compass = try_get_device(robot, NAME_OPTIONS["compass"]) gps = try_get_device(robot, NAME_OPTIONS["gps"]) sonars = [] for n in NAME_OPTIONS["sonar"]: dev = robot.getDevice(n) if robot.getDevice(n) is not None else None if dev: dev.enable(TIMESTEP) sonars.append(dev) if ball_ir: try: ball_ir.enable(TIMESTEP) except: pass if receiver: try: receiver.enable(TIMESTEP) except: pass if compass: compass.enable(TIMESTEP) if gps: gps.enable(TIMESTEP) state = "SEARCH" time_since_ball = 0.0 spin_dir = 1 spin_timer = 0.0 spin_period = 2.8 recent_ball_dirs = deque(maxlen=8) start_x = None target_goal_x = None def read_ball_direction(): if ball_ir: try: v = ball_ir.getValue() if isinstance(v, (int, float)) and v != 0: ang = float(v) if abs(ang) > math.pi * 1.1: ang = math.radians(ang) return smallest_angle_rad(ang), 1.0 except: pass try: vals = ball_ir.getValues() if vals and len(vals) >= 3: bx, by, bz = float(vals[0]), float(vals[1]), float(vals[2]) ang = math.atan2(bx, bz) strength = math.sqrt(bx*bx + bz*bz) return smallest_angle_rad(ang), strength except: pass if receiver and receiver.getQueueLength() > 0: msg = None while receiver.getQueueLength() > 0: data = receiver.getData() try: text = data.decode('utf-8') msg = text except: msg = None if len(data) >= 8: try: import struct bx, bz = struct.unpack('ff', data[:8]) receiver.nextPacket() ang = math.atan2(bx, bz) return smallest_angle_rad(ang), math.hypot(bx, bz) except: pass receiver.nextPacket() if isinstance(msg, str) and msg: if msg.startswith("ANG:"): try: ang = float(msg.split("ANG:")[1].strip()) return smallest_angle_rad(ang), 1.0 except: pass if msg.startswith("DIR:"): try: payload = msg.split("DIR:")[1] parts = payload.split(",") bx = float(parts[0]); bz = float(parts[1]) ang = math.atan2(bx, bz) return smallest_angle_rad(ang), math.hypot(bx, bz) except: pass return None, None while robot.step(TIMESTEP) != -1: dt = TIMESTEP / 1000.0 if gps and start_x is None: p = gps.getValues() start_x = p[0] target_goal_x = FIELD_X if start_x < 0.0 else -FIELD_X elif target_goal_x is None: target_goal_x = FIELD_X heading_deg = None if compass: heading_deg = heading_deg_from_compass(compass.getValues()) pos = None if gps: pos = gps.getValues() left_close = right_close = front_close = False if sonars: svals = [s.getValue() for s in sonars] front_close = any(v > NEAR_FRONT for v in svals[2:6]) if len(svals) >= 6 else False left_close = any(v > NEAR_LEFT for v in svals[:3]) if len(svals) >= 3 else False right_close = any(v > NEAR_RIGHT for v in svals[-3:]) if len(svals) >= 3 else False ball_ang, ball_strength = read_ball_direction() if ball_ang is not None: recent_ball_dirs.append(ball_ang) time_since_ball = 0.0 else: time_since_ball += dt near_wall = False if pos is not None: x, y, z = pos[0], pos[1], pos[2] if (FIELD_X - abs(x)) < WALL_MARGIN or (FIELD_Y - abs(z)) < WALL_MARGIN: near_wall = True if front_close or left_close or right_close or near_wall: state = "AVOID" if state != "AVOID": if ball_ang is None and time_since_ball > BALL_SEEN_TIMEOUT: state = "SEARCH" else: if ball_ang is not None and abs(ball_ang) < BALL_CLOSE_DIR_EPS: state = "ALIGN" else: state = "APPROACH" if state == "AVOID": turn = 0.0 if left_close and not right_close: turn = -TURN_SPEED elif right_close and not left_close: turn = TURN_SPEED else: turn = TURN_SPEED if spin_dir > 0 else -TURN_SPEED set_speed(-0.8 * CRUISE_SPEED + turn, -0.8 * CRUISE_SPEED - turn) if near_wall and heading_deg is not None: to_center_x = 0.0 - (pos[0] if pos else 0.0) to_center_z = 0.0 - (pos[2] if pos else 0.0) desired = math.degrees(math.atan2(to_center_x, to_center_z)) % 360.0 err = smallest_angle_rad(math.radians(desired - heading_deg)) steer = clamp(err * 2.0, -TURN_SPEED, TURN_SPEED) set_speed(-0.5 * CRUISE_SPEED + steer, -0.5 * CRUISE_SPEED - steer) elif state == "SEARCH": spin_timer += dt if spin_timer > spin_period: spin_timer = 0.0 spin_dir *= -1 set_speed(spin_dir * SPIN_SPEED, -spin_dir * SPIN_SPEED) elif state == "APPROACH": if recent_ball_dirs: avg_ang = sum(recent_ball_dirs) / len(recent_ball_dirs) else: avg_ang = ball_ang if ball_ang is not None else 0.0 k_turn = 2.0 steer = clamp(k_turn * avg_ang, -TURN_SPEED, TURN_SPEED) base = CRUISE_SPEED * (1.0 - min(abs(avg_ang) / (math.pi/2), 0.8)) + 1.2 set_speed(base + steer, base - steer) elif state == "ALIGN": if heading_deg is not None and pos is not None: desired_rad = math.atan2(target_goal_x - pos[0], 0.0 - pos[2]) desired_deg = (desired_rad * 180.0 / math.pi) % 360.0 err = smallest_angle_rad(math.radians(desired_deg - heading_deg)) if abs(err) > math.radians(6.0): steer = clamp(err * 2.2, -TURN_SPEED, TURN_SPEED) set_speed(1.6 + steer, 1.6 - steer) else: set_speed(KICK_SPEED, KICK_SPEED) else: set_speed(KICK_SPEED, KICK_SPEED) else: set_speed(0.0, 0.0)