Beispiel #1
0
        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()
Beispiel #2
0
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()