Make robot connections over 2 acceptable

Author: JeongJun-Lee

multi-robots_test.py

@@ -0,0 +1,76 @@
+from neopia import *
+
+socos = (Neosoco(0), Neosoco(1))
+
+# wait until two robots are ready
+wait_until_ready()
+
+def init():
+  for soco in socos:
+    soco.servo_reset_degree() # servo moto init
+
+def ready_condition(soco):
+  print(soco.get_value('in3'))
+  return soco.get_value('in3') < 30
+
+def act_condition(soco):
+  print(soco.get_value('in3'))
+  return soco.get_value('in3') > 70 # when recognize a clap
+
+def led_blink(soco):
+  soco.led_on('out3')
+  wait(500)
+  soco.led_off('out3')
+  wait(500)
+
+def wag_tail(soco):
+  soco.servo_rotate_by_degree('out1', 'forward', '100', '60')
+  soco.servo_rotate_by_degree('out1', 'backward', '100', '60')
+  soco.servo_rotate_by_degree('out1', 'forward', '100', '0')
+
+def move_forth_back(soco, reverse=False):
+  if reverse:
+    dir1 = 'backward'
+    dir2 = 'forward' 
+  else:
+    dir1 = 'forward'
+    dir2 = 'backward'
+
+  soco.motor_move(dir1)
+  wait(1000)
+  soco.motor_move(dir2)
+  wait(1000)
+  soco.motor_stop('both')
+
+def turn_right_left(soco, reverse=False):
+  if reverse:
+    wheel1 = 'right'
+    wheel2 = 'left'
+  else:
+    wheel1 = 'left'
+    wheel2 = 'right'
+    
+  soco.motor_rotate(wheel1, 'forward', '30') # turn right
+  soco.motor_rotate(wheel2, 'backward', '30')
+  wait(2000)
+  soco.motor_rotate(wheel2, 'forward', '30') # turn left
+  soco.motor_rotate(wheel1, 'backward', '30')
+  wait(2000)
+  soco.motor_stop('both')
+
+def dance(soco):
+  move_forth_back(soco)
+  move_forth_back(soco, True)
+  turn_right_left(soco)
+  turn_right_left(soco, True)
+
+  soco.motor_move('forward')
+  wait(1000)
+  soco.motor_stop('both')
+
+init()
+for soco in socos:
+  while_do(ready_condition, led_blink, soco)
+  while_do(ready_condition, wag_tail, soco)
+  when_do(act_condition, dance, soco)
+wait(-1) # wait forever

neopia/neosoco.py

@@ -333,7 +333,7 @@ def _init(self, port_name):
         self._add_neobot(self._neobot)
         Runner.register_robot(self)
         Runner.start()
-        self._neobot._init(port_name)
+        self._neobot._init(port_name, Runner.get_robots())
 
     def find_device_by_id(self, device_id):
         return self._neobot.find_device_by_id(device_id)
@@ -599,6 +599,7 @@ def motor_stop(self, which_motor: str):
         else:
             Runner.shutdown() # The motor will not stop unless the shutdown() function is used
             raise TypeError
+        Runner.wait(100) # Since broadcast from controller is per 100ms
 
     def _convert_sacle_within_100(self, value, cvt_max_val):
         # Map to 0~limited_val from 0~100(max), it's same as Entry

neopia/neosoco_neobot.py

@@ -118,7 +118,7 @@ def _run(self):
         except:
             pass
 
-    def _init(self, port_name=None):
+    def _init(self, port_name=None, reg_neobots=None):
         Runner.register_required()
         self._running = True
         thread = threading.Thread(target=self._run)
@@ -128,7 +128,7 @@ def _init(self, port_name=None):
 
         tag = "Neosoco[{}]".format(self._index)
         self._connector = SerialConnector(tag, NeosocoConnectionChecker(self))
-        result = self._connector.open(port_name)
+        result = self._connector.open(port_name, reg_neobots)
         if result == Result.FOUND:
             while self._ready == False and self._is_disposed() == False:
                 time.sleep(0.01)

neopia/runner.py

@@ -165,6 +165,10 @@ def shutdown():
     def register_robot(robot):
         Runner._added.append(robot)
 
+    @staticmethod
+    def get_robots():
+        return Runner._robots
+
     @staticmethod
     def unregister_robot(robot):
         Runner._removed.append(robot)

neopia/serial_connector.py

@@ -28,7 +28,7 @@
 BAUD_RATE = 115200
 VALID_PACKET_LENGTH = 8
 RETRY = 10
-TIMEOUT = 0.4
+TIMEOUT = 0.5
 START_BYTES = bytearray([0xab, 0xcd])
 
 class SerialConnector(object):
@@ -43,17 +43,26 @@ def __init__(self, tag, connection_checker, loader=None):
         self._timestamp = 0
         self._connected = False
 
-    def open(self, port_name=None):
+    def open(self, port_name=None, reg_robots=None):
         if port_name:
             result = self._open_port(port_name)
             if result != Result.NOT_AVAILABLE:
                 return result
         else:
+            # Get all using ports
+            if reg_robots:
+                reg_ports = []
+                [reg_ports.append(robot._neobot._connector._port_name) for robot in reg_robots]
+            # Get all ports on the computer
             ports = serial.tools.list_ports.comports()
+
             for port in ports:
-                result = self._open_port(port[0])
-                if result != Result.NOT_AVAILABLE:
-                    return result
+                if reg_ports and (port[0] in reg_ports):
+                    continue
+                else:
+                    result = self._open_port(port[0])
+                    if result != Result.NOT_AVAILABLE:
+                        return result
         self._print_error("No available USB to BLE bridge")
         return Result.NOT_AVAILABLE
 
@@ -120,7 +129,7 @@ def _read_line(self, serial, start_byte=None):
                     bufferBytes = serial.inWaiting()
                     if bufferBytes:
                         line = c + serial.read(bufferBytes)
-                        print('Recv: ', line.hex()) # For debug, temporary
+                        # print('Recv: ', line.hex()) # For debug, temporary
                         if line[:2] == start_byte: 
                             return line[:VALID_PACKET_LENGTH]
                 else:
@@ -143,7 +152,7 @@ def write(self, packet):
         if self._serial:
             try:
                 self._serial.write(bytes.fromhex(packet))
-                print('Sent: ', bytes.fromhex(packet)) # For debug, temporary
+                # print('Sent: ', bytes.fromhex(packet)) # For debug, temporary
             except:
                 pass
 

setup.py

@@ -2,9 +2,9 @@
 
 setup(
 	name="neopia",
-	version="0.1.4",
+	version="0.1.5",
 	author="RoboticsWare",
-	author_email="neopia.uz@google.ocm",
+	author_email="neopia.uz@google.com",
 	description="Python library for Neopia neobot",
 	url="https://github.com/roboticsware/pylib_neobot.git",
 	download_url="https://github.com/roboticsware/pylib_neobot/archive/refs/heads/master.zip",