rc.arm()
print(rc.getDeg())
#time.sleep(2)
#rc.forward("time", 6, 0.25)
#rc.yaw("time", 1.2, -0.25)
#rc.forward("time", 3, 0.25)
#rc.yaw("time", 1.2, -0.25)
#rc.forward("time", 6, 0.25)`
start = 72 + 180
rc.setmode('ALT_HOLD')
rc.throttle("time", 3, -0.25)
rc.imu_turn(start)
rc.forward("time", 5, 0.35)
rc.forward("time", 5, 0.35)
rc.yaw("imu", -45, 0.2)
rc.forward("time", 3, 0.25)
rc.yaw("imu", 90, 0.2)
rc.forward("time", 3, 0.25)
rc.yaw("imu", 90, 0.2)
rc.forward("time", 3, 0.25)
rc.yaw("imu", 90, 0.2)
rc.forward("time", 3, 0.25)
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = counter + 1
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - value
print 'Laterl diff ', diff
rc.lateralDist(diff)
# 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", 10, 0.35)
leftAlign(2400)
rc.imu_turn(START)
rc.forward("time", 10, 0.35)
leftAlign(1500)
rc.imu_turn(START)
rc.forward("time", 13, 0.35)
leftAlign(3600)
rc.imu_turn(START + 182)
rc.forward("time", 13, 0.25)
rc.imu_turn(START + 182)
rightAlign(2300)
rc.forward("time", 13, 0.35)
wallDist = 3800
if wallDist < 500:
wallDist = 500
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - value
print 'Laterl diff ', diff
rc.lateralDist(diff)
# 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", 10, 0.32)
leftAlign(2400)
rc.imu_turn(START)
rc.forward("time", 5, 0.32)
leftAlign(2000)
rc.imu_turn(START)
rc.forward("time", 5, 0.32)
leftAlign(1500)
rc.imu_turn(START)
rc.forward("time", 5, 0.32)
leftAlign(1500)
rc.forward("time", 5, 0.32)
leftAlign(3500)
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)
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)
import time
import navigation.imu as imu
from navigation.log import *
log = LogLib()
rc = RCLib(log)
rc.setmode('ALT_HOLD')
rc.arm()
rc.throttle("time", 1, -0.5)
start = rc.getDeg()
rc.imu_turn(start + 90)
#i=0
#while (i<50) :
# rc.getDeg()
# rc.raw("yaw", 1555)
# i = i+1
#start = rc.getDeg()
#print(start)
#print('Now Yawing')
#rc.yaw("imu", 45, 0.2)
#turn=90
#end = int(turn + start)
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = counter + 1
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - value
print 'Laterl diff ', diff
rc.lateralDist(diff)
# 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", 10, 0.35)
leftAlign(2400)
rc.imu_turn(START)
rc.forward("time", 5, 0.35)
leftAlign(2000)
rc.imu_turn(START)
rc.forward("time", 5, 0.35)
leftAlign(1500)
rc.imu_turn(START)
rc.forward("time", 5, 0.35)
leftAlign(1500)
rc.forward("time", 4, 0.35)
leftAlign(2500)
leftAlign(3500)
rc.setmode('MANUAL')
rc.arm()
print(rc.getDeg())
# global "origin" parameter
start = 72
# depth hold and go down
rc.setmode('ALT_HOLD')
rc.throttle("time", 3, -0.25)
# align with the origin
rc.imu_turn(start)
rc.forward("time", 10, 0.35)
rc.imu_turn(start-45)
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)
wallDist = 3800
if wallDist < 500:
wallDist = 500
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - value
print 'Laterl diff ', diff
rc.lateralDist(diff)
# 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", 10, 0.32)
leftAlign(2400)
rc.imu_turn(START)
rc.forward("time", 5, 0.32)
leftAlign(2000)
rc.imu_turn(START)
rc.forward("time", 5, 0.32)
leftAlign(1500)
rc.imu_turn(START)
rc.forward("time", 5, 0.32)
leftAlign(1500)
rc.forward("time", 5, 0.32)
leftAlign(3500)
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = counter + 1
print 'wall', wallDist
print 'counter ', counter
diff = wallDist - value
print 'Laterl diff ', diff
rc.lateralDist(diff)
# 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", 10, 0.35)
leftAlign(GATECENTER)
rc.imu_turn(START)
rc.forward("time", 1, 0.35)
leftAlign(GATECENTER - 750)
rc.imu_turn(START)
rc.forward("time", 1, 0.35)
leftAlign(GATECENTER + 750)
rc.imu_turn(START + 185)
rc.forward("time", 1, 0.25)
rc.imu_turn(START + 185)
rightAlign(GATECENTER)
rc.forward("time", 1, 0.35)