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

Bugfixes and first drive test

Browse Source
This commit is contained in:
Luis Waldhauser 2023-04-22 14:34:46 +02:00
parent 6adc5a3426
commit 307e9fcb7c
1 changed files with 29 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
software/roboter/raspy/mainProg.py
View File

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