Mercurial > ukubik
changeset 91:76811686f4a9 default tip
Better following code
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Sat, 03 Jan 2015 14:51:04 +0100 |
| parents | da950915298d |
| children | |
| files | follow.py |
| diffstat | 1 files changed, 29 insertions(+), 29 deletions(-) [+] |
line wrap: on
line diff
--- a/follow.py Tue Dec 30 14:20:39 2014 +0100 +++ b/follow.py Sat Jan 03 14:51:04 2015 +0100 @@ -1,6 +1,8 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- +import collections + import serial import kubik @@ -17,37 +19,35 @@ self.gait = '' def follow_anim(self): - rotation_count = 30 + rotation_position = 0 + distances = collections.deque(maxlen=10) while True: - distance1 = robot.get_distance() - distance2 = robot.get_distance() - if abs(distance1 - distance2) < 10: - distance = (distance1 + distance2) / 2 - if distance > 80: - self.speed = [0.0, 0.0] - self.gait = '' - if 0 <= rotation_count < 60: - self.rotation = 1.0 - else: - self.rotation = -1.0 - if rotation_count > 19: - rotation_count = -60 - rotation_count += 1 - elif distance > 50: - self.rotation = 0.0 - self.speed = [0.0, 1.0] - self.gait = 'forward' - elif distance < 30: - self.rotation = 0.0 - self.speed = [0.0, -1.0] - self.gait = 'backward' - else: - self.rotation = 0.0 - self.speed = [0.0, 0.0] - self.gait = '' - yield + distances.append(robot.get_distance()) + distance = sum(distances) / len(distances) + print distance + if distance > 100: + self.speed = [0.0, 0.0] + self.gait = '' + if self.rotation == 0.0: + self.rotation = 1.0 + rotation_position += self.rotation + if not -30 < rotation_position < 30: + self.rotation = -self.rotation + elif distance > 40: + rotation_position = 0 + self.rotation = 0.0 + self.speed = [0.0, 0.8] + self.gait = 'forward' + elif distance < 30: + rotation_position = 0 + self.rotation = 0.0 + self.speed = [0.0, -0.8] + self.gait = 'backward' else: - yield + self.rotation = 0.0 + self.speed = [0.0, 0.0] + self.gait = '' + yield