From acccb15f3ea61d0b137c9e68b147148afe23964a Mon Sep 17 00:00:00 2001 From: d3rped Date: Wed, 28 Mar 2018 03:04:27 +0200 Subject: [PATCH] Navigate to path functionality works now! Added try block to modor speed because I encountered some errors (even though there is already a capping function for this). --- src/main.py | 11 ++++++++--- src/move.py | 14 ++++++++++---- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/src/main.py b/src/main.py index 355eaff..eed4e18 100644 --- a/src/main.py +++ b/src/main.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 import uuid import paho.mqtt.client as mqtt -from planet import Direction, Planet -from communication import Mode, Communication +from planet import Direction +from communication import Mode, Command, Communication from move import Move from time import sleep @@ -30,6 +30,11 @@ def run(): while(communication.status() is not Mode.COMPLETE): communication.update(move.getstats()) # correct odometry right here otherwise list of edges will be incorrect + if(communication.planet.getcurnode() == communication.target): + communication._status = Mode.COMPLETE + communication.encode_message(Command.TARGET, None) + communication.encode_message(Command.COMPLETE, None) + break 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 @@ -38,7 +43,7 @@ def run(): move.reset() move.resetwheels() move.traversetonextstation(True) - move.setcurdir(communication.planet._planetmap[communication.navto[0][0]][communication.navto[0][1]]) + move.setcurdir((None, (communication.planet._planetmap[communication.navto[0][0]][communication.navto[0][1]][1] + 180)%360)) communication.update(None) communication.navto.pop(0) if(communication.status() == Mode.TARGET): diff --git a/src/move.py b/src/move.py index 1fa7156..aed6283 100644 --- a/src/move.py +++ b/src/move.py @@ -140,8 +140,11 @@ class Move: self._sensor.getcolor() if(self._sensor.lastcolor is not None): break - 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) + try: + 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) + except OSError: + print("ERROR: Invalid motor speed value") if(not isknownstation): self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) self._wheel_l.stop() @@ -168,8 +171,11 @@ class Move: self._sensor.getcolor() if(self._sensor.lastcolor is not None): break - 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) + try: + 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) + except OSError: + print("ERROR: Invalid motor speed value") if(not isknownstation): self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) self._wheel_l.turnbyamount(200, 90)