Implemented kI + rough PID tuning

Pretty good, I'd say.