def figure8():
def check():
if not running:
thread.exit()
direction = -1
while True:
rocky.fd.x = FAST
# Wait until past home
print('Approach')
old_dist = float('inf')
while True:
dist = rocky.geographic.distance(home, rocky.nav.position)
if dist > old_dist:
break
old_dist = dist
sleep(SLEEP)
check()
# Wait until away from homeposition
print('Retreat')
distance = rocky.geographic.distance(home, rocky.nav.position)
while distance < 7:
distance = rocky.geographic.distance(home, rocky.nav.position)
print('Retreat {} {} {}'.format(round(rocky.fd.x, 3), distance,
rocky.nav.position))
sleep(SLEEP)
check()
# Flip directions
rocky.fd.x = SLOW
direction *= -1
print('Rotate')
diff = float('inf')
while diff > 4:
bear = home_bear()
diff = rocky.nav.heading - bear
diff = diff % 360
diff = diff if diff < 180 else -(diff - 360)
rocky.fd.r = direction * abs(limiter(-0.25, 0.25)(diff / 100))
sleep(SLEEP)
check()
# Stop rotating
rocky.fd.r = 0.0
def range_limit(current, target):
return limiter(-0.2, 0.2)(proportional(0.1 / 50.0)(current, target))