获取机器人指南针的舵转角

Question

有一个角度为1-360度(或0-359度)的电子罗盘。

https://i.stack.imgur.com/Q7RmH.jpg

有变数

• Yaw -指南针读数degrees
• HDG -建立方向舵degrees
• rudder -转动方向盘的度(需要得到)

变量检查

HDG:20偏航:340=需要获得rudder=40

HDG:50偏航:90=需要舵=-40

得到旋转角度的公式是什么？

问题是要在正确的方向上找到最短的转弯。

这看起来很简单，但却让我心碎

就像这样

​

if (Yaw<=180) {
rudder=set HDG-Yaw;
} else {
rudder=360-Yaw+setHDG;
}

​

但是如果setHDG是350而Yaw=20是.给出330，应该是30

Answer 1

if (setHDG>=Yaw) 
      rudder=360-(360-setHDG+Yaw);
else 
      rudder=setHDG-Yaw;