Motor control section first draft

- Also some reorganization