67 lines
2.4 KiB
Python
67 lines
2.4 KiB
Python
#!/usr/bin/env python3
|
|
import uuid
|
|
import paho.mqtt.client as mqtt
|
|
from planet import Direction
|
|
from communication import Mode, Command, Communication
|
|
from move import Move
|
|
from time import sleep
|
|
|
|
# DO NOT EDIT
|
|
client = None
|
|
|
|
|
|
def run():
|
|
# DO NOT EDIT
|
|
global client
|
|
# client_id has to be unique among ALL users
|
|
client = mqtt.Client(client_id=str(uuid.uuid4()),
|
|
clean_session=False,
|
|
protocol=mqtt.MQTTv31)
|
|
|
|
communication = Communication(client)
|
|
move = Move()
|
|
|
|
while(communication.planetname is None):
|
|
communication.process_messages()
|
|
|
|
move.traversetonextstation(True)
|
|
move.resetwheels()
|
|
move.reset()
|
|
while(communication.status() is not Mode.COMPLETE):
|
|
communication.update(move.getstats())
|
|
# correct odometry right here otherwise list of edges will be incorrect
|
|
if(communication.planet.getcurnode() == communication.target):
|
|
communication._status = Mode.COMPLETE
|
|
communication.encode_message(Command.TARGET, None)
|
|
communication.encode_message(Command.COMPLETE, None)
|
|
move.setcurdir(communication.getdir())
|
|
communication.process_edges(move.getstationedges())
|
|
if(communication._status == Mode.COMPLETE):
|
|
break
|
|
if(communication.status() is Mode.GOTOSTATION or communication.status() is Mode.TARGET): # should be triggered if current station has no unexplored edges
|
|
while len(communication.navto) > 0:
|
|
move.turnto(communication.navto[0][1])
|
|
move.reset()
|
|
move.resetwheels()
|
|
move.traversetonextstation(True)
|
|
move.setcurdir((None, (communication.planet._planetmap[communication.navto[0][0]][communication.navto[0][1]][1] + 180)%360))
|
|
communication.update(None)
|
|
communication.navto.pop(0)
|
|
if(communication.status() == Mode.TARGET):
|
|
communication._status = Mode.COMPLETE
|
|
else:
|
|
communication._status = Mode.EXPLORE
|
|
else:
|
|
move.turnto(communication.navto[0][1])
|
|
sleep(4)
|
|
communication.planet.setcurdir(communication.navto[0][1])
|
|
move.setcurdir(communication.getdir())
|
|
move.reset()
|
|
move.resetwheels()
|
|
move.traversetonextstation(False)
|
|
|
|
|
|
# DO NOT EDIT
|
|
if __name__ == '__main__':
|
|
run()
|