Bug fixes, added PID log file, and retuned gains