Modified PID and constants
Partial tuning complete. Many bugs found and fixed.