diff --git a/niryo_one_rpi/scripts/fans_manager.py b/niryo_one_rpi/scripts/fans_manager.py new file mode 100755 index 00000000..8b6256be --- /dev/null +++ b/niryo_one_rpi/scripts/fans_manager.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python + +# fans_manager.py +# Copyright (C) 2017 Niryo +# All rights reserved. +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +import rospy +from std_msgs.msg import Bool + +import RPi.GPIO as GPIO + +FAN_1_GPIO = 27 +FAN_2_GPIO = 23 + +class FansManager: + + def setup_fans(self): + GPIO.setwarnings(False) + GPIO.setmode(GPIO.BCM) + GPIO.setup(FAN_1_GPIO, GPIO.OUT) + GPIO.setup(FAN_2_GPIO, GPIO.OUT) + rospy.sleep(0.05) + rospy.loginfo("------ RPI FANS SETUP OK ------") + + def set_fans(self, activate): + if activate: + GPIO.output(FAN_1_GPIO, GPIO.HIGH) + GPIO.output(FAN_2_GPIO, GPIO.HIGH) + else: + GPIO.output(FAN_1_GPIO, GPIO.LOW) + GPIO.output(FAN_2_GPIO, GPIO.LOW) + + def __init__(self): + self.setup_fans() + self.learning_mode_on = True + self.set_fans(not self.learning_mode_on) + + self.learning_mode_subscriber = rospy.Subscriber( + '/niryo_one/learning_mode', Bool, self.callback_learning_mode) + + + def callback_learning_mode(self, msg): + if msg.data != self.learning_mode_on: + self.learning_mode_on = msg.data + self.set_fans(not self.learning_mode_on) + + diff --git a/niryo_one_rpi/scripts/niryo_one_rpi_node.py b/niryo_one_rpi/scripts/niryo_one_rpi_node.py index 8707cbd9..7ef2e437 100755 --- a/niryo_one_rpi/scripts/niryo_one_rpi_node.py +++ b/niryo_one_rpi/scripts/niryo_one_rpi_node.py @@ -21,34 +21,22 @@ # All nodes related to Raspberry Pi are running here with only one node # ( lower RAM usage ) # -# import rospy import RPi.GPIO as GPIO from ros_log_manager import RosLogManager from led_manager import LEDManager +from fans_manager import FansManager from niryo_one_button import NiryoButton from digital_io_panel import DigitalIOPanel from wifi_connection import WifiConnectionManager from niryo_one_ros_setup import * from niryo_one_modbus.modbus_server import ModbusServer -FAN_1_GPIO = 23 -FAN_2_GPIO = 27 class NiryoOneRpi: - def setup_fans(self): - GPIO.setwarnings(False) - GPIO.setmode(GPIO.BCM) - GPIO.setup(FAN_1_GPIO, GPIO.OUT) - GPIO.setup(FAN_2_GPIO, GPIO.OUT) - rospy.sleep(0.1) - GPIO.output(FAN_1_GPIO, GPIO.HIGH) - GPIO.output(FAN_2_GPIO, GPIO.HIGH) - rospy.loginfo("------ RPI FANS ON ------") - def __init__(self): self.wifi_manager_enabled = rospy.get_param("~wifi_manager_enabled") self.launch_ros_processes = rospy.get_param("~launch_ros_processes") @@ -65,7 +53,7 @@ def __init__(self): if self.wifi_manager_enabled: self.wifi_manager = WifiConnectionManager() - self.setup_fans() + self.fans_manager = FansManager() self.ros_log_manager = RosLogManager() self.led_manager = LEDManager() self.niryo_one_button = NiryoButton()