Moved functions around for readability
Started adding a debugging display for more PID