/
server.py
433 lines (334 loc) · 9.79 KB
/
server.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
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
import socket
from _thread import *
from queue import Queue
import datetime
import tkinter
import RPi.GPIO as GPIO
import time
from mpu import compileData
####################################
# server setup (sets up the server variables)
####################################
HOST = '128.237.238.152'
PORT = 21127
BACKLOG = 2
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server.bind((HOST, PORT))
server.listen(BACKLOG)#number of ppl that the server waits for before it starts
print("looking for connection")
clientList = []
####################################
# Motor Setup
####################################
#these terms refer to certain pin outputs on the raspberry pi
#the pins are hooked up to certain sensors which relay information
#to the pi
AIL = 15 #BCM: 22
ELE = 16 #BCM: 23
THR = 18 #BCM: 24
RUD = 22 #BCM: 25
AUX = 24
LED = 26
GPIO.setmode(GPIO.BOARD)
GPIO.setup(AIL, GPIO.OUT)
GPIO.setup(ELE, GPIO.OUT)
GPIO.setup(THR, GPIO.OUT)
GPIO.setup(RUD, GPIO.OUT)
GPIO.setup(AUX, GPIO.OUT)
GPIO.setup(LED, GPIO.OUT)
#typical outputs are registed with digital outputs, which
#means they can either be 0 or 1, but now we will
#initialize them to input analog outputs, which ranges from
#values 0-255
ail = GPIO.PWM(AIL,100)
ail.start(1)
ele = GPIO.PWM(ELE,100)
ele.start(1)
thr = GPIO.PWM(THR,100)
thr.start(1)
rud = GPIO.PWM(RUD,100)
rud.start(1)
aux = GPIO.PWM(AUX,100)
aux.start(1)
time.sleep(0.1)
#These are magic numbers that I managed to find after alot of data testing
#these are the resting values where the motors won't run.
ailPWM = 7.1
elePWM = 7.1
thrPWM = 5.3
rudPWM = 7.1
auxPWMOn = 8
auxPWMOff = 6
maxPWM = 9.29 #this is 100%
minPWM = 5.3
restPWM = 7.1
thrIdle = 5.1
#the PWM's have a frequency of 50 hz, which is
#specific
ail.ChangeFrequency(50)
ele.ChangeFrequency(50)
thr.ChangeFrequency(50)
rud.ChangeFrequency(50)
aux.ChangeFrequency(50)
ail.ChangeDutyCycle(ailPWM)
ele.ChangeDutyCycle(elePWM)
thr.ChangeDutyCycle(thrPWM)
rud.ChangeDutyCycle(rudPWM)
aux.ChangeDutyCycle(auxPWMOn)
####################################
# Ultrasonic Sensors setup
####################################
#sets up the ultrasonic sensors
# TRIG = 11 #18
# ECHO = 12 #17
TRIG = 13
ECHO = 7
GPIO.setup(TRIG, GPIO.OUT)
GPIO.output(TRIG, 0)
GPIO.setup(ECHO, GPIO.IN)
#this is the initial starting position of the copter. I implement this
#because the gyroscope doesn't read the rest position as perfectly 0,0
#so this basically can account for any slight error in the gyro readings
startingGyroValues = compileData()
startingX = startingGyroValues[0]
startingY = startingGyroValues[1]
startingYaw = startingGyroValues[2]
print('start', startingX, startingY)
####################################
# Initializing global data structures
####################################
keyInputs = {"w": False, "s": False, "d": False, "a": False,
"u":False, "e":False, "r": False, "l": False, "g": True}
msg = {"distance": 0, "ail": 0, "ele": 0, "thr": 0, "rud": 0, "gyroX": 0,
"gyroY": 0, "yaw":0}
isArming = False
####################################
# Handles commands
####################################
#constantly checks for new messages to be sent/relayed
#takes the message and adds it to the queue, which then
#gets sent to everyone connected to the server
def handleClient(client, serverChannel):
global keyInputs
while True:
msg = client.recv(1000).decode('UTF-8')
computeKeys(msg, keyInputs)
def computeMessage(serverChannel):
global keyInputs, thr, thrPWM, ail, ailPWM, restPWM, isArming
localMaxPWM = 7.8
localMinPWM = 6.4
rudMin = 6.8
rudMax = 7.4
while True:
if keyInputs['u']:
if thrPWM <= 6.5: thrPWM += 0.03
thr.ChangeDutyCycle(thrPWM)
print(thrPWM)
elif keyInputs['e']: #e stands for down
if keyInputs['g']: #g stands for grounded
if thrPWM >= 5.3: thrPWM -= 0.03
thr.ChangeDutyCycle(thrPWM)
print(thrPWM)
if keyInputs['d']:
turnRight(localMaxPWM, localMinPWM)
elif keyInputs['a']:
turnLeft(localMaxPWM, localMinPWM)
elif not (keyInputs['d'] and keyInputs['a']):
selfLevel(restPWM)
if keyInputs['l']:
yawRight(rudMin)
elif keyInputs['r']:
yawLeft(rudMax)
elif not (keyInputs['l'] and keyInputs['r']):
if not isArming:
yawStabilize(restPWM)
time.sleep(0.01)
#this will tilt the quad to the right.
def turnRight(max,min):
global ail, ailPWM
error = max - ailPWM
ailPWM += error * 0.04
# print(ailPWM)
ail.ChangeDutyCycle(ailPWM)
#this will tilt the quad to the left
def turnLeft(max, min):
global ail, ailPWM
error = min - ailPWM
ailPWM += error * 0.04
# print(ailPWM)
ail.ChangeDutyCycle(ailPWM)
#this will smoothly reset the x axis tilt of the quad.
def selfLevel(rest):
global ail, ailPWM
error = rest - ailPWM
ailPWM += error * 0.04
ail.ChangeDutyCycle(ailPWM)
#this will smoothly turn the quad to the left
def yawLeft(min):
global rud, rudPWM
error = min - rudPWM
rudPWM += error * 0.05
rud.ChangeDutyCycle(rudPWM)
#this will smoothly turn the quad to the right
def yawRight(max):
global rud, rudPWM
error = max - rudPWM
rudPWM += error * 0.05
rud.ChangeDutyCycle(rudPWM)
#this will reset the yaw back to "rest", or the pwm value of 7.1
#This occurs when there is no user input, so the quad will naturally
#restabilize
def yawStabilize(rest):
global rud, rudPWM
error = rest - rudPWM
rudPWM += error * 0.05
rud.ChangeDutyCycle(rudPWM)
#this will evaluate the keyInputs and set specific values or perform various
#tasks bsed on what key is inputted.
def computeKeys(msg, keyInputs):
for val in msg:
if val == '1':
keyInputs['g'] = True
elif val == '0':
keyInputs['g'] = False
elif val == '8':
armCopter()
elif val == '9':
disArmCopter()
else:
keyInputs[val] = not keyInputs[val]
#the quadcopter has a "safe" setting, like the safety on a gun
#when initially turned on, the copter will be on safe mode
#doing this process will arm the copter
def armCopter():
global rud, rudPWM, thr, thrPWM, restPWM, thrIdle, isArming
isArming = True
rudPWM = restPWM
rud.ChangeDutyCycle(rudPWM)
thrPWM = thrIdle
thr.ChangeDutyCycle(thrPWM)
time.sleep(0.1)
while rudPWM > restPWM - 1:
rudPWM -= 0.1
rud.ChangeDutyCycle(rudPWM)
time.sleep(0.05)
time.sleep(2)
while rudPWM < 7.1:
rudPWM += 0.1
rud.ChangeDutyCycle(rudPWM)
time.sleep(0.05)
isArming = False
#this disarms the copter
def disArmCopter():
global rud, rudPWM, thr, thrPWM, restPWM, thrIdle, isArming
isArming = True
rudPWM = restPWM
rud.ChangeDutyCycle(rudPWM)
thrPWM = thrIdle
thr.ChangeDutyCycle(thrPWM)
time.sleep(0.2)
while rudPWM < restPWM + 1:
rudPWM += 0.1
rud.ChangeDutyCycle(rudPWM)
time.sleep(0.05)
time.sleep(2)
while rudPWM > 7.1:
rudPWM -= 0.1
rud.ChangeDutyCycle(rudPWM)
time.sleep(0.05)
isArming = False
#this is constantly run to check the queue for new messages, if there is,
#it'll put() it in the queue
####################################
# Threading scripts
####################################
#sends back to the user whatever data is in the serverChannel queue
def serverThread(clientList, serverChannel):
while True:
msg = serverChannel.get(True, None)
for client in clientList:
client.send(bytes(msg, "UTF-8"))
serverChannel = Queue(100)
#constantly checks for new server messages
start_new_thread(serverThread, (clientList, serverChannel))
#measures the distance of the signals that the ultrasonic sensors pick up
#the 1700 is the speed in which radio waves travel; essentially this is
#converting pulse widths of the ultra sonic waves and computing
#how far the waves had to travel
def measureDistance():
constant = 17000
GPIO.output(TRIG, 1)
time.sleep(0.00001)
GPIO.output(TRIG, 0)
while GPIO.input(ECHO) == 0:
pass
start = time.time()
while GPIO.input(ECHO) == 1:
pass
stop = time.time()
return ((stop - start) * constant)
#this takes three different outputs and averages them; this makes it more
#accurate
def measureAverage():
dist1 = measureDistance()
time.sleep(0.1)
dist2 = measureDistance()
time.sleep(0.1)
dist3 = measureDistance()
totalDistance = (dist1 + dist2 + dist3) / 3
return totalDistance
#this method will constantly put the global dictionary "msg" in the server
#channel. It doesn't care what state the dictionary is at, it will
#simply send whatever is in it.
def sendData(serverChannel):
global msg
while True:
serverChannel.put(repr(msg))
#this will constantly update the "distance" key in the dictionary.
def addDistance(serverChannel):
global msg
while True:
msg["distance"] = measureAverage()
#this will constantly update the gyroscope and yaw data
def addGyro(serverChannel):
global msg, startingX, startingY
while True:
dataVals = compileData()
# print(dataVals)
x = float(dataVals[0])
y = float(dataVals[1])
yaw = float(dataVals[2])
msg["gyroX"] = x # - startingX
msg["gyroY"] = y #- startingY
msg["yaw"] = yaw - startingYaw
# time.sleep(0.1)
#this will constantly update the motor values to the dictionary
def addMotorVals(serverChannel):
global msg, ailPWM, elePWM, thrPWM, rudPWM
while True:
msg["ail"] = ailPWM
msg["ele"] = elePWM
msg["thr"] = thrPWM
msg["rud"] = rudPWM
#resets the throttle pwm to zero once the client list is zero
#this is a safety mechanism so that once the client disconnects,
#the throttle will shut down.
def resetMotors():
global clientList
while True:
if len(clientList) < 1:
print('nobodys here')
thrPWM = 0
#each of these threads will update the dictionary with its respective
#attributes
start_new_thread(addDistance, (serverChannel,))
start_new_thread(addGyro, (serverChannel,))
start_new_thread(addMotorVals, (serverChannel,))
start_new_thread(sendData, (serverChannel,))
start_new_thread(computeMessage, (serverChannel,))
#this handles the client.
while True:
client, address = server.accept()
clientList.append(client)
print("connection recieved")
start_new_thread(handleClient, (client,serverChannel))