SearhRescue2025/gpio_controller.py at main · DurhamAcademy/SearhRescue2025 · 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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
import atexit
from gpiozero import LED, Servo
import motor_driver

class GamepadGPIO:
    """
    Drives a GPIO pin HIGH while a chosen gamepad button is held, LOW otherwise.
    Uses gpiozero (Pi 5 friendly). If unavailable, becomes a no-op.
    """

    def __init__(self):

        self.mc = motor_driver.Motor()
        self.plow_servo = Servo(18)

        # button states
        self.button_map = {
            0: {'name': 'b',
                'state': False
                },
            1: {'name': 'a',
                'state': False
                },
            8: {'name': 'select',
                'state': False
                },
            9: {'name': 'start',
                'state': False
                },
            12: {'name': 'up',
                 'state': False
                 },
            13: {'name': 'down',
                 'state': False
                 },
            14: {'name': 'right',
                 'state': False
                 },
            15: {'name': 'left',
                 'state': False
                 }
        }

        self.light_pin = 25
        self.light_pin_output = LED(self.light_pin)

    def update_from_gamepad(self, buttons):
        # buttons will be a list of buttons:
        # [{'pressed': False, 'value': 0}, {'pressed': False, 'value': 0}, {'pressed': False, 'value': 0},
        #  {'pressed': False, 'value': 0}, {'pressed': False, 'value': 0}, {'pressed': False, 'value': 0},
        #  {'pressed': False, 'value': 0}, {'pressed': False, 'value': 0}, {'pressed': False, 'value': 0},
        #  {'pressed': False, 'value': 0}, {'pressed': False, 'value': 0}, {'pressed': False, 'value': 0},
        #  {'pressed': False, 'value': 0}, {'pressed': False, 'value': 0}, {'pressed': False, 'value': 0},
        #  {'pressed': False, 'value': 0}]

        for index, button in enumerate(buttons):
            # button will be: {'pressed': False, 'value': 0}
            # when pressed: {'pressed': True, 'value': 1}
            # index is the number in the list

            if index not in self.button_map:
                continue

            try:
                pressed = bool(buttons[index]["pressed"])
            except (IndexError, KeyError, TypeError):
                pressed = False

            if pressed != self.button_map[index]["state"]:
                self.button_map[index]["state"] = pressed

                if self.button_map[index]["name"] == "a":
                    if pressed:
                        print("A- Pressed")
                        self.plow_servo.max()

                if self.button_map[index]["name"] == "b":
                    if pressed:
                        print("B- Pressed")
                        self.plow_servo.mid()

                if self.button_map[index]["name"] == "up":
                    if pressed:
                        print("UP Pressed")
                        self.mc.drive_forward(speed=1.0)
                    else:
                        print("UP Released")
                        self.mc.drive_stop()

                if self.button_map[index]["name"] == "down":
                    if pressed:
                        print("Down Pressed")
                        self.mc.drive_reverse(speed=1.0)
                    else:
                        print("Down Released")
                        self.mc.drive_stop()

                if self.button_map[index]["name"] == "left":
                    if pressed:
                        print("Left Pressed")
                        self.mc.drive_turn_left(speed=0.5)
                    else:
                        print("Left Released")
                        self.mc.drive_stop()

                if self.button_map[index]["name"] == "right":
                    if pressed:
                        print("Right Pressed")
                        self.mc.drive_turn_right(speed=0.5)
                    else:
                        print("Right Released")
                        self.mc.drive_stop()


                print(f">>> GPIO {self.light_pin} -> {'HIGH' if pressed else 'LOW'}")

    def close(self):
        if self.light_pin_output:
            self.light_pin_output.off()
            self.mc.cleanup()