Added Software Serial
Rename servo variables for easy reading
Some tweaks