-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathBasic_movment.py
125 lines (97 loc) · 3.66 KB
/
Basic_movment.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
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
from djitellopy import tello
from time import sleep
import keyboard
drone = tello.Tello()
drone.connect()
print(drone.get_battery())
STATE = "NONE"
droneheight = 0
while True:
if not drone.is_flying:
cmd = input('Enter take off:')
if cmd == "take off":
drone.takeoff()
print('Drón felszállt')
sleep(1)
drone.send_rc_control(0, 0, 0, 0)
STATE = "mode_choose"
if drone.is_flying:
if keyboard.is_pressed('esc'):
print("Interupted")
STATE = "INTERUPT"
if STATE == "mode_choose":
print("Time to choose mode")
cmd_mode = keyboard.read_key()
wait = True
print("Kézi:0 Automata:1")
if cmd_mode == "1":
print("Auto mode")
STATE = "AUTO"
if cmd_mode == "0":
print("Manual mode")
STATE = "MANUAL"
elif STATE == "INTERUPT":
drone.send_rc_control(0, 0, 0, 0)
sleep(1)
drone.land()
elif STATE == "AUTO":
cmd_command_auto = keyboard.read_key()
if cmd_command_auto == 'd':
droneheight = drone.get_height()
print(droneheight)
if cmd_command_auto == 'q':
if droneheight < 20:
drone.send_rc_control(0,0,10,0)
while droneheight < 20:
sleep(1)
droneheight = drone.get_height()
print(droneheight)
if droneheight > 30:
drone.send_rc_control(0,0,-10,0)
while droneheight > 30:
sleep(1)
droneheight = drone.get_height()
print(droneheight)
if 60 < droneheight < 70:
drone.send_rc_control(0,0,0,0)
if cmd_command_auto == 'a':
drone.set_speed(10)
drone.move_forward(100)
sleep(2)
drone.move_left(100)
sleep(2)
drone.move_back(100)
sleep(2)
drone.move_right(100)
if cmd_command_auto == 'x':
sleep(2)
drone.land()
STATE == "mode_choose"
elif STATE == "MANUAL":
cmd_command_manual = keyboard.read_key()
if cmd_command_manual == 'c':
drone.send_rc_control(0, 0, 0, 0)
if cmd_command_manual == 'w':
drone.send_rc_control(0, 50, 0, 0)
if cmd_command_manual == 's':
drone.send_rc_control(0, -50, 0, 0)
if cmd_command_manual == 'a':
drone.send_rc_control(-50, 0, 0, 0)
if cmd_command_manual == 'd':
drone.send_rc_control(50, 0, 0, 0)
if cmd_command_manual == 'g':
drone.send_rc_control(0, 0, 50, 0)
if cmd_command_manual == 't':
drone.send_rc_control(0, 0, -50, 0)
if cmd_command_manual == 'e':
drone.send_rc_control(0, 0, 0, 50)
if cmd_command_manual == 'q':
drone.send_rc_control(0, 0, 0, -50)
if cmd_command_manual == 'x':
drone.send_rc_control(0, 0, 0, 0)
print("Leszállás folyamatban....")
drone.land()
sleep(5)
print("Leszállt")
sleep(1)
STATE = "NONE"