Safety commit.
First PID Controller for cpp version.