-
Notifications
You must be signed in to change notification settings - Fork 0
/
client.py
127 lines (110 loc) · 3.89 KB
/
client.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
import time
import socket
import pyjoy
import serial
import pygame
from math import *
MAX_MOTOR_VALUE = 180 # Sets the highest motor value that is able to be sent to the Arduino
MIN_MOTOR_VALUE = 25 # Sets the lowest motor value that is able to be sent to the Arduino
def getArduinoData(joyAxis, joyButtons):
"""Gets passed all axis and button info from the joystick and returns a list
containing four motor values and a button map to send to the Arduino"""
xAxis = joyAxis[0]
yAxis = joyAxis[1]
rotAxis = joyAxis[2]
# Joystick x and y are converted into polar coordinates and rotated 45 degrees
mag = sqrt(xAxis**2 + yAxis**2)
theta = atan(yAxis/xAxis) + pi/4
motorA = mag*cos(theta)/2
motorB = -mag*sin(theta)/2
motorC = motorB
motorD = motorA
# Create a binary button map from joyButtons
buttonMap = 0
for index, value in enumerate(joyButtons):
if value:
buttonMap += 2**index
# Build and output command list
return [motorA, motorB, motorC, motorD, buttonMap]
joy = []
recvstr = " "
# A list of axis values from the joystick
joyAxis = [0,0,0,0]
# Each boolean represents the value from the corresponding joystick button
joyButtons = [False, False, False, False, False, False]
UDP_IP = "192.168.1.177"
UDP_PORT = 8888
print "UDP target IP:", UDP_IP
print "UDP target port:", UDP_PORT
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
pygame.joystick.init()
pygame.display.init()
if not pygame.joystick.get_count():
print"\nplug in a joystick\n"
quit()
print "\n%d joystick(s) detected" % pygame.joystick.get_count()
for i in range(pygame.joystick.get_count()):
myjoy = pygame.joystick.Joystick(i)
myjoy.init()
joy.append(myjoy)
print "Depress lower rack of buttons to quit\n"
command = [0,0,0,0,0,0]
while True:
e = pygame.event.wait()
if (e.type == pygame.JOYAXISMOTION or e.type == pygame.JOYBUTTONDOWN or e.type == pygame.JOYBUTTONUP):
# Read and change information in joyAxis or joyButtons
if e.type == pygame.JOYBUTTONDOWN:
if (e.dict['button'] in range(6,12)):
print "Bye!\n"
quit()
else:
joyButtons[e.dict['button']] = True
if e.type == pygame.JOYBUTTONUP:
if (e.dict['button'] in range(0,6)):
joyButtons[e.dict['button']] = False
if e.type == pygame.JOYAXISMOTION:
axis = e.dict['axis']
value = e.dict['value']
joyAxis[axis] = value
# Construct Arduino command from all data
xAxis = joyAxis[0]
if xAxis == 0: # Prevents a divide by zero error
xAxis = 10**-24
yAxis = -joyAxis[1]
rotAxis = joyAxis[2]
# Joystick x and y are converted into polar coordinates and rotated 45 degrees
mag = sqrt(xAxis**2 + yAxis**2)
if mag > 1:
mag = 1
theta = atan(yAxis/xAxis) + pi/4
#print theta/pi
motorA = abs(mag*sin(theta)/2)
motorA *= 2 * MAX_MOTOR_VALUE # Values usable by the Arduino Servo library
motorB = abs(mag*cos(theta)/2)
motorB *= 2 * MAX_MOTOR_VALUE
motorC = motorB
motorD = motorA
# Motor direction sent as a binary number with each bit representing the motors A-D
# x x x x
# A B C D
motorDirection = 0
if xAxis > yAxis:
motorDirection &= 0b1001
else:
motorDirection |= 0b0110
if -xAxis > yAxis:
motorDirection &= 0b0110
else:
motorDirection |= 0b1001
# Create a binary button map from joyButtons
buttonMap = 0
for index, value in enumerate(joyButtons):
if value:
buttonMap |= 2**index
# Build and output command list
command = [int(motorA), int(motorB), int(motorC), int(motorD), motorDirection, buttonMap]
MESSAGE = [chr(command[0]), chr(command[1]), chr(command[2]), chr(command[3]), chr(command[4]), chr(command[5])]
sock.sendto("".join(MESSAGE), (UDP_IP, UDP_PORT))
recvstr = sock.recv(4096)
print "MESSAGE = {} {} {} {} {} {}".format(ord(MESSAGE[0]), ord(MESSAGE[1]), ord(MESSAGE[2]), ord(MESSAGE[3]), ord(MESSAGE[4]), ord(MESSAGE[5]))
print "Received: {} {} {} {} {} {}".format(ord(recvstr[0]), ord(recvstr[1]), ord(recvstr[2]), ord(recvstr[3]), ord(recvstr[4]), ord(recvstr[5]))