First try at implementing a PID controller