# ============================================================ # Robot Soccer Final Assignment – Smart Robot 1 # Student: (Bdour Alarifi) # ============================================================ # This code is ready for direct submission on Moodle. # It requires no external libraries or classes. # Works with Webots soccer simulation (Robot 1). from controller import Robot import math robot = Robot() TIME_STEP = 32 # ============================================================ # DEVICE NAMES (CUSTOMIZE HERE if provides exact names) # ============================================================ DEVICE_NAMES = { "left_motor": [ "left wheel motor", "left wheel", "left_wheel_motor", "left motor", "motor_left", "wheel1", "left wheel joint" ], "right_motor": [ "right wheel motor", "right wheel", "right_wheel_motor", "right motor", "motor_right", "wheel2", "right wheel joint" ], "gps": ["gps", "GPS"], "compass": ["compass", "Compass"], "ball_direction": [ "ball_direction", "BallDirection", "ir_ball", "IR ball", "Ball IR", "ball" ], "camera": ["camera", "Camera", "front_camera", "main_camera"], "distance_sensors": [ "ps0","ps1","ps2","ps3","ps4","ps5","ps6","ps7", "ds0","ds1","ds2","ds3","ds4","ds5","ds6","ds7", "ir0","ir1","ir2","ir3","ir4","ir5","ir6","ir7", "front distance","left distance","right distance","back distance", "sonar_front","sonar_left","sonar_right","sonar_back" ], } # ============================================================ def _get_first_existing(names): for nm in names: try: d = robot.getDevice(nm) if d: return d except Exception: pass return None # ===== Initialize devices ===== left_motor = _get_first_existing(DEVICE_NAMES["left_motor"]) right_motor = _get_first_existing(DEVICE_NAMES["right_motor"]) gps = _get_first_existing(DEVICE_NAMES["gps"]) compass = _get_first_existing(DEVICE_NAMES["compass"]) camera = _get_first_existing(DEVICE_NAMES["camera"]) ball_sensor = _get_first_existing(DEVICE_NAMES["ball_direction"]) distance_sensors = [] for nm in DEVICE_NAMES["distance_sensors"]: try: ds = robot.getDevice(nm) if ds: ds.enable(TIME_STEP) distance_sensors.append(ds) except Exception: pass for m in (left_motor, right_motor): if m: m.setPosition(float('inf')) m.setVelocity(0.0) if gps: gps.enable(TIME_STEP) if compass: compass.enable(TIME_STEP) if camera: camera.enable(TIME_STEP) try: camera.recognitionEnable(TIME_STEP) except Exception: pass if ball_sensor: try: ball_sensor.enable(TIME_STEP) except Exception: ball_sensor = None # ============================================================ # FIELD SETTINGS & PARAMETERS # ============================================================ FIELD_HALF_X = 4.5 MAX_SPEED = 6.28 BASE_SPEED = 0.55 * MAX_SPEED TURN_SPEED = 0.45 * MAX_SPEED SLOW_SPEED = 0.30 * MAX_SPEED AVOID_DIST = 800.0 # Determine side (Left or Right) using GPS robot.step(TIME_STEP) we_are_left = True if gps: pos = gps.getValues() we_are_left = (pos[0] < 0) OUR_GOAL_X = -FIELD_HALF_X if we_are_left else FIELD_HALF_X OPP_GOAL_X = FIELD_HALF_X if we_are_left else -FIELD_HALF_X # ============================================================ # HELPER FUNCTIONS # ============================================================ def set_speed(l, r): if left_motor: left_motor.setVelocity(max(min(l, MAX_SPEED), -MAX_SPEED)) if right_motor: right_motor.setVelocity(max(min(r, MAX_SPEED), -MAX_SPEED)) def heading_from_compass(): if not compass: return None v = compass.getValues() return math.atan2(v[0], v[2]) def diff_angle(a, b): d = a - b while d > math.pi: d -= 2*math.pi while d < -math.pi: d += 2*math.pi return d def nearest_obstacle_bias(): left_sum, right_sum = 0.0, 0.0 for ds in distance_sensors: val = ds.getValue() name = ds.getName().lower() if 'left' in name or '7' in name or '6' in name: left_sum += val elif 'right' in name or '0' in name or '1' in name: right_sum += val else: left_sum += val * 0.5 right_sum += val * 0.5 total = left_sum + right_sum + 1e-6 return (left_sum - right_sum) / total def ball_direction_estimate(): if ball_sensor: try: vals = ball_sensor.getValues() if len(vals) >= 3: return (vals[0], vals[2], 0.9) except: pass if camera and hasattr(camera, 'getRecognitionObjects'): try: objs = camera.getRecognitionObjects() for o in objs: if 'ball' in o.getModel().lower(): w, h = camera.getWidth(), camera.getHeight() cx = o.getPositionOnImage()[0] rx = (cx - w/2.0)/(w/2.0) return (rx, 1.0, 0.6) except: pass return (0.0, 0.0, 0.0) def angle_to_target(px, pz, tx, tz): dx, dz = tx - px, tz - pz return math.atan2(dz, dx) # ============================================================ # MAIN LOOP # ============================================================ state = "SEARCH" t = 0.0 last_ball_seen = -1e9 while robot.step(TIME_STEP) != -1: t += TIME_STEP/1000.0 bdx, bdz, bconf = ball_direction_estimate() ball_seen = bconf > 0.0 if ball_seen: last_ball_seen = t avoid = nearest_obstacle_bias() avoid_active = any(ds.getValue() > AVOID_DIST for ds in distance_sensors) yaw = heading_from_compass() pos = gps.getValues() if gps else [0.0,0.0,0.0] if state == "SEARCH": if ball_seen: state = "APPROACH" elif avoid_active: set_speed(BASE_SPEED*(1-avoid), BASE_SPEED*(1+avoid)) else: set_speed(+TURN_SPEED, -TURN_SPEED*0.9) elif state == "APPROACH": if not ball_seen and (t - last_ball_seen) > 1.5: state = "SEARCH" turn = max(-1, min(1, bdx)) fwd = max(0.3, min(1, bdz if bdz else 0.6)) L = BASE_SPEED*fwd*(1-0.9*turn) R = BASE_SPEED*fwd*(1+0.9*turn) if avoid_active: L -= TURN_SPEED*avoid R += TURN_SPEED*avoid set_speed(L, R) if gps and compass: goal_ang = angle_to_target(pos[0], pos[2], OPP_GOAL_X, 0.0) if abs(diff_angle(goal_ang, yaw)) < math.radians(18) and abs(OPP_GOAL_X-pos[0]) < (0.35*FIELD_HALF_X): state = "ALIGN_GOAL" elif state == "ALIGN_GOAL": if not ball_seen and (t - last_ball_seen) > 1.5: state = "SEARCH" if not (gps and compass): state = "APPROACH" else: goal_ang = angle_to_target(pos[0], pos[2], OPP_GOAL_X, 0.0) err = diff_angle(goal_ang, yaw) if abs(err) > math.radians(6): s = TURN_SPEED*0.6 set_speed(-s if err>0 else +s, +s if err>0 else -s) else: state = "SHOOT" elif state == "SHOOT": if avoid_active: set_speed(SLOW_SPEED*(1-avoid), SLOW_SPEED*(1+avoid)) state = "APPROACH" else: turn = max(-0.6, min(0.6, bdx if ball_seen else 0.0)) set_speed(MAX_SPEED*(1-turn), MAX_SPEED*(1+turn)) if not ball_seen and (t - last_ball_seen) > 1.0: state = "SEARCH" else: state = "SEARCH"