def plot(self, color=config.COLOR_POLYGON):
"Desenha o poligono na tela"
p = self.pts
while p.next != self.pts:
self.cid[p] = p.lineto(p.next, color)
p = p.next
self.cid[p] = p.lineto(p.next, color)
control.update()
def plot(self, color=config.COLOR_POLYGON):
"Desenha o poligono na tela"
p = self.pts
while p.next != self.pts:
self.cid[p] = p.lineto(p.next, color)
p = p.next
self.cid[p] = p.lineto(p.next, color)
control.update()
def hilight(self, color_line=config.COLOR_HI_POLYGON, color_point=config.COLOR_HI_POLYGON_POINT):
"Desenha o poligono com destaque na tela"
p = self.pts
while p.next != self.pts:
self.hid[p] = p.lineto(p.next, color_line)
self.hidp[p] = p.hilight(color_point)
p = p.next
self.hid[p] = p.lineto(p.next, color_line)
self.hidp[p] = p.hilight(color_point)
control.update()
def hilight(self,
color_line=config.COLOR_HI_POLYGON,
color_point=config.COLOR_HI_POLYGON_POINT):
"Desenha o poligono com destaque na tela"
p = self.pts
while p.next != self.pts:
self.hid[p] = p.lineto(p.next, color_line)
self.hidp[p] = p.hilight(color_point)
p = p.next
self.hid[p] = p.lineto(p.next, color_line)
self.hidp[p] = p.hilight(color_point)
control.update()
def hide(self):
"Apaga o poligono na tela"
p = self.pts
while p.next != self.pts:
if self.cid.has_key(p):
control.plot_delete(self.cid[p])
del (self.cid[p])
p = p.next
if self.cid.has_key(p):
control.plot_delete(self.cid[p])
del (self.cid[p])
control.update()
def hide(self):
"Apaga o poligono na tela"
p = self.pts
while p.next != self.pts:
if self.cid.has_key(p):
control.plot_delete(self.cid[p])
del (self.cid[p])
p = p.next
if self.cid.has_key(p):
control.plot_delete(self.cid[p])
del (self.cid[p])
control.update()
def triang(a, b, c):
"desenha (e apaga) os lados do triangulo abc"
a.lineto(c, config.COLOR_PRIM)
b.lineto(a, config.COLOR_PRIM)
c.lineto(b, config.COLOR_PRIM)
control.thaw_update()
control.update()
control.freeze_update()
control.sleep()
a.remove_lineto(c)
b.remove_lineto(a)
c.remove_lineto(b)
def triang (a, b, c): "desenha (e apaga) os lados do triangulo abc" a.lineto (c, config.COLOR_PRIM) b.lineto (a, config.COLOR_PRIM) c.lineto (b, config.COLOR_PRIM) control.thaw_update () control.update () control.freeze_update () control.sleep () a.remove_lineto (c) b.remove_lineto (a) c.remove_lineto (b)
def dist2(a, b):
"retorna o quadrado da distancia entre a e b"
ida = a.hilight(config.COLOR_PRIM)
idb = b.hilight(config.COLOR_PRIM)
a.lineto(b, config.COLOR_PRIM)
control.thaw_update()
control.update()
control.freeze_update()
control.sleep()
a.remove_lineto(b)
a.unhilight(ida)
b.unhilight(idb)
return prim.dist2(a, b)
async def realtime_control(host, control, plot, cut):
incoming = asyncio.Queue()
outgoing = asyncio.Queue()
# Keep references to tasks to avoid them being stopped instantly
tc = asyncio.create_task(controld_connection(host, incoming, outgoing))
tg = asyncio.create_task(gps_connection(host, incoming))
# to = asyncio.create_task(heartbeat(outgoing))
# incoming -> track -> control -> outgoing
async for state in play.track(incoming_generator(incoming), 0, 0):
t = time.time()
plot.update(state)
# continue
if control.end(t, state):
break
speed, omega = control.update(t, state)
# logger.debug("Update %s -> %r %r", state, speed, omega)
if speed is not None:
t = datetime.utcnow()
m = RobotState.SpeedCommand(speed)
await outgoing.put((t, m))
if omega is not None:
t = datetime.utcnow()
m = RobotState.OmegaCommand(omega)
await outgoing.put((t, m))
if speed is not None and omega is not None:
t = datetime.utcnow()
m = RobotState.CutCommand(cut)
await outgoing.put((t, m))
# TODO: timeout on commands
t = datetime.utcnow()
m = RobotState.HeartbeatCommand()
await outgoing.put((t, m))
def dist2 (a, b): "retorna o quadrado da distancia entre a e b" ida = a.hilight (config.COLOR_PRIM) idb = b.hilight (config.COLOR_PRIM) a.lineto (b, config.COLOR_PRIM) control.thaw_update () control.update () control.freeze_update () control.sleep () a.remove_lineto (b) a.unhilight (ida) b.unhilight (idb) return prim.dist2(a, b)
def simulated_control(control, plot):
from RobotModel import RobotModel
model = RobotModel(State(-10, -2, 0, 0))
dt = 1
t = 0.0
state = model.get_state()
plot.update(state)
while not control.end(t, state):
t += dt
state = model.update(dt)
plot.update(state)
speed, omega = control.update(t, state)
if speed is not None and omega is not None:
model.set_speed_omega(speed, omega)
color = sf.Color(200, 200, 200)
app.UseVerticalSync(False)
control = control.Control(input)
player = shaolin.Shaolin(control)
next_tick = get_tick()
while True:
app.Clear(color)
app.Draw(player)
app.Draw(player.shadow)
while get_tick() > next_tick:
while app.GetEvent(event):
if event.Type == sf.Event.KeyPressed:
if event.Key.Code == sf.Key.Escape:
app.Close()
sys.exit(0)
elif event.Type == sf.Event.Closed:
sys.exit(0)
next_tick += TICK_STEP
control.update()
player.update()
app.Display()
