Update firmware.ino

Added cleaning servos function to avoid problems with servos on startup.
Added last rotation of 90 degrees to keep the servos on the zero position when are detached.