الگوریتم گل زدن

پیش از اینکه به سراغ الگوریتم گل زدن برویم باید برنامه ای بنویسیم که ربات به طرف توپ حرکت کند. بهتر است کدهای مربوط به خواندن سنسورها از جمله سنسور توپ، قطب نما، GPS را در قالب یک تابع در فایل utils.py بنویسیم زیرا قرار است برای هر سه ربات اجرا شود. نام این تابع را readData() می‌گذاریم. متغیر هایی که در توابع مختلف قرار است استفاده شود نیز بهتر است برای آبجکت robot تعریف شود تا هر ربات پارامترها و متغیرهای خود را داشته باشد و بتوان در توابع مختلف از آنها استفاده کرد. برای تعریف اولیه متغیرها نیز تابعی به نام defineVariables() تعریف شده که مقادیر اولیه همه متغییر ها را برابر صفر قرار می‌دهد.

from math import *

def defineVariables(robot):
    robot.xr = 0
    robot.yr = 0
    robot.xb = 0
    robot.yb = 0
    robot.heading = 0
    robot.ball_angle = 0
    robot.ball_distance = 0
    robot.is_ball = False

def readData(robot):
    gps = robot.get_gps_coordinates()
    robot.heading = degrees(robot.get_compass_heading())
    robot.xr = gps[0]
    robot.yr = gps[1]
    if robot.is_new_ball_data():
        ball_dir = robot.get_new_ball_data()['direction']
        robot.ball_angle = degrees(atan2(ball_dir[1], ball_dir[0]))
        robot.ball_distance = abs(0.0166666666/(abs(ball_dir[2])/sqrt(1 - ball_dir[2]**2)))
        robot.xb = -sin(radians(robot.ball_angle + robot.heading)) * robot.ball_distance + robot.xr
        robot.yb = cos(radians(robot.ball_angle + robot.heading)) * robot.ball_distance + robot.yr
        robot.is_ball = True
    else:
        robot.is_ball = False

def stop(robot):
    robot.left_motor.setVelocity(0)
    robot.right_motor.setVelocity(0)

def motor(robot, left_speed, right_speed):
    if left_speed > 10: left_speed = 10
    if left_speed < -10: left_speed = -10
    if right_speed > 10: right_speed = 10
    if right_speed < -10: right_speed = -10
    robot.left_motor.setVelocity(left_speed)
    robot.right_motor.setVelocity(right_speed)

def move(robot, xt, yt):
    e = degrees(atan2(robot.xr - xt, yt - robot.yr)) - robot.heading
    if e > 180: e -= 360
    elif e <-180: e += 360
    motor(robot, 10 + e, 10 - e)

حال کافی است برای هر کدام از ربات‌ها این دو تابع را فراخانی کنیم. تابع defineVariables() قبل از حلقه while و تابع readData() در ابتدای حلقه while قرار می‌گیرد. این بدین معناست که تعریف متغیرها در ابتدا و پیش از شروع حلقه انجام می‌گردد و خواندن مقادیر سنسورها نیز در ابتدای هر تکرار حلقه. در برنامه زیر ابتدا با شرط self.is_Ball بررسی می‌شود که توپ در دید ربات است یا خیر و سپس به ربات دستور می‌دهیم در صورت وجود توپ به سمت آن حرکت کند و در غیر این صورت بایستد.

from rcj_soccer_robot import RCJSoccerRobot, TIME_STEP
from utils import *

class MyRobot1(RCJSoccerRobot):
    def run(self):
        defineVariables(self)
        while self.robot.step(TIME_STEP) != -1:
            if self.is_new_data():
                readData(self)
                if self.is_ball:
                    move(self, self.xb, self.yb)
                else:
                    stop(self)

تا کنون برنامه‌ای نوشتیم که ربات‌ها به سمت توپ حرکت کنند، اما این برنامه برای گل زدن مناسب نیست و در برخی موقعیت‌ها موجب گل به خودی زدن می‌شود. به همین خاطر در ادامه برنامه ای خواهیم نوشت که ربات ابتدا به نقطه‌ای انتخابی در پشت توپ رفته و سپس از این نقطه به توپ ضربه بزند تا شانس گل زدن به دروازه حریف و کسب امتیاز بیشتر شود. انتخاب نقطه پشت توپ می‌تواند به چندین صورت محاسبه گردد. ساده ترین روش کم کردن یک مقدار مشخص از y توپ است. به عنوان مثال اگر توپ در نقطه (xb, yb) قرار داشته باشد ابتدا به ربات دستور می‌دهیم به نقطه (xb, yb - 0.1) برود و پس از رسیدن به این نقطه به طرف توپ حرکت کند.

from rcj_soccer_robot import RCJSoccerRobot, TIME_STEP
from utils import *

class MyRobot1(RCJSoccerRobot):
    def run(self):
        defineVariables(self)
        while self.robot.step(TIME_STEP) != -1:
            if self.is_new_data():
                readData(self)
                if self.is_ball:
                    if self.ball_distance > 0.15:
                        move(self, self.xb, self.yb - 0.1)
                    else:
                        move(self, self.xb, self.yb)
                else:
                    stop(self)