Still don't think you need to check/enable gripper_cuff_control every half second - why not just once. The re-cal is a good idea, but no need to check cuff_control running?