Got basic winch working, FF still needs to be tuned and PID is totally untuned.