Fixed estop trigger bug

Fixed having to press estop button twice while at a floor