From 307e9fcb7c80a70f2e266740284b0f8cf8d07750 Mon Sep 17 00:00:00 2001
From: waldhauserlu78556 <waldhauserlu78556@th-nuernberg.de>
Date: Sat, 22 Apr 2023 14:34:46 +0200
Subject: [PATCH] Bugfixes and first drive test

---
 software/roboter/raspy/mainProg.py | 50 +++++++++++++++++-------------
 1 file changed, 29 insertions(+), 21 deletions(-)

diff --git a/software/roboter/raspy/mainProg.py b/software/roboter/raspy/mainProg.py
index 7c0c238..29cc48a 100644
--- a/software/roboter/raspy/mainProg.py
+++ b/software/roboter/raspy/mainProg.py
@@ -40,20 +40,22 @@ def measure_send_data(plantID, actionID):
     sensorData = sensors.readSensors()
     sensorData["Plant_ID"] = plantID
     sensorData["Action_ID"] = actionID
-    client.publish("Robot/Data/SensorData", json.dumps(sensorData, indent=4))
+    client.publish("ROBOT/DATA/SENSORDATA", json.dumps(sensorData, indent=4))
 
 
 def drive_home():
     '''Function to drive robot back to starting position in thread'''
-    thread = threading.Thread(target= os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {leftRight}'), daemon=True)
-    thread.start()
+    print("Robot driving home")
+    os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {leftRight}')
+    print("Robot home")
+    #TODO decide about robot occupied message
+    #client.publish("ROBOT/DATA/OCCUPIED", "false")
 
 
-def drive_plant():
+def drive_plant_thread():
     '''Function to drive to plant according to number from MQTT message in thread'''
-    print(f"Driving to Plant {plantNumber}")
     os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantNumber}.py')
-    print("Raising Signal")
+    print("Raising Signal to meassure")
     signal.raise_signal(signal.SIGUSR1)
 
 
@@ -69,16 +71,18 @@ def init_mqtt():
     client.message_callback_add("ROBOT/ACTION/GETPOSITION", get_position)
     client.message_callback_add("ROBOT/ACTION/GETBATTERY", get_BatteryStatus)
 
+    client.connect(mqttBroker)      #Has to be before subscribing
+
     #Subscribe to topics
     client.subscribe("ROBOT/DATA")      #Testing
     client.subscribe("ROBOT/ACTION/DRIVE")
     client.subscribe("ROBOT/Action/GETPOSITION")
     client.subscribe("ROBOT/ACTION/GETBATTERY")
 
-    client.connect(mqttBroker)
+    
 
 
-def signal_measure():
+def signal_measure(signum, frame):
     '''Signal function to measure one plant and start driving home'''
     print("Measuring Sensors")
     #measure_send_data()
@@ -97,17 +101,20 @@ def send_data_json(client, userdata, message):
 
 
 def drive_plant(clients, userdata, message):
-    '''Function to drive to plant according to request'''
-
-    print("received drive to plant")
-
+    '''Function to drive to plant according to request
+    Starting Drive in Thread'''
+    global plantNumber, leftRight
+    #TODO define MQTT message
     plantNumber = int(message.payload.decode("UTF-8"))
+
+    print(f"received drive to plant {plantNumber}")
+    
     if plantNumber % 2 == 0:
         leftRight = -50         #rotating left
     else:
         leftRight = 50          #rotating right
 
-    thread = threading.Thread(target= drive_plant, daemon=True)
+    thread = threading.Thread(target= drive_plant_thread, daemon=True)
     thread.start()
 
 
@@ -118,21 +125,17 @@ def get_position(clients, userdata, message):
     #[ ]TODO handle MQTT message
     gpsPosition["Position"] = sensors.readPosition()
     gpsPosition["Action_ID"] = message 
-    client.publish("Robot/Data/Position", json.dumps(gpsPosition, indent=4))
+    client.publish("ROBOT/DATA/POSITION", json.dumps(gpsPosition, indent=4))
 
 
 def get_BatteryStatus(clients, userdata, message):
-
     '''Callback function for battery status request
     Function to read battery status from ev3 and send via MQTT'''
 
     #[ ]TODO handle MQTT message
     batteryStatus["Battery"] = sensors.readBattery()
-    batteryStatus["Action ID"] = message
-    client.publish("Robot/Data/Battery", json.dumps(batteryStatus, indent=4))
-
-    batteryStatus()
-
+    batteryStatus["Action_ID"] = message
+    client.publish("ROBOT/DATA/BATTERY", json.dumps(batteryStatus, indent=4))
 
     
 #endregion
@@ -140,15 +143,20 @@ def get_BatteryStatus(clients, userdata, message):
 
 
 def main():
+    '''Initialises MQTT and Sensors
+    Runs forever and controlls all robot functions'''
+
     init_mqtt()
     print("MQTT initialized")
     signal.signal(signal.SIGUSR1, signal_measure)
     # dataDict = {}       #Testing
 
-    client.loop_forever()
+    client.loop_start()
 
     print("Starting Loop")
     while True:
+    #     print("Looping")
+    #     time.sleep(1)
         pass