More work towards a working main loop...

This commit is contained in:
d3rped 2018-03-25 15:09:13 +02:00
parent 1dc4f5a2d7
commit 678c94532e
6 changed files with 72 additions and 24 deletions

View file

@ -103,9 +103,15 @@ class Communication:
def status(self): def status(self):
return self._status 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): 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): elif(edges is None):
pass pass
else: else:
@ -145,8 +151,9 @@ 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(',')))
if(self.navto != None):
self._status = Mode.TARGET
elif message.startswith(Command.NOTICE): elif message.startswith(Command.NOTICE):
print(message) print(message)

View file

@ -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 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:
for instructions in communication.navto: for instructions in communication.navto:
communication.update(None) communication.update(None, None)
move.turnto(instructions[1]) move.turnto(instructions[1])
move.traversetonextstation(True) 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 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()) move.setcurdir(communication.getdir())

View file

@ -3,6 +3,8 @@
from wheel import Wheel from wheel import Wheel
from sensor import Sensor from sensor import Sensor
from odometry import Odometry from odometry import Odometry
from planet import Direction
from time import sleep
def capat100(value): def capat100(value):
@ -35,14 +37,22 @@ class Move:
self._sensor.calibrateBrightness() self._sensor.calibrateBrightness()
# pass # pass
''' '''
Function to correct errors should the robot wander astray Function to correct errors should the robot wander astray
''' '''
def _aligntoedge(self): 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 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() Next turn around and detect edges through drops in _sensor.getbrightness()
''' '''
def getstationedges(self): 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): def turnto(self, direction):
turnval = self._odometry.dir2ticks(direction) turnval = self._odometry.dir2ticks(direction)
self._wheel_l.turnbyamount(turnval, self.defaultspeed) self._wheel_l.turnbyamount(turnval, 90)
self._wheel_r.turnbyamount(-turnval, self.defaultspeed) self._wheel_r.turnbyamount(-turnval, 90)
def traversetonextstation(self, isknownstation): def traversetonextstation(self, isknownstation):
self._wheel_l.speed_set(self._sensor.edgebrightness//2) self._wheel_l.speed_set(self._sensor.edgebrightness//2)
@ -72,6 +95,5 @@ class Move:
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()))
self._wheel_l.stop() self._wheel_l.stop()
self._wheel_r.stop() self._wheel_r.stop()

View file

@ -61,6 +61,9 @@ class Odometry:
def angle_set(self, newangle): def angle_set(self, newangle):
self._v_angle = newangle self._v_angle = newangle
def angle_get(self):
return angle2dir(self._v_angle)
def coordinates_get(self): def coordinates_get(self):
return ((posround(self._posxy[0]), posround(self._posxy[1])), angle2dir(self._v_angle)) 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._distance[1] = ticks_wheel[1] * self._distance_per_tick
self._v_length = self._v_length + (self._distance[0] + self._distance[1])/2 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._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._wheel_center = (self._distance[0] + self._distance[1])/2
self.coordinates_update()
print((self._v_length, angle2dir(self._v_angle))) print((self._v_length, angle2dir(self._v_angle)))
def coordinates_update(self): # worked as function outside the class def coordinates_update(self): # worked as function outside the class
@ -79,10 +82,6 @@ class Odometry:
print((self._posxy[0], self._posxy[1])) 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) def dir2ticks(self, destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle)
self._ticksperwheel = 0 turnval = (destdir - self._v_angle) * 2
self._current_Direc = (self._v_angle /self._pi) * 180 self._v_angle = destdir
self._ticks_def = self._current_Direc - destdir return turnval
self._ticksperwheel = self._ticks_def / 2
if self._ticksperwheel < 0:
self._ticksperwheel = - self._ticksperwheel
return ticksperwheel

19
src/odomtest.py Normal file
View file

@ -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()

View file

@ -50,7 +50,7 @@ class Planet:
def __init__(self): def __init__(self):
""" Initializes the data structure """ """ Initializes the data structure """
self._planetmap = {} self._planetmap = {}
self._unexedge = [] self.unexedge = []
self._curnode = None self._curnode = None
self._curdir = Direction.NORTH self._curdir = Direction.NORTH
self.target = None self.target = None
@ -79,10 +79,10 @@ class Planet:
return self._curdir return self._curdir
def addtounexedge(self, node_edge_list): 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): def exploreedge(self, node_edge):
self._unexedge.remove(node_edge) self.unexedge.remove(node_edge)
''' '''
Returns a shortest path between two nodes. Returns a shortest path between two nodes.