Added servo tracking. Placed defines and structs into .h