Switched to a very basic logic, with predictable results, to be able to debug the motion controller accurately and start building a logic strategy step by step.