TG_Robot_Code/My_Custom_Code.py at master · Danny-Dasilva/TG_Robot_Code · 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


from app.Robot import Controller, Py_Hat, Check_Input
from app.Autonomous import Autonomous
# import os
# os.system("sudo pkill -9 python")
from time import sleep


def my_custom_autonomous(hat):
    auto = Autonomous(hat)

    # Takes a value and time

    auto.forward(.8, 2)

    auto.backward(1, 2)

    auto.turn_left(.5, 2)

    auto.turn_right(.5, 2)

    auto.stop()


def my_custom_teleop():
    print("custom code")
    #controller class
    controller = Controller()

    # initialize Pi Hat
    hat = Py_Hat(address=96)

    while True:

        controller.event_get()
        # setup controls
        leftstick = controller.set_axis('leftstick')
        rightstick = controller.set_axis('rightstick')
        LT = controller.set_axis('LT')
        # button example - note set_button vs set_axis
        A = controller.set_button('A')

        # Button press to run Autonomous
        if LT > .75:
            my_custom_autonomous(hat)

        # drivetrain examples
        hat.motor(0, leftstick)
        hat.motor(1, -rightstick)
        # sleep for smooth loops
        sleep(.02)


if __name__ == "__main__":
    import os
    os.system("sudo pkill -9 -f RobotCode.py")

    my_custom_teleop()