forked from bskari/pi-rc
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathinteractive_control.py
158 lines (130 loc) · 4.4 KB
/
interactive_control.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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
"""Interactive control for the remote radio control Raspberry Pi."""
import json
import pygame
import pygame.font
import socket
import sys
UP = LEFT = DOWN = RIGHT = False
def load_configuration(configuration_file):
"""Generates a dict of JSON command messages for each movement."""
configuration = json.loads(configuration_file.read())
base_command = {}
for key in (
'frequency',
'synchronization_burst_us',
'synchronization_spacing_us',
'total_synchronizations',
'signal_burst_us',
'signal_spacing_us',
):
base_command[key] = configuration[key]
dead_frequency = 49.890 if configuration['frequency'] < 38 else 26.995
base_command['dead_frequency'] = dead_frequency
direct_commands = {}
for key in (
'forward',
'forward_left',
'forward_right',
'reverse',
'reverse_left',
'reverse_right',
):
command_dict = base_command.copy()
command_dict['total_signals'] = configuration[key]
direct_commands[key] = json.dumps(command_dict)
# We also need to add an idle command; just broadcast at the dead frequency
command_dict = base_command.copy()
command_dict['frequency'] = dead_frequency
command_dict['total_signals'] = 20 # Doesn't matter
direct_commands['idle'] = json.dumps(command_dict)
return direct_commands
def get_keys():
"""Returns a tuple of (UP, DOWN, LEFT, RIGHT, changed) representing which
keys are UP or DOWN and whether or not the key states changed.
"""
global UP
global DOWN
global LEFT
global RIGHT
change = False
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
elif event.type == pygame.KEYDOWN:
change = True
if event.key == pygame.K_LEFT:
LEFT = True
elif event.key == pygame.K_RIGHT:
RIGHT = True
elif event.key == pygame.K_UP:
UP = True
elif event.key == pygame.K_DOWN:
DOWN = True
elif event.type == pygame.KEYUP:
change = True
if event.key == pygame.K_LEFT:
LEFT = False
elif event.key == pygame.K_RIGHT:
RIGHT = False
elif event.key == pygame.K_UP:
UP = False
elif event.key == pygame.K_DOWN:
DOWN = False
return (UP, DOWN, LEFT, RIGHT, change)
def main(host, port, configuration_file_name):
"""Runs the interactive control."""
pygame.init()
size = (500, 100)
screen = pygame.display.set_mode(size)
background = pygame.Surface(screen.get_size())
clock = pygame.time.Clock()
black = (0, 0, 0)
white = (255, 255, 255)
font = pygame.font.Font(None, 36)
pygame.display.set_caption('rc-pi interactive')
with open(configuration_file_name) as configuration_file:
configuration = load_configuration(configuration_file)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
while True:
up, down, left, right, change = get_keys()
if change:
# Something changed, so send a new command
command = 'idle'
if up or down:
if up:
command = 'forward'
else:
command = 'reverse'
if left:
command += '_left'
elif right:
command += '_right'
sock.sendto(configuration[command], (host, port))
# Show the command and JSON
background.fill(black)
text = font.render(command, 1, white)
text_position = text.get_rect(centerx=size[0] / 2)
background.blit(text, text_position)
screen.blit(background, (0, 0))
print(configuration[command])
pygame.display.flip()
# Limit to 20 frames per second
clock.tick(60)
pygame.quit()
if __name__ == '__main__':
if len(sys.argv) < 2:
sys.stderr.write(
'Usage: {program} <control JSON file>\n'.format(
program=sys.argv[0]
)
)
sys.exit(1)
if len(sys.argv) > 2:
HOST = sys.argv[2]
else:
HOST = '192.168.1.3'
if len(sys.argv) > 3:
PORT = int(sys.argv[3])
else:
PORT = 12345
main(HOST, PORT, sys.argv[1])