-
Notifications
You must be signed in to change notification settings - Fork 9
Expand file tree
/
Copy pathtestMoveByEnd.py
More file actions
55 lines (47 loc) · 1.57 KB
/
Copy pathtestMoveByEnd.py
File metadata and controls
55 lines (47 loc) · 1.57 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
import time
import math
import sys
from core.bebop import *
drone = Bebop()
# Distance to move in [m] and dPsi [rad]
dX = 0
dY = 0
dZ = 0
dPsi = math.pi/2
moveDone = True # Flag to know when movements are done
def moveByFuncition():
try:
drone.takeoff()
drone.wait( 1.0 )
dorne.hover()
for i in range (0,4):
print "Movement: ", i
drone.moveBy( dX, dY, dZ, dPsi) # Command to move to a relative position
moveByControl() # Stops the movements
drone.hover()
drone.wait( 1.0 )
drone.land()
sys.exit(0)
except ManualControlException, e:
print
print "ManualControlException"
if robot.flyingState is None or robot.flyingState == 1: # Taking off
robot.emergency()
robot.land()
def moveByControl(): # While Event != OK or != Interrupted, keep moving
while moveDone:
drone.update()
try:
(dX, dY, dZ, dPsi, Event) = drone.moveByEnd
print "Drone moved [mts, rad]:", dX, dY, dZ, dPsi
Events = ["OK. Relative displacement done", "UNKNOWN", "BUSY", "NOTAVAILABLE", "INTERRUPTED"]
print "Move by event", Event, Events[Event]
if Event == 0 or Event == 5: # Arrived or Interrupted
moveDone = False
except Exception, e: # Catch any error
print "Error getting data from drone, error:", e
pass
moveDone = True
drone.wait( 4.0 ) # Waits () secs after arrive to its position
if __name__ == "__main__":
moveByFuncition()