forked from Menashe-HFR/TelloDroneFaceTraking
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Mapping.py
95 lines (76 loc) · 2.08 KB
/
Mapping.py
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
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
from djitellopy import tello
import KeyPressModule as kp
import numpy as np
from time import sleep
import cv2
import math
#### PARAMETERS ########
fSpeed = 117/10 # Forward speed in cm/s (15cm/s)
aSpeed = 360/10 # Angular speed degrees/s (50d/s)
interval = 0.25
dInterval = fSpeed*interval
aInterval = aSpeed*interval
#####################################
x,y =500,500
a = 0
yaw = 0
kp.init()
mendrone = tello.Tello()
mendrone.connect()
print(mendrone.get_battery())
points = [(0, 0), (0,0)]
def getKeyboardInput():
lr, fb, up, yv = 0, 0, 0, 0
speed = 30
aSpeed= 50
global yaw, x ,y, a
d = 0
if kp.getKey("LEFT"):
lr = -speed
d = dInterval
a = -180
elif kp.getKey("RIGHT"):
lr = speed
d = -dInterval
a = 180
if kp.getKey("UP"):
fb = speed
d = dInterval
a = 270
elif kp.getKey("DOWN"):
fb = -speed
d = -dInterval
a = -90
if kp.getKey("w"):
up = speed
elif kp.getKey("s"):
up = -speed
if kp.getKey("a"):
yv = -aSpeed
yaw -= aInterval
elif kp.getKey("d"):
yv = aSpeed
yaw += aInterval
if kp.getKey("q"): mendrone.land(); sleep(3)
if kp.getKey("e"): mendrone.takeoff()
sleep(interval)
a += yaw
x += int(d*math.cos(math.radians(a)))
y += int(d * math.sin(math.radians(a)))
return [lr, fb, up, yv, x, y]
def drawPoints(img, points):
for point in points:
cv2.circle(img,point, 5,(0,0,255), cv2.FILLED)
cv2.circle(img, points[-1], 8, (0, 255, 0), cv2.FILLED)
cv2.putText(img, f'({(points[-1][0]-500)/100}, {(points[-1][1]-500)/100})m',
(points[-1][0]+10, points[-1][1]+30),cv2.FONT_HERSHEY_PLAIN, 1,
(255, 0, 255), 1)
while True:
vals = getKeyboardInput()
mendrone.send_rc_control(vals[0],vals[1],vals[2],vals[3])
img = np.zeros((1000, 1000, 3), np.uint8)
if(points[-1][0] != vals[4] or points[-1][1] != vals[5]):
points.append((vals[4], vals[5]))
drawPoints(img, points)
cv2.imshow("Output", img)
cv2.waitKey(1)