Fixed some errors/typos which I had overlooked/missed earlier. Stared implementing main loop.
This commit is contained in:
parent
c173917a72
commit
c1049f4a58
4 changed files with 54 additions and 10 deletions
|
@ -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)
|
||||||
|
|
33
src/main.py
33
src/main.py
|
@ -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__':
|
||||||
|
|
11
src/move.py
11
src/move.py
|
@ -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()))
|
||||||
|
|
|
@ -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
|
||||||
|
|
Loading…
Reference in a new issue