Added support for reading speed and tacho count
Added new motor test program