Winch sensor

@camilo86 please confirm that this is the tested code  that is currently on the robot. If it is I'll merge it into master.