Pid control

Implementation of the new I and D gain for the motor control.

- [x] Implement I and D gain in cross-compilation
- [ ] Test with real robot
- [ ] Implement for remote-control
- [ ] Eventually test on real robot

It would be great if the cross-compilation can be tested on the real robot (when someone has time). Warning for test: Too high value of D gain is susceptible to cause some instabilities.