Basic controller, doesn't quite work. Need better gains