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

53
src/odometry.py
View file

@ -26,47 +26,36 @@ Functionality that should be implemented in this class:
class Odometry: class Odometry:
def __init__(self): # planet
def __init__(self, planet):
self._wheel_axis = 11 self._wheel_axis = 11
self._distance_per_tick= 0.0488 self._distance_per_tick = 0.0488
self._pi = 3.141 self._pi = 3.141
self.distance = 0 self.distance = 0
self.alpha = 0
#worked fine outside the class as function in nano, maybe we must fix it a bit :) # 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 def degree(self, delta_rotation_l, delta_rotation_r): # delta_rotation_l/r should work with getmovement method
self.alpha = 0 #should be start_Direc
self.alpha=star_Direc
self.alpha = ((self.delta_rotation_l * self._distance_per_tick - self.delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha self.alpha = ((delta_rotation_l * self._distance_per_tick - 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 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.alpa % (2 * self._pi ) >= 7/4*self._pi and self.alpha %(2 * pi) <2*pi: 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) 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: 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:
return(Direction.EAST,"Distance = ",self.distance) return(Direction.EAST, "Distance = ", self.distance)
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 : 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) return(Direction.SOUTH,"Distance = ",self.distance)
else:
return(Direction.WEST,"Distance = ",self.distance)
def coordinates(self,delta_rota_l,delta_rota_r,Y_koord,X_koord): # worked as function outside the class else:
self.y= self.Y_koord return(Direction.WEST, "Distance = ", self.distance)
self.x= self.X_koord
self.wheel_center = (delta_rota_r + delta_rota_l) /2 def coordinates(self, delta_rota_l, delta_rota_r, Y_koord, X_koord): # worked as function outside the class
self.y = self.Y_koord
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.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.y = self.y + self.wheel_center * math.cos(self.alpha)
self.x = self.x + wheel_center * math.sin(alpha) self.x = self.x + self.wheel_center * math.sin(self.alpha)
return(self.x,self.y) 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._sensor.mode = 'RGB-RAW'
self.RGB = self._sensor.bin_data("hhh") self.RGB = self._sensor.bin_data("hhh")
self.lastcolor = None self.lastcolor = None
self.maxbrightness
self.minbrightness
def refresh(self): def refresh(self):
self.RGB = self._sensor.bin_data("hhh") self.RGB = self._sensor.bin_data("hhh")
def getbrightness(self): def getbrightness(self):
# if(self._sensor.mode == 'COL-REFLECT'): return self._sensor.value()
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.")
def getcolor(self): 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])): if(self.RGB[0] > Color.RED[0] and (self.RGB[1], self.RGB[2]) < (Color.RED[1], Color.RED[2])):
self.lastcolor = Color.RED self.lastcolor = Color.RED
if(self.RGB[0] < Color.BLUE[0] and (self.RGB[1], self.RGB[2]) > (Color.BLUE[1], Color.BLUE[2])): if(self.RGB[0] < Color.BLUE[0] and (self.RGB[1], self.RGB[2]) > (Color.BLUE[1], Color.BLUE[2])):
self.lastcolor = Color.BLUE self.lastcolor = Color.BLUE
def setmode(self, newmode):
self._sensor.mode = newmode
def calibrateRGB(self): def calibrateRGB(self):
# Enums are uncangeable right? Dunno how to calibrate like that # Enums are unchangeable right? Dunno how to calibrate like that
pass pass