Modularized the (already existing) odometry class. Added some handy functions to reset loop control condition variables. Added second bumper to sensor class.

This commit is contained in:
d3rped 2018-03-24 04:43:01 +01:00
parent 97af61e5e1
commit c173917a72
3 changed files with 84 additions and 73 deletions

View file

@ -22,10 +22,8 @@ class Move:
self._sensor = Sensor() self._sensor = Sensor()
except OSError: except OSError:
print("ERROR: Cannot find Motor/Sensor") print("ERROR: Cannot find Motor/Sensor")
self.maxbrightness = 0 self.defaultspeed = 20
self.minbrightness = 0 self._odometry = Odometry()
self.edgebrightness = 0
self._odometry = Odometry()
''' '''
determine maximum and minimum brightness of lines/white space determine maximum and minimum brightness of lines/white space
@ -36,31 +34,7 @@ class Move:
self.calibrateBrightness() self.calibrateBrightness()
# pass # 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 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() Next turn around and detect edges through drops in _sensor.getbrightness()
''' '''
def getstationedges(self): def getstationedges(self):
self._wheel_l.turnbyamount(500, 250) pass
self._wheel_r.turnbyamount(-500, 250)
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): def traversetonextstation(self, isknownstation):
self._wheel_l.speed_set(self.edgebrightness//2) self._wheel_l.speed_set(self.edgebrightness//2)
self._wheel_r.speed_set(self.edgebrightness//2) 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 (not self._sensor.bumperwaspressed and self._sensor.lastcolor is None):
while (self._sensor.bumperispressed() is not True and self._sensor.lastcolor is None): self._sensor.refresh()
self._camera.refresh() self._sensor.checkbumper()
self._wheel_l.speed_set(capat100(self.maxbrightness/2 - self._sensor.getbrightness()/2)) 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._wheel_r.speed_set(capat100(self._sensor.getbrightness()/2 - self.maxbrightness/2)+self.defaultspeed)
self._sensor.getcolor() self._sensor.getcolor()
if(not isknownstation): 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_l.stop()
self._wheel_r.stop() self._wheel_r.stop()

View file

@ -5,6 +5,7 @@
import time import time
from planet import Direction from planet import Direction
import math import math
import decimal
''' '''
Documentation: Documentation:
@ -16,7 +17,7 @@ axis a (measure distance where wheels have most friction)
calibration factor c = diameter/ticks_per_turn calibration factor c = diameter/ticks_per_turn
distance d = ticks wheel * c (calculate for each wheel individually) distance d = ticks wheel * c (calculate for each wheel individually)
vector_length = (d_l + d_r)/2 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: Functionality that should be implemented in this class:
- tracking of relative distances - 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: 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 = math.pi
self.distance = 0 self._posxy = [0, 0]
self.alpha = 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 angle_set(self, newangle):
def degree(self, delta_rotation_l, delta_rotation_r): # delta_rotation_l/r should work with getmovement method 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 def coordinates_get(self):
self.distance = (delta_rotation_r * self._distance_per_tick + delta_rotation_l * self._distance_per_tick)/2 + self.distance return ((posround(self._posxy[0]), posround(self._posxy[1])), angle2dir(self._v_angle))
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: def pos_update(self, ticks_wheel):
return(Direction.EAST, "Distance = ", self.distance) 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: def coordinates_update(self): # worked as function outside the class
return(Direction.SOUTH, "Distance = ", self.distance) 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: def dir2ticks(destdir): # return amount of ticks to turn to a given direction (current direction should be _v_angle)
return(Direction.WEST, "Distance = ", self.distance) ticksperwheel = None
return ticksperwheel
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)

View file

@ -24,25 +24,40 @@ Y=(0.299 x R) + (0.587 x G) + (0.114 x B);
class Color(tuple, Enum): class Color(tuple, Enum):
RED = (90, 40, 25) RED = (85, 40, 25)
BLUE = (25, 75, 90) BLUE = (25, 70, 85)
class Sensor: class Sensor:
def __init__(self): def __init__(self):
self._bumper = ev3.TouchSensor() try:
self._camera = ev3.ColorSensor() self._bumper_l = ev3.TouchSensor("in1")
self._camera.mode = 'RGB-RAW' 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.RGB = self._camera.bin_data("hhh")
self._brightness = self._camera.value() self._brightness = self._camera.value()
def reset(self):
self.bumperwaspressed = False
self.lastcolor = None self.lastcolor = None
def bumperispressed(self): def calibrateBrightness(self):
self._bumper.value() self.refresh()
self.edgebrightness = self.getbrightness()
def refresh(self): # do we still want to check if the propper camera/sensor mode is set? def checkbumper(self):
self.RGB = self._camera.bin_data("hhh") if(self._bumper_l.value() or self._bumper_r.value()):
self._brightness = self._camera.value() self.bumperwaspressed = True
def getbrightness(self): def getbrightness(self):
return self._brightness return self._brightness
@ -55,5 +70,4 @@ class Sensor:
self.lastcolor = Color.BLUE self.lastcolor = Color.BLUE
def calibrateRGB(self): def calibrateRGB(self):
# Enums are unchangeable right? Dunno how to calibrate like that
pass pass