Servo test functionality
- Test class Servo_teset
- Fixed pin assignment of servos in config.h
- Increased window size
- Debugging method in Servo_wrapper to set the servo position
- reset() jumps into the bootloader for easy flashing