diff --git a/launch/personal_food_computer_var_types.yaml b/launch/personal_food_computer_var_types.yaml index c484139..8c90c41 100644 --- a/launch/personal_food_computer_var_types.yaml +++ b/launch/personal_food_computer_var_types.yaml @@ -23,6 +23,11 @@ environment_variables: description: "The amount of Carbon Dioxide in the air" units: "ppm" type: "std_msgs/Float64" + air_oxygen: + name: air_oxygen + description: "Oxygen density in the air" + units: "percent" + type: "std_msgs/Float64" air_flush_on: name: air_flush_on description: "Turn on air flush (off by default)" diff --git a/launch/python_peripherals_only.launch b/launch/python_peripherals_only.launch new file mode 100644 index 0000000..eefa95d --- /dev/null +++ b/launch/python_peripherals_only.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nodes/actuator_hid_relay_16.py b/nodes/actuator_hid_relay_16.py new file mode 100755 index 0000000..bd333d6 --- /dev/null +++ b/nodes/actuator_hid_relay_16.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python +""" +`actuator_hid_relay_16.py` handles communication with the +`Sainsmart USB HID programmable control relay module + +that connects the the Sainsmart 16-channel relay module +`_. +""" +import rospy +from std_msgs.msg import Bool +from openag_brain.peripherals.hid_relay_16 import HidRelay16 + +if __name__ == '__main__': + rospy.init_node("actuator_hid_relay_16") + default_relay_map = {"red_light_intensity/commanded": "2", # red light is connected to relay 1 + "white_light_intensity/commanded": "3", + "blue_light_intensity/commanded": "4"} + + relay_map = rospy.get_param("~relay_map", default_relay_map) + + hid_relay_16 = HidRelay16() + + def on_set(msg, relay_id): + hid_relay_16.set(int(relay_id), msg.data) + + for topic in relay_map: + subscriber = rospy.Subscriber(topic, Bool, callback=on_set, callback_args=relay_map[topic]) + + rospy.spin() diff --git a/nodes/actuator_relay.py b/nodes/actuator_relay.py new file mode 100755 index 0000000..d9c248d --- /dev/null +++ b/nodes/actuator_relay.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python +""" +`actuator_relay.py` handles communication with an active low relay +driven by GPIO pins +""" +import rospy +from std_msgs.msg import Float64 +from openag_brain.peripherals.relay import Relay + +if __name__ == '__main__': + rospy.init_node("relay") + topic = rospy.get_param("~topic", "red_light_intensity/commanded") + pin = rospy.get_param("~pin", 27) # BCM pin + relay = Relay(pin) + + def on_set(msg): + cmd = msg.data > 0.0 + relay.set(cmd) + + subscriber = rospy.Subscriber(topic, Float64, callback=on_set) + + rospy.spin() diff --git a/nodes/sensor_am2315.py b/nodes/sensor_am2315.py new file mode 100755 index 0000000..4fd5db9 --- /dev/null +++ b/nodes/sensor_am2315.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python +""" +`sensor_am2315.py` handles communication with the +`AM2315 temperature and humidity sensor `_. +""" +import rospy +from std_msgs.msg import Float64 +from openag_brain.peripherals.am2315 import AM2315 + +if __name__ == '__main__': + rospy.init_node("sensor_am2315") + i2c_addr = rospy.get_param("~i2c_addr", 0x5c) + i2c_bus = rospy.get_param("~i2c_bus", "/dev/i2c-1") + temp_pub = rospy.Publisher("air_temperature/raw", Float64, queue_size=10) + humid_pub = rospy.Publisher("air_humidity/raw", Float64, queue_size=10) + rate = rospy.get_param("~rate_hz", 1) + r = rospy.Rate(rate) + + with AM2315(i2c_addr, i2c_bus) as am2315: + while not rospy.is_shutdown(): + am2315.poll() + if am2315.temperature is not None: + temp_pub.publish(am2315.temperature) + if am2315.humidity is not None: + humid_pub.publish(am2315.humidity) + + # Use rate timer instance to sleep until next turn + r.sleep() diff --git a/nodes/sensor_atlas_ec.py b/nodes/sensor_atlas_ec.py new file mode 100755 index 0000000..67fc152 --- /dev/null +++ b/nodes/sensor_atlas_ec.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python +""" +`sensor_atlas_ec.py` handles communication with the Atlas EC sensor +` connected +to the Atlas Isolated USB EZO Board `_. +""" +import rospy +from std_msgs.msg import Float64 +from openag_brain.peripherals.atlas_ec import AtlasEc + +if __name__ == '__main__': + rospy.init_node("sensor_atlas_ec") + ec_pub = rospy.Publisher("water_electrical_conductivity/raw", Float64, queue_size=10) + rate = rospy.get_param("~rate_hz", 1) + r = rospy.Rate(rate) + + while not rospy.get_param("atlas/ready", False): + pass + + rospy.set_param("atlas/ready", False) + with AtlasEc() as atlas_ec: + while not rospy.is_shutdown(): + atlas_ec.poll() + if atlas_ec.ec is not None: + rospy.set_param("atlas/ready", True) + ec_pub.publish(atlas_ec.ec) + + r.sleep() diff --git a/nodes/sensor_atlas_ph.py b/nodes/sensor_atlas_ph.py new file mode 100755 index 0000000..7cfa1bd --- /dev/null +++ b/nodes/sensor_atlas_ph.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python +""" +`sensor_atlas_ph.py` handles communication with the Atlas pH sensor +` connected +to the Atlas Isolated USB EZO Board `_. +""" +import rospy +from std_msgs.msg import Float64 +from openag_brain.peripherals.atlas_ph import AtlasPh + +if __name__ == '__main__': + rospy.init_node("sensor_atlas_ph") + ph_pub = rospy.Publisher("water_potential_hydrogen/raw", Float64, queue_size=10) + rate = rospy.get_param("~rate_hz", 1) + r = rospy.Rate(rate) + + while not rospy.get_param("atlas/ready", False): + pass + + rospy.set_param("atlas/ready", False) + with AtlasPh() as atlas_ph: + + while not rospy.is_shutdown(): + atlas_ph.poll() + if atlas_ph.ph is not None: + rospy.set_param("atlas/ready", True) + ph_pub.publish(atlas_ph.ph) + + r.sleep() diff --git a/nodes/sensor_ds18b20.py b/nodes/sensor_ds18b20.py new file mode 100755 index 0000000..7838f25 --- /dev/null +++ b/nodes/sensor_ds18b20.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python +""" +`sensor_ds18b20.py` handles communication with the +`DS18B20 temperature sensor `_. +""" +import rospy +from std_msgs.msg import Float64 +from openag_brain.peripherals.ds18b20 import DS18B20 + +if __name__ == '__main__': + rospy.init_node("sensor_ds18b20") + temp_pub = rospy.Publisher("water_temperature/raw", Float64, queue_size=10) + rate = rospy.get_param("~rate_hz", 1) + r = rospy.Rate(rate) + + with DS18B20() as ds18b20: + while not rospy.is_shutdown(): + ds18b20.poll() + if ds18b20.temperature is not None: + temp_pub.publish(ds18b20.temperature) + r.sleep() diff --git a/nodes/sensor_grove_o2.py b/nodes/sensor_grove_o2.py new file mode 100755 index 0000000..5b01068 --- /dev/null +++ b/nodes/sensor_grove_o2.py @@ -0,0 +1,21 @@ +#!/usr/bin/python +""" +This module consists of code for interacting with a Grove O2 sensor. +""" + +import rospy +from std_msgs.msg import Float64 +from openag_brain.peripherals.grove_o2 import GroveO2 + +if __name__ == "__main__": + rospy.init_node("sensor_grove_o2") + o2_pub = rospy.Publisher("air_oxygen/raw", Float64, queue_size=10) + rate = rospy.get_param("~rate_hz", 1) + r = rospy.Rate(rate) + + with GroveO2() as grove_o2: + while not rospy.is_shutdown(): + grove_o2.poll() + if grove_o2.o2 is not None: + o2_pub.publish(grove_o2.o2) + r.sleep() diff --git a/nodes/sensor_mhz16.py b/nodes/sensor_mhz16.py new file mode 100755 index 0000000..daa7218 --- /dev/null +++ b/nodes/sensor_mhz16.py @@ -0,0 +1,25 @@ +#!/usr/bin/env python +""" +`sensor_mhz16.py` handles communication with the +`MHZ16 co2 sensor `_. +""" +import rospy +from std_msgs.msg import Float64 +from openag_brain.peripherals.mhz16 import MHZ16 + +if __name__ == '__main__': + rospy.init_node("sensor_mhz16") + i2c_addr = 0x4d + i2c_bus = rospy.get_param("~i2c_bus", "/dev/i2c-1") + co2_pub = rospy.Publisher("air_carbon_dioxide/raw", Float64, queue_size=10) + rate = rospy.get_param("~rate_hz", 1) + r = rospy.Rate(rate) + + with MHZ16(i2c_addr, i2c_bus) as mhz16: + while not rospy.is_shutdown(): + mhz16.poll() + if mhz16.co2 is not None: + co2_pub.publish(mhz16.co2) + + # Use rate timer instance to sleep until next turn + r.sleep() diff --git a/nodes/ui_touchscreen.py b/nodes/ui_touchscreen.py new file mode 100755 index 0000000..7c9ffac --- /dev/null +++ b/nodes/ui_touchscreen.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python +""" +`ui_touchscreen.py` handles communication with an raspberry pi 7" touchcreen +""" +import rospy +from std_msgs.msg import Float64 +from openag_brain.peripherals.touchscreen import Touchscreen + +if __name__ == '__main__': + rospy.init_node("touchscreen") + + + touchscreen = Touchscreen() + + def set_air_temp(msg): + touchscreen.air_temp = msg.data + + def set_humidity(msg): + touchscreen.humidity= msg.data + + def set_co2(msg): + touchscreen.co2 = msg.data + + def set_o2(msg): + touchscreen.o2 = msg.data + + def set_water_temp(msg): + touchscreen.water_temp = msg.data + + def set_ph(msg): + touchscreen.ph = msg.data + + def set_ec(msg): + touchscreen.ec = msg.data + + def set_cmd_temp(msg): + touchscreen.cmd_temp = msg.data + + def set_cmd_hum(msg): + touchscreen.cmd_hum = msg.data + + + air_temp_sub = rospy.Subscriber("air_temperature/raw", Float64, callback=set_air_temp) + humidity_sub = rospy.Subscriber("air_humidity/raw", Float64, callback=set_humidity) + co2_sub = rospy.Subscriber("air_carbon_dioxide/raw", Float64, callback=set_co2) + o2_sub = rospy.Subscriber("air_oxygen/raw", Float64, callback=set_o2) + water_temp_sub = rospy.Subscriber("water_temperature/raw", Float64, callback=set_water_temp) + ph_sub = rospy.Subscriber("water_potential_hydrogen/raw", Float64, callback=set_ph) + ec_sub = rospy.Subscriber("water_electrical_conductivity/raw", Float64, callback=set_ec) + + temp_pub = rospy.Publisher("air_temperature/desired", Float64, queue_size=10) + hum_pub = rospy.Publisher("air_humidity/desired", Float64, queue_size=10) + + temp_cmd_sub = rospy.Subscriber("air_temperature/commanded", Float64, callback=set_cmd_temp) + hum_cmd_sub = rospy.Subscriber("air_humidity/commanded", Float64, callback=set_cmd_hum) + + # Closures are passed by reference such that any new substitutions are interpreted + # as declarations, causing prev_time to be "Referenced before declaration". + # This can be bypassed using an object reference + # https://stackoverflow.com/questions/3190706/nonlocal-keyword-in-python-2-x + def ros_next(rate_hz): + ros_next.prev_time = rospy.get_time() + timeout = 1 / rate_hz + def closure(): + curr_time = rospy.get_time() + if curr_time - ros_next.prev_time > timeout: + ros_next.prev_time = curr_time + return True + else: + return False + return closure + rate = rospy.get_param("~rate_hz", 1) + is_pub = ros_next(rate) + r = rospy.Rate(60) # Frame rate + + while not rospy.is_shutdown(): + if is_pub(): + temp_pub.publish(Float64(touchscreen.desired_temp)) + hum_pub.publish(Float64(touchscreen.desired_hum)) + touchscreen.refresh() + r.sleep() diff --git a/package.xml b/package.xml index da6a4a6..c1c80e1 100644 --- a/package.xml +++ b/package.xml @@ -53,8 +53,16 @@ python-gevent python-flask python-couchdb + python-lxml + python-periphery-pip + python-pylibftdi-pip + python-w1thermsensor-pip + python-usb + python-numpy + python-pygame rosunit rostest + diff --git a/scripts/init_dev b/scripts/init_dev index 5fe691a..fec40be 100755 --- a/scripts/init_dev +++ b/scripts/init_dev @@ -48,7 +48,7 @@ sudo easy_install pip sudo pip install rosdep rosinstall_generator wstool rosinstall # Install some python dependencies that there aren't ros packages for. -sudo pip install voluptuous HTTPretty pytest +sudo pip install voluptuous HTTPretty pytest # Initialize rosdep if it hasn't been already. # Checking for the .list file first makes this script idempotent. @@ -81,4 +81,11 @@ echo "Installing binary dependencies with rosdep" # Install dependency binary deps rosdep install --from-paths ~/catkin_ws/src --ignore-src --rosdistro indigo -y -r --os=debian:jessie +# Let pi user access hid relay device without having to be root +sudo sh -c 'echo SUBSYSTEM=="usb", ATTR{idVendor}=="0416", ATTR{idProduct}=="5020", MODE="0666", GROUP="plugdev" > /etc/udev/rules.d/99-hidrelay.rules' + +# Let pi user access gpio without having to be root +echo SUBSYSTEM=="gpio*", PROGRAM="/bin/sh -c 'chown -R root:gpio /sys/class/gpio && chmod -R 770 /sys/class/gpio; chown -R root:gpio /sys/devices/virtual/gpio && chmod -R 770 /sys/devices/virtual/gpio'" | sudo tee -a /etc/udev/rules.d/99-com.rules > /dev/null +sudo usermod -a -G gpio pi + echo "Finished setting up development environment. Ready to build with catkin_make." diff --git a/scripts/install_dev b/scripts/install_dev index d462262..68a8520 100755 --- a/scripts/install_dev +++ b/scripts/install_dev @@ -18,15 +18,15 @@ echo "Location: /opt/ros/indigo" echo "~~~" # Install packages in system directory -cd ~/catkin_ws && sudo ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/indigo -j2 +cd ~/catkin_ws && ./src/catkin/bin/catkin_make -j2 # Install openag_brain startup service ~/catkin_ws/src/openag_brain/scripts/install_openag_brain_service # Activate the ROS environment -source /opt/ros/indigo/setup.bash -echo "Adding /opt/ros/indigo/setup.bash to ~/.bashrc" -echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc +source ~/catkin_ws/devel/setup.bash +echo "Adding ~/catkin_ws/devel/setup.bash to ~/.bashrc" +echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc # Install platformio rosrun openag_brain install_pio diff --git a/scripts/install_ros b/scripts/install_ros new file mode 100755 index 0000000..7323a33 --- /dev/null +++ b/scripts/install_ros @@ -0,0 +1,41 @@ +#!/bin/bash +# This script installs ROS from source +# in a catkin workspace. + +# Add ROS apt source +sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu jessie main" > /etc/apt/sources.list.d/ros-latest.list' +wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add - + +# Update apt +sudo apt-get update +sudo apt-get -y upgrade + +# Install bootstrapping dependencies +sudo apt-get install -y python-pip +sudo pip install rosdep rosinstall_generator wstool + +# Initialize rosdep. +# This has to happen just once, so we make it +# idempotent by removing the old list if it exists. +if [ -f /etc/ros/rosdep/sources.list.d/20-default.list ]; + then sudo rm /etc/ros/rosdep/sources.list.d/20-default.list +fi +sudo rosdep init +rosdep update + +# Make catkin workspace +mkdir ~/ros_catkin_ws +cd ~/ros_catkin_ws + +# Create a rosinstall file for ros indigo core +rosinstall_generator ros_comm --rosdistro indigo --deps --wet-only --exclude roslisp --tar > indigo_ros_comm_wet.rosinstall + +# Use .rosinstall file to fetch ros source +wstool init src indigo_ros_comm_wet.rosinstall + +# Install dist ros dependencies +rosdep install --from-paths src --ignore-src --rosdistro indigo -y -r --os=debian:jessie + +# Source ROS environment +source /opt/ros/indigo/setup.bash +echo "source /opt/ros/indigo/setup.bash" >> ~/.bashrc diff --git a/setup.py b/setup.py index fdf018e..947d94c 100644 --- a/setup.py +++ b/setup.py @@ -8,7 +8,9 @@ 'openag_lib', 'openag_lib.firmware', 'openag_lib.firmware.plugins', - 'openag_lib.db_bootstrap' + 'openag_lib.db_bootstrap', + 'openag_brain.commands', + 'openag_brain.peripherals' ], package_dir={'': 'src'}, ) diff --git a/src/openag_brain/peripherals/README.md b/src/openag_brain/peripherals/README.md new file mode 100644 index 0000000..0e4db42 --- /dev/null +++ b/src/openag_brain/peripherals/README.md @@ -0,0 +1,3 @@ +# Peripherals + +This directory contains supporting libraries for sensor and actuator peripherals. \ No newline at end of file diff --git a/src/openag_brain/peripherals/__init__.py b/src/openag_brain/peripherals/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/openag_brain/peripherals/am2315.py b/src/openag_brain/peripherals/am2315.py new file mode 100644 index 0000000..d030e95 --- /dev/null +++ b/src/openag_brain/peripherals/am2315.py @@ -0,0 +1,160 @@ +""" +This module consists of code for interacting with the AM2315 temperature and +humidity sensor. +""" + +import rospy +import time +from periphery import I2C, I2CError + +class AM2315: + """ + Class that represents a AM2315 temperature and humidity sensor instance + and provides functions for interfacing with the sensor. + + One instance = 1 successful i2c connection. + """ + + def __init__(self, i2c_addr = 0x5c, i2c_bus = "/dev/i2c-1"): + self.__i2c_master = None + self.i2c_addr = i2c_addr + self.i2c_bus = i2c_bus + self.temperature = None + self.humidity = None + self.regRhMSB = 0x00 + self.regRhLSB = 0x01 + self.regTmpMSB = 0x02 + self.regTmpLSB = 0x03 + self.regModelHi = 0x08 + self.regModelLo = 0x09 + self.regVersion = 0x0a + self.regIDA = 0x0b + self.regIDB = 0x0c + self.regIDD = 0x0d + self.regIDE = 0x0e + self.regStat = 0x0f + self.regUsrAMSB = 0x10 + self.regUsrALSB = 0x11 + self.regUsrBMSB = 0x12 + self.regUsrBLSB = 0x13 + self.cmdReadReg = 0x03 + # Attempt I2C connect. + self.connect() + + def __del__(self): + """ + Destructor will close the I2C manager if instance is garbage collected. + """ + if self.__i2c_master: + self.__i2c_master.close() + + def __enter__(self): + """ + We use the __enter__ and exit methods to manage the lifecycle of + i2c managers. This class can be used with the ``with`` keyword. + See https://www.python.org/dev/peps/pep-0343/ for more on `with`. + + Example:: + + with AM2315() as am2315: + am2315.poll() + """ + return self + + def __exit__(self, exception_type, exception_value, exception_traceback): + # Close the I2C manager on exit. + if self.__i2c_master: + self.__i2c_master.close() + + def connect(self): + # Instantiate the periphery I2C manager. If it fails it will raise + # an I2CError, which we then catch. + # If we have an ``__i2c_master`` it means we have a successful + # connection. + # See http://python-periphery.readthedocs.io/en/latest/i2c.html + try: + self.__i2c_master = I2C(self.i2c_bus) + rospy.loginfo("Connected to AM2315") + except I2CError as e: + rospy.logwarn("Failed to connect to AM2315: {}".format(e)) + self.__i2c_master = None + pass + + def poll(self): + if self.__i2c_master is not None: + try: + self.temperature, self.humidity = self.get_temp_humid() + except: + self.temperature = None + self.humidity = None + else: + self.connect() + + def get_signed(self, unsigned): + """ + Converts the temp reading from the AM2315 to a signed int. + """ + + signednum = 0 + + # If we have the negative temp bit set + if (unsigned & 0x8000) == 0x8000: + # Clear the negative sign bit, and make the number negative. + signednum = 0 - (unsigned & 0x7fff) + else: + signednum = unsigned + + # Return the unsigned int. + return signednum + + def get_temp_humid(self): + """ + get_temp_humid() + + Get the temperature and humidity from the sensor. Returns an array with two integers - temp. [0] and humidity [1] + """ + # Loop sentinel values + failCount = 0 + # Commands to get data temp and humidity data from AM2315 + cmd_data = bytearray((self.cmdReadReg, 0x00, 0x04)) + + # If we have failed more than twice to read the data, or have finished getting data break the loop. + while (failCount < 2): + try: + cmd_msgs = [I2C.Message(cmd_data)] + self.__i2c_master.transfer(self.i2c_addr, cmd_msgs) + + # Wait for the sensor to supply data to read. + time.sleep(0.01) + + # Now read 8 bytes from the AM2315. + read_data = bytearray((0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00)) + read_msgs = [I2C.Message(read_data, read=True)] + self.__i2c_master.transfer(self.i2c_addr, read_msgs) + + # Break the string we want out of the array the transaction returns. + rawTH = bytearray(read_msgs[0].data) + + # Confirm the command worked by checking the response for the command we executed + # and the number of bytes we asked for. + if ((rawTH[0] == self.cmdReadReg) or (rawTH[0] == self.cmdReadReg + 0x80)) and (rawTH[1] == 0x04): + + # And the MSB and LSB for each value together to yield our raw values. + humidRaw = (rawTH[2] << 8) | rawTH[3] + + # Get signed int from AND'd temperature bytes. + tempRaw = self.get_signed((rawTH[4] << 8) | rawTH[5]) + + # The return data is scaled up by 10x, so compensate. + return (tempRaw / 10.0, humidRaw / 10.0) + # No connection yet + except AttributeError: + pass + # We usually fail to read data 50% of the time because the sensor goes to sleep, so try twice. + except IOError: + if failCount > 1: + raise IOError("am2315 IO Error: failed to read from sensor.") + else: + failCount = failCount + 1 + return None, None diff --git a/src/openag_brain/peripherals/atlas_device.py b/src/openag_brain/peripherals/atlas_device.py new file mode 100644 index 0000000..dfb524c --- /dev/null +++ b/src/openag_brain/peripherals/atlas_device.py @@ -0,0 +1,59 @@ +""" +Code for interfacing with atlas scientific devices +""" +from pylibftdi.device import Device +from pylibftdi._base import FtdiError + +class AtlasDevice(Device): + def __init__(self, sn): + Device.__init__(self, mode='t', device_id=sn) + + def read_line(self, size=0): + """ + taken from the ftdi library and modified to + use the ezo line separator "\r" + """ + lsl = len('\r') + line_buffer = [] + while True: + next_char = self.read(1) + if next_char == '' or (size > 0 and len(line_buffer) > size): + break + line_buffer.append(next_char) + if (len(line_buffer) >= lsl and + line_buffer[-lsl:] == list('\r')): + break + return ''.join(line_buffer) + + def read_lines(self): + """ + also taken from ftdi lib to work with modified readline function + """ + lines = [] + try: + while True: + line = self.read_line() + if not line: + self.flush_input() + break + lines.append(line) + return lines + + except FtdiError: + print("Failed to read from the sensor.") + return '' + + def send_cmd(self, cmd): + """ + Send command to the Atlas Sensor. + Before sending, add Carriage Return at the end of the command. + :param cmd: + :return: + """ + buf = cmd + "\r" # add carriage return + try: + self.write(buf) + return True + except FtdiError: + print ("Failed to send command to the sensor.") + return False diff --git a/src/openag_brain/peripherals/atlas_ec.py b/src/openag_brain/peripherals/atlas_ec.py new file mode 100644 index 0000000..ccffd28 --- /dev/null +++ b/src/openag_brain/peripherals/atlas_ec.py @@ -0,0 +1,77 @@ +""" +This module consists of code for interacting with the Atlas electrical +conductivity (EC) sensor. +""" + +""" +IMPORTANT: Need to configure FTDI on raspi for this module to work properly. +See: https://github.com/OpenAgInitiative/openag_atlas_ph_python/blob/master/README.md +""" + +import rospy +import time +from atlas_device import AtlasDevice +from pylibftdi.examples import list_devices + +class AtlasEc: + """ + Class that represents an Atlas EC sensor instance and provides functions + for interfacing with the sensor. + """ + + def __init__(self, device_id = None): + self.device_id = device_id + self.ec = None + self.connect() + + def __enter__(self): + return self + + def __exit__(self, exception_type, exception_value, exception_traceback): + return self + + def connect(self): + try: + if self.device_id is None: + # Try to find pH device id automatically + devices = list_devices.get_ftdi_device_list() + + # Check if any atlas devices are connected to device + if len(devices) == 0: + raise IOError("No atlas device found on system") + + for device in devices: + # Extract id + device_id = (device.split("FTDI:FT230X Basic UART:"))[1] + + # Check if device is ph sensor + with AtlasDevice(device_id) as atlas_device: + atlas_device.send_cmd("i") # get device information + time.sleep(1) + lines = atlas_device.read_lines() + if len(lines) == 0: + continue + if "EC" in lines[0]: + self.device_id = device_id + rospy.loginfo("Automatically found device id: {}".format(device_id)) + + self.device = AtlasDevice(self.device_id) + self.device.send_cmd("C,0") # turn off continuous mode + time.sleep(1) + self.device.flush() + rospy.loginfo("Connected to AtlasEc") + + except Exception as e: + rospy.logwarn("Failed to connect to AtlasEc. Error: {}".format(e)) + pass + + def poll(self): + try: + self.device.send_cmd("R") + lines = self.device.read_lines() + for i in range(len(lines)): + if lines[i] != u'*OK\r': + floats = [float(x) for x in lines[i].split(',')] + self.ec = floats[0] / 1000 # ms/cm + except: + self.ec = None diff --git a/src/openag_brain/peripherals/atlas_ph.py b/src/openag_brain/peripherals/atlas_ph.py new file mode 100644 index 0000000..6b1d770 --- /dev/null +++ b/src/openag_brain/peripherals/atlas_ph.py @@ -0,0 +1,78 @@ +""" +This module consists of code for interacting with the Atlas pH sensor. +""" + +""" +IMPORTANT: Need to configure FTDI on raspi for this module to work properly. +See: https://github.com/OpenAgInitiative/openag_atlas_ph_python/blob/master/README.md +""" + +import rospy +import time +import os +from atlas_device import AtlasDevice +from pylibftdi.examples import list_devices + +class AtlasPh: + """ + Class that represents an Atlas pH sensor instance and provides functions + for interfacing with the sensor. + """ + + def __init__(self, device_id = None): + self.device_id = device_id + self.ph = None + self.connect() + + def __enter__(self): + return self + + def __exit__(self, exception_type, exception_value, exception_traceback): + return self + + def connect(self): + try: + if self.device_id is None: + # Try to find pH device id automatically + devices = list_devices.get_ftdi_device_list() + + # Check if any atlas devices are connected to device + if len(devices) == 0: + raise IOError("No atlas device found on system") + + for device in devices: + # Extract id + device_id = (device.split("FTDI:FT230X Basic UART:"))[1] + + # Check if device is ph sensor + with AtlasDevice(device_id) as atlas_device: + atlas_device.send_cmd("i") # get device information + time.sleep(1) + lines = atlas_device.read_lines() + if len(lines) == 0: + continue + if "pH" in lines[0]: + self.device_id = device_id + rospy.loginfo("Automatically found device id: {}".format(device_id)) + + self.device = AtlasDevice(self.device_id) + self.device.send_cmd("C,0") # turn off continuous mode + time.sleep(1) + self.device.flush() + rospy.loginfo("Connected to AtlasPh") + + except Exception as e: + rospy.logwarn("Failed to connect to AtlasPh. Error: {}".format(e)) + pass + + + def poll(self): + try: + self.device.send_cmd("R") + lines = self.device.read_lines() + for i in range(len(lines)): + if lines[i] != u'*OK\r': + self.ph = float(lines[i]) + self.ph = float("{0:.2f}".format(self.ph)) + except: + self.ph = None diff --git a/src/openag_brain/peripherals/ds18b20.py b/src/openag_brain/peripherals/ds18b20.py new file mode 100644 index 0000000..ad60c2a --- /dev/null +++ b/src/openag_brain/peripherals/ds18b20.py @@ -0,0 +1,40 @@ +""" +This module consists of code for interacting with the DS18B20 temperature sensor. +""" + +import rospy +import time +from w1thermsensor import W1ThermSensor + +class DS18B20: + """ + Class that represents a DS18B20 temperature sensor instance + and provides functions for interfacing with the sensor. + """ + + def __init__(self): + self.temperature = None + self.connect() + + def __del__(self): + return self + + def __enter__(self): + return self + + def __exit__(self, exception_type, exception_value, exception_traceback): + return self + + def connect(self): + try: + self.sensor = W1ThermSensor() + rospy.loginfo("Connected to DS18B20") + except I2CError: + rospy.logwarn("Failed to connect to DS18B20") + pass + + def poll(self): + try: + self.temperature = self.sensor.get_temperature() + except: + self.temperature = None diff --git a/src/openag_brain/peripherals/grove_o2.py b/src/openag_brain/peripherals/grove_o2.py new file mode 100644 index 0000000..810b612 --- /dev/null +++ b/src/openag_brain/peripherals/grove_o2.py @@ -0,0 +1,63 @@ +import serial +import rospy + +class GroveO2: + """ + Class that represents a Grove O2 sensor instance and provides functions + for interfacing with the sensor. + """ + + def __init__(self, analog_port=0, serial_port='/dev/serial/by-id/usb-Numato_Systems_Pvt._Ltd._Numato_Lab_8_Channel_USB_GPIO_Module-if00', pseudo=False): + self.analog_port = analog_port + self.serial_port = serial_port + self.pseudo = pseudo + self.o2 = None + self.sensor_is_connected = True + self.connect() + + def __del__(self): + if self.serial: + self.serial.close() + + def __enter__(self): + return self + + def __exit__(self, exception_type, exception_value, exception_traceback): + if self.serial: + self.serial.close() + return self + + def connect(self): + if self.pseudo: + rospy.loginfo('Connected to pseudo sensor') + return + try: + self.serial = serial.Serial(self.serial_port, 19200, timeout=1) + rospy.logdebug("self.serial.isOpen() = {}".format(self.serial.isOpen())) + if not self.sensor_is_connected: + self.sensor_is_connected = True + rospy.loginfo('Connected to sensor') + except: + if self.sensor_is_connected: + self.sensor_is_connected = False + rospy.logwarn('Unable to connect to sensor') + + def poll(self): + if self.pseudo: + self.o2 = 19.3 + return + if self.sensor_is_connected: + try: + self.serial.write(('adc read {}\r'.format(self.analog_port)).encode()) + response = self.serial.read(25) + voltage = float(response[10:-3]) * 5 / 1024 + if voltage == 0: + return + self.o2 = voltage * 0.21 / 2.0 * 100 # percent + rospy.logdebug('o2 = {}'.format(self.o2)) + except: + rospy.logwarn("O2 SENSOR> Failed to read value during poll") + self.o2 = None + self.sensor_is_connected = False + else: + self.connect() diff --git a/src/openag_brain/peripherals/hid_relay_16.py b/src/openag_brain/peripherals/hid_relay_16.py new file mode 100644 index 0000000..ffe2c16 --- /dev/null +++ b/src/openag_brain/peripherals/hid_relay_16.py @@ -0,0 +1,234 @@ +""" +This module consists of code for interacting with the USB HID 16-Channel +relay module. +""" + +import rospy +import time +import os +import sys +import math +import numpy +import usb.core +import usb.util + +# TODO: Sometimes this class misses...need to add support to handle better... + +class HidRelay16: + """HID USB SainSmart 16 Relay Module using NUC122ZC1 - AN413AD22 - 31B033 - ARM""" + + print_cfg = False + + vid = 0x0416 + pid = 0x5020 + + hid = None + ep_in = None # From Device to Host Computer. + ep_in_address = 0x84 # bEndpointAddress -> 0x84 EP 4 IN + ep_out = None # From Host Computer to Device. + ep_out_address = 0x05 # bEndpointAddress -> 0x05 EP 5 OUT + + relayBitmap = None + relayBitmap16 = [128, 256, 64, 512, 32, 1024, 16, 2048, 8, 4096, 4, 8192, 2, 16384, 1, 32768] + relayBitmap8 = [128, 256, 64, 512, 32, 1024, 16, 2048, 8] + + max_relay_id = None + max_relay_id16 = 15 + max_relay_id8 = 7 + + packet_len = 64 + + """Initiates de device. Ends the process if no device was found.""" + def __init__(self, version=16): + rospy.loginfo("Initializing HidRelay16") + + self.relayBitmap = self.relayBitmap16 + self.max_relay_id = self.max_relay_id16 + + init_ok = self.get_device() + + return None + + """Small function to pack array of bytes""" + def pack_bytes(self, bytesArray): + packet = [0x0] * self.packet_len + i = 0 + for byte in bytesArray: + packet[i] = byte + i += 1 + + bytesString = ''.join([chr(c) for c in packet]) + return bytesString + + """Small function to unpack array of bytes""" + def unpack_bytes(self, bytesString): + bytesArray = bytearray() + bytesArray.extend(bytesString) + + return bytesArray + + """Gets the devices and sets the two endpoints.""" + def get_device(self): + rospy.loginfo("Getting device") + + # initialising debuging - don't have a clue if this will work + self.hid = usb.core.find(idVendor=self.vid, idProduct=self.pid) + + # was it found? + if self.hid is None: + rospy.loginfo("Device not found") + return False + + try: + self.hid.detach_kernel_driver(0) + except: # this usually mean that kernel driver has already been dettached + pass + + if self.print_cfg is True: + for cfg in self.hid: + sys.stdout.write('cfg.bConfigurationValue: ' + str(cfg.bConfigurationValue) + '\n') + for intf in cfg: + sys.stdout.write('\t' + \ + 'intf.bInterfaceNumber: ' + str(intf.bInterfaceNumber) + \ + ',' + \ + 'intf.bAlternateSetting: ' + str(intf.bAlternateSetting) + \ + '\n') + for ep in intf: + sys.stdout.write('\t\t' + \ + 'ep.bEndpointAddress: ' + str(ep.bEndpointAddress) + \ + '\n') + + # access the second configuration + cfg = self.hid[0] + # access the first interface + intf = cfg[(0,0)] + # IN endpoint + self.ep_in = intf[0] + # OUT endpoint + self.ep_out = intf[1] + + # Set the first configuration + self.hid.set_configuration() + + return True + + def read(self): + + readCmd = [0xD2, 0x0E, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x11, 0x48, 0x49, 0x44, + 0x43, 0x80, 0x02, 0x00, 0x00, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, + 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, + 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, + 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC] + + readCmd_pack = self.pack_bytes(readCmd) + # assert len(self.hid.write(self.ep_out_address, readCmd_pack, 100)) == len(readCmd_pack) + self.hid.write(self.ep_out_address, readCmd_pack, 100) + + data_pack = self.hid.read(self.ep_in_address, self.packet_len, 100) + + np_arr8 = numpy.uint8([data_pack[2], data_pack[3]]) + + arr16 = np_arr8.view('uint16') + + mask = arr16[0] + + return mask + + def reset(self): + + resetCmd = [0x71, 0x0E, 0x71, 0x00, 0x00, 0x00, 0x11, 0x11, 0x00, 0x00, 0x48, 0x49, 0x44, + 0x43, 0x2A, 0x02, 0x00, 0x00, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, + 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, + 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, + 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC] + + resetCmd_pack = self.pack_bytes(resetCmd) + + self.hid.write(self.ep_out_address, resetCmd_pack, 100) + + def write(self, mask): + if (type(mask) is not int) or (mask < 0) or (mask > 0xFFFF): + raise ValueError('Invalid write mask: ', mask) + + writeCmd = [0xC3, 0x0E, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x48, 0x49, 0x44, + 0x43, 0xEE, 0x01, 0x00, 0x00, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, + 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, + 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, + 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC, 0xCC]; + + np_arr16 = numpy.uint16([mask]) + np_arr8 = np_arr16.view('uint8') + + writeCmd[2] = np_arr8[0] + writeCmd[3] = np_arr8[1] + + size = writeCmd[1] + + writeCmd_sliced = writeCmd[0:size] + checksum = reduce((lambda a, b: a + b), writeCmd_sliced) + + np_arr32 = numpy.uint32([checksum]) + arr8 = np_arr32.view('uint8') + + for x in range(0, 3): + writeCmd[size + x] = arr8[x] + + writeCmd_pack = self.pack_bytes(writeCmd) + self.hid.write(self.ep_out_address, writeCmd_pack, 100) + + return True + + def set(self, relay_id, state): + try: + if state: + rospy.loginfo("Turning ON Relay {}".format(relay_id)) + else: + rospy.loginfo("Turning OFF Relay {}".format(relay_id)) + + if type(relay_id) is not int: + raise ValueError('Invalid relay ID type: ', relay_id) + + if (relay_id < 0) or (relay_id > self.max_relay_id): + raise ValueError('Invalid relay ID: ', relay_id) + + if type(state) is not bool: + raise ValueError('Invalid state: ', state) + + readmask = self.read() + rospy.loginfo("Readmask: {}".format(readmask)) + + bit = int(math.pow(2, relay_id)) + + mask = 0 + + isOn = False + + for x in range(0, self.max_relay_id): + if (readmask & self.relayBitmap[x]) != 0: + currentRelayMask = int(math.pow(2, x)) + mask = mask | currentRelayMask + if currentRelayMask == int(bit): + isOn = True + rospy.loginfo("Led {} is ON".format(x)) + + rospy.loginfo("Current Mask: {}".format(mask)) + + if state is True: + mask = mask | bit + elif state is False: + if isOn is True: + mask = mask ^ bit + + + rospy.loginfo("Writing Mask: {}".format(mask)) + writemask = self.write(mask) + rospy.loginfo("Writemask: {}".format(writemask)) + + new_readmask = self.read() + + rospy.loginfo("New Readmask: {}".format(new_readmask)) + + return writemask + + except: + return None diff --git a/src/openag_brain/peripherals/mhz16.py b/src/openag_brain/peripherals/mhz16.py new file mode 100644 index 0000000..042011d --- /dev/null +++ b/src/openag_brain/peripherals/mhz16.py @@ -0,0 +1,148 @@ +""" +This module consists of code for interacting with the MHZ16 co2 sensor. +""" + +import rospy +import time +from periphery import I2C, I2CError + +class MHZ16: + """ + Class that represents a MHZ16 co2 sensor instance + and provides functions for interfacing with the sensor. + + One instance = 1 successful i2c connection. + """ + + def __init__(self, i2c_addr = 0x4d, i2c_bus = "/dev/i2c-1"): + self.__i2c_master = None + self.i2c_addr = i2c_addr + self.i2c_bus = i2c_bus + self.co2 = None + self.cmd_measure = [0xFF,0x01,0x9C,0x00,0x00,0x00,0x00,0x00,0x63] + self.ppm = 0 + self.IOCONTROL = 0X0E << 3 + self.FCR = 0X02 << 3 + self.LCR = 0X03 << 3 + self.DLL = 0x00 << 3 + self.DLH = 0X01 << 3 + self.THR = 0X00 << 3 + self.RHR = 0x00 << 3 + self.TXLVL = 0X08 << 3 + self.RXLVL = 0X09 << 3 + + # Attempt I2C connect. + self.connect() + + def __del__(self): + """ + Destructor will close the I2C manager if instance is garbage collected. + """ + if self.__i2c_master: + self.__i2c_master.close() + + def __enter__(self): + """ + We use the __enter__ and exit methods to manage the lifecycle of + i2c managers. This class can be used with the ``with`` keyword. + See https://www.python.org/dev/peps/pep-0343/ for more on `with`. + + Example:: + + with MHZ16() as mhz16: + mhz16.poll() + """ + return self + + def __exit__(self, exception_type, exception_value, exception_traceback): + # Close the I2C manager on exit. + if self.__i2c_master: + self.__i2c_master.close() + + def connect(self): + # Instantiate the periphery I2C manager. If it fails it will raise + # an I2CError, which we then catch. + # If we have an ``__i2c_master`` it means we have a successful + # connection. + # See http://python-periphery.readthedocs.io/en/latest/i2c.html + try: + self.__i2c_master = I2C(self.i2c_bus) + rospy.loginfo("Connected to MHZ16") + self.configure() + except I2CError as e: + rospy.logwarn("Failed to connect to MHZ16{}".format(e)) + self.__i2c_master = None + pass + + def poll(self): + if self.__i2c_master is not None: + try: + self.co2 = self.get_co2() + except: + self.co2 = None + else: + self.connect() + + + def configure(self): + try: + self.write_register(self.IOCONTROL, 0x08) + except IOError: + pass + self.write_register(self.FCR, 0x07) + self.write_register(self.LCR, 0x83) + self.write_register(self.DLL, 0x60) + self.write_register(self.DLH, 0x00) + self.write_register(self.LCR, 0x03) + + def get_co2(self): + try: + # Set the FCR register + self.write_register(self.FCR, 0x07) + # cmd_data = bytearray((self.FCR, 0x07)) + # cmd_msgs = [I2C.Message(cmd_data)] + # self.__i2c_master.transfer(self.i2c_addr, cmd_msgs) + + # Read TXLVL register + msgs = [I2C.Message([self.TXLVL]), I2C.Message([0x00], read=True)] + self.__i2c_master.transfer(self.i2c_addr, msgs) + txlvl = msgs[1].data[0] + + # Verify TXLVL + if txlvl >= len(self.cmd_measure): + # Send Command Bytes to THR Register + cmd_data = bytearray([self.THR] + self.cmd_measure) + cmd_msgs = [I2C.Message(cmd_data)] + self.__i2c_master.transfer(self.i2c_addr, cmd_msgs) + + # Wait for the sensor to supply data to read. + time.sleep(0.1) + + # Now read 9 bytes from the MHZ16. + read_data = bytearray((0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00)) + read_msgs = [I2C.Message(read_data, read=True)] + self.__i2c_master.transfer(self.i2c_addr, read_msgs) + + # Break the string we want out of the array the transaction returns. + response = bytearray(read_msgs[0].data) + + ## Compute checksum + checksum = 0 + for i in range (0, 9): + checksum += response[i] + + # Confirm the command worked by checking the response for the command we executed + # and validating checksum + if (response[0] == 0xFF) and (response[1] == 0x9C) and (checksum % 256 == 0xFF): + return (response[2]<<24) + (response[3]<<16) + (response[4]<<8) + response[5] + + except IOError: + raise IOError("mhz16 IO Error: failed to read from sensor.") + + return None + + def write_register(self, reg_addr, val): + time.sleep(0.001) + cmd_data = bytearray((reg_addr, val)) + cmd_msgs = [I2C.Message(cmd_data)] + self.__i2c_master.transfer(self.i2c_addr, cmd_msgs) diff --git a/src/openag_brain/peripherals/relay.py b/src/openag_brain/peripherals/relay.py new file mode 100644 index 0000000..2b2a367 --- /dev/null +++ b/src/openag_brain/peripherals/relay.py @@ -0,0 +1,27 @@ +""" +This module consists of code for interacting with an active low relay. +""" + +import rospy +from periphery import GPIO + +class Relay(): + def __init__(self, pin): + rospy.loginfo("Initializing relay") + self.pin = pin + self.gpio_out = GPIO(self.pin, "out") + self.gpio_out.write(True) # initialize relay to be OFF + + def __exit__(self): + self.gpio_out.close() + + def set(self, state): + try: + if state: + rospy.loginfo("Turning ON relay") + self.gpio_out.write(False) # active low + else: + rospy.loginfo("Turning OFF relay") + self.gpio_out.write(True) + except Exception as e: + rospy.logwarn("Unable to set relay. Error: {}".format(e)) diff --git a/src/openag_brain/peripherals/touchscreen.py b/src/openag_brain/peripherals/touchscreen.py new file mode 100644 index 0000000..8f0ca4e --- /dev/null +++ b/src/openag_brain/peripherals/touchscreen.py @@ -0,0 +1,167 @@ +""" +This module consists of code for running a ui on the official +raspberry pi 7" touchscreen +""" + +import rospy +import pygame + +WIDTH = 800 +HEIGHT = 480 + +class Touchscreen(): + def __init__(self): + self.air_temp = None + self.humidity = None + self.co2 = None + self.o2 = None + self.water_temp = None + self.ph = None + self.ec = None + self.desired_temp = 25 + self.desired_hum = 50 + self.cmd_temp = 0 + self.cmd_hum = 0 + self.white = [255,255,255] + self.black = [0,0,0] + self.event_queue = None + + rospy.loginfo("Initializing touchscreen") + pygame.init() + # pygame.mouse.set_visible(False) + self.screen = pygame.display.set_mode((WIDTH,HEIGHT),pygame.NOFRAME) + + def refresh(self): + try: + self.event_queue = pygame.event.get() + self.screen.fill(self.black) + self.blitSensorValues() + self.blitDesiredUI() + pygame.display.update() + except Exception as e: + rospy.logwarn("Refresh crashed. Error: {}".format(e)) + + + def blitSensorValues(self): + if self.air_temp is not None: + self.createSensorCard(0, 'Air Temp: {0:.1f} C'.format(self.air_temp), self.black, self.white) + else: + self.createSensorCard(0, 'Air Temp: {}'.format(self.air_temp), self.black, self.white) + + if self.humidity is not None: + self.createSensorCard(1, 'Humidity: {0:.1f} %'.format(self.humidity), self.black, self.white) + else: + self.createSensorCard(1, 'Humidity: {}'.format(self.humidity), self.black, self.white) + + if self.co2 is not None: + self.createSensorCard(2, 'CO2: {0:.0f} ppm'.format(self.co2), self.black, self.white) + else: + self.createSensorCard(2, 'CO2: {}'.format(self.co2), self.black, self.white) + + if self.o2 is not None: + self.createSensorCard(3, 'O2: {0:.1f} %'.format(self.o2), self.black, self.white) + else: + self.createSensorCard(3, 'O2: {}'.format(self.o2), self.black, self.white) + + if self.water_temp is not None: + self.createSensorCard(4, 'Water Temp: {0:.1f} C'.format(self.water_temp), self.black, self.white) + else: + self.createSensorCard(4, 'Water Temp: {}'.format(self.water_temp), self.black, self.white) + + if self.ph is not None: + self.createSensorCard(5, 'pH: {0:.1f}'.format(self.ph), self.black, self.white) + else: + self.createSensorCard(5, 'pH: {}'.format(self.ph), self.black, self.white) + + if self.ec is not None: + self.createSensorCard(6, 'EC: {0:.1f} ms/cm'.format(self.ec), self.black, self.white) + else: + self.createSensorCard(6, 'EC: {}'.format(self.ec), self.black, self.white) + + def blitDesiredUI(self): + def plus_temp(): + self.desired_temp += 1 + def minus_temp(): + self.desired_temp -= 1 + self.createSetPointUI(0, "Air Temperature: {}".format(self.desired_temp), plus_temp, minus_temp, self.black, self.white) + + def plus_hum(): + self.desired_hum += 1 + def minus_hum(): + self.desired_hum -= 1 + self.createSetPointUI(1, "Humidity: {}".format(self.desired_hum), plus_hum, minus_hum, self.black, self.white) + + self.createSensorCard(2, "Heater on: {}".format(bool(self.cmd_temp)), self.black, self.white, x_offset=(WIDTH/2)) + self.createSensorCard(3, "Humidifier on: {}".format(bool(self.cmd_hum)), self.black, self.white, x_offset=(WIDTH/2)) + + def createSensorCard(self, pos, msg, box_color=None, text_color=None, x_offset=0): + spacing = 6 + width = (WIDTH / 2) - spacing + height = 50 #64 + box_colors = [[255,255,255], [0,0,0]] + text_colors = [[0,0,0], [255,255,255]] + x = x_offset + y_offset = 50 + y = y_offset + (height + spacing) * pos + font_style = 'monospace' # monospace or 'freesans.ttf' + font_size = 25 #23 or 30 + + if box_color is None: + box_color = box_colors[pos%2] + if text_color is None: + text_color = text_colors[pos%2] + + pygame.draw.rect(self.screen, box_color, (x,y,width,height)) + font = pygame.font.SysFont(font_style, font_size) + text_surf, text_rect = self.textObjects(msg, font, text_color) + text_rect.center = ( (x+(width/2)), (y+(height/2)) ) + self.screen.blit(text_surf, text_rect) + + def button(self,msg,x,y,w,h,inactive_color,active_color, action=None): + clicked = False + for event in self.event_queue: + if event.type == pygame.MOUSEBUTTONDOWN: + clicked = True + mouse = pygame.mouse.get_pos() + + if x+w > mouse[0] > x and y+h > mouse[1] > y: + pygame.draw.rect(self.screen, active_color,(x,y,w,h)) + if clicked and action is not None: + action() + else: + pygame.draw.rect(self.screen, inactive_color,(x,y,w,h)) + + font = pygame.font.SysFont('monospace', 25) + textSurf, textRect = self.textObjects(msg, font, [255,255,255]) + textRect.center = ( (x+(w/2)), (y+(h/2)) ) + self.screen.blit(textSurf, textRect) + + + def createSetPointUI(self, pos, msg, plus, minus, box_color=None, text_color=None, x_offset=0): + spacing = 6 + width = (WIDTH/2) - spacing + height = 50 #64 + box_colors = [[255,255,255], [0,0,0]] + text_colors = [[0,0,0], [255,255,255]] + x = (WIDTH/2) + spacing + x_offset + y_offset = 50 + y = y_offset + (height + spacing) * pos + font_style = 'monospace' # monospace or 'freesans.ttf' + font_size = 25 #23 or 30 + + if box_color is None: + box_color = box_colors[pos%2] + if text_color is None: + text_color = text_colors[pos%2] + + pygame.draw.rect(self.screen, box_color, (x,y,width,height)) + font = pygame.font.SysFont(font_style, font_size) + text_surf, text_rect = self.textObjects(msg, font, text_color) + text_rect.center = ( (x+(width/2)), (y+(height/2)) ) + self.button("-", x, y, height, height, [50,50,50], [200,200,200], minus) + self.button("+", WIDTH - height - spacing, y, height, height, [50,50,50], [200,200,200], plus) + self.screen.blit(text_surf, text_rect) + + def textObjects(self, text, font, color): + text_surface = font.render(text, True, color) + return text_surface, text_surface.get_rect()