Replace floating point numbers with integers

[stenbergd]

Since integer operations are less expensive, we should try to replace alot of our float values with ints. This goes for:

• Sensor readings (rad/s could me millirad/s)
• Forces/torques
• Control reference signals

possibly more.