diff --git a/requirements.txt b/requirements.txt
index 04e64e1..c7cb356 100644
--- a/requirements.txt
+++ b/requirements.txt
@@ -29,4 +29,8 @@ urllib3==1.26.14
 Werkzeug==2.2.3
 zipp==3.15.0
 python-ev3dev2==2.1.0.post1
-pytest==7.3.1
\ No newline at end of file
+pytest==7.3.1
+pip==23.1.2
+# spidev==3.6
+# picamera==1.13
+numpy==1.19.5
diff --git a/software/roboter/raspy/defines.py b/software/roboter/raspy/defines.py
index 974eca1..929cf64 100644
--- a/software/roboter/raspy/defines.py
+++ b/software/roboter/raspy/defines.py
@@ -19,6 +19,9 @@ Topics = {
     "ROBOT_DATA_BATTERY": "ROBOT/DATA/BATTERY",
     "ROBOT_DATA_POSITION": "ROBOT/DATA/POSITION",
     "ROBOT_DATA_PICTURE": "ROBOT/DATA/PICTURE",
+    "ROBOT_DATA_ERROR": "ROBOT/DATA/ERROR",
+    "ROBOT_DATA_ROBOTREADY": "ROBOT/DATA/ROBOTREADY",
+    "ROBOT_DATA_PICTURE": "ROBOT/DATA/PICTURE",
 
     "BACKEND_ACTION_DRIVE": "BACKEND/ACTION/DRIVE",
     "BACKEND_ACTION_DRIVEPALL": "BACKEND/ACTION/DRIVEALL",
@@ -36,6 +39,8 @@ Topics = {
     "BACKEND_DATA_BATTERY": "BACKEND/DATA/BATTERY",
     "BACKEND_DATA_PICTURE": "BACKEND/DATA/PICTURE",
     "BACKEND_DATA_PLANTCOUNT": "BACKEND/DATA/PLANTCOUNT",
+    "BACKEND_DATA_ERROR": "BACKEND/DATA/ERROR",
+    "BACKEND_DATA_ROBOTREADY": "BACKEND/DATA/ROBOTREADY"
 
 
 }
@@ -49,7 +54,7 @@ SENSORDATA = {
     "SoilMoisture": 0.0,
     "Brightness": 0,
     "PlantID": 0,
-    "ActionID": 0
+    "ActionID": ""
 }
 
 # TODO When sensor is available
@@ -67,7 +72,7 @@ BATTERY = {
 
 DRIVE = {
     "PlantID": 0,
-    "ActionID": 0
+    "ActionID": ""
 }
 
 # GETPOSITION -> no message needed
diff --git a/software/roboter/raspy/functions.py b/software/roboter/raspy/functions.py
index 338340e..b43382a 100644
--- a/software/roboter/raspy/functions.py
+++ b/software/roboter/raspy/functions.py
@@ -20,12 +20,12 @@ def measure_send_data(plantID, actionID, client: mqtt.Client):
     try:
         sensorData = Sensors.readSensors()
     except Exception as e:
-        print(str(e))
-        # TODO Error message MQTT
+        logging.error(str(e))
+        client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
 
     sensorData["PlantID"] = plantID
     sensorData["ActionID"] = actionID
-    client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4))
+    client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4), qos=1)
 
 
 def drive_plant_thread(plantID, actionID, client: mqtt.Client):
@@ -43,49 +43,56 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
     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:
         logging.error(f"Robot Error {errorCode} occurred")
-        logging.error("Drive Plant aborted, Robot at starting position")
+        logging.info("Drive Plant aborted, Robot at starting position")
+        client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1)
+        client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
+
         return
    
     logging.info("Measuring Sensors")
     measure_send_data(plantID, actionID, client)
 
+    logging.info("Taking Picture")
+    sendPicture(client)
+
     logging.info("Robot driving home")
     errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}')
 
-    # TODO Error Message
     if errorCode != 0:
         logging.error(f"Robot Error {errorCode} occurred")
-        logging.error(f"Drive Home aborted, Robot at plant {plantID}")
+        logging.info(f"Drive Home aborted, Robot at plant {plantID}")
+        client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plantID}", qos=1)
+        client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
         return
 
     logging.info("Robot home")
 
-    #TODO decide about robot occupied message
-    #client.publish("ROBOT/DATA/OCCUPIED", "false")
+    client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
 
 
 #region MQTT callbacks
 
-def drive_plant(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
+def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
     """
     Function to drive to plant according to request
     Starting Drive in Thread to not block main programm
 
     Args:
-        clients (mqtt.Client): current mqtt client
+        client (mqtt.Client): current mqtt client
         userdata (_type_): _description_
         message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID
     """
     dictMessage = json.loads(str(message.payload.decode("UTF-8")))
+
     plantID = dictMessage["PlantID"]
     actionID = dictMessage["ActionID"]
 
     logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}")
+    client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
     
-    thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, clients), daemon=True)
+    thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, client), daemon=True)
     thread.start()
 
 
@@ -95,19 +102,21 @@ def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
     Function to send actual GPS position via MQTT in form of POSITION
 
     Args:
-        clients (mqtt.Client): current mqtt client
+        client (mqtt.Client): current mqtt client
         userdata (_type_): _description_
         message (mqtt.MQTTMessage): received message
     """
+    logging.info("Received position request")
+
     # TODO Write GPS Sensor Function
     position = {
         "Position": ""
     }
 
-    clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4))
+    clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4), qos=1)
 
 
-def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
+def get_BatteryStatus(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
     """
     Callback function for battery status request
     Function to read battery status from ev3 and send via MQTT in form of BATTERY
@@ -115,7 +124,7 @@ def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage)
     5V -> 0%
 
     Args:
-        clients (mqtt.Client): current mqtt client
+        client (mqtt.Client): current mqtt client
         userdata (_type_): _description_
         message (mqtt.MQTTMessage): received message
     """
@@ -125,7 +134,7 @@ def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage)
         batteryLevel = int(os.popen('sshpass -p maker ssh robot@ev3dev.local cat /sys/devices/platform/battery/power_supply/lego-ev3-battery/voltage_now').read())
     except:
         logging.error("Robot not connected")
-        #TODO Error Message
+        client.publish(Topics["ROBOT_DATA_ERROR"], "Robot not connected", qos=1)
         return
 
     batteryLevel = round(batteryLevel / 1000000, 2)       # Voltage
@@ -135,6 +144,28 @@ def get_BatteryStatus(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage)
     battery = {
         "Battery": batteryLevel
     }
-    clients.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4))
+    client.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4), qos=1)
+    logging.info(f"Battery done {batteryLevel}")
+
+
+def sendPicture(client: mqtt.Client):
+    """
+    Takes picture and publish via MQTT
+
+    Args:
+        client (mqtt.Client): current mqtt client
+    """
+    try:
+        Sensors.takePicture()
+    except Exception as e:
+        logging.error(str(e))
+        client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
+        return
+
+    with open("picture.png", "rb") as f:
+        file = f.read()
+        byteArr = bytearray(file)
+        client.publish(Topics["ROBOT_DATA_PICTURE"], byteArr)
+        logging.info("Picture Published")
 
 #endregion
\ No newline at end of file
diff --git a/software/roboter/raspy/mainProg.py b/software/roboter/raspy/mainProg.py
index fdc1a2b..4edc26a 100644
--- a/software/roboter/raspy/mainProg.py
+++ b/software/roboter/raspy/mainProg.py
@@ -57,7 +57,7 @@ def main():
     # CHECK forever or start
     client.loop_start()
 
-    logging.info("Starting Loop")
+    logging.info("Robot initialised")
     while True:
         pass
 
diff --git a/software/roboter/raspy/raspy_sensors.py b/software/roboter/raspy/raspy_sensors.py
index 0500902..42ad204 100644
--- a/software/roboter/raspy/raspy_sensors.py
+++ b/software/roboter/raspy/raspy_sensors.py
@@ -3,6 +3,8 @@ import adafruit_dht
 import adafruit_tsl2561
 import board
 import json
+import spidev
+from picamera import PiCamera
 from defines import SENSORDATA
 
 
@@ -34,7 +36,7 @@ def readDHT22():
 
 def readTSL2561():
     """
-    Reads TSL2561 brightness in Lux
+    Reads TSL2561 brightness in Lux and returns integer value
 
     Raises:
         Exception: If TSL2561 not connected properly
@@ -57,8 +59,36 @@ def readTSL2561():
     return brightness
 
 
+def readMCP3008():
+    """
+    Reads YL-69 via MCP3008 ADC soil moisture in percent and returns float value
+    
+    Raises:
+        Exception: If YL-69 not connected properly
+
+    Returns:
+        float: soil moisture [%]
+    """
+
+    channel = 0                                         # Input channel into MCP3008 ADC
+    try:
+        spi = spidev.SpiDev()
+        spi.open(0,0)
+        spi.max_speed_hz = 1000000
+    except:
+        raise Exception("YL69 not connected")
+
+    val = spi.xfer2([1,(8+channel) << 4, 0])
+    data = ((val[1] & 3) << 9) +val[2]
+    percentage = data - 680                             # Return values between ~1780 and ~680
+    percentage = round((1100 - percentage) / 1100, 2)   # 680 -> 100% moisture, 1780 -> 0% moisture
+
+    if percentage > 1 or percentage < 0:                # If not connected values above 100% appear  
+        percentage = 0
+        raise Exception("YL69 not connected")
+
+    return percentage
 
-# TODO SoilMoisture Sensor Function
 
 
 # TODO Function for all sensors
@@ -69,6 +99,7 @@ def readSensors():
     Raises:
         Exception: DHT22 not connected
         Exception: TSL2561 not connected
+        Exception: YL69 not connected
 
     Returns:
         dict: Sensordata
@@ -76,6 +107,7 @@ def readSensors():
     sensorData = SENSORDATA
     errorMessage = ""
 
+    # read DHT22
     try:
         sensorData["AirTemperature"], sensorData["AirHumidity"] = readDHT22()   
     except Exception as e:
@@ -83,12 +115,20 @@ def readSensors():
         sensorData["AirTemperature"] = 0
         errorMessage = str(e) + "\n"
 
+    # read TSL2561
     try:
         sensorData["Brightness"] = readTSL2561()
     except Exception as e:
         sensorData["Brightness"] = 0                   # otherwise old value
         errorMessage = errorMessage + str(e) + "\n"
 
+    # read YL-69
+    try:
+        sensorData["SoilMoisture"] = readMCP3008()
+    except Exception as e:
+        sensorData["SoilMoisture"] = 0
+        errorMessage = errorMessage + str(e) + "\n"
+
     # combined error message
     if errorMessage != "":
         raise Exception(errorMessage)
@@ -96,16 +136,22 @@ def readSensors():
     return sensorData
 
 
-# TODO - take picture function
 def takePicture():
     """
     Take picture and return picture
     Returns:
         _type_: _description_
     """
-    picture = ""
+    try:
+        camera = PiCamera()
+    except:
+        raise Exception("Camera not connected")
+    
+    camera.start_preview()
+    camera.capture("picture.png")
+    camera.stop_preview()
 
-    return picture
+    return 
 
 
 # TODO - read position with sensor
@@ -118,3 +164,18 @@ def readPosition():
      """
     position = ""
     return position
+
+
+# Testing
+
+def main():
+    value = SENSORDATA
+    try:
+        readSensors()
+    except Exception as e:
+        print(str(e))
+
+    print(value)
+
+if __name__ == "__main__":
+    main()
\ No newline at end of file