Check closed loop logic

[FrancoPalau]

The actual logic of the closed loop needs to be checked with the prototype. Specifically, the signs of the respective signal to the motors