class Robo:
"""
Robo
Variaveis do estado:
x , y , angulo: posicao do robo no ambiente.
raio: tamanho do robo.
vel_motor_direita , vel_motor_esquerda: utilizadas na dinamica do robo
"""
class Params:
VELOCIDADE_MAXIMA_MOTOR = 2
REDUCAO_MOTOR_CORREDOR = 0.99 # reducao percentual da velocidade maxima usada no controle. Padrao: 0.99
REDUCAO_MOTOR_CAMPO_ABERTO = 0.90 # reducao percentual da velocidade maxima usada no controle. Padrao: 0.99
VARIACAO_MOTORES = 0.1 # variacao aleatoria na velocidade dos motores. Padrao: 0.1 (10% maximo)
DISTANCIA_MINIMA_PARADA = 15 # distancia minima de uma parede a frente. Padrao: 15
LARGURA_CORREDOR = 60 # Padrao: 60
def __init__(self, x, y, angulo, raio):
self.coord_x = x
self.coord_y = y
self.angulo = angulo
self.direcao = Direcoes.LESTE
self.raio = raio
self.vel_motor_direita = 0
self.vel_motor_esquerda = 0
self.dist_esquerda_anterior = 0
self.dist_direita_anterior = 0
self.mapa = Mapa()
self.mapa.inicializa()
self.lista_de_comandos = []
self.algoritmo_de_movimentacao = algoritmo_de_movimentacao
self.algoritmo_de_movimentacao.setup(self)
self.estado = self.Estados.PARA
self.contagem = 0
self.sensores_distancia = []
self.sensores_distancia.append(SensorDistancia(raio, 0,
0)) # sensor frontal
self.sensores_distancia.append(SensorDistancia(0, raio, math.pi /
2)) # sensor esquerdo
self.sensores_distancia.append(SensorDistancia(0, -raio, -math.pi /
2)) # sensor direito
self.retas = []
self.retas.append([-raio, -raio, raio, -raio, None]) # Roda direita
self.retas.append([-raio, raio, raio, raio, None]) # Roda esquerda
self.referencia_figura = None
self.ideal = True
def mostra_direcao(self):
if self.direcao == Direcoes.NORTE:
print("NORTE")
if self.direcao == Direcoes.SUL:
print("SUL")
if self.direcao == Direcoes.LESTE:
print("LESTE")
if self.direcao == Direcoes.OESTE:
print("OESTE")
if self.direcao == 0:
print("INDEFINIDA")
def determina_direcao(self):
self.direcao = 0
if (self.angulo <= math.pi / 4) and (self.angulo > -math.pi / 4):
self.direcao = Direcoes.LESTE
if (self.angulo > math.pi / 4) and (self.angulo <= 3 * math.pi / 4):
self.direcao = Direcoes.NORTE
if (self.angulo < -math.pi / 4) and (self.angulo >= -3 * math.pi / 4):
self.direcao = Direcoes.SUL
if (self.angulo > 3 * math.pi / 4) or (self.angulo < -3 * math.pi / 4):
self.direcao = Direcoes.OESTE
class Estados(enum.Enum):
ANDA = 1 # anda indefinidamente
ANDA1 = 2 # anda uma celula
PARA = 3
GIRA_DIREITA = 4
GIRA_ESQUERDA = 5
def mostra_estado(self):
if self.estado == self.Estados.ANDA:
print("ANDA")
if self.estado == self.Estados.ANDA1:
print("ANDA1")
elif self.estado == self.Estados.PARA:
print("PARA")
elif self.estado == self.Estados.GIRA_DIREITA:
print("GIRA_DIREITA")
elif self.estado == self.Estados.GIRA_ESQUERDA:
print("GIRA_ESQUERDA")
elif self.estado == self.Estados.GIRA_PARA:
print("PARA")
@staticmethod
def rotaciona(x, y, angulo):
x_rotacionado = x * math.cos(angulo) - y * math.sin(angulo)
y_rotacionado = x * math.sin(angulo) + y * math.cos(angulo)
return x_rotacionado, y_rotacionado
def atualiza_sensores(self, lista_obstaculos):
"""
Atualiza a leitura dos sensores
"""
for sensor in self.sensores_distancia:
sensor.inicializa()
for obstaculo in lista_obstaculos:
for sensor in self.sensores_distancia:
coord_x_rotacionada, coord_y_rotacionada = self.rotaciona(
sensor.coord_x_relativa, sensor.coord_y_relativa,
self.angulo)
coord_x_absoluta = self.coord_x + coord_x_rotacionada
coord_y_absoluta = self.coord_y + coord_y_rotacionada
sensor.determina_distancia_obstaculo(
coord_x_absoluta, coord_y_absoluta,
self.angulo + sensor.angulo_relativo, obstaculo)
def verifica_colisao(self, lista_obstaculos, coord_x_prevista,
coord_y_prevista):
restringe_x = False
restringe_y = False
for obstaculo in lista_obstaculos:
x1 = obstaculo[0]
y1 = obstaculo[1]
x2 = obstaculo[2]
y2 = obstaculo[3]
limite_direito = coord_x_prevista + self.raio
limite_esquerdo = coord_x_prevista - self.raio
limite_superior = coord_y_prevista - self.raio
limite_inferior = coord_y_prevista + self.raio
if ((limite_direito > x1) and
(limite_direito < x2)) or ((limite_esquerdo > x1) and
(limite_esquerdo < x2)):
if (limite_inferior < y1) and (limite_inferior > y2) or (
limite_superior < y1) and (limite_superior > y2):
restringe_x = True
restringe_y = True
if restringe_x:
coord_x_final = self.coord_x
else:
coord_x_final = coord_x_prevista
if restringe_y:
coord_y_final = self.coord_y
else:
coord_y_final = coord_y_prevista
return coord_x_final, coord_y_final
def dinamica_robo(self, lista_obstaculos):
"""
Atualiza a posicao e angulo do robo com base no algoritmo de controle.
"""
self.atualiza_sensores(lista_obstaculos)
self.controle()
velocidade = (self.vel_motor_direita + self.vel_motor_esquerda) / 2
self.angulo = self.angulo + (self.vel_motor_direita -
self.vel_motor_esquerda) * math.pi / 24
if self.angulo > math.pi:
self.angulo -= 2 * math.pi
if self.angulo < -math.pi:
self.angulo += 2 * math.pi
coord_x_prevista = self.coord_x + velocidade * math.cos(self.angulo)
coord_y_prevista = self.coord_y + velocidade * math.sin(self.angulo)
self.coord_x, self.coord_y = self.verifica_colisao(
lista_obstaculos, coord_x_prevista, coord_y_prevista)
self.determina_direcao()
# self.mostra_direcao()
dist_frente = self.sensores_distancia[0].distancia
dist_esquerda = self.sensores_distancia[1].distancia
dist_direita = self.sensores_distancia[2].distancia
if (self.estado == self.Estados.ANDA) or (self.estado
== self.Estados.ANDA1):
self.mapa.atualiza(self.coord_x, self.coord_y, dist_frente,
dist_esquerda, dist_direita, self.direcao)
if self.estado == self.Estados.PARA:
self.avanca_sequencia_de_comandos()
self.algoritmo_de_movimentacao.loop(self)
def avanca_sequencia_de_comandos(self):
#self.mostra_estado()
if len(self.lista_de_comandos) > 0:
self.estado = self.lista_de_comandos[0]
del self.lista_de_comandos[0]
if self.estado == self.Estados.ANDA1:
if (self.direcao == Direcoes.LESTE) or (self.direcao
== Direcoes.OESTE):
self.contagem = int(
self.mapa.fator_escala_x /
self.Params.VELOCIDADE_MAXIMA_MOTOR) + 1
if (self.direcao == Direcoes.NORTE) or (self.direcao
== Direcoes.SUL):
self.contagem = int(
self.mapa.fator_escala_y /
self.Params.VELOCIDADE_MAXIMA_MOTOR) + 1
if self.estado == self.Estados.GIRA_DIREITA:
if self.ideal:
self.contagem = 9
else:
self.contagem = 8 + random.randint(0, 2)
if self.estado == self.Estados.GIRA_ESQUERDA:
if self.ideal:
self.contagem = 9
else:
self.contagem = 8 + random.randint(0, 2)
#self.mostra_estado()
def aberto_direita(self):
celula_atual = self.mapa.celula_atual()
if self.direcao == Direcoes.OESTE:
if celula_atual.aberto_norte:
return True
if self.direcao == Direcoes.LESTE:
if celula_atual.aberto_sul:
return True
if self.direcao == Direcoes.NORTE:
if celula_atual.aberto_leste:
return True
if self.direcao == Direcoes.SUL:
if celula_atual.aberto_oeste:
return True
return False
def aberto_esquerda(self):
celula_atual = self.mapa.celula_atual()
if self.direcao == Direcoes.OESTE:
if celula_atual.aberto_sul:
return True
if self.direcao == Direcoes.LESTE:
celula_atual = self.mapa.celula_atual()
if celula_atual.aberto_norte:
return True
if self.direcao == Direcoes.NORTE:
celula_atual = self.mapa.celula_atual()
if celula_atual.aberto_oeste:
return True
if self.direcao == Direcoes.SUL:
celula_atual = self.mapa.celula_atual()
if celula_atual.aberto_leste:
return True
return False
def aberto_adiante(self):
celula_atual = self.mapa.celula_atual()
if self.direcao == Direcoes.OESTE:
if celula_atual.aberto_oeste:
return True
if self.direcao == Direcoes.LESTE:
if celula_atual.aberto_leste:
return True
if self.direcao == Direcoes.NORTE:
if celula_atual.aberto_norte:
return True
if self.direcao == Direcoes.SUL:
if celula_atual.aberto_sul:
return True
return False
def controle(self):
"""
Algoritmo de controle dos motores do robo.
O robo eh controlado por:
self.vel_motor_direita
self.vel_motor_esquerda
Os sensores são lidos na configuracao atual do robo em:
self.sensores_distancia[0].distancia # frontal;
self.sensores_distancia[1].distancia # esquerdo;
self.sensores_distancia[2].distancia # direito;
O estado atual do robo pode ser lido em:
self.estado
e pode ser:
self.Estados.ANDA - anda ate encontrar uma parede;
self.Estados.ANDA1 - anda uma celula a frente e para;
self.Estados.GIRA_DIREITA - gira para a direita;
self.Estados.GIRA_ESQUERDA - gira para a esquerda;
O tempo em cada etapa eh controlado por
self.contagem
"""
dist_frontal = self.sensores_distancia[0].distancia
dist_esquerda = self.sensores_distancia[1].distancia
dist_direita = self.sensores_distancia[2].distancia
#self.mostra_estado()
if (self.estado == self.Estados.ANDA) or (self.estado
== self.Estados.ANDA1):
if dist_frontal > self.Params.DISTANCIA_MINIMA_PARADA:
if self.ideal:
self.vel_motor_direita = self.Params.VELOCIDADE_MAXIMA_MOTOR
self.vel_motor_esquerda = self.Params.VELOCIDADE_MAXIMA_MOTOR
else:
self.vel_motor_direita = self.Params.VELOCIDADE_MAXIMA_MOTOR + self.Params.VARIACAO_MOTORES * random.random(
)
self.vel_motor_esquerda = self.Params.VELOCIDADE_MAXIMA_MOTOR + self.Params.VARIACAO_MOTORES * random.random(
)
if not self.ideal:
if dist_direita + dist_esquerda < self.Params.LARGURA_CORREDOR:
if dist_direita > dist_esquerda:
self.vel_motor_direita = self.Params.VELOCIDADE_MAXIMA_MOTOR * self.Params.REDUCAO_MOTOR_CORREDOR
if dist_esquerda > dist_direita:
self.vel_motor_esquerda = self.Params.VELOCIDADE_MAXIMA_MOTOR * self.Params.REDUCAO_MOTOR_CORREDOR
else:
if dist_direita > dist_esquerda:
if dist_esquerda > self.dist_esquerda_anterior:
self.vel_motor_esquerda = self.Params.VELOCIDADE_MAXIMA_MOTOR * self.Params.REDUCAO_MOTOR_CAMPO_ABERTO
self.vel_motor_direita = self.Params.VELOCIDADE_MAXIMA_MOTOR
else:
self.vel_motor_direita = self.Params.VELOCIDADE_MAXIMA_MOTOR * self.Params.REDUCAO_MOTOR_CAMPO_ABERTO
self.vel_motor_esquerda = self.Params.VELOCIDADE_MAXIMA_MOTOR
if dist_esquerda > dist_direita:
if dist_direita > self.dist_direita_anterior:
self.vel_motor_direita = self.Params.VELOCIDADE_MAXIMA_MOTOR * self.Params.REDUCAO_MOTOR_CAMPO_ABERTO
self.vel_motor_esquerda = self.Params.VELOCIDADE_MAXIMA_MOTOR
else:
self.vel_motor_esquerda = self.Params.VELOCIDADE_MAXIMA_MOTOR * self.Params.REDUCAO_MOTOR_CAMPO_ABERTO
self.vel_motor_direita = self.Params.VELOCIDADE_MAXIMA_MOTOR
else:
if self.direcao == Direcoes.LESTE:
self.angulo = 0
elif self.direcao == Direcoes.OESTE:
self.angulo = math.pi
elif self.direcao == Direcoes.NORTE:
self.angulo = math.pi / 2
elif self.direcao == Direcoes.LESTE:
self.angulo = -math.pi / 2
if dist_frontal < self.Params.DISTANCIA_MINIMA_PARADA:
self.vel_motor_direita = 0
self.vel_motor_esquerda = 0
self.estado = self.Estados.PARA
if self.estado == self.Estados.ANDA1:
self.contagem -= 1
if self.contagem == 0:
self.estado = self.Estados.PARA
if self.estado == self.Estados.PARA:
self.vel_motor_direita = 0
self.vel_motor_esquerda = 0
if self.estado == self.Estados.GIRA_DIREITA:
self.vel_motor_direita = -self.Params.VELOCIDADE_MAXIMA_MOTOR / 3
self.vel_motor_esquerda = self.Params.VELOCIDADE_MAXIMA_MOTOR / 3
self.contagem = self.contagem - 1
if self.contagem == 0:
self.estado = self.Estados.PARA
if self.estado == self.Estados.GIRA_ESQUERDA:
self.vel_motor_direita = self.Params.VELOCIDADE_MAXIMA_MOTOR / 3
self.vel_motor_esquerda = -self.Params.VELOCIDADE_MAXIMA_MOTOR / 3
self.contagem = self.contagem - 1
if self.contagem == 0:
self.estado = self.Estados.PARA
#self.mostra_estado()
self.dist_esquerda_anterior = self.dist_esquerda_anterior * 0.7 + dist_esquerda * 0.3
self.dist_direita_anterior = self.dist_direita_anterior * 0.7 + dist_direita * 0.3
