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 - 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.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)
from navigation.rc import RCLib
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
from navigation.rc import RCLib
import time
import navigation.imu as imu
from navigation.log import *
log = LogLib()
rc = RCLib(log)
rc.setmode('ALT_HOLD')
rc.arm()
print 'Throttle'
rc.throttle("time", 3, -0.20)
rc.throttle("time", 120, 0)
# elif (steerinp < 0.15) :
# steer = 0.15
# return steer
#def fixAngle():
# error = rc.getDeg() - 48
#
# while (abs(error) > 3):
# steer = abs(error)*kP
# steer = clipSteer(steer)
#
# if (error>0) rc.raw('yaw', (1500 - (400*steer)) elif (error<0) rc.raw('yaw', (1500 + (400*steer)
# error = rc.getDeg() -48
originAngle = 48
rc = RCLib()
rc.setmode('ALT_HOLD')
rc.arm()
#fixAngle()
#endTime = time.time() + 7
#while(time.time() < endTime):
# rc.raw('pitch', 1700)
rc.throttle("time", 2.5, -0.5)
rc.forward("time", 8, 0.4)