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 4.3KB

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