derped/robolab
1
0
Fork 0
Code Pull requests Releases Activity

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).

Browse source
This commit is contained in:
d3rped 2018-03-28 03:04:27 +02:00
parent 11f061287e
commit acccb15f3e
2 changed files with 18 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

11
src/main.py
View file

@ -1,8 +1,8 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
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
from communication import Mode, Communication from communication import Mode, Command, Communication
from move import Move from move import Move
from time import sleep from time import sleep
@ -30,6 +30,11 @@ def run():
while(communication.status() is not Mode.COMPLETE): while(communication.status() is not Mode.COMPLETE):
communication.update(move.getstats()) communication.update(move.getstats())
# correct odometry right here otherwise list of edges will be incorrect # 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()) move.setcurdir(communication.getdir())
communication.process_edges(move.getstationedges()) 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 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.reset()
move.resetwheels() move.resetwheels()
move.traversetonextstation(True) 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.update(None)
communication.navto.pop(0) communication.navto.pop(0)
if(communication.status() == Mode.TARGET): if(communication.status() == Mode.TARGET):

6
src/move.py
View file

@ -140,8 +140,11 @@ class Move:
self._sensor.getcolor() self._sensor.getcolor()
if(self._sensor.lastcolor is not None): if(self._sensor.lastcolor is not None):
break break
try:
self._wheel_l.speed_set(capat100(self._sensor.edgebrightness/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._sensor.edgebrightness/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): 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()))
self._wheel_l.stop() self._wheel_l.stop()
@ -168,8 +171,11 @@ class Move:
self._sensor.getcolor() self._sensor.getcolor()
if(self._sensor.lastcolor is not None): if(self._sensor.lastcolor is not None):
break break
try:
self._wheel_l.speed_set(capat100(self._sensor.edgebrightness/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._sensor.edgebrightness/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): 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()))
self._wheel_l.turnbyamount(200, 90) self._wheel_l.turnbyamount(200, 90)