Mostly have trajectory controller working