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)
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
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)
def initialize(self):
""" Initialise Hardware.
Step :
- Initialise Sensor
- Position at origin
"""
logger.debug("Initialize Winch hardware...")
self.__changeState(State.INIT)
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
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())
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.")
def getThrottleValue(self):
logger.debug("IO : Throttle to %s" % self.__value)
def __pressedLeft(self):
logger.debug("IO : Move Left pressed !")
self._lcd.enter(InputType.LEFT)
def __initialize(self):
logger.debug("Initialize mode.")
self._speed_current = 0
self._board.initialize()
self._winch.initialized()
def getDistance(self) -> float:
logger.debug("Calculate distance.")
return rotate2distance(self._board.getRotationFromBegin())
def __pressedRight(self):
logger.debug("IO : Move Right pressed !")
self._lcd.enter(InputType.RIGHT)
def applyThrottleValue(self):
logger.debug("Calculate & apply throttle value.")
value = self.__speed_ratio * self._speed_current
self._board.setThrottleValue(value)
def __pressedEnter(self):
logger.debug("IO : Enter pressed !")
self._lcd.enter(InputType.ENTER)
def setSpeedMode(self, speed_mode):
self._speed_mode = speed_mode
logger.debug("IO : Change Speed mode to %s" % self.getSpeedMode())
def setReverse(self, enable):
self._reverse = enable
logger.debug("IO : Change Reverse mode to : %s" % self.isReverse())
def setThrottleValue(self, value):
if (self.__throttle_cmd.value != value):
logger.debug("IO : Throttle to %s" % value)
self.__throttle_cmd.value = value
def emergency(self):
logger.debug("IO : Shutdown power !")
self.__power_cmd.off()