Simple Gimbal control

proof of concept for integrating gimbal control
Some debugging lines to prove the output