Update motor controller (handle nonlinearity in motor control and others)

[izsoandras]

The motors have been identified with the Hammerstein-Wiener model, but the implementation have been done with a simple PID. A new controller shall be designed and implemented together with the handling of the input and output nonlinearity.

TODO:

• check identificaiont whether it uses SI units, if not fix it (and re-identify)
  • collect the ratio between wheel rotation and encoder -> count pulses and rotate the wheel around many times (so the fact that the rotation is not precise has less effect)
• design PID controller for the linear part of the system
• test the design in Simulink
• implement input and output nonlinearity in STM
• test the results on the robot
• adjust if needed
• make controller capable of being disabled
• move matlab folder apart from the STM source files
• document work