89 lines
3.0 KiB
Python
89 lines
3.0 KiB
Python
from robot import Robot
|
|
from parser import aip_parser
|
|
from laby import Laby
|
|
|
|
from subprocess import Popen, PIPE
|
|
import fcntl
|
|
import os
|
|
from time import sleep
|
|
|
|
# Pygame
|
|
from math import degrees, cos, sin
|
|
import sys, pygame
|
|
from pygame.locals import *
|
|
pygame.init()
|
|
|
|
size = width, height = 500, 300
|
|
black = 0, 0, 0
|
|
|
|
screen = pygame.display.set_mode(size)
|
|
|
|
# on lance le programme arduino
|
|
ard_process = Popen(["build/app"], stdin=PIPE, stdout=PIPE);
|
|
|
|
# stdout du programme arduino en non bloquant pour pouvoir arreter la lecture si il y as plus rien à lire
|
|
fd = ard_process.stdout.fileno()
|
|
fl = fcntl.fcntl(fd, fcntl.F_GETFL)
|
|
fcntl.fcntl(fd, fcntl.F_SETFL, fl | os.O_NONBLOCK)
|
|
|
|
laby = Laby((1.5,0))
|
|
|
|
# le robot
|
|
r = Robot(laby)
|
|
r.light_pos = (laby.pos[0]+laby.L8+laby.L3/2,laby.L17+laby.L4/2)
|
|
r.pos = (0.1, laby.L17+laby.L4/2)
|
|
|
|
# le parser wrapper arduino->simulateur
|
|
parser = aip_parser(ard_process.stdin, ard_process.stdout, r);
|
|
|
|
|
|
while True:
|
|
for event in pygame.event.get():
|
|
if event.type == pygame.QUIT: sys.exit()
|
|
if event.type == pygame.KEYDOWN:
|
|
if event.key == K_q:
|
|
sys.exit()
|
|
|
|
# On envoie le états des pins
|
|
parser.send_pin_state()
|
|
|
|
# On demarre un tour de simulation
|
|
ard_process.stdin.write(b'@')
|
|
ard_process.stdin.flush()
|
|
|
|
# tant qu'il y as qqu chose sur le bus on le parse
|
|
while(parser.poll()):
|
|
pass
|
|
|
|
# on met à jour la simulation
|
|
r.update()
|
|
|
|
# On dessine le bordel
|
|
screen.fill(black)
|
|
laby.draw(screen)
|
|
robotShape = pygame.Rect(20,15, 10,20);
|
|
rsurf = pygame.Surface((50,50)).convert_alpha();
|
|
rsurf.fill((255,255,255,0))
|
|
# robot
|
|
pygame.draw.rect(rsurf, (255,255,255), robotShape);
|
|
# heading
|
|
pygame.draw.line(rsurf, (255,255,255), (25,25), (25,50))
|
|
# dir2light
|
|
pygame.draw.line(rsurf, (255,255,255), (25,25), (25+25*sin(r.angle2spot),25+25*cos(r.angle2spot)))
|
|
# light
|
|
pygame.draw.polygon(screen, (255,255,0), ([100*a for a in r.light_pos],[100*(a+b) for a,b in zip(r.light_pos,(-0.1,0.1))],[100*(a+b) for a,b in zip(r.light_pos,(-0.1,-0.1))]))
|
|
# ultrasonic sensors
|
|
for usrs in r.ultr_sense_rays:
|
|
endl = (r.pos[0]+r.ultr_sense_max*sin(r.rot+usrs+r.ultr_sense_scatter), r.pos[1]+r.ultr_sense_max*cos(r.rot+usrs+r.ultr_sense_scatter))
|
|
endr = (r.pos[0]+r.ultr_sense_max*sin(r.rot+usrs-r.ultr_sense_scatter), r.pos[1]+r.ultr_sense_max*cos(r.rot+usrs-r.ultr_sense_scatter))
|
|
pygame.draw.line(screen, (255,0,0), [p*100 for p in r.pos], [p*100 for p in endl])
|
|
pygame.draw.line(screen, (255,0,0), [p*100 for p in r.pos], [p*100 for p in endr])
|
|
pygame.draw.line(screen, (255,0,0), [p*100 for p in endl], [p*100 for p in endr])
|
|
for c in r.ci:
|
|
if c:
|
|
pygame.draw.circle(screen, (255,0,0), [p*100 for p in c], 3)
|
|
rsurf_r = pygame.transform.rotate(rsurf, degrees(r.rot));
|
|
center = rsurf_r.get_rect().center
|
|
screen.blit(rsurf_r, (r.pos[0]*100-center[0],r.pos[1]*100-center[1]))
|
|
pygame.display.flip()
|