Update controller.cpp

Also implemented the PID Controller :) 
Chihi yours was missing the time in the calculation