Have working code. Need to play with the pid stuff.