Browse Source

Bugfixes and first drive test

master
Luis Waldhauser 1 year ago
parent
commit
307e9fcb7c
1 changed files with 29 additions and 21 deletions
  1. 29
    21
      software/roboter/raspy/mainProg.py

+ 29
- 21
software/roboter/raspy/mainProg.py View File

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




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




def drive_plant(clients, 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")) 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()




#[ ]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
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 #endregion




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





Loading…
Cancel
Save