From 58553e38d0fd667f626e07ba21e42baa9b79e665 Mon Sep 17 00:00:00 2001 From: d3rped Date: Mon, 26 Mar 2018 14:23:23 +0200 Subject: [PATCH] Now it kind of works? There are still some bugs to squash --- src/communication.py | 3 ++- src/main.py | 13 +++++++------ src/move.py | 4 ++++ src/planet.py | 2 +- 4 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/communication.py b/src/communication.py index c2be708..5cee521 100644 --- a/src/communication.py +++ b/src/communication.py @@ -108,11 +108,12 @@ class Communication: def update(self, odometry): # TODO: FINISH if(self.isfirstrun): - pass + self.isfirstrun = False elif(odometry is not None): # use odometry to send data self.encode_message(Command.PATH, ((self.planet.getcurnode(), self.planet.getcurdir()), ((self.planet.getcurnode()[0] + odometry[0][0], self.planet.getcurnode()[1] + odometry[0][1]), (odometry[1] - 180) % 360), odometry[2])) elif(self._status is Mode.GOTOSTATION or self._status is Mode.TARGET): self.encode_message(Command.PATH, ((self.navto[0][0], self.navto[0][1]), (self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][0], self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][1]), self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][2])) + # self.encode_message(Command.PATH, ((self.navto[0][0], self.navto[0][1]), (self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][0], self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][1]), self.planet._planetmap[self.navto[0][0]][self.navto[0][1]][2])) self.process_messages() # this should update curnode and curdir def process_edges(self, edges): diff --git a/src/main.py b/src/main.py index 1d9833d..2f5d4f0 100644 --- a/src/main.py +++ b/src/main.py @@ -24,20 +24,21 @@ def run(): while(communication.planetname is None): communication.process_messages() - move.traversetonextstation(False) - + move.traversetonextstation(True) + move.resetwheels() + move.reset() while(communication.status() is not Mode.COMPLETE): communication.update(move.getstats()) # TODO: Register in map # correct odometry right here otherwise list of edges will be incorrect move.setcurdir(communication.getdir()) communication.process_edges(move.getstationedges()) if(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: - move.turnto(instructions[1]) + while len(communication.navto) > 0: + move.turnto(communication.navto[0][1]) move.traversetonextstation(True) - move.setcurdir(communication.planet._planetmap[instructions[0]][instructions[1]][1]) + move.setcurdir(communication.planet._planetmap[communication.navto[0][0]][communication.navto[0][1]]) communication.update(None) - communication.navto.clear() + communication.navto.pop(0) communication._status = Mode.EXPLORE else: move.turnto(communication.navto[0][1]) diff --git a/src/move.py b/src/move.py index 79298dc..ca15735 100644 --- a/src/move.py +++ b/src/move.py @@ -124,6 +124,10 @@ class Move: self._odometry.reset() self._sensor.reset() + def resetwheels(self): + self._wheel_l._motor.position = 0 + self._wheel_r._motor.position = 0 + def traversetonextstation(self, isknownstation): self._wheel_l.speed_set(self._sensor.edgebrightness//2) self._wheel_r.speed_set(self._sensor.edgebrightness//2) diff --git a/src/planet.py b/src/planet.py index 04f6f33..8c46193 100644 --- a/src/planet.py +++ b/src/planet.py @@ -89,7 +89,7 @@ class Planet: testnode.remove(edges) if(testnode != [] and self._curnode not in self.unexedge): self.unexedge.append(self._curnode) - if(testnode == [] and self._curnode in self.unexedge): + if(len(testnode) <= 1 and self._curnode in self.unexedge): self.unexedge.remove(self._curnode) print("MAP:", self._planetmap[self._curnode]) print("NODES:", testnode)