From 4cadbf9c293e712ee0058ea2a47170aa74611c64 Mon Sep 17 00:00:00 2001 From: waldhauserlu78556 Date: Tue, 23 May 2023 18:08:41 +0200 Subject: [PATCH] Drive all Test and bugfix --- software/roboter/ev3/drive_arm.py | 17 +++++++++++++++ software/roboter/ev3/drive_back_straight.py | 24 +++++++++++++++++++++ software/roboter/raspy/defines.py | 6 ++++++ software/roboter/raspy/functions.py | 21 +++++++----------- 4 files changed, 55 insertions(+), 13 deletions(-) create mode 100644 software/roboter/ev3/drive_arm.py create mode 100644 software/roboter/ev3/drive_back_straight.py diff --git a/software/roboter/ev3/drive_arm.py b/software/roboter/ev3/drive_arm.py new file mode 100644 index 0000000..d3955a7 --- /dev/null +++ b/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) diff --git a/software/roboter/ev3/drive_back_straight.py b/software/roboter/ev3/drive_back_straight.py new file mode 100644 index 0000000..a23b75c --- /dev/null +++ b/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() \ No newline at end of file diff --git a/software/roboter/raspy/defines.py b/software/roboter/raspy/defines.py index 1a69b4e..a002e9a 100644 --- a/software/roboter/raspy/defines.py +++ b/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": "" diff --git a/software/roboter/raspy/functions.py b/software/roboter/raspy/functions.py index 14b1468..1656382 100644 --- a/software/roboter/raspy/functions.py +++ b/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):