Branch data Line data Source code
1 : : #include "joystick.hpp"
2 : :
3 : 0 : Joystick::Joystick(CarMove& car) : gameController(nullptr), remote(nullptr), car_ref(&car) {
4 : 0 : if (SDL_Init(SDL_INIT_GAMECONTROLLER) < 0) {
5 : 0 : throw std::runtime_error("Failed to initialize SDL2 GameController: " +
6 : 0 : std::string(SDL_GetError()));
7 : : }
8 : 0 : remote = new RemoteMove(car, "tcp://*:5555");
9 : :
10 : 0 : for (int i = 0; i < SDL_NumJoysticks(); ++i) {
11 : 0 : if (SDL_IsGameController(i) != 0U) {
12 : 0 : gameController = SDL_GameControllerOpen(i);
13 : 0 : if (gameController != nullptr) {
14 : 0 : setAxisMapping(car);
15 : : std::cout << "Game controller connected: " << SDL_GameControllerName(gameController)
16 : 0 : << '\n';
17 : 0 : setButtonAction(1, Actions{[this, &car]() {
18 : 0 : std::cout << "Autonomous driving on\n";
19 : 0 : if (remote) {
20 : 0 : remote->start();
21 : : }
22 : 0 : },
23 : 0 : []() {}});
24 : :
25 : 0 : setButtonAction(3, Actions{[this, &car]() {
26 : 0 : std::cout << "Autonomous driving out\n";
27 : 0 : if (remote) {
28 : 0 : remote->stop();
29 : : }
30 : 0 : },
31 : 0 : [&car]() {
32 : : std::cout
33 : 0 : << "Button 3 released - Returning to neutral\n";
34 : 0 : car.setServoAngle(0);
35 : 0 : }});
36 : :
37 : 0 : printButtonStates();
38 : 0 : break;
39 : : }
40 : : }
41 : : }
42 : 0 : }
43 : :
44 : 0 : Joystick::~Joystick() {
45 : 0 : delete remote;
46 : 0 : if (gameController != nullptr) {
47 : 0 : SDL_GameControllerClose(gameController);
48 : 0 : gameController = nullptr;
49 : : }
50 : 0 : SDL_Quit();
51 : 0 : }
52 : :
53 : 0 : void Joystick::setButtonAction(int button, Actions actions) {
54 : 0 : buttonActions[button] = std::move(actions);
55 : 0 : }
56 : :
57 : 0 : void Joystick::setAxisAction(int axis, std::function<void(int)> action) {
58 : 0 : axisActions[axis] = std::move(action);
59 : 0 : }
60 : :
61 : 0 : void Joystick::setAxisMapping(CarMove& car) {
62 : 0 : setAxisAction(0, [&car](int value) {
63 : 0 : float fvalue = value * 1.0;
64 : 0 : fvalue = fvalue / 32000.0 * 55;
65 : 0 : car.setServoAngle(fvalue);
66 : 0 : });
67 : :
68 : 0 : setAxisAction(5, [&car](int value) {
69 : 0 : value -= 16319;
70 : 0 : value = (value / 165) * -1;
71 : 0 : car.setMotorSpeed(value);
72 : 0 : });
73 : 0 : }
74 : :
75 : 0 : void Joystick::processEvent(const SDL_Event& event) {
76 : 0 : if (event.type == SDL_CONTROLLERBUTTONDOWN || event.type == SDL_CONTROLLERBUTTONUP) {
77 : 0 : bool is_pressed = (event.type == SDL_CONTROLLERBUTTONDOWN);
78 : 0 : int button = event.cbutton.button;
79 : :
80 : 0 : if (static_cast<std::size_t>(button) < buttonStates.size()) {
81 : 0 : buttonStates[button] = is_pressed;
82 : 0 : if (buttonActions.find(button) != buttonActions.end()) {
83 : 0 : if (is_pressed) {
84 : 0 : buttonActions[button].onPress();
85 : : } else {
86 : 0 : buttonActions[button].onRelease();
87 : : }
88 : : }
89 : : }
90 : 0 : } else if (event.type == SDL_CONTROLLERAXISMOTION) {
91 : 0 : int axis = event.caxis.axis;
92 : 0 : int value = event.caxis.value;
93 : :
94 : 0 : if (axisActions.find(axis) != axisActions.end()) {
95 : 0 : axisActions[axis](value);
96 : : }
97 : 0 : } else if (event.type == SDL_CONTROLLERDEVICEREMOVED) {
98 : 0 : std::cout << "Game controller disconnected." << '\n';
99 : :
100 : 0 : if (gameController != nullptr) {
101 : 0 : SDL_GameControllerClose(gameController);
102 : 0 : gameController = nullptr;
103 : : }
104 : :
105 : 0 : std::cout << "Exiting the program due to controller disconnection." << '\n';
106 : 0 : exit(1);
107 : : }
108 : 0 : }
109 : :
110 : 0 : void Joystick::listen() {
111 : : SDL_Event event;
112 : : while (true) {
113 : 0 : while (SDL_PollEvent(&event) != 0) {
114 : 0 : processEvent(event);
115 : : }
116 : : // Original exit conditions
117 : 0 : if (buttonStates[4] && buttonStates[6]) {
118 : 0 : if (car_ref) {
119 : 0 : car_ref->setMotorSpeed(0);
120 : 0 : car_ref->setServoAngle(0);
121 : : }
122 : 0 : if (remote) {
123 : 0 : remote->stop();
124 : 0 : delete remote;
125 : 0 : remote = nullptr;
126 : : }
127 : 0 : if (gameController) {
128 : 0 : SDL_GameControllerClose(gameController);
129 : 0 : gameController = nullptr;
130 : : }
131 : 0 : SDL_Quit();
132 : 0 : std::cout << "All resources cleaned up. Exiting.\n";
133 : 0 : std::exit(EXIT_SUCCESS);
134 : : }
135 : 0 : if (gameController == nullptr) {
136 : 0 : std::cout << "No controller connected, stopping listen.\n";
137 : 0 : break;
138 : : }
139 : 0 : SDL_Delay(10);
140 : : }
141 : 0 : }
142 : :
143 : 0 : void Joystick::printButtonStates() {
144 : 0 : std::cout << "======= CONTROLLER BUTTON SETUP =======" << '\n';
145 : 0 : std::cout << "Button 1: Autonomous driving ON" << '\n';
146 : 0 : std::cout << "Button 3: Autonomous driving OFF" << '\n';
147 : 0 : std::cout << "Buttons 4+6 together: Exit program" << '\n';
148 : 0 : std::cout << "=====================================" << '\n';
149 : :
150 : 0 : std::cout << "Current button states:" << '\n';
151 : 0 : for (size_t i = 0; i < buttonStates.size(); ++i) {
152 : 0 : if (buttonStates[i]) {
153 : 0 : std::cout << "Button " << i << ": PRESSED" << '\n';
154 : : }
155 : : }
156 : 0 : }
|
