#2、10、16进制转换部分代码来源deepseek，已验证准确可用
#https://chat.deepseek.com/a/chat/s/b41c3c2a-6f50-468e-9c2c-b2bd2fdf3548
#代码作用
#设定好阿克曼车的轮距、轴距、目标前进速度，右前轮角度（左转为正）
#计算车体的旋转角速度，16位整数转换为16进制数，用于向轮趣运动底盘发送车体控制命令
#目标前进速度、横向移动速度，同样可以用这里的函数把16位整数转换为16进制数

import math

PI=3.1415926535
H=0.322 #轴距 单位m
W=0.322 #轮距 单位m
Vx=-0.08 #目标前进速度 单位m/s
AngleR=22 #目标右前轮角度 单位 度


#整数转换为16位的二进制数
def int_to_16bit_binary(n):
    return format(n & 0xFFFF, '016b')

#二进制数转换为16进制数
def bin_to_hex(bin_str):
    return format(int(bin_str, 2), "04X")  # 输入需为16位二进制字符串

#计算车体旋转角速度，输入：轴距 轮距 前进速度 右前轮角度（左转为正）
def calc_vz(H, W, Vx, AngleR):	
	AngleR_in_rad=AngleR/180*PI #转换单位弧度
	turn_R=H/(math.tan(AngleR_in_rad))+0.5*W #计算转弯半径
	Vz=Vx/turn_R #计算车体对应旋转角速度 单位rad/s
	Vz=int(Vz*1000) #单位转换rad/s->0.001rad/s

	#整数转换为16位的二进制数
	Vz_in_binary=int_to_16bit_binary(Vz)

	#二进制数转换为16进制数
	Vz_in_hex=bin_to_hex(Vz_in_binary)

	print("AngleR: "+str(AngleR)+"°")
	print("turn_R: "+str(turn_R))
	print("Vz: "+str(Vz))
	print("Vz_in_binary: "+str(Vz_in_binary))
	print("Vz_in_hex: "+str(Vz_in_hex))
	print("\t")

calc_vz(H, W, Vx, 23)
calc_vz(H, W, Vx, 25)
calc_vz(H, W, Vx, 28)
#16进制数转换10进制正负数，输入：16进制数，要转换位数
#用于反向验证结果
def hex_to_signed_int(hex_str, bits=16):
    value = int(hex_str.strip().replace('0x', ''), 16)
    max_positive = (1 << (bits - 1)) - 1  # 如16位时为0x7FFF
    return value if value <= max_positive else value - (1 << bits)