|
|
|
|
|
|
|
|
""" |
|
|
""" |
|
|
Reads all sensors and publishes data via MQTT in form of SENSORDATA |
|
|
Reads all sensors and publishes data via MQTT in form of SENSORDATA |
|
|
|
|
|
|
|
|
|
|
|
***Function is only neccessary for driving without color codes*** |
|
|
|
|
|
|
|
|
Args: |
|
|
Args: |
|
|
plantID (_type_): plant to measure |
|
|
plantID (_type_): plant to measure |
|
|
actionID (_type_): current ID of driving action |
|
|
actionID (_type_): current ID of driving action |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/plant_{plantID}.py') |
|
|
|
|
|
|
|
|
|
|
|
# Error handling for drive operation |
|
|
|
|
|
# Abort when drive operation at start when error occurs |
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
if errorCode == 65280: |
|
|
if errorCode == 65280: |
|
|
errorMessage = "EV3 not connected" |
|
|
errorMessage = "EV3 not connected" |
|
|
|
|
|
|
|
|
logging.info(f"{errorMessage}, Drive Plant aborted, Robot at starting position") |
|
|
logging.info(f"{errorMessage}, Drive Plant aborted, Robot at starting position") |
|
|
client.publish(Topics["ROBOT_DATA_ERROR"], f"{errorMessage}, Drive Plant aborted, Robot at starting position", qos=1) |
|
|
client.publish(Topics["ROBOT_DATA_ERROR"], f"{errorMessage}, Drive Plant aborted, Robot at starting position", qos=1) |
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) |
|
|
client.publish(Topics["ROBOT_DATA_ROBOTREADY"], "True", qos=1) |
|
|
|
|
|
|
|
|
return |
|
|
return |
|
|
|
|
|
|
|
|
logging.info("Measuring Sensors") |
|
|
logging.info("Measuring Sensors") |
|
|
measure_send_data(plantID, actionID, client) |
|
|
measure_send_data(plantID, actionID, client) |
|
|
|
|
|
|
|
|
# TODO How to Handle Pictures and PlantID |
|
|
|
|
|
logging.info("Taking Picture") |
|
|
|
|
|
sendPicture(client) |
|
|
|
|
|
|
|
|
# Taking Picture not fully implemented |
|
|
|
|
|
# logging.info("Taking Picture") |
|
|
|
|
|
# sendPicture(client) |
|
|
|
|
|
|
|
|
logging.info("Robot driving home") |
|
|
logging.info("Robot driving home") |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back.py {plantID}') |
|
|
|
|
|
|
|
|
|
|
|
# Error handling for drive operation |
|
|
|
|
|
# Abort when drive operation at start when error occurs |
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
if errorCode == 65280: |
|
|
if errorCode == 65280: |
|
|
errorMessage = "EV3 not connected" |
|
|
errorMessage = "EV3 not connected" |
|
|
|
|
|
|
|
|
allPlantData["ActionID"] = actionID |
|
|
allPlantData["ActionID"] = actionID |
|
|
|
|
|
|
|
|
for plant in plantIDs: |
|
|
for plant in plantIDs: |
|
|
# FIXME Only possible with color codes |
|
|
|
|
|
logging.info(f"Robot Driving to plant {plant}") |
|
|
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}') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_plant.py {plant}') |
|
|
|
|
|
|
|
|
|
|
|
# Error handling for drive operation |
|
|
|
|
|
# Abort when drive operation at start when error occurs |
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
if errorCode == 65280: |
|
|
if errorCode == 65280: |
|
|
errorMessage = "EV3 not connected" |
|
|
errorMessage = "EV3 not connected" |
|
|
|
|
|
|
|
|
allPlantData["SensorData"].append(sensordata) |
|
|
allPlantData["SensorData"].append(sensordata) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# TODO How to Handle Pictures and PlantID |
|
|
|
|
|
logging.info("Taking Picture") |
|
|
|
|
|
sendPicture(client) |
|
|
|
|
|
|
|
|
# Taking Picture not fully implemented |
|
|
|
|
|
# logging.info("Taking Picture") |
|
|
|
|
|
# sendPicture(client) |
|
|
|
|
|
|
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_arm.py {plant}') |
|
|
|
|
|
# Error handling for drive operation |
|
|
|
|
|
# Abort when drive operation at start when error occurs |
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
if errorCode == 65280: |
|
|
if errorCode == 65280: |
|
|
errorMessage = "EV3 not connected" |
|
|
errorMessage = "EV3 not connected" |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
logging.info("Robot driving home") |
|
|
logging.info("Robot driving home") |
|
|
|
|
|
# Error handling for drive operation |
|
|
|
|
|
# Abort when drive operation at start when error occurs |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py') |
|
|
errorCode = os.system(f'sshpass -p maker ssh robot@ev3dev.local python3 /home/robot/Programme/drive_back_straight.py') |
|
|
if errorCode != 0: |
|
|
if errorCode != 0: |
|
|
if errorCode == 65280: |
|
|
if errorCode == 65280: |
|
|
|
|
|
|
|
|
thread.start() |
|
|
thread.start() |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# FIXME Only possible with color codes |
|
|
|
|
|
def drive_plant_all(client: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
def drive_plant_all(client: mqtt.Client, userdata, message: mqtt.MQTTMessage): |
|
|
""" |
|
|
""" |
|
|
Function to drive to plants according to request |
|
|
Function to drive to plants according to request |
|
|
|
|
|
|
|
|
""" |
|
|
""" |
|
|
logging.info("Received position request") |
|
|
logging.info("Received position request") |
|
|
|
|
|
|
|
|
# TODO Write GPS Sensor Function |
|
|
|
|
|
position = { |
|
|
position = { |
|
|
"Position": "" |
|
|
"Position": "" |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|
""" |
|
|
""" |
|
|
Callback function for battery status request |
|
|
Callback function for battery status request |
|
|
Function to read battery status from EV3 and send via MQTT in form of BATTERY |
|
|
Function to read battery status from EV3 and send via MQTT in form of BATTERY |
|
|
|
|
|
Current battery level is stored in "voltage_now" file |
|
|
Reading battery level via SSH script execution |
|
|
Reading battery level via SSH script execution |
|
|
Battery level shown in Volts: |
|
|
Battery level shown in Volts: |
|
|
8,5V -> 100% |
|
|
8,5V -> 100% |