import api.ps_drone as ps_drone
from tools import emergency
def getFrameGround():
IMC = drone.VideoImageCount
while drone.VideoImageCount == IMC:
time.sleep(0.01)
drone.groundVideo()
img = drone.VideoImage
return img
print "Booting up the drone"
drone = ps_drone.Drone() # Start using drone
thread = emergency.keyThread(drone)
drone.startup() # Connects to drone and starts subprocesses
drone.trim()
drone.getSelfRotation(5)
drone.reset() # Sets drone's status to good
while (drone.getBattery()[0] == -1):
time.sleep(0.1) # Wait until drone has done its reset
drone.useDemoMode(False) # Set 15 basic dataset/sec
drone.setConfigAllID()
drone.getNDpackage(["demo", "pressure_raw", "altitude", "magneto", "wifi"])
print "BATERIA ACTUAL: ", drone.getBattery()[0]
print "Booting up the camera"
drone.sdVideo() # Choose lower resolution (try hdVideo())
drone.groundCam() # Choose front view
CDC = drone.ConfigDataCount
def main():
drone = ps_drone.Drone() # Start using drone
tof = VL53L0X.VL53L0X()
thread = emergency.keyThread(drone)
print "Configuracion del drone"
drone.startup() # Connects to drone and starts subprocesses
drone.reset() # Always good, at start
drone.trim() # Recalibrate sensors
#drone.getSelfRotation(5)
time.sleep(0.5)
thread.start()
print "BATERIA ACTUAL: ", drone.getBattery()[0]
print "Comienzo el programa"
print "takeoff"
drone.takeoff()
time.sleep(2)
drone.hover()
time.sleep(2)
#drone.setSpeed(0.1)
print "hovering"
tof.start_ranging(4)
time.sleep(0.001)
drone.useDemoMode(False)
drone.getNDpackage(["demo", "pressure_raw", "altitude", "magneto", "wifi"])
time.sleep(1.0)
#THIS PART GETS NEAR THE OBSTACLE
for i in range(0, 2):
distance = tof.get_distance()
if i == 0:
target = 1000
else:
target = 600
while distance > target:
print "Distance: "
distance = tof.get_distance()
print distance
if distance <= target:
drone.moveBackward(0.5)
else:
drone.move(0.02, 0.1, 0.0, 0.0)
print "next"
#time.sleep(3)
print "back"
drone.moveBackward(0.5)
time.sleep(0.5)
drone.hover()
time.sleep(2)
#THIS PART GOES UP 1.2 METERS USING DRONES INTEGRATED ULTRASONIC SENSOR
NDC = drone.NavDataCount
alti = 0.0
while alti < 1100:
while drone.NavDataCount == NDC:
time.sleep(0.001)
if drone.getKey(): stop = True
NDC = drone.NavDataCount
alti = drone.NavData["altitude"][3]
print "Altitude: " + str(alti)
drone.move(0.00, 0.02, 0.6, 0.0)
drone.hover()
time.sleep(2)
drone.moveLeft(0.3)
time.sleep(2)
drone.hover()
time.sleep(2)
drone.land()
def main():
drone = ps_drone.Drone() # Start using drone
tof = VL53L0X.VL53L0X()
thread = emergency.keyThread(drone)
tof.start_ranging(4)
time.sleep(0.001)
if(tof.get_distance()==-1):
print "No se inicializo correctamente el tof"
return
print "Configuracion del drone"
drone.startup() # Connects to drone and starts subprocesses
drone.reset() # Always good, at start
drone.trim() # Recalibrate sensors
#drone.getSelfRotation(5)
time.sleep(0.5)
thread.start()
print "BATERIA ACTUAL: ", drone.getBattery()[0]
#NAV DATA
print "Obteniendo paquete de medidas"
drone.useDemoMode(False)
drone.getNDpackage(["demo","pressure_raw","altitude","magneto","wifi"])
print "Comienzo el programa"
print "takeoff"
drone.takeoff()
time.sleep(2)
drone.hover()
time.sleep(2)
#drone.setSpeed(0.1)
print "hovering"
#THIS PART GETS NEAR THE OBSTACLE
distance = tof.get_distance()
print "Distance before while: ",distance
while distance > 1300 and distance!=0:
distance = tof.get_distance()
print "Distance: ", distance
if distance < 1301:
drone.moveBackward(0.5)
else:
drone.moveForward(0.1)
print "next"
#time.sleep(3)
print "back"
drone.moveBackward(0.5)
time.sleep(0.5)
print "hover"
drone.hover()
time.sleep(5)
#THIS PART GOES UP 1600 mm
NDC = drone.NavDataCount
alti = 0.0
while alti < 1400:
while drone.NavDataCount == NDC: time.sleep(0.001)
NDC = drone.NavDataCount
alti = drone.NavData["altitude"][3]
print "Altitude: " + str(alti)
drone.move(0,0.05,0.99,0.0)
# #THIS PART GOES UP WHILE SEEIONG THE OBSTACLE AND GOES FORWARD
# #WITH A TIME OUT
# thisTime = time.time()
# distance = tof.get_distance()
# while distance < 5000:
# print "Distancia subiendo ", distance
# drone.moveUp(0.7)
# distance = tof.get_distance()
# actualTime = time.time()
# if (actualTime - thisTime) > 7 :
# drone.land()
# print "TIME OUT"
# break
# print "FINISHED GOING UP"
drone.moveDown(0.2)
time.sleep(0.5)
drone.hover()
time.sleep(2)
#THIS LANDS THE DRONE AFTER THE TIME OUT OR WHEN
#IT DETECTS A WALL
thisTime = time.time()
distance = tof.get_distance()
print "Distance before while: ",distance
while distance > 1000 or distance < 1:
distance = tof.get_distance()
print "Distance: ",distance
if distance < 1001:
drone.moveBackward(0.5)
else:
drone.moveForward(0.1)
actualTime = time.time()
if (actualTime - thisTime) > 6 :
drone.land()
print "TIME OUT"
break
print "back"
drone.moveBackward(0.5)
time.sleep(0.5)
drone.hover()
time.sleep(1)
print "land"
drone.land()
def main():
drone = ps_drone.Drone() # Start using drone
# tof = VL53L0X.VL53L0X()
thread = emergency.keyThread(drone)
print "Configuracion del drone"
drone.startup() # Connects to drone and starts subprocesses
drone.reset() # Always good, at start
drone.trim() # Recalibrate sensors
#drone.getSelfRotation(5)
time.sleep(0.5)
thread.start()
print "BATERIA ACTUAL: ", drone.getBattery()[0]
print "Comienzo el programa"
print "takeoff"
drone.takeoff()
time.sleep(2)
drone.hover()
time.sleep(2)
#drone.setSpeed(0.1)
print "hovering"
# tof.start_ranging(4)
time.sleep(0.001)
drone.useDemoMode(False)
drone.getNDpackage(["demo", "pressure_raw", "altitude", "magneto", "wifi"])
time.sleep(1.0)
'''
#THIS PART GETS NEAR THE OBSTACLE
distance = tof.get_distance()
print "Distance before while: ",distance
while distance > 1300 and distance!=0:
distance = tof.get_distance()
print "Distance: ", distance
if distance < 1301:
drone.moveBackward(0.5)
else:
drone.moveForward(0.1)
print "next"
#time.sleep(3)
print "back"
drone.moveBackward(0.5)
time.sleep(0.5)
print "hover"
drone.hover()
time.sleep(3)
'''
for i in range(0, 2):
#THIS PART GOES UP 1600 mm
NDC = drone.NavDataCount
alti = 0.0
while alti < 1100:
while drone.NavDataCount == NDC:
time.sleep(0.001)
NDC = drone.NavDataCount
alti = drone.NavData["altitude"][3]
print "Altitude UP: " + str(alti)
drone.move(0, -0.03, 0.6, 0.0)
drone.moveDown(0.2)
time.sleep(0.5)
drone.hover()
time.sleep(1)
NDC = drone.NavDataCount
while alti > 800:
while drone.NavDataCount == NDC:
time.sleep(0.001)
NDC = drone.NavDataCount
alti = drone.NavData["altitude"][3]
print "Altitude FORWARD: " + str(alti)
drone.move(0.025, 0.07, 0.0, 0.0)
print "back"
drone.moveBackward(0.1)
time.sleep(0.5)
drone.hover()
time.sleep(2)
print "hover"
drone.hover()
time.sleep(2)
print "left"
drone.moveLeft(0.2)
time.sleep(1)
drone.hover()
time.sleep(2)
print "land"
drone.land()