From 9c222b10990d70b062be2b8c6dca0158a6d9b308 Mon Sep 17 00:00:00 2001
From: spirkelmann <simon.pirkelmann@gmail.com>
Date: Fri, 28 Jun 2019 10:28:51 +0200
Subject: [PATCH] fixed problems with simultaneous inputs not being handled and
 gamepads not detected

---
 remote_control/class_control_joystick.py | 113 +++++++++++++----------
 1 file changed, 66 insertions(+), 47 deletions(-)

diff --git a/remote_control/class_control_joystick.py b/remote_control/class_control_joystick.py
index 4cfc5d5..77d6a0f 100644
--- a/remote_control/class_control_joystick.py
+++ b/remote_control/class_control_joystick.py
@@ -13,15 +13,34 @@ class robot:  # we have 4 arg for this class, because joysticks get the same (va
         self.robot0_stopped_3 = True
         self.robot0_stopped_4 = True
         self.rc_socket = socket.socket()
+        self.u1 = 0
+        self.u2 = 0
         try:
             self.rc_socket.connect((self.ip, self.port))
         except socket.error():
             print("couldn't connect to socket")
 
+        self.check_joystick = False
     def joystick_init(self):  # Joystick's initialisation
-        self.joystick = pygame.joystick.Joystick(self.joy)
-        self.joystick.init()
-        self.axes = self.joystick.get_numaxes()
+        joystick_count = pygame.joystick.get_count()
+        for count in range(joystick_count):
+            if joystick_count == 2:
+                self.joystick = pygame.joystick.Joystick(self.joy)
+                print("{}-->joystick count".format(joystick_count))
+                self.joystick.init()
+                self.check_joystick = True
+            elif joystick_count == 1:
+                joystick = pygame.joystick.Joystick(0)
+                joystick.init()
+                print("connected only 1 joystick - {}".format(joystick))
+                self.check_joystick = False
+            elif not joystick_count:
+                print("no joysticks connected")
+                self.check_joystick = False
+
+
+
+
 
     def control(self, event):  # the control of two robots with joysticks
         joy = event.joy
@@ -74,54 +93,55 @@ class robot:  # we have 4 arg for this class, because joysticks get the same (va
                     self.robot0_stopped_4 = True
 
     def control_keyboard(self, event):  # keyboard control for robot1
-        command_received = False
-        if event.key == pygame.K_LEFT:
+
+        if pressed[pygame.K_LEFT]:
             u1 = -1.0
             u2 = 1.0
-            command_received = True
-        elif event.key == pygame.K_RIGHT:
-            u1 = 1.0
-            u2 = -1.0
-            command_received = True
-        elif event.key == pygame.K_UP:
-            u1 = -1.0
-            u2 = -1.0
-            command_received = True
-        elif event.key == pygame.K_DOWN:
-            u1 = 1.0
-            u2 = 1.0
-            command_received = True
-        if command_received:
             self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
-    def control_keyboard_2(self, event):  # keyboard control for robot2
-
-        command_received = False
-        if event.key == pygame.K_a:
-            u1 = -1.0
-            u2 = 1.0
-            command_received = True
-        elif event.key == pygame.K_d:
+        elif pressed[pygame.K_RIGHT]:
             u1 = 1.0
             u2 = -1.0
-            command_received = True
-        elif event.key == pygame.K_w:
-            u1 = -1.0
-            u2 = -1.0
-            command_received = True
-        elif event.key == pygame.K_s:
-            u1 = 1.0
-            u2 = 1.0
-            command_received = True
-        if command_received:
             self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
-    def control_keyboard_stop(self):  # stop for both robot
+        elif pressed[pygame.K_UP]:
+            u1 = -1.0
+            u2 = -1.0
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+
+        elif pressed[pygame.K_DOWN]:
+            u1 = 1.0
+            u2 = 1.0
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+
+        elif event.type == pygame.KEYUP:
+            self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode())
+
+    def control_keyboard_2(self, event):  # keyboard control for robot1
+
+        if pressed[pygame.K_a]:
+            u1 = -1.0
+            u2 = 1.0
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+
+        elif pressed[pygame.K_d]:
+            u1 = 1.0
+            u2 = -1.0
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+
+        elif pressed[pygame.K_w]:
+            u1 = -1.0
+            u2 = -1.0
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+
+        elif pressed[pygame.K_s]:
+            u1 = 1.0
+            u2 = 1.0
+            self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
+
+        elif event.type == pygame.KEYUP:
+            self.rc_socket.send('({},{})\n'.format(self.u1, self.u2).encode())
 
-        u1 = 0
-        u2 = 0
-        print("key released, resetting: ({},{})".format(u1, u2))
-        self.rc_socket.send('({},{})\n'.format(u1, u2).encode())
 
 def main():
     pygame.init()
@@ -131,19 +151,18 @@ def main():
     robot_1.joystick_init()
     robot_2 = robot(1, '192.168.1.103', 1234)
     robot_2.joystick_init()
-
     while True:
         events = pygame.event.get()
+        global pressed
+        pressed = pygame.key.get_pressed()
         for event in events:
             if event.type == pygame.JOYAXISMOTION:
                 robot_1.control(event)
                 robot_2.control(event)
-            elif event.type == pygame.KEYDOWN:
+            else:
                 robot_1.control_keyboard(event)
                 robot_2.control_keyboard_2(event)
-            elif event.type == pygame.KEYUP:
-                robot_1.control_keyboard_stop()
-                robot_2.control_keyboard_stop()
+
 
 
 if __name__ == '__main__':