GamepadSTM32/controller.py at dev · IsaiahPaget/GamepadSTM32 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
import math

class Gamepad:

    def __init__(self, buttons, left_stick, right_stick):
        self.buttons = buttons
        self.vry, self.vrx = right_stick
        self.vly, self.vlx = left_stick

        # ring buf
        self.buf_len = 8 # lower value means more responsive, high value means less noisy
        self.lx_buf = [128] * self.buf_len
        self.ly_buf = [128] * self.buf_len
        self.rx_buf = [128] * self.buf_len
        self.ry_buf = [128] * self.buf_len
        self.buf_idx = 0

def _smooth(gamepad, buf, new_val):
    buf[gamepad.buf_idx % gamepad.buf_len] = new_val
    return sum(buf) // gamepad.buf_len

def read(gamepad):
    buttons = [
        not p.value() if p else False
        for p in gamepad.buttons
    ]
    lx, ly = _normalize_stick(
        _smooth(gamepad, gamepad.lx_buf, _map_adc_to_axis(gamepad.vlx.read())),
        _smooth(gamepad, gamepad.ly_buf, _map_adc_to_axis(gamepad.vly.read()))
    )
    rx, ry = _normalize_stick(
        _smooth(gamepad, gamepad.rx_buf, _map_adc_to_axis(gamepad.vrx.read())),
        _smooth(gamepad, gamepad.ry_buf, _map_adc_to_axis(gamepad.vry.read()))
    )
    gamepad.buf_idx += 1
    return buttons, lx, ly, rx, ry


def _normalize_stick(x, y):
    # shift from 0-255 to -127 to 127 so that centre is 0,0
    cx = x - 128
    cy = y - 128

    magnitude = math.sqrt(cx * cx + cy * cy)

    # circular clamp
    if magnitude > 127:
        cx = int(cx * 127 / magnitude)
        cy = int(cy * 127 / magnitude)

    # shift back to 0-255
    return cx + 128, cy + 128

def _make_report(x, y, z, rz, buttons):
    btn = 0
    for i, pressed in enumerate(buttons):
        if pressed:
            btn |= (1 << i)
    # btn is now a 12-bit number, split across 2 bytes
    btn_low  = btn & 0xFF         # buttons 1-8
    btn_high = (btn >> 8) & 0x0F  # buttons 9-12 (upper 4 bits are padding)
    return bytes([x, y, z, rz, btn_low, btn_high])

def _map_adc_to_axis(adc_value):
    return adc_value >> 4  # 12-bit to 8-bit


def get_report(gamepad):
    buttons, lx, ly, rx, ry = read(gamepad)
    return _make_report(lx, ly, rx, ry, buttons)