# -*- 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)