self.past_diffs = deque(maxlen=deque_length)
def step(self):
print '\033[1;36m{}\033[0m'.format('TURNING START')
if self.target_yaw is None:
self.target_yaw = p.yaw.get() + self.want_degree
deg = p.yaw.get()
diff = constrain(self.c.run(self.target_yaw - deg), 0.4, -0.4)
# print deg, diff
p.wheels.set_diff(speed=0, diff=-diff)
print 'deg: {:8f} target yaw:{:8f} '.format(self.target_yaw, deg),
self.past_diffs.append(self.target_yaw - deg)
average_diff = abs(sum([angle for angle in self.past_diffs])) / float(
len(self.past_diffs))
print 'avg: {}'.format(average_diff)
self.finish_flag = average_diff < self.tolerant_angle
print '\033[1;36m{}\033[0m'.format('TURNING END')
return self.finish_flag
if __name__ == '__main__':
tt = Tortoise()
tt.task = Turning(90)
tt.walk()
def record_working_loop(name='default_rec'):
t = Tortoise()
t.task = RecordingTask(name)
t.walk()
self._a2 = self._kd
def constrain(a, l, u):
if a > u:
return u
elif a < l:
return l
else:
return a
class Turning(Task):
def __init__(self):
super(Turning, self).__init__()
self.c = PIDController(0.04, 0, 0)
self.target_yaw = 0
def step(self):
deg = p.yaw.get()
diff = constrain(self.c.run(self.target_yaw - deg), -0.4, 0.4)
print deg, diff
p.wheels.set_diff(speed=0, diff=-diff)
if __name__ == '__main__':
tt = Tortoise()
tt.task = Turning()
tt.walk()
