コード例 #1
0
    def __initControlLoop(self):
        """ Initialize Control Loop thread. """

        logger.debug("Initialize Control Loop...")
        self.__controlLoop = threading.Thread(target=self.__mode.runControlLoop, name="Ctrl", args=(), daemon=True)
        self.__controlLoop.start()
        self.__changeState(State.BOOTED)
コード例 #2
0
    def __init__(self, winch: Winch):
        logger.debug("IO : Initialize Hardware...")
        super().__init__(winch)

        # Power
        self.__power_cmd = OutputDevice(OUT_PWR)
        self.__power_cmd.off()

        # Reverse
        self.__reverse_cmd = OutputDevice(OUT_REVERSE)

        # Speed mode (Lo, Medium, Hi)
        self.__speed_cmd = OutputDevice(OUT_SPD)

        # Throlle
        self.__throttle_cmd = PWMOutputDevice(OUT_THROTTLE)

        # Move
        self.__key_enter_btn = Button(IN_KEY_ENTER)
        self.__key_left_btn = Button(IN_KEY_LEFT)
        self.__key_right_btn = Button(IN_KEY_RIGHT)

        # Register event
        self.__key_enter_btn.when_pressed = self.__pressedEnter
        self.__key_left_btn = self.__pressedLeft
        self.__key_right_btn = self.__pressedRight
コード例 #3
0
    def setThrottleValue(self, value):
        if (self.__value > 0):
            self._rotation_from_init -= 1
        elif (self.__init):
            self._rotation_from_init += 1

        if (self.__value != value):
            self.__value = value
            logger.debug("IO : Throttle to %s" % self.__value)
コード例 #4
0
    def initialize(self):
        """ Initialise Hardware.

        Step :
        - Initialise Sensor
        - Position at origin
        """

        logger.debug("Initialize Winch hardware...")
        self.__changeState(State.INIT)
コード例 #5
0
    def __changeState(self, state):
        """ Change State of machine-state

        Parameters
        ----------
        mode : State Enum
            Mode to enable.
        """
        if (self.__state != state):
            logger.debug("Switch state : %s", state)
            self.__state = state
コード例 #6
0
    def __loadConfig(self):
        logger.debug("Gui config : %s" % config.GUI)
        self.__gui = Gui(self)
        self.__gui.boot()
        self._input = Keyboard(self, self.__gui)

        logger.debug("Board config : %s" % config.BOARD)
        self.__board = loadClass(config.BOARD, self)
        logger.info("Board : %s" % type(self.__board).__name__)

        logger.debug("Mode config : %s" % config.MODE)
        self.__mode = ModeFactory.modeFactory(self, self.__board, config.MODE)
        logger.info("Mode : %s" % self.getMode())
コード例 #7
0
    def runControlLoop(self):
        """ Main Loop to control hardware. """

        t = threading.currentThread()
        logger.debug("Starting Control Loop.")

        while getattr(t, "do_run", True):
            logger.debug("Current state : %s - speed : %s - limit : %s" %
                         (self._winch.getState(), self._speed_current,
                          self._board.getRotationFromBegin()))

            # INIT
            if (self._winch.getState().isInit):
                self.__initialize()

            # STARTING or RUNNING
            if (self._winch.getState().isRun):
                self.__starting()

            # STOP
            if (self._winch.getState().isStop):
                self.__stopping()

            # Specifical mode
            self._extraMode()

            # EMERGENCY
            if (self._winch.getState().isFault):
                self.__fault()

            self.applyThrottleValue()

            # CPU idle
            time.sleep(LOOP_DELAY)

        logger.debug("Stopping Control Loop.")
コード例 #8
0
 def getThrottleValue(self):
     logger.debug("IO : Throttle to %s" % self.__value)
コード例 #9
0
 def __pressedLeft(self):
     logger.debug("IO : Move Left pressed !")
     self._lcd.enter(InputType.LEFT)
コード例 #10
0
 def __initialize(self):
     logger.debug("Initialize mode.")
     self._speed_current = 0
     self._board.initialize()
     self._winch.initialized()
コード例 #11
0
 def getDistance(self) -> float:
     logger.debug("Calculate distance.")
     return rotate2distance(self._board.getRotationFromBegin())
コード例 #12
0
 def __pressedRight(self):
     logger.debug("IO : Move Right pressed !")
     self._lcd.enter(InputType.RIGHT)
コード例 #13
0
 def applyThrottleValue(self):
     logger.debug("Calculate & apply throttle value.")
     value = self.__speed_ratio * self._speed_current
     self._board.setThrottleValue(value)
コード例 #14
0
 def __pressedEnter(self):
     logger.debug("IO : Enter pressed !")
     self._lcd.enter(InputType.ENTER)
コード例 #15
0
 def setSpeedMode(self, speed_mode):
     self._speed_mode = speed_mode
     logger.debug("IO : Change Speed mode to %s" % self.getSpeedMode())
コード例 #16
0
 def setReverse(self, enable):
     self._reverse = enable
     logger.debug("IO : Change Reverse mode to : %s" % self.isReverse())
コード例 #17
0
 def setThrottleValue(self, value):
     if (self.__throttle_cmd.value != value):
         logger.debug("IO : Throttle to %s" % value)
         self.__throttle_cmd.value = value
コード例 #18
0
 def emergency(self):
     logger.debug("IO : Shutdown power !")
     self.__power_cmd.off()