50 lines
1.6 KiB
Python
50 lines
1.6 KiB
Python
'''
|
|
Author: WZX 17839623189@168.com
|
|
Date: 2023-12-03 17:23:03
|
|
LastEditors: WZX 17839623189@168.com
|
|
LastEditTime: 2023-12-03 17:35:15
|
|
FilePath: /wzx/zkmetaapi/ZKMetaUnit/utils/gyro.py
|
|
Description:
|
|
|
|
Copyright (c) 2023 by LLM, All Rights Reserved.
|
|
'''
|
|
class Gyro:
|
|
def __init__(self, yaw_start=0):
|
|
self.yaw_start = yaw_start
|
|
self.yaw_current = yaw_start
|
|
self.left_turn_angle = 0
|
|
self.right_turn_angle = 0
|
|
|
|
def update(self, yaw):
|
|
# 计算当前角度与起始角度的差值
|
|
# 左转
|
|
if self.yaw_current > 170 and yaw < 0:
|
|
delta_yaw = yaw+360 - self.yaw_current
|
|
self.left_turn_angle += delta_yaw
|
|
# 右转
|
|
elif self.yaw_current < -170 and yaw >0:
|
|
delta_yaw = 360 - (yaw - self.yaw_current)
|
|
self.right_turn_angle += delta_yaw
|
|
# 左转
|
|
elif self.yaw_current < yaw:
|
|
delta_yaw = yaw - self.yaw_current
|
|
self.left_turn_angle += delta_yaw
|
|
# 右转
|
|
elif self.yaw_current >= yaw:
|
|
delta_yaw = - (yaw - self.yaw_current)
|
|
self.right_turn_angle += delta_yaw
|
|
|
|
# 更新当前角度为新的角度
|
|
self.yaw_current = yaw
|
|
|
|
def get_left_turn_angle(self):
|
|
return self.left_turn_angle
|
|
|
|
def get_right_turn_angle(self):
|
|
return self.right_turn_angle
|
|
|
|
def get_corr_left_turn_angle(self):
|
|
return self.left_turn_angle - self.right_turn_angle
|
|
|
|
def get_corr_right_turn_angle(self):
|
|
return self.right_turn_angle - self.left_turn_angle |