Algorithm 计算圆周运动中的角度变化

我正在尝试推导一种算法，它允许我计算圆周运动中的角位移。这是一个物理设备，我正在编程，所以我有从0-2pi当前角度位置的反馈。我附上了一张照片，将帮助你看到我试图实现的目标（见下文）。每次处理器扫描时，程序都会更新标记，然后通过取θ2-θ1计算δθ，然后将其存储到δθ标记中。问题是，当我完成一次旋转时，θ2变成0，θ1仍然是3pi/2左右，因此我得到了一个比我应该得到的更大的δθ。每次角度达到2π标记时，都会重置为零。我正在使用这个程序在罗克韦尔CompactLogix PLC上实现一个傅里叶分析算法。对于你们中足够关心的人，我正在使用“结构化文本”语言用RSLogix5000编程。如果需要，请访问此网站以获取有关该语言的手册。 请参见下面的示例代码

ThetaOld = Theta;
Theta = SpindleActPosition; //This is feedback from the motor 0-2*pi
DeltaTheta = Theta2 - Theta1; //This is the "algorithm" I need help with

我也尝试过这个算法，但发现它的循环漏洞不允许它工作

DeltaTheta = [(Theta2 - Theta1) + 2*pi] MOD(2*pi)

我相信MOD操作员将是我们在这方面的朋友，但我无法找到解决问题的方法

代码的其余部分实际上与解决此问题无关。不幸的是，由于这台机器的性质，角度从12点钟位置开始，然后顺时针旋转。我不能改变这个

您只需添加：

IF DeltaTheta < 0 THEN
    DeltaTheta = DeltaTheta + 2*pi;
END_IF;

如果DeltaTheta<0，则
德尔塔赫塔=德尔塔赫塔+2*pi；
结束(如有)；

保持DeltaTheta=Theta2-Theta1，然后实现此条件？是。只需将此添加到原来的3行。