Odometry now has less of an offset, still needs to be improved. Switched back to hard coded colors.

This commit is contained in:
d3rped 2018-03-27 19:51:10 +02:00
parent eabe7c650f
commit 11f061287e
5 changed files with 32 additions and 17 deletions

View file

@ -102,6 +102,7 @@ class Communication:
self._status = Mode.EXPLORE self._status = Mode.EXPLORE
self.navto = [] self.navto = []
self.isfirstrun = True self.isfirstrun = True
self.target = None
def status(self): def status(self):
return self._status return self._status
@ -173,6 +174,7 @@ 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.target = str2tuple(*message[7:].rsplit(','))
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 is not None): if(self.navto is not None):
self._status = Mode.TARGET self._status = Mode.TARGET

View file

@ -28,24 +28,30 @@ def run():
move.resetwheels() move.resetwheels()
move.reset() 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())
# 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
while len(communication.navto) > 0: while len(communication.navto) > 0:
move.turnto(communication.navto[0][1]) move.turnto(communication.navto[0][1])
move.reset()
move.resetwheels()
move.traversetonextstation(True) move.traversetonextstation(True)
move.setcurdir(communication.planet._planetmap[communication.navto[0][0]][communication.navto[0][1]]) move.setcurdir(communication.planet._planetmap[communication.navto[0][0]][communication.navto[0][1]])
communication.update(None) communication.update(None)
communication.navto.pop(0) communication.navto.pop(0)
communication._status = Mode.EXPLORE if(communication.status() == Mode.TARGET):
communication._status = Mode.COMPLETE
else:
communication._status = Mode.EXPLORE
else: else:
move.turnto(communication.navto[0][1]) move.turnto(communication.navto[0][1])
sleep(4) sleep(4)
communication.planet.setcurdir(communication.navto[0][1]) communication.planet.setcurdir(communication.navto[0][1])
move.setcurdir(communication.getdir()) move.setcurdir(communication.getdir())
move.reset() move.reset()
move.resetwheels()
move.traversetonextstation(False) move.traversetonextstation(False)

View file

@ -40,7 +40,8 @@ class Move:
self._sensor.reset() self._sensor.reset()
sleep(1) sleep(1)
self._sensor.calibrateBrightness() self._sensor.calibrateBrightness()
sleep(2) sleep(5)
'''
ev3.Sound.beep() ev3.Sound.beep()
while(not self._sensor.bumperwaspressed): while(not self._sensor.bumperwaspressed):
self._sensor.checkbumper() self._sensor.checkbumper()
@ -55,7 +56,7 @@ class Move:
sleep(1) sleep(1)
self._sensor.calibrateRGB(self._sensor.blue) self._sensor.calibrateRGB(self._sensor.blue)
sleep(5) sleep(5)
'''
''' '''
Function to correct errors should the robot wander astray Function to correct errors should the robot wander astray
''' '''
@ -68,6 +69,8 @@ class Move:
self._sensor.refresh() self._sensor.refresh()
while(self._sensor.getbrightness() > self._sensor.edgebrightness): while(self._sensor.getbrightness() > self._sensor.edgebrightness):
self._sensor.refresh() self._sensor.refresh()
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
self._odometry.angle_set(self._odometry.angle_get())
self._wheel_l.stop() self._wheel_l.stop()
self._wheel_r.stop() self._wheel_r.stop()
self._wheel_l.turnbyamount(20, 40) self._wheel_l.turnbyamount(20, 40)
@ -88,8 +91,6 @@ class Move:
found_edges = [] found_edges = []
self._odometry.reset() self._odometry.reset()
while(True): while(True):
self._wheel_l.turnbyamount(40, 60)
self._wheel_r.turnbyamount(-40, 60)
self._aligntoedge() self._aligntoedge()
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
if(self._odometry.angle_get() in found_edges): if(self._odometry.angle_get() in found_edges):
@ -100,9 +101,6 @@ class Move:
self._wheel_l.turnbyamount(20, 40) self._wheel_l.turnbyamount(20, 40)
self._wheel_r.turnbyamount(-20, 40) self._wheel_r.turnbyamount(-20, 40)
sleep(2) sleep(2)
self._aligntoedge()
self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement()))
self._odometry.angle_set(self._odometry.angle_get())
return found_edges return found_edges
def setcurdir(self, newdir): def setcurdir(self, newdir):
@ -149,6 +147,13 @@ class Move:
self._wheel_l.stop() self._wheel_l.stop()
self._wheel_r.stop() self._wheel_r.stop()
if(self._sensor.bumperwaspressed is True): if(self._sensor.bumperwaspressed is True):
ev3.Sound.beep()
sleep(0.2)
ev3.Sound.beep()
sleep(0.2)
ev3.Sound.beep()
sleep(0.2)
ev3.Sound.beep()
self._wheel_l.turnbyamount(-200, 120) self._wheel_l.turnbyamount(-200, 120)
self._wheel_r.turnbyamount(-200, 120) self._wheel_r.turnbyamount(-200, 120)
sleep(1) sleep(1)
@ -167,7 +172,6 @@ class Move:
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)
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)
self._wheel_r.turnbyamount(200, 90) self._wheel_r.turnbyamount(200, 90)
sleep(2) sleep(2)

View file

@ -80,4 +80,4 @@ class Odometry:
self._posxy[1] = self._posxy[1] + self._wheel_center * math.cos(self._v_angle) self._posxy[1] = self._posxy[1] + self._wheel_center * math.cos(self._v_angle)
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)
return (destdir - (math.degrees(self._v_angle)%360)) * 2 return (destdir - (math.degrees(self._v_angle) % 360)) * 2

View file

@ -42,8 +42,8 @@ class Sensor:
print("Error while setting up sensors. Please make sure that everything is plugged in properly.") print("Error while setting up sensors. Please make sure that everything is plugged in properly.")
self.lastcolor = None self.lastcolor = None
self.edgebrightness = None self.edgebrightness = None
self.red = None self.red = (40, 30, 30)
self.blue = None self.blue = (40, 40, 70)
def refresh(self): def refresh(self):
self.RGB = self._camera.bin_data("hhh") self.RGB = self._camera.bin_data("hhh")
@ -66,16 +66,19 @@ class Sensor:
def getcolor(self): def getcolor(self):
if(self._camera.mode == 'RGB-RAW'): if(self._camera.mode == 'RGB-RAW'):
if(self.RGB[0] > self.red[0] and (self.RGB[1], self.RGB[2]) < (self.red[1], self.red[2])): if(self.RGB[0] > self.red[0] and self.RGB[1] < self.red[1] and self.RGB[2] < self.red[2]):
print("red: ", self.RGB)
self.lastcolor = self.red self.lastcolor = self.red
if(self.RGB[0] < self.blue[0] and (self.RGB[1], self.RGB[2]) > (self.blue[1], self.blue[2])): if(self.RGB[0] < self.blue[0] and self.RGB[1] > self.blue[1] and self.RGB[2] > self.blue[2]):
print("blue ", self.RGB)
self.lastcolor = self.blue self.lastcolor = self.blue
def calibrateRGB(self, color): def calibrateRGB(self, color):
self.refresh() self.refresh()
print("Color:", self.RGB)
if (color is self.red): if (color is self.red):
newRED = (self.RGB[0]-5, self.RGB[1]+5, self.RGB[2]+5) newRED = (self.RGB[0]-20, self.RGB[1]+15, self.RGB[2]+15)
self.red = newRED self.red = newRED
elif (color is self.blue): elif (color is self.blue):
newBLUE = (self.RGB[0]+5, self.RGB[1]-5, self.RGB[2]-5) newBLUE = (self.RGB[0]+10, self.RGB[1]-10, self.RGB[2])
self.blue = newBLUE self.blue = newBLUE