repository to manage all files related to the makeathon farm bot project (Software + Documentation).
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

functions.py 5.3KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154
  1. import paho.mqtt.client as mqtt
  2. import json
  3. import threading
  4. import os
  5. import logging
  6. import raspy_sensors as Sensors
  7. from defines import Topics, SENSORDATA
  8. def measure_send_data(plantID, actionID, client: mqtt.Client):
  9. """
  10. Reads all sensors and publishes data via MQTT in form of SENSORDATA
  11. Args:
  12. plantID (_type_): plant to measure
  13. actionID (_type_): current ID of driving action
  14. client (mqtt.Client): current mqtt client for publishing
  15. """
  16. sensorData = SENSORDATA
  17. try:
  18. sensorData = Sensors.readSensors()
  19. except Exception as e:
  20. logging.error(str(e))
  21. # TODO Error message MQTT
  22. client.publish(Topics["ROBOT_DATA_ERROR"], str(e), qos=1)
  23. sensorData["PlantID"] = plantID
  24. sensorData["ActionID"] = actionID
  25. client.publish(Topics["ROBOT_DATA_SENSORDATA"], json.dumps(sensorData, indent=4), qos=1)
  26. def drive_plant_thread(plantID, actionID, client: mqtt.Client):
  27. """
  28. Function to drive to plant according to number from MQTT message in thread
  29. Meassure and publish data via MQTT
  30. Drive home to starting point
  31. Args:
  32. plantID (_type_): plant to measure
  33. actionID (_type_): current ID of driving action
  34. client (mqtt.Client): current mqtt client for publishing
  35. """
  36. # FIXME Change to color code driving
  37. errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py')
  38. # errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plantID}')
  39. if errorCode != 0:
  40. logging.error(f"Robot Error {errorCode} occurred")
  41. logging.info("Drive Plant aborted, Robot at starting position")
  42. client.publish(Topics["ROBOT_DATA_ERROR"], "Drive Plant aborted, Robot at starting position", qos=1)
  43. #TODO decide about robot occupied message
  44. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  45. return
  46. logging.info("Measuring Sensors")
  47. measure_send_data(plantID, actionID, client)
  48. logging.info("Robot driving home")
  49. errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}')
  50. if errorCode != 0:
  51. logging.error(f"Robot Error {errorCode} occurred")
  52. logging.info(f"Drive Home aborted, Robot at plant {plantID}")
  53. client.publish(Topics["ROBOT_DATA_ERROR"], f"Drive Home aborted, Robot at plant {plantID}", qos=1)
  54. #TODO decide about robot occupied message
  55. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  56. return
  57. logging.info("Robot home")
  58. #TODO decide about robot occupied message
  59. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
  60. #region MQTT callbacks
  61. def drive_plant(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  62. """
  63. Function to drive to plant according to request
  64. Starting Drive in Thread to not block main programm
  65. Args:
  66. clients (mqtt.Client): current mqtt client
  67. userdata (_type_): _description_
  68. message (mqtt.MQTTMessage): received DRIVE message with PlantID and ActionID
  69. """
  70. dictMessage = json.loads(str(message.payload.decode("UTF-8")))
  71. print(dictMessage)
  72. plantID = dictMessage["PlantID"]
  73. actionID = dictMessage["ActionID"]
  74. logging.info(f"Received Drive-request to plant {plantID}, ActionID: {actionID}")
  75. #TODO decide about robot occupied message
  76. client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
  77. thread = threading.Thread(target= drive_plant_thread, args=(plantID, actionID, client), daemon=True)
  78. thread.start()
  79. def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  80. """
  81. Callback function for GPS position request
  82. Function to send actual GPS position via MQTT in form of POSITION
  83. Args:
  84. clients (mqtt.Client): current mqtt client
  85. userdata (_type_): _description_
  86. message (mqtt.MQTTMessage): received message
  87. """
  88. logging.info("Received position request")
  89. # TODO Write GPS Sensor Function
  90. position = {
  91. "Position": ""
  92. }
  93. clients.publish(Topics["ROBOT_DATA_POSITION"], json.dumps(position, indent=4), qos=1)
  94. def get_BatteryStatus(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
  95. """
  96. Callback function for battery status request
  97. Function to read battery status from ev3 and send via MQTT in form of BATTERY
  98. 8,5V -> 100%
  99. 5V -> 0%
  100. Args:
  101. clients (mqtt.Client): current mqtt client
  102. userdata (_type_): _description_
  103. message (mqtt.MQTTMessage): received message
  104. """
  105. logging.info("Received battery status request")
  106. try:
  107. batteryLevel = int(os.popen('sshpass -p maker ssh robot@ev3dev.local cat /sys/devices/platform/battery/power_supply/lego-ev3-battery/voltage_now').read())
  108. except:
  109. logging.error("Robot not connected")
  110. client.publish(Topics["ROBOT_DATA_ERROR"], "Robot not connected", qos=1)
  111. return
  112. batteryLevel = round(batteryLevel / 1000000, 2) # Voltage
  113. batteryLevel = batteryLevel - 5
  114. batteryLevel = round(batteryLevel / 3.5, 3) *100 # Percentage
  115. battery = {
  116. "Battery": batteryLevel
  117. }
  118. client.publish(Topics["ROBOT_DATA_BATTERY"], json.dumps(battery, indent=4), qos=1)
  119. #endregion