92 lines
3.9 KiB
Python
92 lines
3.9 KiB
Python
# Left Stick Calibration
|
|
stick_cal_left = [0xBA, 0xF5, 0x62, 0x6F, 0xC8, 0x77, 0xED, 0x95, 0x5B]
|
|
# Right Stick Calibration
|
|
stick_cal_right = [0x16, 0xD8, 0x7D, 0xF2, 0xB5, 0x5F, 0x86, 0x65, 0x5E]
|
|
data_left = [0] * 6
|
|
data_right = [0] * 6
|
|
|
|
# Left stick uint16 conversion
|
|
data_left[0] = (stick_cal_left[1] << 8) & 0xF00 | stick_cal_left[0]
|
|
data_left[1] = (stick_cal_left[2] << 4) | (stick_cal_left[1] >> 4)
|
|
data_left[2] = (stick_cal_left[4] << 8) & 0xF00 | stick_cal_left[3]
|
|
data_left[3] = (stick_cal_left[5] << 4) | (stick_cal_left[4] >> 4)
|
|
data_left[4] = (stick_cal_left[7] << 8) & 0xF00 | stick_cal_left[6]
|
|
data_left[5] = (stick_cal_left[8] << 4) | (stick_cal_left[7] >> 4)
|
|
|
|
# Right stick uint16 conversion
|
|
data_right[0] = (stick_cal_right[1] << 8) & 0xF00 | stick_cal_right[0]
|
|
data_right[1] = (stick_cal_right[2] << 4) | (stick_cal_right[1] >> 4)
|
|
data_right[2] = (stick_cal_right[4] << 8) & 0xF00 | stick_cal_right[3]
|
|
data_right[3] = (stick_cal_right[5] << 4) | (stick_cal_right[4] >> 4)
|
|
data_right[4] = (stick_cal_right[7] << 8) & 0xF00 | stick_cal_right[6]
|
|
data_right[5] = (stick_cal_right[8] << 4) | (stick_cal_right[7] >> 4)
|
|
|
|
# Left Stick Decode
|
|
left_center_x = data_left[2]
|
|
left_center_y = data_left[3]
|
|
left_x_min = (left_center_x - data_left[0]) - left_center_x
|
|
left_x_max = (left_center_x + data_left[4]) - left_center_x
|
|
left_y_min = (left_center_y - data_left[1]) - left_center_y
|
|
left_y_max = (left_center_y + data_left[5]) - left_center_y
|
|
|
|
print("Left Stick Values:")
|
|
print("~~~~~~~~~~~~~~~~~~")
|
|
print("Left Center X and Y:", left_center_x, left_center_y)
|
|
print("Left X Min/Max: ", left_x_min, "", left_x_max)
|
|
print("Left Y Min/Max: ", left_y_min, "", left_y_max)
|
|
|
|
# Right Stick Decode
|
|
right_center_x = data_right[0]
|
|
right_center_y = data_right[1]
|
|
right_x_min = (right_center_x - data_right[2]) - right_center_x
|
|
right_x_max = (right_center_x + data_right[4]) - right_center_x
|
|
right_y_min = (right_center_y - data_right[3]) - right_center_y
|
|
right_y_max = (right_center_y + data_right[5]) - right_center_y
|
|
|
|
print("\nRight Stick Values:")
|
|
print("~~~~~~~~~~~~~~~~~~~")
|
|
print("Right Center X and Y:", right_center_x, right_center_y)
|
|
print("Right X Min/Max: ", right_x_min, "", right_x_max)
|
|
print("Right Y Min/Max: ", right_y_min, "", right_y_max)
|
|
|
|
# Sample Stick Data Conversion:
|
|
stick_data = [0xB3, 0x32, 0x6C]
|
|
stick_horizontal = stick_data[0] | ((stick_data[1] & 0xF) << 8)
|
|
stick_vertical = (stick_data[1] >> 4) | (stick_data[2] << 4)
|
|
|
|
print("\nExample Left Stick Data to Ratio Conversion:")
|
|
print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
|
|
print("Raw X/Y Uint16 Values:", stick_horizontal, stick_vertical)
|
|
ratio_x = abs((stick_horizontal - left_center_x)) / (left_x_min - left_center_x)
|
|
ratio_y = (stick_vertical - left_center_y) / (left_y_min - left_center_y)
|
|
print("Relative X/Y Values", ratio_x, ratio_y)
|
|
|
|
print("\nExample Left Stick Ratio to Data Conversion:")
|
|
print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
|
|
if ratio_x < 0:
|
|
data_x_converted = (
|
|
abs(ratio_x) * (left_x_min - left_center_x) + left_center_x)
|
|
else:
|
|
data_x_converted = (
|
|
abs(ratio_x) * (left_x_max - left_center_x) + left_center_x)
|
|
data_x_converted = int(round(data_x_converted))
|
|
|
|
if ratio_y < 0:
|
|
data_y_converted = (
|
|
abs(ratio_y) * (left_y_min - left_center_y) + left_center_y)
|
|
else:
|
|
data_y_converted = (
|
|
abs(ratio_y) * (left_y_max - left_center_y) + left_center_y)
|
|
data_y_converted = int(round(data_y_converted))
|
|
print("X/Y Converted Values:", data_x_converted, data_y_converted)
|
|
|
|
# Converting the two X/Y uint16 values to 3 uint8 Little Endian values
|
|
converted_values = [
|
|
# Get the last two hex digits
|
|
hex(data_x_converted & 0xFF),
|
|
# Combine the last digit of the Y uint16 and the first digit
|
|
# of the X uint16
|
|
hex(((data_y_converted & 0xF) << 4) + (data_x_converted >> 8)),
|
|
# Get the first two digits of the Y uint16
|
|
hex(data_y_converted >> 4)]
|
|
print("Uint8 Converted Values:", converted_values)
|