Drive all Test and bugfix

software/roboter/ev3/drive_arm.py

@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+from ev3dev2.motor import Motor, OUTPUT_B, OUTPUT_C, SpeedPercent
+import sys
+
+motorUpDown = Motor(OUTPUT_C)
+motorLeftRight = Motor(OUTPUT_B)
+
+plantID = int(sys.argv[1])
+
+# Set direction of arm rotation back to middle
+if plantID % 2 == 0:
+    leftRight = 50     # rotating right
+else:
+    leftRight = -50      # rotating left
+
+motorUpDown.on_for_seconds(SpeedPercent(-50), seconds=2.8)
+motorLeftRight.on_for_degrees(SpeedPercent(leftRight), 400)

software/roboter/ev3/drive_back_straight.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python3
+from ev3dev2.motor import LargeMotor, OUTPUT_A, OUTPUT_D, SpeedPercent
+from ev3dev2.sensor.lego import TouchSensor, UltrasonicSensor  
+from ev3dev2.sensor import INPUT_1, INPUT_4
+
+motorLeft = LargeMotor(OUTPUT_D)
+motorRight = LargeMotor(OUTPUT_A)
+sensorUltraSonic = UltrasonicSensor(INPUT_4)
+sensorTouch = TouchSensor(INPUT_1)
+
+motorLeft.on(SpeedPercent(-70), block=False)
+motorRight.on(SpeedPercent(-70))
+
+while True:
+    if sensorUltraSonic.distance_centimeters < 10:
+        motorLeft.on(SpeedPercent(-25), block=False)
+        motorRight.on(SpeedPercent(-25))
+        break
+
+
+    
+sensorTouch.wait_for_pressed(timeout_ms=None, sleep_ms=10)
+motorLeft.off()
+motorRight.off()

software/roboter/raspy/defines.py

@@ -23,6 +23,7 @@ Topics = {
     "ROBOT_DATA_ERROR": "ROBOT/DATA/ERROR",
     "ROBOT_DATA_ROBOTREADY": "ROBOT/DATA/ROBOTREADY",
     "ROBOT_DATA_PICTURE": "ROBOT/DATA/PICTURE",
+    "ROBOT_DATA_ALL": "ROBOT/DATA/ALL",
 
     "BACKEND_ACTION_DRIVE": "BACKEND/ACTION/DRIVE",
     "BACKEND_ACTION_DRIVEPALL": "BACKEND/ACTION/DRIVEALL",
@@ -58,6 +59,11 @@ SENSORDATA = {
     "ActionID": ""
 }
 
+ALLSENSORDATA = {
+    "SensorData": [],
+    "ActionID": ""
+}
+
 # TODO When sensor is available
 POSITION = {
     "Position": ""

software/roboter/raspy/functions.py

@@ -4,7 +4,7 @@ import threading
 import os
 import logging
 import raspy_sensors as Sensors
-from defines import Topics, SENSORDATA, ALLPLANTDATA
+from defines import Topics, SENSORDATA, ALLSENSORDATA
 
 def measure_send_data(plantID, actionID, client: mqtt.Client):
     """
@@ -72,8 +72,7 @@ def drive_plant_thread(plantID, actionID, client: mqtt.Client):
     client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
 
 
-# TODO thread all plants
-def drive_plant_all(plantIDs: list, actionID, client: mqtt.Client):
+def drive_plant_all_thread(plantIDs: list, actionID, client: mqtt.Client):
     """
     Function to drive to plants according to numbers from MQTT message in thread
     Meassure and publish data for all plants via MQTT
@@ -85,14 +84,14 @@ def drive_plant_all(plantIDs: list, actionID, client: mqtt.Client):
         client (mqtt.Client): current MQTT client
     """
 
-    allPlantData = ALLPLANTDATA
+    allPlantData = ALLSENSORDATA
     allPlantData["ActionID"] = actionID
+    logging.debug(f"plants {plantIDs}, type {type(plantIDs)}")
 
-    # TODO loop through plantID array
     for plant in plantIDs:
          # FIXME Only possible with color codes
+        logging.info(f"Robot Driving to plant {plant}")
         errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plant}')
-
         if errorCode != 0:
             logging.error(f"Robot Error {errorCode} occurred")
             logging.info("Drive Plant aborted, Robot at starting position")
@@ -114,23 +113,19 @@ def drive_plant_all(plantIDs: list, actionID, client: mqtt.Client):
         logging.info("Taking Picture")
         sendPicture(client)
 
-        # TODO Test Drive arm back to middle
         errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}')
-
         if errorCode != 0:
             logging.error(f"Robot Error {errorCode} occurred")
             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_ERROR"], f"Rotate arm aborted, Robot at plant {plant}", qos=1)
             client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1)
             return
 
     logging.info("All Plants measured, sending data")
-    client.publish(Topics["ROBOT_DATA_DRIVEALL"], json.dumps(allPlantData, indent=4), qos=1)
+    client.publish(Topics["ROBOT_DATA_ALL"], json.dumps(allPlantData, indent=4), qos=1)
 
-    # TODO Test Drive back straight without arm
     logging.info("Robot driving home")
     errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py')
-
     if errorCode != 0:
         logging.error(f"Robot Error {errorCode} occurred")
         logging.info(f"Drive Home aborted, Robot at plant {plant}")
@@ -186,7 +181,7 @@ def drive_plant_all(client: mqtt.Client, userdata, message: mqtt.MQTTMessage):
     logging.info(f"Received Drive-request to plants {plantIDs}, ActionID: {actionID}")
     client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "False", qos=1)
     
-    thread = threading.Thread(target= drive_plant_all, args=(plantIDs, actionID, client), daemon=True)
+    thread = threading.Thread(target= drive_plant_all_thread, args=(plantIDs, actionID, client), daemon=True)
     thread.start()
 
 def get_position(clients: mqtt.Client, userdata, message: mqtt.MQTTMessage):