# Mawaddah Ahmed - Final Robot 1 (Webots Soccer) # Notes: # - No external libraries. # - Uses enabled sensors only: IR ball sensors, Compass, Sonar, GPS. # - Simple, robust logic; small helper functions only. # - Safe play: avoids walls/opponent, prevents own goals, recovers when stuck. from controller import Robot robot = Robot() TIME_STEP = 64 # --- Devices --- left_motor = robot.getDevice("left wheel motor") right_motor = robot.getDevice("right wheel motor") # 12 IR ball sensors named IR0 .. IR11 (common in template) ball_sensors = [] for i in range(12): name = "IR" + str(i) if robot.hasDevice(name): s = robot.getDevice(name) s.enable(TIME_STEP) ball_sensors.append(s) # Two front sonars named sonar0 / sonar1 (left/right) sonars = [] for i in range(2): name = "sonar" + str(i) if robot.hasDevice(name): s = robot.getDevice(name) s.enable(TIME_STEP) sonars.append(s) # Compass + GPS compass = robot.getDevice("compass") if robot.hasDevice("compass") else None gps = robot.getDevice("gps") if robot.hasDevice("gps") else None if compass: compass.enable(TIME_STEP) if gps: gps.enable(TIME_STEP) # --- Motors setup --- left_motor.setPosition(float('inf')) right_motor.setPosition(float('inf')) left_motor.setVelocity(0.0) right_motor.setVelocity(0.0) MAX_SPEED = 6.0 CRUISE_SPEED= 5.2 TURN_SPEED = 3.0 # --- Internal state --- init_pos = None if gps: # wait a few steps to get a valid first reading for _ in range(3): robot.step(TIME_STEP) init_pos = gps.getValues() field_x_limit = 0.75 # conservative margins to avoid walls (adjust if needed) field_y_limit = 0.55 # Determine which goal is "ours" by starting side along X. # If we started with x > 0: our goal is +X, opponent is -X (and vice versa). # This avoids accidentally pushing towards our own goal. def get_goal_sign(): if not gps or init_pos is None: return 1 # default return 1 if init_pos[0] >= 0.0 else -1 OUR_GOAL_SIGN = get_goal_sign() # +1 means our goal at +X, opponent at -X OPP_GOAL_SIGN = -OUR_GOAL_SIGN # --- Helpers --- def heading_x_sign(): """Return +1 if heading points roughly to +X, -1 if to -X, 0 if unknown.""" if not compass: return 0 # Webots compass returns a 3D vector pointing north. Heading yaw can be derived. # Here we just use x-component sign approximation via atan2 of components. c = compass.getValues() # [x, y, z] pointing north # Robot forward vector in Webots standard: (-c[0], 0, -c[2]) when facing north. # Approximate projection on X axis: fx = -c[0] if fx > 0.2: # facing +X return 1 elif fx < -0.2: # facing -X return -1 return 0 def ball_direction_index(): """Return index of strongest IR, or None if no ball detected.""" if not ball_sensors: return None values = [s.getValue() for s in ball_sensors] strongest = max(values) if values else 0.0 if strongest < 50.0: # detection threshold (tune as needed) return None return values.index(strongest) def obstacle_ahead(): """Return True if an obstacle is close in front using sonars.""" if not sonars: return False thresh = 800.0 # tune for your sonar scale for s in sonars: try: if s.getValue() < thresh: return True except: pass return False # Edge avoidance using GPS def near_wall(): if not gps: return False x, _, y = gps.getValues() if abs(x) > field_x_limit or abs(y) > field_y_limit: return True return False # Simple stuck detection via GPS movement prev_pos = gps.getValues() if gps else None still_counter = 0 def update_stuck(): global prev_pos, still_counter if not gps: return False pos = gps.getValues() if prev_pos is None: prev_pos = pos return False dx = pos[0] - prev_pos[0] dy = pos[2] - prev_pos[2] if len(pos) > 2 else 0.0 dist2 = dx*dx + dy*dy prev_pos = pos if dist2 < 1e-4: # barely moved still_counter += 1 else: still_counter = 0 return still_counter >= 8 # stuck if ~8 cycles with almost no movement def set_speed(l, r): # clamp speeds if l > MAX_SPEED: l = MAX_SPEED if r > MAX_SPEED: r = MAX_SPEED if l < -MAX_SPEED: l = -MAX_SPEED if r < -MAX_SPEED: r = -MAX_SPEED left_motor.setVelocity(l) right_motor.setVelocity(r) # --- Behavior loop --- spin_dir = 1 # used when searching while robot.step(TIME_STEP) != -1: # 1) Safety first: walls & obstacles if near_wall(): # Back off then turn toward center set_speed(-2.0, -2.0) # brief back step for _ in range(3): robot.step(TIME_STEP) # turn inwards (toward opposite of current Y sign) turn = TURN_SPEED set_speed(-turn, turn) for _ in range(4): robot.step(TIME_STEP) continue if obstacle_ahead(): # Quick avoidance turn set_speed(-1.5, 3.5) continue # 2) Ball handling bdir = ball_direction_index() # 2.a) Recovery if stuck if update_stuck(): # wiggle: reverse then arc set_speed(-2.5, -2.5) for _ in range(3): robot.step(TIME_STEP) set_speed(2.0, -2.0) for _ in range(6): robot.step(TIME_STEP) continue # 3) If no ball seen: search smartly (prefer turning toward opponent goal) if bdir is None: # bias the spin to face opponent goal direction hx = heading_x_sign() desired = OPP_GOAL_SIGN if hx != desired and hx != 0: # rotate toward opponent goal set_speed(TURN_SPEED, -TURN_SPEED) else: # slow spin to scan set_speed(spin_dir*2.0, -spin_dir*2.0) # flip spin direction occasionally to avoid looping spin_dir *= -1 continue # 4) Ball seen: align & push toward opponent goal, avoid own-goal # Map 12 IR sectors: 0..11 around robot. We consider 5..6 as forward-ish. if bdir <= 4: # ball to left set_speed(2.2, 4.6) continue elif bdir >= 7: # ball to right set_speed(4.6, 2.2) continue else: # Ball roughly ahead (5..6): check heading vs opponent goal hx = heading_x_sign() if hx == OUR_GOAL_SIGN: # We are aiming at OUR goal -> arc to swing behind the ball set_speed(2.5, -2.5) # quick pivot continue # Good heading: drive forward to push set_speed(CRUISE_SPEED, CRUISE_SPEED)