-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathsend-joystick.py
More file actions
50 lines (39 loc) · 1.62 KB
/
send-joystick.py
File metadata and controls
50 lines (39 loc) · 1.62 KB
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
#!/usr/bin/env python
"""joystick-sender.py: Reads a joystick device using pygame and sends the information via UDP."""
import socket, struct, time
import pygame
from modules.utils import *
# Main configuration
#UDP_IP = "127.0.0.1" # Localhost (for testing)
rapi = raw_input("Ingrese ip de raspberry o cliente: ")
UDP_IP = rapi # Vehicle IP address
UDP_PORT = 51001 # This port match the ones using on other scripts
update_rate = 0.01 # 100 hz loop cycle
# Create UDP socket
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
try:
pygame.init()
pygame.joystick.init()
joystick = pygame.joystick.Joystick(0)
joystick.init()
except Exception,error:
print "No hay joystick conectedo en la computadora, "+str(error)
while True:
current = time.time()
elapsed = 0
# Joystick reading
pygame.event.pump()
roll = mapping(joystick.get_axis(0),-1.0,1.0,1000,2000)
pitch = mapping(joystick.get_axis(1),-1.0,1.0,1000,2000)
yaw = mapping(joystick.get_axis(2),-1.0,1.0,1000,2000)
throttle = mapping(joystick.get_axis(3),-1.0,1.0,1000,2000)
mode = joystick.get_button(1)
# Be sure to always send the data as floats
# The extra zeros on the message are there in order for the other scripts to do not complain about missing information
message = [roll, pitch, yaw, throttle, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.0,0.0,0.0]
buf = struct.pack('>' + 'd' * len(message), *message)
sock.sendto(buf, (UDP_IP, UDP_PORT))
print message
# Make this loop work at update_rate
while elapsed < update_rate:
elapsed = time.time() - current