Cleaned code, began adding PID output display