Fixed some errors/typos which I had overlooked/missed earlier. Stared implementing main loop.

This commit is contained in:
d3rped 2018-03-24 06:09:48 +01:00
parent c173917a72
commit c1049f4a58
4 changed files with 54 additions and 10 deletions

View file

@ -72,6 +72,14 @@ class Command(str, Enum):
NOTICE = "notice" NOTICE = "notice"
class Mode(str, Enum):
EXPLORE = "explore"
GOTOSTATION = "gotostation"
TARGET = "target"
TARGET2 = "target2"
COMPLETE = "complete"
class Communication: class Communication:
""" Initializes communication module, connect to server, subscribe, etc. """ """ Initializes communication module, connect to server, subscribe, etc. """
def __init__(self, mqtt_client): def __init__(self, mqtt_client):
@ -91,6 +99,13 @@ class Communication:
self.subscribetochannel("explorer/" + self.uid) self.subscribetochannel("explorer/" + self.uid)
self.encode_message(Command.READY, None) self.encode_message(Command.READY, None)
self.client.loop_start() self.client.loop_start()
self._status = Mode.EXPLORE
def status(self):
return self._status
def update(self):
pass
# THIS FUNCTIONS SIGNATURE MUST NOT BE CHANGED # THIS FUNCTIONS SIGNATURE MUST NOT BE CHANGED
""" Handles the callback if any message arrived """ """ Handles the callback if any message arrived """
@ -126,6 +141,7 @@ class Communication:
elif message.startswith(Command.PATH): elif message.startswith(Command.PATH):
self.stringtopath(*message[5:].rsplit(' ')) self.stringtopath(*message[5:].rsplit(' '))
elif message.startswith(Command.TARGET): elif message.startswith(Command.TARGET):
self._status = Mode.TARGET
self.navto = self.planet.shortest_path(self.planet.getcurnode(), str2tuple(*message[7:].rsplit(','))) self.navto = self.planet.shortest_path(self.planet.getcurnode(), str2tuple(*message[7:].rsplit(',')))
elif message.startswith(Command.NOTICE): elif message.startswith(Command.NOTICE):
print(message) print(message)

View file

@ -1,10 +1,8 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
import ev3dev.ev3 as ev3
import uuid import uuid
import paho.mqtt.client as mqtt import paho.mqtt.client as mqtt
from planet import Direction, Planet from planet import Direction, Planet
from communication import Communication from communication import Mode, Communication
from move import Move from move import Move
# DO NOT EDIT # DO NOT EDIT
@ -19,6 +17,35 @@ def run():
clean_session=False, clean_session=False,
protocol=mqtt.MQTTv31) protocol=mqtt.MQTTv31)
communication = Communication(client)
move = Move()
while(communication.planetname is None):
communication.process_messages()
print("received Planet Name")
while(communication.status() is not Mode.COMPLETE):
move.getstationedges() # TODO: Register in map
move.turnto(90) # TODO: add control mechanism to communication class
move.traversetonextstation(False) # TODO: let same control mechanism decide if station is known, also traversetonextstation needs a return value
while(communication.status() is Mode.GOTOSTATION): # should be triggered if current station has no unexplored edges
for instructions in communication.navto:
move.follow(instructions)
communication.update()
while(communication.status() is Mode.TARGET): # should be triggered when communication gets TARGET signal from server
for instructions in communication.navto:
move.follow(instructions)
communication.update() # TODO: implement update function that communicates with server and waits/checks for new instructions
if(communication.status() == Mode.TARGET2): # TODO: implement Target change
break
communication.update()
# DO NOT EDIT # DO NOT EDIT
if __name__ == '__main__': if __name__ == '__main__':

View file

@ -20,6 +20,7 @@ class Move:
self._wheel_l = Wheel('outB') self._wheel_l = Wheel('outB')
self._wheel_r = Wheel('outC') self._wheel_r = Wheel('outC')
self._sensor = Sensor() self._sensor = Sensor()
self._calibrate()
except OSError: except OSError:
print("ERROR: Cannot find Motor/Sensor") print("ERROR: Cannot find Motor/Sensor")
self.defaultspeed = 20 self.defaultspeed = 20
@ -31,7 +32,7 @@ class Move:
''' '''
def _calibrate(self): def _calibrate(self):
self._sensor.calibrateRGB() self._sensor.calibrateRGB()
self.calibrateBrightness() self._sensor.calibrateBrightness()
# pass # pass
@ -59,15 +60,15 @@ class Move:
self._wheel_r.turnbyamount(-turnval, self.defaultspeed) self._wheel_r.turnbyamount(-turnval, self.defaultspeed)
def traversetonextstation(self, isknownstation): def traversetonextstation(self, isknownstation):
self._wheel_l.speed_set(self.edgebrightness//2) self._wheel_l.speed_set(self._sensor.edgebrightness//2)
self._wheel_r.speed_set(self.edgebrightness//2) self._wheel_r.speed_set(self._sensor.edgebrightness//2)
self._wheel_l.run() self._wheel_l.run()
self._wheel_r.run() self._wheel_r.run()
while (not self._sensor.bumperwaspressed and self._sensor.lastcolor is None): while (not self._sensor.bumperwaspressed and self._sensor.lastcolor is None):
self._sensor.refresh() self._sensor.refresh()
self._sensor.checkbumper() self._sensor.checkbumper()
self._wheel_l.speed_set(capat100(self.maxbrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed) self._wheel_l.speed_set(capat100(self._sensor.edgebrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed)
self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self.maxbrightness/2)+self.defaultspeed) self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self._sensor.edgebrightness/2)+self.defaultspeed)
self._sensor.getcolor() self._sensor.getcolor()
if(not isknownstation): if(not isknownstation):
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))

View file

@ -76,6 +76,6 @@ class Odometry:
self._posxy[1] = self._posxy[1] + self._v_length * math.cos(self._v_angle) self._posxy[1] = self._posxy[1] + self._v_length * math.cos(self._v_angle)
print((self._posxy[0], self._posxy[1])) print((self._posxy[0], self._posxy[1]))
def dir2ticks(destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle) def dir2ticks(self, destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle)
ticksperwheel = None ticksperwheel = 0
return ticksperwheel return ticksperwheel