def resurface():
global tolerance
global location
while location[2] > tolerance:
cmdAgent.cmdAgent(Rise)
getPosition.getPosition()
return 0
#functional loop
while i < 11:
elapsetime+=time.clock()
#check if destination has been reached
if (location == destination) or (location > bounds):
while difftime < 15:
ser.write(Hover)
difftime+=time.clock()
#write to sonar
#retreat
resurface.resurface()
response = cmdAgent.cmdAgent(PwrOff)
else:
#check the state of the agent
if response == "" or response == "Ready":
getPosition.getPosition()
# cmdAgent(Fwd)
# time.sleep(1)
# cmdAgent(PwrOff)
#cmdAgent(PwrOn)
#print summ
#getHeading()
#calculate the change in position
diff[0] = location[0] - prevlocation[0]
diff[1] = location[1] - prevlocation[1]
diff[2] = location[2] - prevlocation[2]
maglocation = math.sqrt((math.pow(diff[0],2))+(math.pow(diff[1],2))+(math.pow(diff[2],2)))
#functional loop
while i < 11:
elapsetime += time.clock()
#check if destination has been reached
if (location == destination) or (location > bounds):
while difftime < 15:
ser.write(Hover)
difftime += time.clock()
#write to sonar
#retreat
resurface.resurface()
response = cmdAgent.cmdAgent(PwrOff)
else:
#check the state of the agent
if response == "" or response == "Ready":
getPosition.getPosition()
# cmdAgent(Fwd)
# time.sleep(1)
# cmdAgent(PwrOff)
#cmdAgent(PwrOn)
#print summ
#getHeading()
#calculate the change in position
diff[0] = location[0] - prevlocation[0]
diff[1] = location[1] - prevlocation[1]
diff[2] = location[2] - prevlocation[2]
maglocation = math.sqrt((math.pow(diff[0], 2)) +
(math.pow(diff[1], 2)) +
