Very messy and could use some cleanup, but works (SERVO)