From d0608fd87084dfbd7d4cd0e7c94a7d35c542a3ac Mon Sep 17 00:00:00 2001 From: haselnussbier Date: Fri, 23 Mar 2018 19:09:49 +0100 Subject: [PATCH] reapplied commit: 0124e57 --- src/move.py | 41 +++++++++++++++++++---------------- src/odometry.py | 57 ++++++++++++++++++++----------------------------- src/sensor.py | 15 ++----------- 3 files changed, 48 insertions(+), 65 deletions(-) diff --git a/src/move.py b/src/move.py index 4ae4cbe..c2352c9 100644 --- a/src/move.py +++ b/src/move.py @@ -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() diff --git a/src/odometry.py b/src/odometry.py index 9a2ff49..a2fac38 100644 --- a/src/odometry.py +++ b/src/odometry.py @@ -26,47 +26,36 @@ Functionality that should be implemented in this class: class Odometry: - - def __init__(self, planet): - self._wheel_axis = 11 - self._distance_per_tick= 0.0488 - self._pi = 3.141 - + 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 +# 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): # 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) = 7/4*self._pi and self.alpha %(2 * pi) <2*pi: - return(Direction.NORTH,"Distance = ",self.distance) + 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: - return(Direction.EAST,"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: + 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 : 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 - self.y= self.Y_koord - self.x= self.X_koord - self.wheel_center = (delta_rota_r + delta_rota_l) /2 + 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 + 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.y = self.y + wheel_center * math.cos(alpha) - self.x = self.x + wheel_center * math.sin(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) + 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) diff --git a/src/sensor.py b/src/sensor.py index 9dde436..a7b6c9e 100644 --- a/src/sensor.py +++ b/src/sensor.py @@ -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