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

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

Browse source
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
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/communication.py
View file

@ -102,6 +102,7 @@ class Communication:
self._status = Mode.EXPLORE
self.navto = []
self.isfirstrun = True
self.target = None
def status(self):
return self._status
@ -173,6 +174,7 @@ class Communication:
elif message.startswith(Command.PATH):
self.stringtopath(*message[5:].rsplit(' '))
elif message.startswith(Command.TARGET):
self.target = str2tuple(*message[7:].rsplit(','))
self.navto = self.planet.shortest_path(self.planet.getcurnode(), str2tuple(*message[7:].rsplit(',')))
if(self.navto is not None):
self._status = Mode.TARGET

10
src/main.py
View file

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

20
src/move.py
View file

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

2
src/odometry.py
View file

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

15
src/sensor.py
View file

@ -42,8 +42,8 @@ class Sensor:
print("Error while setting up sensors. Please make sure that everything is plugged in properly.")
self.lastcolor = None
self.edgebrightness = None
self.red = None
self.blue = None
self.red = (40, 30, 30)
self.blue = (40, 40, 70)
def refresh(self):
self.RGB = self._camera.bin_data("hhh")
@ -66,16 +66,19 @@ class Sensor:
def getcolor(self):
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
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
def calibrateRGB(self, color):
self.refresh()
print("Color:", self.RGB)
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
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