rc = RCLib()
ac = ACLib()
rc.setmode('ALT_HOLD')
rc.arm()
rc.throttle("time", 3, -0.20)
#i = 0
#while i<500:
# ac.get_distance_right()
# rc.setmode('ALT_HOLD')
# time.sleep(0.1)
# i = i + 1
rc.forward("time", 6, 0.1)
wallDist = 0
counter = 0
while ((ac.get_distance_right()[1] < 70) and (counter < 50)):
wallDist = ac.get_distance_right()[0]
counter = counter +1
print 'wall' , wallDist
print 'counter ' , counter
diff = ac.get_distance_right()[0] - 1500
print 'Laterl diff ' , diff
rc.lateralDist(diff)
rc.close()
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 45)
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 135)
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 225)
rc.forward("time", 5, 0.25)
rc.imu_turn(START + 175)
rc.forward("time", 5, 0.35)
pingReturn = ac.get_distance_right()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = 0
while ((confidence < 70) and (counter < 50)):
pingReturn = ac.get_distance_right()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = counter + 1
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - GATECENTER
print 'Laterl diff ', diff
rc.lateralDist(diff)
from navigation.ac import ACLib
import time
ac = ACLib()
i = 0
#while i < 10:
while (1):
#print 'left' , ac.get_distance_left()[0], ac.get_distance_left()[1]
print 'right', ac.get_distance_right()[0], ac.get_distance_right()[1]
#print 'fwd' , ac.get_distance_fwd()[0], ac.get_distance_fwd()[1]
print '--'