编码器过零点（359-360/0-1）角度解缠绕问题

使用编码器，磁编码器给出角度时在跨越360°时会有个问题，

Preview	Current	Distance
350	2	+12
4	358	-6

• 这两个角度实际可能是车辆转向起始角度和目标角度，可能是机械臂当前位置和目标位置，前者需要计算出最小转向角度，后者需要计算出两次采样角度和目标值差多少然后给pid用，而且这两个计算出来的值最好带方向，例如车的情境里得知道往哪个方向转

• 你会发现用两个角度没法直接的给出角度差，直接相减便是2-350=-348，358-4=354，这种大数肯定不是我们想要的，转348°肯定不如转12°，我们更想要上表的distance那样的数据

• 不上答案和长篇大论，要理解如何解决这个问题可以换个角度：

  • 因为0-360这个表示方式天生不支持表示跨越360/0°，
  • 所以可以假设360°/0°这里有个挡板无法通过，就好像一个虚拟的单圈音量旋钮，那么我们无法跨越360°位置，我们只能从pre反方向旋转很大一圈来达到current值来实现
  • 350->2 便350-300-100-2的顺序到达，等于反方向走了348
  • 4->358 等于正着354°，
  • 从数字上看从pre到cur的距离可以分为实际距离（短）和绕远距离（长），而这两者相加正好等于一圈360，而这两个距离中我们一般要的是实际距离

• 所以如果我们计算出是长的距离，一般来说是两次采样差的绝对值大于180，便对应的加减360，差值>180就-360，<180就+360，多退少补 来拒去留嘛

def angle_unwrapping(current_angle, prev_angle):
    """
    处理角度循环跳变
    current_angle: 当前角度 [0, 360)
    prev_angle: 上一时刻的连续角度
    return: 连续角度
    """
    # 计算原始差值
    delta = current_angle - (prev_angle % 360)
    
    # 处理跳变
    if delta > 180:
        delta -= 360
    elif delta < -180:
        delta += 360
    
    # 返回连续角度
    return prev_angle + delta

# 使用示例
prev_angle = 359.0
current_angle = 1.0  # 实际传感器读数，发生了359→1的跳变
continuous_angle = angle_unwrapping(current_angle, prev_angle)
# continuous_angle = 361.0，而不是1.0