Added PI (Feedback Control) functionality [Tested]
TBD: PID Tuning...
Also resetting the timers and all temporary counters for each run