rad.vg
this repo has no description
1import cv2
2import pygame
3import time
4
5SPEED = 50
6FPS = 120
7
8class FrontEnd(object):
9 def __init__(self):
10 pygame.init()
11 pygame.display.set_caption("Tello video stream")
12 self.screen = pygame.display.set_mode([960, 720])
13 self.joystick = pygame.joystick.Joystick(0)
14
15 self.for_back_velocity = 0
16 self.left_right_velocity = 0
17 self.up_down_velocity = 0
18 self.yaw_velocity = 0
19 self.speed = 10
20
21 self.send_rc_control = False
22
23 pygame.time.set_timer(pygame.USEREVENT + 1, 1000 // FPS)
24
25 def run(self):
26 should_stop = False
27 while not should_stop:
28 for event in pygame.event.get():
29 if event.type == pygame.USEREVENT + 1:
30 self.update()
31 elif event.type == pygame.QUIT:
32 should_stop = True
33 elif event.type == pygame.KEYDOWN:
34 if event.key == pygame.K_ESCAPE:
35 should_stop = True
36 else:
37 self.keydown(event.key)
38 elif event.type == pygame.KEYUP:
39 self.keyup(event.key)
40 elif event.type == pygame.JOYAXISMOTION:
41 self.stickmotion(event)
42 elif event.type == pygame.JOYBUTTONDOWN:
43 self.buttonup(event.button)
44
45 self.screen.fill([0, 0, 0])
46
47 time.sleep(1 / FPS)
48
49 def keydown(self, key):
50 if key == pygame.K_UP: # set forward velocity
51 self.for_back_velocity = SPEED
52 elif key == pygame.K_DOWN: # set backward velocity
53 self.for_back_velocity = -SPEED
54 elif key == pygame.K_LEFT: # set left velocity
55 self.left_right_velocity = -SPEED
56 elif key == pygame.K_RIGHT: # set right velocity
57 self.left_right_velocity = SPEED
58 elif key == pygame.K_w: # set up velocity
59 self.up_down_velocity = SPEED
60 elif key == pygame.K_s: # set down velocity
61 self.up_down_velocity = -SPEED
62 elif key == pygame.K_a: # set yaw counter clockwise velocity
63 self.yaw_velocity = -SPEED
64 elif key == pygame.K_d: # set yaw clockwise velocity
65 self.yaw_velocity = SPEED
66
67 def keyup(self, key):
68 if key == pygame.K_UP or key == pygame.K_DOWN: # set zero forward/backward velocity
69 self.for_back_velocity = 0
70 elif key == pygame.K_LEFT or key == pygame.K_RIGHT: # set zero left/right velocity
71 self.left_right_velocity = 0
72 elif key == pygame.K_w or key == pygame.K_s: # set zero up/down velocity
73 self.up_down_velocity = 0
74 elif key == pygame.K_a or key == pygame.K_d: # set zero yaw velocity
75 self.yaw_velocity = 0
76 elif key == pygame.K_t: # takeoff
77 print("Taking off")
78 self.send_rc_control = True
79 elif key == pygame.K_l: # land
80 print("Landing")
81 self.send_rc_control = False
82
83 def stickmotion(self, event):
84 print(f"Motion: {event.axis}")
85 if event.axis == 0: # left stick left/right
86 self.yaw_velocity = round(self.joystick.get_axis(event.axis), 3) * SPEED
87 elif event.axis == 3: # right stick left/right
88 self.left_right_velocity = round(self.joystick.get_axis(event.axis), 3) * SPEED
89 elif event.axis == 4: # right stick up/down
90 self.for_back_velocity = -round(self.joystick.get_axis(event.axis), 3) * SPEED
91 elif event.axis == 2: # left trigger
92 self.up_down_velocity = -min(round(self.joystick.get_axis(event.axis), 3) + 1, 1) * SPEED
93 elif event.axis == 5: # right trigger
94 self.up_down_velocity = min(round(self.joystick.get_axis(event.axis), 3) + 1, 1) * SPEED
95
96 def buttonup(self, button):
97 if button == 2:
98 print("Landing")
99 self.send_rc_control = False
100 elif button == 0:
101 print("Taking off")
102 self.send_rc_control = True
103
104 def update(self):
105 if self.send_rc_control:
106 print(f"Left/Right: {self.left_right_velocity}, Front/Back: {self.for_back_velocity}, Up/Down: {self.up_down_velocity}, Yaw: {self.yaw_velocity}")
107
108if __name__ == '__main__':
109 FrontEnd().run()