234 lines
8.0 KiB
Python
Executable File
234 lines
8.0 KiB
Python
Executable File
# -*- coding: utf-8 -*-
|
|
|
|
########################################
|
|
# HXbot controller module #
|
|
# EoF 2016 EoF@itphx.ru #
|
|
########################################
|
|
|
|
import HX
|
|
import threading
|
|
import array
|
|
import struct
|
|
import select
|
|
import time
|
|
from fcntl import ioctl
|
|
|
|
|
|
class HXjoystickClass():
|
|
|
|
def __init__(self):
|
|
super(HXjoystickClass, self).__init__()
|
|
|
|
self.__e = None
|
|
self.__t = None
|
|
self.ready = False
|
|
|
|
self.device = open(HX.JOYSTICK_PATH, 'rb')
|
|
|
|
self.name = ""
|
|
self.axis_states = {}
|
|
self.button_states = {}
|
|
self.axis_map = []
|
|
self.button_map = []
|
|
self.axis_names = HX.JOYSTICK_AXIS_NAMES
|
|
self.button_names = HX.JOYSTICK_BUTTON_NAMES
|
|
|
|
self.online = False
|
|
self.is_running = False
|
|
self.last_command_time = 0.0
|
|
|
|
# Get the device name.
|
|
buf = array.array('B', [0] * 64)
|
|
# JSIOCGNAME(len)
|
|
ioctl(self.device, 0x80006a13 + (0x10000 * len(buf)), buf)
|
|
self.name = buf.tostring()
|
|
#print('Device name: %s' % js_name)
|
|
|
|
# Get number of axes and buttons.
|
|
buf = array.array('B', [0])
|
|
ioctl(self.device, 0x80016a11, buf) # JSIOCGAXES
|
|
num_axes = buf[0]
|
|
|
|
buf = array.array('B', [0])
|
|
ioctl(self.device, 0x80016a12, buf) # JSIOCGBUTTONS
|
|
num_buttons = buf[0]
|
|
|
|
# Get the axis map.
|
|
buf = array.array('B', [0] * 0x40)
|
|
ioctl(self.device, 0x80406a32, buf) # JSIOCGAXMAP
|
|
|
|
for axis in buf[:num_axes]:
|
|
axis_name = self.axis_names.get(axis, 'unknown(0x%02x)' % axis)
|
|
self.axis_map.append(axis_name)
|
|
self.axis_states[axis_name] = 0.0
|
|
|
|
# Get the button map.
|
|
buf = array.array('H', [0] * 200)
|
|
ioctl(self.device, 0x80406a34, buf) # JSIOCGBTNMAP
|
|
|
|
for btn in buf[:num_buttons]:
|
|
btn_name = self.button_names.get(btn, 'unknown(0x%03x)' % btn)
|
|
self.button_map.append(btn_name)
|
|
self.button_states[btn_name] = 0
|
|
|
|
#print('%d axes found: %s' % (num_axes, ', '.join(self.axis_map)))
|
|
#print('%d btns found: %s' % (num_buttons, ', '.join(self.button_map)))
|
|
|
|
def start(self):
|
|
self.__e = threading.Event()
|
|
self.__t = threading.Thread(target=self.__process, args=(self.__e, ))
|
|
|
|
self.__t.start()
|
|
self.is_running = True
|
|
|
|
def stop(self):
|
|
self.__e.set()
|
|
self.__t.join()
|
|
self.is_running = False
|
|
|
|
self.__e = None
|
|
self.__t = None
|
|
|
|
def __process(self, stopRequest):
|
|
while not stopRequest.is_set():
|
|
r, w, e = select.select([self.device], [], [], 0.8)
|
|
if self.device in r:
|
|
try:
|
|
evbuf = self.device.read(8)
|
|
self.online = True
|
|
except OSError:
|
|
self.online = False
|
|
self.ready = False
|
|
continue
|
|
|
|
if evbuf:
|
|
time_, value, type, number = struct.unpack('IhBB', evbuf)
|
|
|
|
# Save last command time
|
|
self.last_command_time = time.clock()
|
|
|
|
#if type & 0x80:
|
|
#print("(initial)")
|
|
|
|
if type & 0x01:
|
|
button = self.button_map[number]
|
|
if button:
|
|
self.button_states[button] = value
|
|
#if value:
|
|
#print("%s pressed" % (button))
|
|
#else:
|
|
#print("%s released" % (button))
|
|
|
|
if type & 0x02:
|
|
axis = self.axis_map[number]
|
|
if axis:
|
|
fvalue = value / 32767.0
|
|
self.axis_states[axis] = fvalue
|
|
#print("%s: %.3f" % (axis, fvalue))
|
|
|
|
# Switch ready property
|
|
if (self.button_states[HX.JOYSTICK_START_BTN_1] and
|
|
self.button_states[HX.JOYSTICK_START_BTN_2]):
|
|
if self.ready:
|
|
self.ready = False
|
|
else:
|
|
self.ready = True
|
|
|
|
# Timeout? Relax...
|
|
elif time.clock() - self.last_command_time >= \
|
|
HX.JOYSTICK_READY_TIMEOUT / 100:
|
|
self.ready = False
|
|
|
|
def get_m(self):
|
|
axis_move = self.axis_states[HX.JOYSTICK_MOVE_AXIS]
|
|
axis_steer = self.axis_states[HX.JOYSTICK_STEER_AXIS]
|
|
|
|
axis_head_v = self.axis_states[HX.JOYSTICK_HEAD_V_AXIS]
|
|
axis_head_h = self.axis_states[HX.JOYSTICK_HEAD_H_AXIS]
|
|
|
|
# STOP (FLOAT)
|
|
if (axis_move >= -1 / 8) and (axis_move <= 1 / 8):
|
|
move = HX.MOVE_FLOAT
|
|
|
|
# MOVE FORWARD
|
|
elif (axis_move >= -2 / 8) and (axis_move < -1 / 8):
|
|
move = HX.MOVE_FWD1
|
|
elif (axis_move >= -3 / 8) and (axis_move < -2 / 8):
|
|
move = HX.MOVE_FWD2
|
|
elif (axis_move >= -4 / 8) and (axis_move < -3 / 8):
|
|
move = HX.MOVE_FWD3
|
|
elif (axis_move >= -5 / 8) and (axis_move < -4 / 8):
|
|
move = HX.MOVE_FWD4
|
|
elif (axis_move >= -6 / 8) and (axis_move < -5 / 8):
|
|
move = HX.MOVE_FWD5
|
|
elif (axis_move >= -7 / 8) and (axis_move < -6 / 8):
|
|
move = HX.MOVE_FWD6
|
|
elif (axis_move >= -1) and (axis_move < -7 / 8):
|
|
move = HX.MOVE_FWD7
|
|
|
|
# MOVE BACKWARD
|
|
elif (axis_move > 1 / 8) and (axis_move <= 2 / 8):
|
|
move = HX.MOVE_REV1
|
|
elif (axis_move > 2 / 8) and (axis_move <= 3 / 8):
|
|
move = HX.MOVE_REV2
|
|
elif (axis_move > 3 / 8) and (axis_move <= 4 / 8):
|
|
move = HX.MOVE_REV3
|
|
elif (axis_move > 4 / 8) and (axis_move <= 5 / 8):
|
|
move = HX.MOVE_REV4
|
|
elif (axis_move > 5 / 8) and (axis_move <= 6 / 8):
|
|
move = HX.MOVE_REV5
|
|
elif (axis_move > 6 / 8) and (axis_move <= 7 / 8):
|
|
move = HX.MOVE_REV6
|
|
elif (axis_move > 7 / 8) and (axis_move <= 1):
|
|
move = HX.MOVE_REV7
|
|
|
|
# CENTER (FLOAT)
|
|
if (axis_steer >= -1 / 8) and (axis_steer <= 1 / 8):
|
|
steer = HX.STEER_CENTER
|
|
|
|
# STEER LEFT
|
|
elif (axis_steer >= -2 / 8) and (axis_steer < -1 / 8):
|
|
steer = HX.STEER_LEFT1
|
|
elif (axis_steer >= -3 / 8) and (axis_steer < -2 / 8):
|
|
steer = HX.STEER_LEFT2
|
|
elif (axis_steer >= -4 / 8) and (axis_steer < -3 / 8):
|
|
steer = HX.STEER_LEFT3
|
|
elif (axis_steer >= -5 / 8) and (axis_steer < -4 / 8):
|
|
steer = HX.STEER_LEFT4
|
|
elif (axis_steer >= -6 / 8) and (axis_steer < -5 / 8):
|
|
steer = HX.STEER_LEFT5
|
|
elif (axis_steer >= -7 / 8) and (axis_steer < -6 / 8):
|
|
steer = HX.STEER_LEFT6
|
|
elif (axis_steer >= -1) and (axis_steer < -7 / 8):
|
|
steer = HX.STEER_LEFT7
|
|
|
|
# STEER RIGHT
|
|
elif (axis_steer > 1 / 8) and (axis_steer <= 2 / 8):
|
|
steer = HX.STEER_RIGHT1
|
|
elif (axis_steer > 2 / 8) and (axis_steer <= 3 / 8):
|
|
steer = HX.STEER_RIGHT2
|
|
elif (axis_steer > 3 / 8) and (axis_steer <= 4 / 8):
|
|
steer = HX.STEER_RIGHT3
|
|
elif (axis_steer > 4 / 8) and (axis_steer <= 5 / 8):
|
|
steer = HX.STEER_RIGHT4
|
|
elif (axis_steer > 5 / 8) and (axis_steer <= 6 / 8):
|
|
steer = HX.STEER_RIGHT5
|
|
elif (axis_steer > 6 / 8) and (axis_steer <= 7 / 8):
|
|
steer = HX.STEER_RIGHT6
|
|
elif (axis_steer > 7 / 8) and (axis_steer <= 1):
|
|
steer = HX.STEER_RIGHT7
|
|
|
|
# Head V
|
|
if axis_head_v > 0:
|
|
head_v = int(HX.HEAD_V_CENTER - HX.HEAD_V_UP_RANGE * axis_head_v)
|
|
else:
|
|
head_v = int(HX.HEAD_V_CENTER - HX.HEAD_V_DOWN_RANGE * axis_head_v)
|
|
|
|
# Head H
|
|
if axis_head_h > 0:
|
|
head_h = int(HX.HEAD_H_CENTER - HX.HEAD_H_R_RANGE * axis_head_h)
|
|
else:
|
|
head_h = int(HX.HEAD_H_CENTER - HX.HEAD_H_L_RANGE * axis_head_h)
|
|
|
|
# Return
|
|
return (True, move, steer, head_v, head_h) |