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.5KB

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