hxbot/HXbot/REflex.py

# -*- coding: utf-8 -*-

########################################
# REflex communication module          #
# EoF 2016 EoF@itphx.ru                #
########################################

import threading
import time
import HX


class REflexClass():

    def __init__(self, hxbot):
        self.i2c_bus = hxbot.i2c_bus
        self.hxbot = hxbot

        self.__e = None
        self.__t = None
        self.lock = hxbot.reflex_lock

        self.joystick = None
        self.hxpower = None
        self.control = HX.CONTROL_SRC_DEFAULT

        self.error_count = 0
        self.online = False
        self.is_running = False

        self.ready_to_move = False
        self.last_command_time = 0.0

        self.__move_float = 0
        self.__steer_center = 0

        # Tuple of commands for execute
        self.m = (
            False,  # Check
            HX.MOVE_FLOAT,  # Move
            HX.STEER_CENTER,  # Steer
            HX.HEAD_V_CENTER,  # Head vertical orientation
            HX.HEAD_H_CENTER  # Head horizontal orientation
            )

        # Tuple with results of executions
        self.s = (
            HX.NO_RSP,
            0,
            0,
            0,
            0
            )

    def start(self):
        self.joystick = self.hxbot.joystick
        self.hxpower = self.hxbot.hxpower

        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

        self.joystick = None
        self.hxpower = None

    def start_move(self):
        result = HX.OK_RSP

        # Enable moving
        if not self.hxpower.mv_enabled:
            result = self.hxpower.enable_mv()

        if result != HX.OK_RSP:
            return

        self.ready_to_move = True

    def stop_move(self, non_block=False):
        result = HX.OK_RSP

        # Disable moving
        if self.hxpower.mv_blocked and not non_block:
            return

        if self.hxpower.mv_enabled:
            result = self.hxpower.disable_mv()

        if result != HX.OK_RSP:
            return

        self.ready_to_move = False

    def get_temp(self):
        return self.__send_command(HX.COM_GET_TEMP)

    def __send_command(self, command):
        try:
            with self.lock:
                self.i2c_bus.write_word_data(HX.REFLEX_ADDRESS, command, 0)
                time.sleep(HX.REFLEX_RESPONSE_DELAY)
                response = self.i2c_bus.read_byte(HX.REFLEX_ADDRESS)
        except IOError:
            if self.online:
                self.error_count += 1
            if self.error_count >= HX.REFLEX_ERROR_LIMIT:
                self.online = False
            return HX.IOE_RSP
        else:
            self.error_count = 0
            self.online = True
            return response

        return HX.NO_RSP

    def __send_ex_command(self, flag, command):
        try:
            with self.lock:
                self.i2c_bus.write_word_data(HX.REFLEX_ADDRESS, HX.EXT_COM,
                    flag << 8 | command)
                time.sleep(HX.REFLEX_RESPONSE_DELAY)
                response = self.i2c_bus.read_byte(HX.REFLEX_ADDRESS)
        except IOError:
            if self.online:
                self.error_count += 1
            if self.error_count >= HX.REFLEX_ERROR_LIMIT:
                self.online = False
            return HX.IOE_RSP
        else:
            self.error_count = 0
            self.online = True
            return response

        return HX.NO_RSP

    def __get_byte(self):
        try:
            with self.lock:
                response = self.i2c_bus.read_byte(HX.REFLEX_ADDRESS)
        except IOError:
            if self.online:
                self.error_count += 1
            if self.error_count >= HX.REFLEX_ERROR_LIMIT:
                self.online = False
            return HX.IOE_RSP
        else:
            self.error_count = 0
            self.online = True
            return response

    def __process(self, stopRequest):
        # Process loop
        while not stopRequest.is_set():
            # Init cycle
            have_commands = False

            # Ping REflex
            self.__send_command(HX.COM_PING)

            # Joystick
            if self.control == HX.CONTROL_SRC_JOYSTICK and self.joystick:
                if self.joystick.ready:
                    # Joystick ready! Let's go!
                    if not self.ready_to_move:
                        self.start_move()

                    # Get m
                    self.m = self.joystick.get_m()
                    have_commands = self.m[0]
                else:
                    if self.ready_to_move:
                        self.stop_move(True)
                    time.sleep(0.5)

            # Execute received commands
            if have_commands:
                self.s = self.execute(self.m)

            # No commands? Stop move...
            elif (time.clock() - self.last_command_time >=
                HX.REFLEX_MOVE_TIMEOUT / 100):
                self.stop_move()

    def execute(self, m):
        # Move
        if m[1] != HX.MOVE_FLOAT:
            move_rsp = self.__send_ex_command(HX.MOVE_FLAG, m[1])
            self.__move_float = 0
        elif self.__move_float < 3:
            move_rsp = self.__send_ex_command(HX.MOVE_FLAG, m[1])
            self.__move_float += 1
        else:
            move_rsp = HX.OK_RSP

        # Steer
        if m[2] != HX.STEER_CENTER:
            steer_rsp = self.__send_ex_command(HX.STEER_FLAG, m[2])
            self.__steer_center = 0
        elif self.__steer_center < 3:
            steer_rsp = self.__send_ex_command(HX.STEER_FLAG, m[2])
            self.__steer_center += 1
        else:
            steer_rsp = HX.OK_RSP

        # Head vertiacal orientation
        head_v_rsp = self.__send_ex_command(HX.HEAD_V_FLAG, m[3])

        # Head horizontal orientation
        head_h_rsp = self.__send_ex_command(HX.HEAD_H_FLAG, m[4])

        # Set last command time
        self.last_command_time = time.clock()

        # Return execution status
        return (HX.OK_RSP, move_rsp, steer_rsp, head_v_rsp, head_h_rsp)