iframe-proxy | Sunbelt Computer Software

146 lines (113 loc) · 4.01 KB
from lib.pipeline import Observer, UARTSource, Packetizer, PacketProcessor, StdOutObserver
from lib.orbhid import OrbHid
from lib.sensitivity import SensitivityAdjustment
from lib.chording import ChordingAdjustment
Packet_lengths_spaceorb = {
    'D': 13,
    'K': 6,
    '.': 4,
    'C': 4,
    'F': 4,
    'M': 5,
    'N': 4,
    'P': 6,
    '\v': 1,
    '\n': 1,
    '\r': 1,
    '@': 62,
    'E': 5,
    'Z': 14,
    'd': 26,
def process_spaceorb_buttondata(buf):
    buffer[2] now has data in the form 1<rezero><F><E><D><C><B><A>
    return dict(buttons=buf[2] & 63,
                reset=(buf[2] & 64) != 0)
def process_spaceorb_balldata(buf):
    D<button status><D1><D2><D3><D4><D5><D6><D7><D8><D9><xor>
    from elsewhere:
    <D1> to <D9> are data bytes containing the force and torque components.
    These values are generated by first packing the six 10 bit values then
    xoring them with the characters "SpaceWare". Packed data has the form:
    1XXX XXXX 1XXX YYYY 1YYY YYYZ 1ZZZ ZZZZ1ZZx
    xxxx 1xxx xxyy 1yyy yyyy 1yzz zzzz1zzz z000
    where XXXXXXXXXX , YYYYYYYYYY , ZZZZZZZZZZ are the components of force.
    xxxxxxxxxx , yyyyyyyyyy , zzzzzzzzzz are the components of torque.
    Note the "at rest" packet, the packet with all force and torque
    components at zero, will contain the word "SpaceWare".
    s = bytearray('SpaceWare')
    for i in range(9):
        buf[i+2] ^= s[i]
    # now process the button character
    buttons = buf[1] & 63
    # now process the ball components
    def mshl(b, mask, shift):
        return (int(b) & mask) << shift
    def mshr(b, mask, shift):
        return (int(b) & mask) >> shift
    def resign(i):
        # we have signed 10-bit integers; need to fix that
        schar = 0x0200
        smask = 0xfe00
        if (i & schar):
            return (i & (~schar))-512
        return i
    axes = [0]*6
    axes[0] = resign(mshl(buf[2], 0x7f, 3) | mshr(buf[3], 0x70, 4))
    axes[1] = resign(mshl(buf[3], 0x0f, 6) | mshr(buf[4], 0x7e, 1))
    axes[2] = resign(mshl(buf[4], 0x01, 9) | mshl(buf[5], 0x7f, 2))
    axes[3] = resign(mshl(buf[6], 0x1f, 5) | mshr(buf[7], 0x7c, 2))
    axes[4] = resign(mshl(buf[7], 0x03, 8) | mshl(buf[8], 0x7f, 1))
    axes[5] = resign(mshl(buf[9], 0x3f, 4) | mshr(buf[10], 0x78, 3))
    return dict(buttons=buttons, axes=axes)
Packet_processors_spaceorb = {
    'K': process_spaceorb_buttondata,
    'D': process_spaceorb_balldata
def connect_pipeline(stages):
    pairs = [(stages[i], stages[i+1]) for i in range(len(stages)-1)]
    for pair in pairs:
        pair[0].attach(pair[1])
    return stages
class HidReporterObserver(Observer):
    def __init__(self):
        self.orb = OrbHid()
    def receive(self, msg):
        if 'buttons' in msg:
            self.orb._buttons_state = msg['buttons']
        if 'axes' in msg:
            self.orb._joy_x = msg['axes'][0] + 512
            self.orb._joy_y = msg['axes'][1] + 512
            self.orb._joy_z = msg['axes'][2] + 512
            self.orb._joy_rx = msg['axes'][3] + 512
            self.orb._joy_ry = msg['axes'][4] + 512
            self.orb._joy_rz = msg['axes'][5] + 512
        self.orb._send()
class MemObserver:
    def __init__(self):
        pass
    def receive(self, msg):
        print("Mem: {0}".format(gc.mem_free()))
def main():
    pipeline = connect_pipeline([
        UARTSource(),
        Packetizer(Packet_lengths_spaceorb),
        PacketProcessor(Packet_processors_spaceorb),
        ChordingAdjustment(),
        SensitivityAdjustment(lambda x: int(x**3 / 512**2))
    pipeline[2].attach(StdOutObserver("Processed Packet"))
    pipeline[3].attach(StdOutObserver("Adjusted Packet"))
    pipeline[3].attach(MemObserver())
    pipeline[3].attach(HidReporterObserver())
    while True:
        pipeline[0].tick()
if __name__ == "__main__":
Sunbelt Computer Software

PL/B Language Development and Support

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Sunbelt Computer Software

PL/B Language Development and Support

Uh oh!

FilesExpand file tree

code.py

Latest commit

History

code.py

File metadata and controls
