diff --git a/src/communication.py b/src/communication.py index a11ffd1..f227ac4 100644 --- a/src/communication.py +++ b/src/communication.py @@ -72,6 +72,14 @@ class Command(str, Enum): NOTICE = "notice" +class Mode(str, Enum): + EXPLORE = "explore" + GOTOSTATION = "gotostation" + TARGET = "target" + TARGET2 = "target2" + COMPLETE = "complete" + + class Communication: """ Initializes communication module, connect to server, subscribe, etc. """ def __init__(self, mqtt_client): @@ -91,6 +99,13 @@ class Communication: self.subscribetochannel("explorer/" + self.uid) self.encode_message(Command.READY, None) 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 """ Handles the callback if any message arrived """ @@ -126,6 +141,7 @@ class Communication: elif message.startswith(Command.PATH): self.stringtopath(*message[5:].rsplit(' ')) elif message.startswith(Command.TARGET): + self._status = Mode.TARGET self.navto = self.planet.shortest_path(self.planet.getcurnode(), str2tuple(*message[7:].rsplit(','))) elif message.startswith(Command.NOTICE): print(message) diff --git a/src/main.py b/src/main.py index f2e7dae..dae34e7 100644 --- a/src/main.py +++ b/src/main.py @@ -1,10 +1,8 @@ #!/usr/bin/env python3 - -import ev3dev.ev3 as ev3 import uuid import paho.mqtt.client as mqtt from planet import Direction, Planet -from communication import Communication +from communication import Mode, Communication from move import Move # DO NOT EDIT @@ -19,6 +17,35 @@ def run(): clean_session=False, 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 if __name__ == '__main__': diff --git a/src/move.py b/src/move.py index 62c6dfd..6c94eed 100644 --- a/src/move.py +++ b/src/move.py @@ -20,6 +20,7 @@ class Move: self._wheel_l = Wheel('outB') self._wheel_r = Wheel('outC') self._sensor = Sensor() + self._calibrate() except OSError: print("ERROR: Cannot find Motor/Sensor") self.defaultspeed = 20 @@ -31,7 +32,7 @@ class Move: ''' def _calibrate(self): self._sensor.calibrateRGB() - self.calibrateBrightness() + self._sensor.calibrateBrightness() # pass @@ -59,15 +60,15 @@ class Move: self._wheel_r.turnbyamount(-turnval, self.defaultspeed) def traversetonextstation(self, isknownstation): - self._wheel_l.speed_set(self.edgebrightness//2) - self._wheel_r.speed_set(self.edgebrightness//2) + self._wheel_l.speed_set(self._sensor.edgebrightness//2) + self._wheel_r.speed_set(self._sensor.edgebrightness//2) self._wheel_l.run() self._wheel_r.run() while (not self._sensor.bumperwaspressed and self._sensor.lastcolor is None): self._sensor.refresh() self._sensor.checkbumper() - self._wheel_l.speed_set(capat100(self.maxbrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed) - self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self.maxbrightness/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._sensor.edgebrightness/2)+self.defaultspeed) self._sensor.getcolor() if(not isknownstation): self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) diff --git a/src/odometry.py b/src/odometry.py index 1565138..f3dce02 100644 --- a/src/odometry.py +++ b/src/odometry.py @@ -76,6 +76,6 @@ class Odometry: self._posxy[1] = self._posxy[1] + self._v_length * math.cos(self._v_angle) 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) - ticksperwheel = None + def dir2ticks(self, destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle) + ticksperwheel = 0 return ticksperwheel