usb-rc/test_3channel.py at master · colzilla/usb-rc · 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
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
import board
import digitalio
import pulseio
from adafruit_hid.gamepad import Gamepad
from time import sleep
import sys


startpause = 3
print("usb-rc - an adapter from RC car receiver to USB-gamepad/joystick. https://github.com/colzilla/usb-rc")
print("starting in {} sec...".format(startpause))
sleep(startpause)

debug=False
shush=False

#create the gampepad object
gp = Gamepad()

#setup the pulse readers
ch1 = pulseio.PulseIn(board.A3, 5)
ch2 = pulseio.PulseIn(board.A4, 5)
ch3 = pulseio.PulseIn(board.A2, 5)

#used in auto-calibration
ch1min = 1499
ch1max = 1501
ch2min = 1499
ch2max = 1501
ch3min = 1499
ch3max = 1501
allowed_min = 800
allowed_max = 2200
loop=0

def range_map(x, in_min, in_max, out_min, out_max):
    joy_value =  (x - in_min) * (out_max - out_min) // (in_max - in_min) + out_min
    return joy_value

while True:
    if(debug): print("\nloop[{}]".format(loop))
    loop += 1

    if len(ch1) > 0:
        ch1in = ch1[0]
        if(debug): print("ch1 was ", ch1in)
    else:
        if(debug): print("ch1 was empty")
        ch1in = 1500

    if len(ch2) > 0:
        ch2in = ch2[0]
        if(debug): print("ch2 was ", ch2in)
    else:
        if(debug): print("ch2 was empty")
        ch2in = 1500

    if len(ch3) > 0:
        ch3in = ch3[0]
        if(debug): print("ch3 was ", ch3in)
    else:
        if(debug): print("ch3 was empty")
        ch3in = 1500

#    and xin > allowed_min and xin < allowed_max and yin > allowed_min and yin < allowed_max:
    try:
        if ch1in < ch1min: ch1min = ch1in
        if ch1in > ch1max and ch1in < 2200: ch1max = ch1in
        if ch2in < ch2min: ch2min = ch2in
        if ch2in > ch2max and ch2in < 2200: ch2max = ch2in
        if ch3in < ch3min: ch3min = ch3in
        if ch3in > ch3max and ch3in < 2200: ch3max = ch3in

        x=range_map(ch1in, ch1min, ch1max, -127, 127)
        y=range_map(ch2in, ch2min, ch2max, -127, 127) * -1
        z=range_map(ch3in, ch3min, ch3max, -127, 127) * -1

        gp.move_joysticks(x, y, z)
        if(debug): print("ch1min", ch1min, "ch1in", ch1in, "ch1max", ch1max,
                         "ch2min", ch2min, "ch2in", ch2in, "ch2max", ch2max,
                         "ch3min", ch3min, "ch3in", ch3in, "ch3max", ch3max,
                         "joy_x", x, "joy_y", y, "joy_z", z)
        if(not shush): print("x", x, ", y", y, ", z", z)
        sleep(0.005)
    except Exception as e:
        if(debug): print("ax", ch1in, "ay", ch2in, "az", ch3in, e)
        pass

    # if(debug): sleep(0.5)