added servos_move and refactored a lot, made all commands remember the last servoe pos. Not tested yet