This should help a lot. It explains a lot about holonomic drives and it literally provides the exact code.