Implemented PID algorithm

Some testing done and results can be seen in PID test.ods