More work towards a working main loop...
This commit is contained in:
parent
1dc4f5a2d7
commit
678c94532e
6 changed files with 72 additions and 24 deletions
|
@ -103,8 +103,14 @@ 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):
|
||||||
|
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
|
self._status = Mode.GOTOSTATION
|
||||||
elif(edges is None):
|
elif(edges is None):
|
||||||
pass
|
pass
|
||||||
|
@ -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)
|
||||||
|
|
||||||
|
|
|
@ -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())
|
||||||
|
|
||||||
|
|
||||||
|
|
36
src/move.py
36
src/move.py
|
@ -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()
|
||||||
|
|
|
@ -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
19
src/odomtest.py
Normal 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()
|
||||||
|
|
|
@ -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.
|
||||||
|
|
Loading…
Reference in a new issue