Servo functionality extension

- Add SetSpeed function
- Custom timer channel support
- Little refactor
- Fix bugs