Added proper conversion from angular wheel speeds to PWM.
Testing with hardware (not stable yet).