From c173917a729b9e24075555656f680eecad810d75 Mon Sep 17 00:00:00 2001 From: d3rped Date: Sat, 24 Mar 2018 04:43:01 +0100 Subject: [PATCH] Modularized the (already existing) odometry class. Added some handy functions to reset loop control condition variables. Added second bumper to sensor class. --- src/move.py | 52 +++++++++++-------------------------- src/odometry.py | 69 +++++++++++++++++++++++++++++++------------------ src/sensor.py | 36 ++++++++++++++++++-------- 3 files changed, 84 insertions(+), 73 deletions(-) diff --git a/src/move.py b/src/move.py index 24851e9..62c6dfd 100644 --- a/src/move.py +++ b/src/move.py @@ -22,10 +22,8 @@ class Move: self._sensor = Sensor() except OSError: print("ERROR: Cannot find Motor/Sensor") - self.maxbrightness = 0 - self.minbrightness = 0 - self.edgebrightness = 0 - self._odometry = Odometry() + self.defaultspeed = 20 + self._odometry = Odometry() ''' determine maximum and minimum brightness of lines/white space @@ -36,31 +34,7 @@ class Move: self.calibrateBrightness() # pass - def calibrateBrightness(self): - i = 0 - self.maxbrightness = self._sensor.getbrightness() - self.minbrightness = self._sensor.getbrightness() - self._wheel_l.speed_set(15) - self._wheel_r.speed_set(-15) - self._wheel_r.run() - self._wheel_l.run() - tempvalue = 0 - while(i < 500): - self._sensor.refresh() - tempvalue = self._sensor.getbrightness() - if(tempvalue > self.maxbrightness): - self.maxbrightness = tempvalue - if(tempvalue < self.minbrightness): - self.minbrightness = tempvalue - i = i + 1 - self._wheel_l.stop() - self._wheel_r.stop() - self.edgebrightness = (self.maxbrightness + self.minbrightness)//2 - while(not (self._sensor.getbrightness() == self.edgebrightness)): - self._wheel_l.speed_set(15) - self._wheel_r.speed_set(-15) - self._wheel_l.stop() - self._wheel_r.stop() + ''' Function to correct errors should the robot wander astray @@ -77,22 +51,26 @@ class Move: Next turn around and detect edges through drops in _sensor.getbrightness() ''' def getstationedges(self): - self._wheel_l.turnbyamount(500, 250) - self._wheel_r.turnbyamount(-500, 250) + pass + + def turnto(self, direction): + turnval = self._odometry.dir2ticks(direction) + self._wheel_l.turnbyamount(turnval, self.defaultspeed) + self._wheel_r.turnbyamount(-turnval, self.defaultspeed) def traversetonextstation(self, isknownstation): 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._sensor.bumperispressed() is not True and self._sensor.lastcolor is None): - self._camera.refresh() - self._wheel_l.speed_set(capat100(self.maxbrightness/2 - self._sensor.getbrightness()/2)) - self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2)) + while (not self._sensor.bumperwaspressed and self._sensor.lastcolor is None): + self._sensor.refresh() + self._sensor.checkbumper() + self._wheel_l.speed_set(capat100(self.maxbrightness/2 - self._sensor.getbrightness()/2)+self.defaultspeed) + self._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self.maxbrightness/2)+self.defaultspeed) self._sensor.getcolor() if(not isknownstation): - print(self._odometry.degree(self._wheel_l.getmovement(), self._wheel_r.getmovement(), 0)) + self._odometry.pos_update((self._wheel_l.getmovement(), self._wheel_r.getmovement())) self._wheel_l.stop() self._wheel_r.stop() diff --git a/src/odometry.py b/src/odometry.py index ec37ad7..1565138 100644 --- a/src/odometry.py +++ b/src/odometry.py @@ -5,6 +5,7 @@ import time from planet import Direction import math +import decimal ''' Documentation: @@ -16,7 +17,7 @@ axis a (measure distance where wheels have most friction) calibration factor c = diameter/ticks_per_turn distance d = ticks wheel * c (calculate for each wheel individually) vector_length = (d_l + d_r)/2 -vector_angle ~= (d_l + d_r)/a (only applicable for small distances) +vector_angle ~= (d_l - d_r)/a (only applicable for small distances) Functionality that should be implemented in this class: - tracking of relative distances @@ -26,37 +27,55 @@ Functionality that should be implemented in this class: ''' +def angle2dir(angle): + if angle % (2 * math.pi) >= 0 and angle % (2 * math.pi) < math.pi * 0.25 or angle % (2 * math.pi) >= 7 / 4 * math.pi and angle % (2 * math.pi) < 2 * math.pi: + return Direction.NORTH + elif angle % (2 * math.pi) >= math.pi * 0.25 and angle % (2 * math.pi) < math.pi * 0.5 or angle % (2 * math.pi) >= math.pi * 0.5 and angle % (2 * math.pi) < math.pi * 0.75: + return Direction.EAST + elif angle % (2 * math.pi) >= math.pi * 0.75 and angle % (2 * math.pi) < math.pi or angle % (2 * math.pi) >= math.pi and angle % (2 * math.pi) < 5 / 4 * math.pi: + return Direction.SOUTH + else: + return Direction.WEST + + +def posround(pos): + return decimal.Decimal(pos/50).quantize(0, rounding=decimal.ROUND_HALF_UP) + + class Odometry: def __init__(self): # planet self._wheel_axis = 11 self._distance_per_tick = 0.0488 - self._pi = 3.141 - self.distance = 0 - self.alpha = 0 + self._pi = math.pi + self._posxy = [0, 0] + self._distance = [0, 0] + self._v_length = 0 + self._v_angle = Direction.NORTH + def reset(self): + self._distance = [0, 0] + self._v_length = 0 + self._posxy = [0, 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): # delta_rotation_l/r should work with getmovement method + def angle_set(self, newangle): + self._v_angle = newangle - 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) + def coordinates_get(self): + return ((posround(self._posxy[0]), posround(self._posxy[1])), angle2dir(self._v_angle)) - 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) + def pos_update(self, ticks_wheel): + self._distance[0] = ticks_wheel[0] * self._distance_per_tick + self._distance[1] = ticks_wheel[1] * self._distance_per_tick + self._v_length = self._v_length + (self._distance[0] + self._distance[1])/2 + self._v_angle = self._v_angle + (ticks_wheel[0] - ticks_wheel[1])/self._wheel_axis + self.coordinates_update() + print((self._v_length, angle2dir(self._v_angle))) - 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 % (2 * self._pi) < 5 / 4 * self._pi: - return(Direction.SOUTH, "Distance = ", self.distance) + def coordinates_update(self): # worked as function outside the class + self._posxy[0] = self._posxy[0] + self._v_length * math.sin(self._v_angle) + self._posxy[1] = self._posxy[1] + self._v_length * math.cos(self._v_angle) + print((self._posxy[0], self._posxy[1])) - 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 + self.wheel_center * math.cos(self.alpha) - self.x = self.x + self.wheel_center * math.sin(self.alpha) - return(self.x, self.y) + def dir2ticks(destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle) + ticksperwheel = None + return ticksperwheel diff --git a/src/sensor.py b/src/sensor.py index ca520e4..3ad2f2c 100644 --- a/src/sensor.py +++ b/src/sensor.py @@ -24,25 +24,40 @@ Y=(0.299 x R) + (0.587 x G) + (0.114 x B); class Color(tuple, Enum): - RED = (90, 40, 25) - BLUE = (25, 75, 90) + RED = (85, 40, 25) + BLUE = (25, 70, 85) class Sensor: def __init__(self): - self._bumper = ev3.TouchSensor() - self._camera = ev3.ColorSensor() - self._camera.mode = 'RGB-RAW' + try: + self._bumper_l = ev3.TouchSensor("in1") + self._bumper_r = ev3.TouchSensor("in4") + self.bumperwaspressed = False + self._camera = ev3.ColorSensor() + self._camera.mode = 'RGB-RAW' + self.RGB = self._camera.bin_data("hhh") + self._brightness = self._camera.value() + except OSError: + print("Error while setting up sensors. Please make sure that everything is plugged in properly.") + self.lastcolor = None + self.edgebrightness = None + + def refresh(self): # do we still want to check if the propper camera/sensor mode is set? self.RGB = self._camera.bin_data("hhh") self._brightness = self._camera.value() + + def reset(self): + self.bumperwaspressed = False self.lastcolor = None - def bumperispressed(self): - self._bumper.value() + def calibrateBrightness(self): + self.refresh() + self.edgebrightness = self.getbrightness() - def refresh(self): # do we still want to check if the propper camera/sensor mode is set? - self.RGB = self._camera.bin_data("hhh") - self._brightness = self._camera.value() + def checkbumper(self): + if(self._bumper_l.value() or self._bumper_r.value()): + self.bumperwaspressed = True def getbrightness(self): return self._brightness @@ -55,5 +70,4 @@ class Sensor: self.lastcolor = Color.BLUE def calibrateRGB(self): - # Enums are unchangeable right? Dunno how to calibrate like that pass