Working servos and elevator

Signed-off-by: Nikita 'webconn' Maslov <wkernelteam@gmail.com>