fixed bugs found from testing, put all servo commands in one loop