diff --git a/software/roboter/raspy/functions.py b/software/roboter/raspy/functions.py
index 4336049..686a6f4 100644
--- a/software/roboter/raspy/functions.py
+++ b/software/roboter/raspy/functions.py
@@ -3,7 +3,7 @@ import json
 import threading
 import os
 import raspy_sensors as Sensors
-from defines import Topics
+from defines import Topics, SENSORDATA
 
 def measure_send_data(plantID, actionID, client: mqtt.Client):
     """
@@ -14,13 +14,13 @@ def measure_send_data(plantID, actionID, client: mqtt.Client):
         actionID (_type_): current ID of driving action
         client (mqtt.Client): current mqtt client for publishing
     """
+    sensorData = SENSORDATA
+
     try:
         sensorData = Sensors.readSensors()
-        
     except Exception as e:
         print(str(e))
-
-    
+        # TODO Error message MQTT
 
     sensorData["PlantID"] = plantID
     sensorData["ActionID"] = actionID
@@ -39,14 +39,14 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
         client (mqtt.Client): current mqtt client for publishing
     """
     # FIXME Change to color code driving
-    errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py')
-    # errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}')
+    # errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py')
+    # # errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}')
 
-    # # TODO Error Message
-    if errorCode != 0:
-        print(f"Robot Error {errorCode} occurred")
-        print("Drive Plant aborted, Robot at starting position")
-        return
+    # TODO Error Message
+    # if errorCode != 0:
+    #     print(f"Robot Error {errorCode} occurred")
+    #     print("Drive Plant aborted, Robot at starting position")
+    #     return
    
     print("Measuring Sensors")
 
diff --git a/software/roboter/raspy/raspy_sensors.py b/software/roboter/raspy/raspy_sensors.py
index 15c658c..e6bc905 100644
--- a/software/roboter/raspy/raspy_sensors.py
+++ b/software/roboter/raspy/raspy_sensors.py
@@ -3,40 +3,59 @@ import adafruit_dht
 import adafruit_tsl2561
 import board
 import json
+from defines import SENSORDATA
 
 
-# TODO 
-def readDHT22():
-    # Air Temperature & Humidity
+def readDHT22(): 
+    """
+    Reads DHT22 air temperature and air humidity and return values as float
+
+    Raises:
+        Exception: If DHT22 not connected properly
+
+    Returns:
+        float: air temperature in °C
+        float: air humidity %
+    """
     try:
         dht22 = adafruit_dht.DHT22(board.D4, use_pulseio=False)
     except:
         raise Exception("DHT22 not connected")
     
     # read DHT22
-    # if Error reading Data try again
-    while True:
-        try:
-            sensorData["AirTemperature"] = dht22.temperature
-            sensorData["AirHumidity"] = dht22.humidity
-        except:
-            continue
+    try:
+        temperature = dht22.temperature
+        humidity = dht22.humidity
+    except:
+        raise Exception("DHT22 not connected")
+
+    return temperature, humidity
 
-        break
 
-# TODO
 def readTSL2561():
-    # Brightness
+    """
+    Reads TSL2561 brightness in Lux
+
+    Raises:
+        Exception: If TSL2561 not connected properly
+
+    Returns:
+        int: brightness in Lux
+    """
     try:
         tsl2561 = adafruit_tsl2561.TSL2561(board.I2C())
     except:
         raise Exception("TSL2561 not connected")
     
     # read TSL2561
+    brightness = 0
     if type(tsl2561.lux) == type(None):  # Max Value 40.000 -> above error
-        sensorData["Brightness"] = 40000
+        brightness = 40000
     else:
-        sensorData["Brightness"] = int(tsl2561.lux)
+        brightness = int(tsl2561.lux)
+
+    return brightness
+
 
 
 # TODO SoilMoisture Sensor Function
@@ -45,25 +64,34 @@ def readTSL2561():
 # TODO Function for all sensors
 def readSensors():
     """
-    Read all Sensors and return Dictionary with data
+    Read DHT22, TSL2561 and Humidity Sensor
+
+    Raises:
+        Exception: DHT22 not connected
+        Exception: TSL2561 not connected
 
     Returns:
-        sensordata (dict): all data of sensors
+        dict: Sensordata
     """
-    
+    sensorData = SENSORDATA
+    errorMessage = ""
 
-    # global Variables
-    sensorData = {
-        "AirTemperature": 0.0,
-        "AirHumidity": 0.0,
-        "SoilMoisture": 0.0,
-        "Brightness": 0,
-        "PlantID": 0,
-        "ActionID": 0,
-    }
+    try:
+        sensorData["AirTemperature"], sensorData["AirHumidity"] = readDHT22()   
+    except Exception as e:
+        sensorData["AirHumidity"] = 0                   # otherwise old value
+        sensorData["AirTemperature"] = 0
+        errorMessage = str(e) + "\n"
 
-    print("TEST")
+    try:
+        sensorData["Brightness"] = readTSL2561()
+    except Exception as e:
+        sensorData["Brightness"] = 0                   # otherwise old value
+        errorMessage = errorMessage + str(e) + "\n"
 
+    # combined error message
+    if errorMessage != "":
+        raise Exception(errorMessage)
 
     return sensorData