Now it kind of works? There are still some bugs to squash
This commit is contained in:
parent
6b2a19ebdc
commit
58553e38d0
4 changed files with 14 additions and 8 deletions
|
@ -108,11 +108,12 @@ class Communication:
|
||||||
|
|
||||||
def update(self, odometry): # TODO: FINISH
|
def update(self, odometry): # TODO: FINISH
|
||||||
if(self.isfirstrun):
|
if(self.isfirstrun):
|
||||||
pass
|
self.isfirstrun = False
|
||||||
elif(odometry is not None): # use odometry to send data
|
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]))
|
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):
|
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.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
|
self.process_messages() # this should update curnode and curdir
|
||||||
|
|
||||||
def process_edges(self, edges):
|
def process_edges(self, edges):
|
||||||
|
|
13
src/main.py
13
src/main.py
|
@ -24,20 +24,21 @@ def run():
|
||||||
while(communication.planetname is None):
|
while(communication.planetname is None):
|
||||||
communication.process_messages()
|
communication.process_messages()
|
||||||
|
|
||||||
move.traversetonextstation(False)
|
move.traversetonextstation(True)
|
||||||
|
move.resetwheels()
|
||||||
|
move.reset()
|
||||||
while(communication.status() is not Mode.COMPLETE):
|
while(communication.status() is not Mode.COMPLETE):
|
||||||
communication.update(move.getstats()) # TODO: Register in map
|
communication.update(move.getstats()) # TODO: Register in map
|
||||||
# correct odometry right here otherwise list of edges will be incorrect
|
# correct odometry right here otherwise list of edges will be incorrect
|
||||||
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
|
||||||
for instructions in communication.navto:
|
while len(communication.navto) > 0:
|
||||||
move.turnto(instructions[1])
|
move.turnto(communication.navto[0][1])
|
||||||
move.traversetonextstation(True)
|
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.update(None)
|
||||||
communication.navto.clear()
|
communication.navto.pop(0)
|
||||||
communication._status = Mode.EXPLORE
|
communication._status = Mode.EXPLORE
|
||||||
else:
|
else:
|
||||||
move.turnto(communication.navto[0][1])
|
move.turnto(communication.navto[0][1])
|
||||||
|
|
|
@ -124,6 +124,10 @@ class Move:
|
||||||
self._odometry.reset()
|
self._odometry.reset()
|
||||||
self._sensor.reset()
|
self._sensor.reset()
|
||||||
|
|
||||||
|
def resetwheels(self):
|
||||||
|
self._wheel_l._motor.position = 0
|
||||||
|
self._wheel_r._motor.position = 0
|
||||||
|
|
||||||
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)
|
||||||
self._wheel_r.speed_set(self._sensor.edgebrightness//2)
|
self._wheel_r.speed_set(self._sensor.edgebrightness//2)
|
||||||
|
|
|
@ -89,7 +89,7 @@ class Planet:
|
||||||
testnode.remove(edges)
|
testnode.remove(edges)
|
||||||
if(testnode != [] and self._curnode not in self.unexedge):
|
if(testnode != [] and self._curnode not in self.unexedge):
|
||||||
self.unexedge.append(self._curnode)
|
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)
|
self.unexedge.remove(self._curnode)
|
||||||
print("MAP:", self._planetmap[self._curnode])
|
print("MAP:", self._planetmap[self._curnode])
|
||||||
print("NODES:", testnode)
|
print("NODES:", testnode)
|
||||||
|
|
Loading…
Reference in a new issue