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

reapplied commit: 0124e57

Browse source
This commit is contained in:
haselnussbier 2018-03-23 19:09:49 +01:00 committed by d3rped
parent 31cfdf50d3
commit d0608fd870
3 changed files with 48 additions and 65 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

41
src/move.py
View file

@ -3,6 +3,7 @@
import ev3dev.ev3 as ev3
from wheel import Wheel
from sensor import Sensor
from odometry import Odometry
class Move:
@ -12,9 +13,11 @@ class Move:
self._wheel_r = Wheel('outC')
self._camera = Sensor()
self._bumper = ev3.TouchSensor()
self.maxbrightness
self.minbrightness
self.edgebrightness
self.maxbrightness = 0
self.minbrightness = 0
self.edgebrightness = 0
self._odometry = Odometry()
except OSError:
print("ERROR: Cannot find Motor/Sensor")
@ -29,13 +32,13 @@ class Move:
def calibrateBrightness(self):
i = 0
self._camera.refresh()
maxbrightness, minbrightness = self._camera.getbrightness()
self._wheel_l.speed_set(5)
self._wheel_r.speed_set(-5)
self.maxbrightness = self._camera.getbrightness()
self.minbrightness = self._camera.getbrightness()
self._wheel_l.speed_set(15)
self._wheel_r.speed_set(-15)
self._wheel_r.run()
self._wheel_l.run()
tempvalue
tempvalue = 0
while(i < 500):
self._camera.refresh()
tempvalue = self._camera.getbrightness()
@ -46,10 +49,10 @@ class Move:
i = i + 1
self._wheel_l.stop()
self._wheel_r.stop()
self.edgebrightness = (self.maxbrightness + self.minbrightness)/2
while(not (self.edgebrightness - 10 < self._camera.getbrightness() < self.edgebrightness + 10))
self._wheel_l.speed_set(5)
self._wheel_r.speed_set(-5)
self.edgebrightness = (self.maxbrightness + self.minbrightness)//2
while(not (self._camera.getbrightness() == self.edgebrightness)):
self._wheel_l.speed_set(15)
self._wheel_r.speed_set(-15)
self._wheel_l.stop()
self._wheel_r.stop()
# pass
@ -73,16 +76,18 @@ class Move:
self._wheel_r.turnbyamount(-500, 250)
def traversetonextstation(self, isknownstation):
self._wheel_l.speed_set(24)
self._wheel_r.speed_set(24)
self._wheel_l.speed_set(self.edgebrightness//2)
self._wheel_r.speed_set(self.edgebrightness//2)
self._wheel_l.run()
self._wheel_r.run()
while (self._bumper.value() == False and self._camera.lastcolor == None):
while (not self._bumper.value() and self._camera.lastcolor == None):
self._camera.refresh()
self._wheel_l.speed_set((60 - (self._camera.getbrightness()/5))-4)
self._wheel_r.speed_set((self._camera.getbrightness()/5))
self._wheel_l.speed_set(self.maxbrightness//2 - self._camera.getbrightness()//2)
self._wheel_r.speed_set(self._camera.getbrightness()//2)
self._camera.getcolor()
if(not isknownstation):
pass # run odometry stuff here
print(self._odometry.degree(self._wheel_l.getmovement(), self._wheel_r.getmovement(), 0))
# pass # run odometry stuff here
self._wheel_l.stop()
self._wheel_r.stop()

29
src/odometry.py
View file

@ -26,23 +26,20 @@ Functionality that should be implemented in this class:
class Odometry:
def __init__(self, planet):
def __init__(self): # planet
self._wheel_axis = 11
self._distance_per_tick = 0.0488
self._pi = 3.141
self.distance = 0
self.alpha = 0
# worked fine outside the class as function in nano, maybe we must fix it a bit :)
def degree(self, delta_rotation_l, delta_rotation_r,star_Direc): #delta_rotation_l/r should work with getmovement method
self.alpha = 0 #should be start_Direc
self.alpha=star_Direc
def degree(self, delta_rotation_l, delta_rotation_r): # delta_rotation_l/r should work with getmovement method
self.alpha = ((self.delta_rotation_l * self._distance_per_tick - self.delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha
self.distance=(self.delta_rotation_r * self._distance_per_tick + self.delta_rotation_l * self._distance_per_tick)/2 + self.distance
if self.alpha % (2 * self._pi) >=0 and self.alpha % (2 * self._pi) <self._pi*0.25 or self.alpa % (2 * self._pi ) >= 7/4*self._pi and self.alpha %(2 * pi) <2*pi:
self.alpha = ((delta_rotation_l * self._distance_per_tick - delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha
self.distance = (delta_rotation_r * self._distance_per_tick + delta_rotation_l * self._distance_per_tick)/2 + self.distance
if self.alpha % (2 * self._pi) >= 0 and self.alpha % (2 * self._pi) < self._pi * 0.25 or self.alpha % (2 * self._pi) >= 7 / 4 * self._pi and self.alpha % (2 * self._pi) < 2 * self._pi:
return(Direction.NORTH, "Distance = ", self.distance)
elif self.alpha % (2 * self._pi) >= self._pi * 0.25 and self.alpha % (2 * self._pi) < self._pi * 0.5 or self.alpha % (2 * self._pi) >= self._pi * 0.5 and self.alpha % (2 * self._pi) < self._pi * 0.75:
@ -50,6 +47,7 @@ class Odometry:
elif self.alpha % (2 * self._pi)>=self._pi * 0.75 and self.alpha % (2* self._pi)< self._pi or self.alpha % (2 * self._pi) >=self._pi and self.alpha < self.alpha * 5/4 pi :
return(Direction.SOUTH,"Distance = ",self.distance)
else:
return(Direction.WEST, "Distance = ", self.distance)
@ -58,15 +56,6 @@ class Odometry:
self.x = self.X_koord
self.wheel_center = (delta_rota_r + delta_rota_l) / 2
self.alpha = ((self.delta_rotation_l * self._distance_per_tick - self.delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha
self.y = self.y + wheel_center * math.cos(alpha)
self.x = self.x + wheel_center * math.sin(alpha)
self.y = self.y + self.wheel_center * math.cos(self.alpha)
self.x = self.x + self.wheel_center * math.sin(self.alpha)
return(self.x, self.y)
elif self.alpha % (2 * self._pi)>=self._pi * 0.75 and self.alpha % (2* self._pi)< self._pi or self.alpha % (2 * self._pi) >=self._pi and self.alpha < self.alpha * 5/4 pi :
return(Direction.SOUTH,"Distance = ",self.distance)
else:
return(Direction.WEST,"Distance = ",self.distance)

15
src/sensor.py
View file

@ -34,30 +34,19 @@ class Sensor:
self._sensor.mode = 'RGB-RAW'
self.RGB = self._sensor.bin_data("hhh")
self.lastcolor = None
self.maxbrightness
self.minbrightness
def refresh(self):
self.RGB = self._sensor.bin_data("hhh")
def getbrightness(self):
# if(self._sensor.mode == 'COL-REFLECT'):
if(self._sensor.mode == 'RGB-RAW'):
return ((0.299 * self.RGB[0]) + (0.587 * self.RGB[1]) + (0.114 * self.RGB[2]))
# return self._sensor.value()
else:
print("ERROR: Wrong Sensor Mode.")
return self._sensor.value()
def getcolor(self):
if(self._sensor.mode == 'RGB-RAW'):
if(self.RGB[0] > Color.RED[0] and (self.RGB[1], self.RGB[2]) < (Color.RED[1], Color.RED[2])):
self.lastcolor = Color.RED
if(self.RGB[0] < Color.BLUE[0] and (self.RGB[1], self.RGB[2]) > (Color.BLUE[1], Color.BLUE[2])):
self.lastcolor = Color.BLUE
def setmode(self, newmode):
self._sensor.mode = newmode
def calibrateRGB(self):
# Enums are uncangeable right? Dunno how to calibrate like that
# Enums are unchangeable right? Dunno how to calibrate like that
pass