Implement constant acceleration, compiles, not working in sim