caliskanbi83990/projektarbeit_duelger_waldhauser_caliskan
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Drive all Test and bugfix

Browse Source
This commit is contained in:
Luis Waldhauser 2023-05-23 18:08:41 +02:00
parent 409c38a96d
commit 4cadbf9c29
4 changed files with 55 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
software/roboter/ev3/drive_arm.py Normal file
View File

@ -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)

24
software/roboter/ev3/drive_back_straight.py Normal file
View File

@ -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()

6
software/roboter/raspy/defines.py
View File

@ -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": ""

21
software/roboter/raspy/functions.py
View File

@ -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):