Working code for gimbal control