This repository has been archived by the owner on Jun 7, 2018. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 7
/
JoystickInterface.py
231 lines (203 loc) · 7.97 KB
/
JoystickInterface.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
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
import pygame
import math
import socket
# Joystick Constants
angle = 0
speed = 0
joysticks_connected = False
joystick = []
joynum = 0
joycommands = ["Drive", "Arm", "Science", "Joystick 4"]
# Start Pygame
pygame.init()
# Loop until the user clicks the close button.
done = False
# UDP Constants
TARGET_IP = "192.168.1.51"
#TARGET_IP = "173.250.159.234"
UDP_PORT = 8888
MAX_BUFFER_SIZE = 24
#UDP Defaults message
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
message = ""
# Define some colors
WHITE = (255, 255, 255)
PURPLE = (60, 45, 112)
GOLD = (213, 202, 148)
RED = (205, 40, 40)
BLACK = (0, 0, 0)
RECTCOLOR = PURPLE
TEXTCOLOR = GOLD
POTCOLOR = RED
STOPCOLOR = RED
PI = math.pi
# Define initial characteristics of the screen
size = (700, 500)
screen = pygame.display.set_mode(size)
screen.fill(WHITE)
BUTTON = pygame.Rect(100, 100, 225, 200)
DISPLAY = pygame.Rect(100, 350, 400, 100)
STOP = pygame.Rect(475, 100, 200, 200)
POT = pygame.Rect(340, 100, 125, 200)
pygame.display.set_caption("The Interactive Joystick Interface")
font = pygame.font.SysFont('Arial', 25)
# Used to manage how fast the screen updates
clock = pygame.time.Clock()
# Processes the joystick values. Doubles if button for extra thrust is down, rounds values below 0.5 to 0
def joy2value(value, half_control=False):
if half_control:
value /= 2.0
if abs(value - 0) < 0.05:
value = 0
return value
# Maps values to range from 0 to 255
def float256(value, low, high):
value = 256 * (value - low) / (high - low)
value = max([value, 0])
value = min([value, 255])
return int(value)
# Method to redraw the default, commonly used screen
def redraw_screen(is_adding_joysticks_now):
screen.fill(WHITE)
button = pygame.draw.rect(screen, RECTCOLOR, BUTTON, 0)
stop = pygame.draw.rect(screen, STOPCOLOR, STOP, 0)
pot = pygame.draw.rect(screen, POTCOLOR, POT, 0)
display = pygame.draw.rect(screen, RECTCOLOR, DISPLAY, 0)
pygame.draw.rect(screen, RECTCOLOR, DISPLAY, 0)
pygame.draw.rect(screen, STOPCOLOR, STOP, 0)
pygame.draw.rect(screen, POTCOLOR, POT, 0)
text = font.render('Connect Joysticks!', 1, TEXTCOLOR)
textpos = text.get_rect()
textpos.centerx = button.centerx
textpos.centery = button.centery
screen.blit(text, (textpos.centerx - textpos.width/2, textpos.centery - textpos.height/2))
# logo = pygame.image.load("robotics.jpeg")
# logorect = logo.get_rect()
# logorect.centerx = 575
# logorect.centery = 400
# screen.blit(logo, logorect)
text = font.render('Emergency Stop', 1, TEXTCOLOR)
textpos = text.get_rect()
textpos.centerx = stop.centerx
textpos.centery = stop.centery
screen.blit(text, (textpos.centerx - textpos.width/2, textpos.centery - textpos.height/2))
text = font.render('Pot Stop', 1, TEXTCOLOR)
textpos = text.get_rect()
textpos.centerx = pot.centerx
textpos.centery = pot.centery
screen.blit(text, (textpos.centerx - textpos.width/2, textpos.centery - textpos.height/2))
if not is_adding_joysticks_now:
text = font.render('Number of Connected Joysticks: ' + str(joynum), 1, TEXTCOLOR)
textpos = text.get_rect()
textpos.centerx = display.centerx
textpos.centery = display.centery
screen.blit(text, (textpos.centerx - textpos.width/2, textpos.centery - textpos.height/2))
pygame.display.flip()
# Method to create the commands for connecting different joysticks
def create_user_commands():
for i in range(0, len(joycommands)):
joycommands[i] = "Connect " + joycommands[i] + " Joystick"
# Handles the connecting of the joysticks
def connect_joysticks(rect):
try:
# create the command to tell user to connect a certain joystick
create_user_commands()
font = pygame.font.Font(None, 36)
text = font.render(joycommands[0], 1, (10, 10, 10))
textpos = text.get_rect()
textpos.centerx = rect.centerx
textpos.centery = rect.centery
# Find out how many joysticks are connected to the computer
pygame.joystick.init()
pygame.display.init()
joynum = pygame.joystick.get_count()
print(str(joynum) + " joysticks connected.")
# Initialize joysticks
for i in range(joynum):
pygame.joystick.Joystick(i).init()
# Loop through joysticks to connect each one to interface individually
while len(joystick) < joynum:
pygame.event.pump()
pygame.draw.rect(screen, RECTCOLOR, DISPLAY, 0)
text = font.render(joycommands[len(joystick)], 1, TEXTCOLOR) #array[len(joystick)]
textpos = text.get_rect()
textpos.centerx = DISPLAY.centerx
textpos.centery = DISPLAY.centery
screen.blit(text, (textpos.centerx - textpos.width/2, textpos.centery - textpos.height/2))
pygame.display.flip()
for i in range(joynum):
if pygame.joystick.Joystick(i).get_button(0):
redraw_screen(True)
joystick.append(pygame.joystick.Joystick(i))
pygame.time.wait(500)
for i in range(joynum):
print("Joystick " + str(i) + " is: " + joystick[i].get_name() + " with " + str(joystick[i].get_numaxes()) + " axes.")
return joynum
except KeyboardInterrupt:
pygame.quit()
# UDP initialization stuff
if __name__ == '__main__':
# UDP
print("UDP Port: ", UDP_PORT)
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
sock.bind(('0.0.0.0', UDP_PORT))
sock.settimeout(0.01)
# ------ Main Program Loop -------
while not done:
# Reset screen
redraw_screen(False)
# --- Main event loop
for event in pygame.event.get():
if event.type == pygame.QUIT:
done = True
elif event.type == pygame.MOUSEBUTTONDOWN:
click = BUTTON.collidepoint(pygame.mouse.get_pos())
if click == 1:
joynum = connect_joysticks(BUTTON)
if joynum != 0:
joysticks_connected = True
click = STOP.collidepoint(pygame.mouse.get_pos()) # ESTOP button clicked
if click == 1:
if STOPCOLOR == RED:
STOPCOLOR = BLACK
else:
STOPCOLOR = RED
click = POT.collidepoint(pygame.mouse.get_pos()) # Pot Stop button clicked
if click == 1:
if POTCOLOR == RED:
POTCOLOR = BLACK
else:
POTCOLOR = RED
# -- update screen
pygame.display.flip()
# -- limit to 60 frames per second
clock.tick(60)
# Get and convert joystick value, print
pygame.event.pump()
if joysticks_connected == True:
# Process values for the joysticks
for i in range(0, len(joystick)):
angle = (joy2value(joystick[i].get_axis(0), True))
speed = (joy2value(joystick[i].get_axis(1), (not joystick[i].get_button(0))))
angle = float256(angle, -1, 1)
speed = float256(speed, -1, 1)
print("ANGLE: " + str(ord(chr(angle))))
print("SPEED: " + str(ord(chr(speed))))
# send a 1 if potentiometer is in use (red), 0 if we need to shut off (black)
pot_flag = 1
if POTCOLOR == BLACK:
pot_flag = 0
# send a 1 if emergency stop has not been clicked (red), 0 if everything needs to be turned off (black)
stop_flag = 1
if STOPCOLOR == BLACK:
stop_flag = 0
# send message in form of characters for the potentiometer flag, emergency stop flag, angle, and speed
message = ''.join([chr(pot_flag), chr(stop_flag), chr(angle), chr(speed)])
print(message)
print(pot_flag)
#message = ''.join([str(pot_flag), str(stop_flag), str(angle), str(speed)])
# Send data over UDP, print recv
sock.sendto(message, (TARGET_IP, UDP_PORT))
pygame.time.wait(100)
pygame.time.wait(100)
pygame.quit()