from navigation.rc import RCLib
from navigation.ac import ACLib
import time
import navigation.imu as imu
rc = RCLib()
ac = ACLib()
rc.setmode('MANUAL')
rc.arm()
print(rc.getDeg())
# global "origin" parameter
START = 239
DEPTH_SEC = 5.2
SEC_PER_METER = 1
GATECENTER = 2500
def leftAlign(value):
pingReturn = ac.get_distance_fwd()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = 0
while ((confidence < 70) and (counter < 50)):
pingReturn = ac.get_distance_fwd()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = counter + 1
from navigation.rc import RCLib
from navigation.ac import ACLib
import time
import navigation.imu as imu
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
from navigation.rc import RCLib
from navigation.ac import ACLib
import time
import navigation.imu as imu
GATECENTER = 2000
rc = RCLib()
ac = ACLib()
rc.setmode('MANUAL')
rc.arm()
print(rc.getDeg())
# global "origin" parameter
START = 67
DEPTH_SEC = 3
SEC_PER_METER = 1
# depth hold and go down
rc.setmode('ALT_HOLD')
rc.throttle("time", DEPTH_SEC, -0.25)
# align with the origin
rc.imu_turn(START)
rc.forward("time", 6, 0.35)
while (ac.get_distance_fwd()[0] > 7000):
rc.forward("time", 1, 0.35)
rc.imu_turn(START - 45)
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 '--'