From 678c94532e75231304df23ae5a76bf864dc3a1c1 Mon Sep 17 00:00:00 2001 From: d3rped Date: Sun, 25 Mar 2018 15:09:13 +0200 Subject: [PATCH] More work towards a working main loop... --- src/communication.py | 13 ++++++++++--- src/main.py | 7 ++++--- src/move.py | 36 +++++++++++++++++++++++++++++------- src/odometry.py | 15 +++++++-------- src/odomtest.py | 19 +++++++++++++++++++ src/planet.py | 6 +++--- 6 files changed, 72 insertions(+), 24 deletions(-) create mode 100644 src/odomtest.py diff --git a/src/communication.py b/src/communication.py index b3ffbd9..0bf4993 100644 --- a/src/communication.py +++ b/src/communication.py @@ -103,9 +103,15 @@ class Communication: def status(self): return self._status - def update(self, edges): # TODO: FINISH + def update(self, odometry, edges): # TODO: FINISH + if(odometry != None): + self.planet.setcurnode(self.planet.getcurnode() + odometry[0]) if(edges is None and self._status is Mode.EXPLORE): - self._status = Mode.GOTOSTATION + if(self.planet.unexedge == []): + self.encode_message(Command.COMPLETE, None) + else: + self.navto = next(filter(lambda x: x is not None, map(lambda edge: self.planet.shortest_path(self.planet.getcurnode(), edge), self.planet.unexedge))) + self._status = Mode.GOTOSTATION elif(edges is None): pass else: @@ -145,8 +151,9 @@ 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(','))) + if(self.navto != None): + self._status = Mode.TARGET elif message.startswith(Command.NOTICE): print(message) diff --git a/src/main.py b/src/main.py index 273b1e6..e374a65 100644 --- a/src/main.py +++ b/src/main.py @@ -27,13 +27,14 @@ def run(): while(communication.status() is Mode.GOTOSTATION or communication.status() is Mode.TARGET): # should be triggered if current station has no unexplored edges for instructions in communication.navto: for instructions in communication.navto: - communication.update(None) + communication.update(None, None) move.turnto(instructions[1]) move.traversetonextstation(True) - communication.update(move.getstationedges()) # TODO: Register in map + communication.update(move._odometry.coordinates_get(), move.getstationedges()) # TODO: Register in map + move.updatevars() move.turnto(communication.navto[0][1]) # TODO: add control mechanism to communication class - if(communication.check(move.traversetonextstation(False)) is False): # TODO: let same control mechanism decide if station is known, also traversetonextstation needs a return value + if(communication.check(move.traversetonextstation(False)) is False): move.setcurdir(communication.getdir()) diff --git a/src/move.py b/src/move.py index 6c94eed..8ec4068 100644 --- a/src/move.py +++ b/src/move.py @@ -3,6 +3,8 @@ from wheel import Wheel from sensor import Sensor from odometry import Odometry +from planet import Direction +from time import sleep def capat100(value): @@ -35,14 +37,22 @@ class Move: self._sensor.calibrateBrightness() # pass - - ''' Function to correct errors should the robot wander astray ''' def _aligntoedge(self): - pass + self._wheel_l.speed_set(20) + self._wheel_r.speed_set(-20) + self._wheel_l.run() + self._wheel_r.run() + self._sensor.refresh() + while(self._sensor.getbrightness() > self._sensor.edgebrightness): + self._sensor.refresh() + self._wheel_l.stop() + self._wheel_r.stop() + self._wheel_l.turnbyamount(20, 20) + self._wheel_r.turnbyamount(-20, 20) ''' uses odometry and the planet object to map edges that are connected to @@ -52,12 +62,25 @@ class Move: Next turn around and detect edges through drops in _sensor.getbrightness() ''' def getstationedges(self): - pass + found_edges = [] + self._odometry.reset() + while(True): + self._aligntoedge() + self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) + if(self._odometry.angle_get() in found_edges): + break + else: + found_edges.append(self._odometry.angle_get()) + self._wheel_l.turnbyamount(20, 20) + self._wheel_r.turnbyamount(-20, 20) + sleep(2) + return found_edges + def turnto(self, direction): turnval = self._odometry.dir2ticks(direction) - self._wheel_l.turnbyamount(turnval, self.defaultspeed) - self._wheel_r.turnbyamount(-turnval, self.defaultspeed) + self._wheel_l.turnbyamount(turnval, 90) + self._wheel_r.turnbyamount(-turnval, 90) def traversetonextstation(self, isknownstation): self._wheel_l.speed_set(self._sensor.edgebrightness//2) @@ -72,6 +95,5 @@ class Move: self._sensor.getcolor() if(not isknownstation): self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) - self._wheel_l.stop() self._wheel_r.stop() diff --git a/src/odometry.py b/src/odometry.py index 78f26db..150b7f4 100644 --- a/src/odometry.py +++ b/src/odometry.py @@ -61,6 +61,9 @@ class Odometry: def angle_set(self, newangle): self._v_angle = newangle + def angle_get(self): + return angle2dir(self._v_angle) + def coordinates_get(self): return ((posround(self._posxy[0]), posround(self._posxy[1])), angle2dir(self._v_angle)) @@ -69,8 +72,8 @@ class Odometry: self._distance[1] = ticks_wheel[1] * self._distance_per_tick self._v_length = self._v_length + (self._distance[0] + self._distance[1])/2 self._v_angle = self._v_angle + (self._distance[0] - self._distance[1])/self._wheel_axis - self.coordinates_update() self._wheel_center = (self._distance[0] + self._distance[1])/2 + self.coordinates_update() print((self._v_length, angle2dir(self._v_angle))) def coordinates_update(self): # worked as function outside the class @@ -79,10 +82,6 @@ class Odometry: print((self._posxy[0], self._posxy[1])) def dir2ticks(self, destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle) - self._ticksperwheel = 0 - self._current_Direc = (self._v_angle /self._pi) * 180 - self._ticks_def = self._current_Direc - destdir - self._ticksperwheel = self._ticks_def / 2 - if self._ticksperwheel < 0: - self._ticksperwheel = - self._ticksperwheel - return ticksperwheel + turnval = (destdir - self._v_angle) * 2 + self._v_angle = destdir + return turnval diff --git a/src/odomtest.py b/src/odomtest.py new file mode 100644 index 0000000..0b821ea --- /dev/null +++ b/src/odomtest.py @@ -0,0 +1,19 @@ +from move import Move + +mymove = Move() +mymove._sensor.refresh() +mymove.maxbrightness = mymove._sensor.getbrightness() +mymove.maxbrightness = 50 +mymove._wheel_l.getmovement() +mymove._wheel_r.getmovement() + + + +try: + mymove.traversetonextstation(False) +except: + mymove._wheel_l.stop() + mymove._wheel_r.stop() + mymove._wheel_l.getmovement() + mymove._wheel_r.getmovement() + diff --git a/src/planet.py b/src/planet.py index 24c0ce0..81b64b7 100644 --- a/src/planet.py +++ b/src/planet.py @@ -50,7 +50,7 @@ class Planet: def __init__(self): """ Initializes the data structure """ self._planetmap = {} - self._unexedge = [] + self.unexedge = [] self._curnode = None self._curdir = Direction.NORTH self.target = None @@ -79,10 +79,10 @@ class Planet: return self._curdir def addtounexedge(self, node_edge_list): - self._unexedge = self._unexedge + node_edge_list + self.unexedge = self.unexedge + node_edge_list def exploreedge(self, node_edge): - self._unexedge.remove(node_edge) + self.unexedge.remove(node_edge) ''' Returns a shortest path between two nodes.