Functional version of basic dynamics code (does not account for motor inertia yet)
What issues does this fix?