Skip to content

Commit

Permalink
feat(strategy/iron2023): implement robot get item dunder method
Browse files Browse the repository at this point in the history
  • Loading branch information
jpkleal committed Mar 1, 2023
1 parent 058002f commit 8c42a86
Showing 1 changed file with 24 additions and 16 deletions.
40 changes: 24 additions & 16 deletions entities/Robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
from commons.math import angular_speed, rotate_via_numpy
from commons.math import speed as avg_speed


class Robot(object):

def __init__(self, game, robot_id, team_color):
Expand Down Expand Up @@ -50,7 +51,7 @@ def __init__(self, game, robot_id, team_color):

def start(self):
self.strategy.start(self)

def get_name(self):
return 'ROBOT_{}_{}'.format(self.robot_id, self.team_color)

Expand All @@ -75,14 +76,13 @@ def update(self, frame):
self.update_stuckness()

def get_speed(self):
return (self.vx**2 + self.vy**2)**.5
return (self.vx ** 2 + self.vy ** 2) ** .5

def _update_speeds(self):
self._frames['x'].append(self.current_data['x'])
self._frames['y'].append(self.current_data['y'])
self._frames['theta'].append(self.current_data['orientation'])


self.theta = self.current_data['orientation']

self.x = self.current_data['x']
Expand All @@ -92,34 +92,33 @@ def _update_speeds(self):
self.vy = avg_speed(self._frames['y'], self.game.vision._fps)
self.vtheta = angular_speed(self._frames['theta'], self.game.vision._fps)

self.speed = math.sqrt(self.vx**2 + self.vy**2)
self.speed = math.sqrt(self.vx ** 2 + self.vy ** 2)

def update_stuckness(self):
MIN_STUCK_SPEED = 0.005

if self.game.use_referee and not self.game.referee.can_play:
self.stuck_time = 0

if(self.speed <= MIN_STUCK_SPEED):
if (self.speed <= MIN_STUCK_SPEED):
self.stuck_time += 1
else:
self.stuck_time = 0

def is_stuck(self):
MIN_STUCK_TIME = 1 # in seconds
MIN_STUCK_TIME = 1 # in seconds
if self.game.vision._fps > 0:
time_in_seconds = self.stuck_time/self.game.vision._fps
time_in_seconds = self.stuck_time / self.game.vision._fps
if time_in_seconds > MIN_STUCK_TIME:
return True
return False


def _get_desired_differential_robot_speeds(self, vx, vy, theta):
'''
Entradas: velocidades no eixo X, Y
Saidas: velocidades linear, angular
'''

speed_vector = np.array([vx, vy])
speed_norm = np.linalg.norm(speed_vector)
robot_world_speed = list(rotate_via_numpy(speed_vector, theta))
Expand All @@ -129,14 +128,13 @@ def _get_desired_differential_robot_speeds(self, vx, vy, theta):
if robot_world_speed[0] > 0.0:
robot_world_speed[1] = -robot_world_speed[1]
robot_world_speed[0] = -robot_world_speed[0]

robot_angle_speed = -math.atan2(robot_world_speed[1], robot_world_speed[0])

# va = signal of robot_angle_speed {-1, 1} * robot_world_speed.y [0, 1] * math.pi (max_speed = PI rad/s )
va = (robot_angle_speed/abs(robot_angle_speed)) * robot_world_speed[1] * math.pi
va = (robot_angle_speed / abs(robot_angle_speed)) * robot_world_speed[1] * math.pi
return vl, va


def _get_differential_robot_speeds(self, vx, vy, theta):
'''
Entradas: velocidades no eixo X, Y
Expand All @@ -149,9 +147,8 @@ def _get_differential_robot_speeds(self, vx, vy, theta):
vl = robot_world_speed[0] * speed_norm

va = self.vtheta

return vl, va


def decide(self):
desired = self.strategy.decide()
Expand All @@ -160,11 +157,22 @@ def decide(self):

return self._get_command(self.power_left, self.power_right)


def _get_command(self, power_left, power_right):
return {
'robot_id': self.robot_id,
'wheel_left': power_left,
'wheel_right': power_right,
'color': self.team_color
}

def __getitem__(self, item):
if item == 0:
return self.x

if item == 1:
return self.y

if item == 2:
return self.theta

raise IndexError("Robot only has 3 coordinates")

0 comments on commit 8c42a86

Please sign in to comment.