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 = sensors.readSensors()
sensorData["Plant_ID"] = plantID sensorData["Plant_ID"] = plantID
sensorData["Action_ID"] = actionID 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(): def drive_home():
'''Function to drive robot back to starting position in thread''' '''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) print("Robot driving home")
thread.start() 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''' '''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') 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) 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/GETPOSITION", get_position)
client.message_callback_add("ROBOT/ACTION/GETBATTERY", get_BatteryStatus) client.message_callback_add("ROBOT/ACTION/GETBATTERY", get_BatteryStatus)
client.connect(mqttBroker) #Has to be before subscribing
#Subscribe to topics #Subscribe to topics
client.subscribe("ROBOT/DATA") #Testing client.subscribe("ROBOT/DATA") #Testing
client.subscribe("ROBOT/ACTION/DRIVE") client.subscribe("ROBOT/ACTION/DRIVE")
client.subscribe("ROBOT/Action/GETPOSITION") client.subscribe("ROBOT/Action/GETPOSITION")
client.subscribe("ROBOT/ACTION/GETBATTERY") 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''' '''Signal function to measure one plant and start driving home'''
print("Measuring Sensors") print("Measuring Sensors")
#measure_send_data() #measure_send_data()
@ -97,17 +101,20 @@ def send_data_json(client, userdata, message):
def drive_plant(clients, userdata, message): def drive_plant(clients, userdata, message):
'''Function to drive to plant according to request''' '''Function to drive to plant according to request
Starting Drive in Thread'''
print("received drive to plant") global plantNumber, leftRight
#TODO define MQTT message
plantNumber = int(message.payload.decode("UTF-8")) plantNumber = int(message.payload.decode("UTF-8"))
print(f"received drive to plant {plantNumber}")
if plantNumber % 2 == 0: if plantNumber % 2 == 0:
leftRight = -50 #rotating left leftRight = -50 #rotating left
else: else:
leftRight = 50 #rotating right leftRight = 50 #rotating right
thread = threading.Thread(target= drive_plant, daemon=True) thread = threading.Thread(target= drive_plant_thread, daemon=True)
thread.start() thread.start()
@ -118,21 +125,17 @@ def get_position(clients, userdata, message):
#[ ]TODO handle MQTT message #[ ]TODO handle MQTT message
gpsPosition["Position"] = sensors.readPosition() gpsPosition["Position"] = sensors.readPosition()
gpsPosition["Action_ID"] = message 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): def get_BatteryStatus(clients, userdata, message):
'''Callback function for battery status request '''Callback function for battery status request
Function to read battery status from ev3 and send via MQTT''' Function to read battery status from ev3 and send via MQTT'''
#[ ]TODO handle MQTT message #[ ]TODO handle MQTT message
batteryStatus["Battery"] = sensors.readBattery() batteryStatus["Battery"] = sensors.readBattery()
batteryStatus["Action ID"] = message batteryStatus["Action_ID"] = message
client.publish("Robot/Data/Battery", json.dumps(batteryStatus, indent=4)) client.publish("ROBOT/DATA/BATTERY", json.dumps(batteryStatus, indent=4))
batteryStatus()
#endregion #endregion
@ -140,15 +143,20 @@ def get_BatteryStatus(clients, userdata, message):
def main(): def main():
'''Initialises MQTT and Sensors
Runs forever and controlls all robot functions'''
init_mqtt() init_mqtt()
print("MQTT initialized") print("MQTT initialized")
signal.signal(signal.SIGUSR1, signal_measure) signal.signal(signal.SIGUSR1, signal_measure)
# dataDict = {} #Testing # dataDict = {} #Testing
client.loop_forever() client.loop_start()
print("Starting Loop") print("Starting Loop")
while True: while True:
# print("Looping")
# time.sleep(1)
pass pass