forked from CRLS-MATE/underwater-robotics
-
Notifications
You must be signed in to change notification settings - Fork 0
/
joystick_controller.py
executable file
·229 lines (182 loc) · 6.97 KB
/
joystick_controller.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
#CRLS UNDERWATER ROBOTICS TEAM 2015
import os
import sys
import time
import select #not important
import pygame
import serial
import requests
import urllib.parse
# ip of rpi
rovIP = "192.168.4.1"
# this is the number of dots that will be used to display the output of an axis
dots = 8
# this is for pygameeeeee
screen = None
# write to stderr. pygame clutters up stdout because of a bug
def wrerr(msg):
sys.stderr.write(msg)
# print to stderr to avoid SDL messages
def prerr(msg):
wrerr(msg + "\r\n")
# write out all the information we know about a joystick
def printJoystickInfo(index, js):
prerr("\n")
prerr("Joystick %d: %s" % (index, js.get_name()))
prerr("\tAxes:\t%d" % js.get_numaxes())
prerr("\tBalls:\t%d" % js.get_numballs())
prerr("\tButtons:\t%d" % js.get_numbuttons())
prerr("\tHats:\t%d" % js.get_numhats())
# read joystick info and print it out on the screen
def readJoystick(js):
pygame.event.pump() # get joystick events from pygame
output = []
# cycle through all joystick axes
for i, axis in enumerate([js.get_axis(i) for i in range(js.get_numaxes())]):
norm = (axis + 1.0) * (180/ 2.0) # normalize to the range 0-180
# if((not js.get_button(7)) and not (js.get_button(6) and i != 2)):
# norm = 88
output.append(round(norm))
# print the axis number for debugging
wrerr(str(i) + ": ")
# print an exclamation point if the value is above this dot, otherwise a period
for d in range(dots):
if d < (norm * dots)/179:
wrerr("!")
else:
wrerr(".")
wrerr("\t")
output.append([]) # last value in array will be list of buttons
for j, button in enumerate([js.get_button(k) for k in range(js.get_numbuttons())]):
output[4].append(button)
wrerr("Button " + str(j) + ": " + str(button))
#prerr("")
#pygame.event.clear()
return output
def constrain(val, low=0, high=180):
return max(min(val, high), low)
last_query = {
3: 90, # Motor A/Arm Motor
4: 92, # right
5: 92, # left
6: 92, # up-right
7: 92, # up-left
8: 45, # Arm Servo
9: 45, # Linear Actuator
10: 90, # Camera Servo
11: 90 # Motor B/Mini-bot
}
# normal script entry
if __name__ == "__main__":
# set up pygame, including a screen (even though we don't need it)
global Screen
pygame.init()
#screen = pygame.display.set_mode((640, 480))
#ser = serial.Serial(2)
# print error message if no joysticks were connected
if pygame.joystick.get_count() < 1:
prerr("No joysticks were found :(")
exit(1)
# print info about each joystick that was found
for i, js in enumerate([pygame.joystick.Joystick(i) for i in range(pygame.joystick.get_count())]):
js.init()
printJoystickInfo(i, js)
prerr("")
# print an error message if no joystick was specified
if len(sys.argv) < 3:
pygame.quit()
prerr("You need to choose a specific joystick with the command 'python %s <joystick number> 1>out.log'" % sys.argv[0])
prerr("")
exit(1)
joystickNum = int(sys.argv[1])
joystickNumAux = int(sys.argv[2])
# initialize the joystick that was specified on the command line
js = pygame.joystick.Joystick(joystickNum)
if not js.get_init():
js.init()
# initialize the joystick that was specified on the command line
js_aux = pygame.joystick.Joystick(joystickNumAux)
if not js_aux.get_init():
js_aux.init()
n = 0
# loop and read input
while True:
values = readJoystick(js) # get array of axis values
values_aux = readJoystick(js_aux)
print('\n\n\n', values, '\n\n\n')
print('\n\n\n', values_aux, '\n\n\n')
# map joystick axis values to servos
my_query = {
3: 90, # Motor A/Arm Motor
4: str(180 - round((values[1] - 90 + values[0] - 90)/2 + 90)), # right
5: str(180 - round((values[1] - values[0])/2 + 90)), # left
6: str(180-values[2]), # up-right
7: str(180-values[2]), # up-left
8: last_query[8], # Arm Servo
9: last_query[9], # Linear Actuator
10: last_query[10], # Camera Servo
11: values_aux[3] # Motor B/Mini-bot
}
# add something at index 0 to make buttons match visible buttons
values_aux[4].insert(0, 0)
values[4].insert(0, 0)
# Check button 7 for thruster kill
if(values[4][7] != 1):
my_query[4] = 92
my_query[5] = 92
my_query[6] = 92
my_query[7] = 92
# Check aux buttons 7/8 for Motor A/Arm Motor
if(values_aux[4][7] == 1): # button 7 pressed
my_query[3] = 179
elif(values_aux[4][8] == 1):
my_query[3] = 0
# Check aux buttons 9/10 for Arm Servo
if(values_aux[4][9] == 1):
my_query[8] += 3
if(values_aux[4][10] == 1):
my_query[8] -= 3
# Check aux buttons 5/6 for Linear Actuator
if(values_aux[4][5] == 1):
my_query[9] += 3
if(values_aux[4][6] == 1):
my_query[9] -= 3
# Check aux buttons 2/4 for Camera Servo
if(values_aux[4][2] == 1):
my_query[10] += 1
if(values_aux[4][4] == 1):
my_query[10] -= 1
# Constraints
my_query[8] = constrain(my_query[8], 45, 80)
my_query[9] = constrain(my_query[9], 45, 80)
# create a copy to be referenced in the next run
last_query = my_query.copy()
my_query = urllib.parse.urlencode(my_query)
prerr("about to send: " + my_query)
try:
r = requests.get("http://" + rovIP + ":5000" + "/?" + my_query)
prerr("sent")
# prerr(r.text)
except Exception as e:
prerr(str(e))
time.sleep(0.1)
##### SCRAPS #####
#letters = ["w", "s", "h", "y"] # labels we send to serial port that the arduino expect
# The arduinocamand will call for the label (letters) and the value of the speed(0 - 127)
# the speed is coded as a char
# example of arduinocommand: ::wJsOhUyP
# y -> yaw (spinning in place)
# h -> height?
# s -> speed (forward)?
# w -> ?
# building the arduino command
#myArduinoCommand = ""#"::" # resetting the state of the ardunio
#for i, v in enumerate(values): # going through the values index and its axis value
# myArduinoCommand = myArduinoCommand + letters[i] # setting up the arduino state
# myArduinoCommand = myArduinoCommand + str(int(v))#chr(int(v)) # setting up the arduino spead
# # chr(int(v)) is going to convert v as integer with the equivalent value of it as a character
# arduino command is now complete
#ser.write(myArduinoCommand) # send the command
## going through the values index and its axis value
#for i, v in enumerate(values):
# my_query[i] = v