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

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

Browse source
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
Show stats Download patch file Download diff file Expand all files Collapse all files

50
src/move.py
View file

@ -22,9 +22,7 @@ class Move:
self._sensor = Sensor()
except OSError:
print("ERROR: Cannot find Motor/Sensor")
self.maxbrightness = 0
self.minbrightness = 0
self.edgebrightness = 0
self.defaultspeed = 20
self._odometry = Odometry()
'''
@ -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()

69
src/odometry.py
View file

@ -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

28
src/sensor.py
View file

@ -24,26 +24,41 @@ 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()
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
def bumperispressed(self):
self._bumper.value()
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 calibrateBrightness(self):
self.refresh()
self.edgebrightness = self.getbrightness()
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