Fixed some sign issues, tuned PID as much as possible