@Lauszus,

Okay, it seems alright then.

the code is correct, but the result is not correct.

But I really see no point of converting it to deg/s, unless you want a speedometer or something like that?

Because it really doesn't matter if just need it to make it balance.

But of course it's always nice to know what your robot is capable of

Is necessary to know the speed, because my control system are a LQR and pole placement.

These two control systems, can be applied to a dynamic system whose equations are written in the form of state.

In fact, after writing the dynamical system in the form of state (taking into account the masses, the moment of inertia, the motor inertia, the height of the robot, the radius of the wheels, the weight of the robot) I get the equations whose variables are precisely the state of the system. In my case they are

1) the position that the robot reaches (practically is a linear position, obtained by knowing the angular position multiplied by the radius, obtained in meters) = x1

2) the speed of the robot (ie, angular velocity multiplied by the radius obtained in m / s) = x2

3) the angular position (obtained in rad / s) = x3

4) the angular acceleration (obtained in rad / (s ^ 2)) = x4

Knowing these variables that represent the state vector x, I can provide a control action u =-k * x, where exploding are: u =-k1x1-k2x2-k3x3-k4x4,

then it is necessary that the values ??x1, x2, x3, x4 are in units of measurement right.

The various k (k1, k2, k3, k4) are obtained by means of the algorithms LQR, or pole placement, which can be applied directly from matlab

I hope I made you understand something