سنسور تشخیص توپ

سنسور تشخیص توپ به ما جهت توپ نسبت به ربات و فاصله آن از ربات را می‌دهد. جهت توپ یک بردار نرمال با سه مولفه x و y و z می‌باشد.اندازه بردار نرمال همیشه برابر 1 است. با استفاده از یک رابطه مثلثاتی ساده می‌توان زاویه توپ را بر حسب رادیان یا درجه بدست آورد. خروجی این سنسور یک دیکشنری با دو عضو direction و strength بوده و برای خواندن خروجی سنسور از دستور self.get_new_ball_data() استفاده می‌کنیم. پیش از استفاده از این دستور باید در یک شرط بررسی کنیم که توپ توسط سنسور دیده شده یا خیر. بدین منظور از تابع self.is_new_ball_data() استفاده می‌کنیم. کد زیر نحوه خواندن سنسور و چاپ مقدار خروجی را نشان می‌دهد.

from rcj_soccer_robot import RCJSoccerRobot, TIME_STEP
from utils import *

class MyRobot1(RCJSoccerRobot):
    def run(self):
        while self.robot.step(TIME_STEP) != -1:
            if self.is_new_data():
                if self.is_new_ball_data():
                    ball_data = self.get_new_ball_data()
                    print(ball_data)
                else:
                    print('No Ball')

محاسبه مختصات توپ

همانطور که گفته شد بردار direction یک بردار نرمال بوده و اندازه آن برابر 1 است. در صورتی که بخواهیم مختصات توپ را نسبت به مبدا مختصات یعنی مرکز زمین محاسبه کنیم باید مقدار دقیق فاصله توپ از ربات را داشته باشیم. در برنامه زیر زاویه و فاصله توپ نسبت به ربات محاسبه می‌گردد و سپس با توجه به موقعیت فعلی ربات مختصات دقیق توپ با روابط مثلثاتی محاسبه می‌گردد.

from rcj_soccer_robot import RCJSoccerRobot, TIME_STEP
from utils import *

class MyRobot1(RCJSoccerRobot):
    def run(self):
        xb = 0
        yb = 0
        while self.robot.step(TIME_STEP) != -1:
            if self.is_new_data():
                gps = self.get_gps_coordinates()
                heading = self.get_compass_heading()
                xr = gps[0]
                yr = gps[1]
                if self.is_new_ball_data():
                    ball_dir = self.get_new_ball_data()['direction']
                    ball_angle = degrees(atan2(ball_dir[1], ball_dir[0]))
                    ball_distance = abs(0.0166666666/(abs(ball_dir[2])/sqrt(1 - ball_dir[2]**2)))
                    xb = -sin(radians(ball_angle + heading)) * ball_distance + xr
                    yb = cos(radians(ball_angle + heading)) * ball_distance + yr
                    print(xb, yb)
                else:
                    print('No Ball')