PTCS Update 4

ptcs/ptcs_bridge/bridge2.py

@@ -1,20 +1,33 @@
 import asyncio
 
 from .train_base import TrainBase
+from .wire_pole_client import WirePoleClient
 
 
 class Bridge2:
     trains: dict[str, TrainBase]
+    obstacles: dict[str, WirePoleClient]
 
     def __init__(self) -> None:
         self.trains = {}
+        self.obstacles = {}
 
     def add_train(self, train: TrainBase) -> None:
         assert train.id not in self.trains
         self.trains[train.id] = train
 
+    def add_obstacle(self, obstacle: WirePoleClient) -> None:
+        assert obstacle.id not in self.obstacles
+        self.obstacles[obstacle.id] = obstacle
+
     async def connect_all(self) -> None:
-        await asyncio.gather(*(train.connect() for train in self.trains.values()))
+        await asyncio.gather(
+            *(train.connect() for train in self.trains.values()),
+            *(obstacle.connect() for obstacle in self.obstacles.values()),
+        )
 
     async def disconnect_all(self) -> None:
-        await asyncio.gather(*(train.disconnect() for train in self.trains.values()))
+        await asyncio.gather(
+            *(train.disconnect() for train in self.trains.values()),
+            *(obstacle.disconnect() for obstacle in self.obstacles.values()),
+        )

ptcs/ptcs_bridge/train_base.py

@@ -13,7 +13,10 @@ async def connect(self) -> None:
     async def disconnect(self) -> None:
         raise NotImplementedError()
 
-    async def send_speed(self, speed: int) -> None:
+    async def send_speed(self, speed: float) -> None:
+        raise NotImplementedError()
+
+    async def send_motor_input(self, motor_input: int) -> None:
         raise NotImplementedError()
 
     async def start_notify_position_id(self, callback: NotifyPositionIdCallback) -> None:

ptcs/ptcs_bridge/train_client.py

@@ -7,7 +7,7 @@
 
 from .train_base import NotifyPositionIdCallback, NotifyRotationCallback, TrainBase
 
-SERVICE_UUID = UUID("63cb613b-6562-4aa5-b602-030f103834a4")
+SERVICE_TRAIN_UUID = UUID("63cb613b-6562-4aa5-b602-030f103834a4")
 CHARACTERISTIC_MOTOR_INPUT_UUID = UUID("88c9d9ae-bd53-4ab3-9f42-b3547575a743")
 CHARACTERISTIC_POSITION_ID_UUID = UUID("8bcd68d5-78ca-c1c3-d1ba-96d527ce8968")
 CHARACTERISTIC_ROTATION_UUID = UUID("aab17457-2755-8b50-caa1-432ff553d533")
@@ -35,13 +35,13 @@ async def disconnect(self) -> None:
         await self._client.disconnect()
         logger.info("%s disconnected", self)
 
-    def _get_service(self) -> BleakGATTService:
-        service = self._client.services.get_service(SERVICE_UUID)
+    def _get_service_train(self) -> BleakGATTService:
+        service = self._client.services.get_service(SERVICE_TRAIN_UUID)
         assert service is not None
         return service
 
     def _get_characteristic(self, uuid: UUID) -> BleakGATTCharacteristic:
-        service = self._get_service()
+        service = self._get_service_train()
         characteristic = service.get_characteristic(uuid)
         assert characteristic is not None
         return characteristic
@@ -55,11 +55,12 @@ def _get_characteristic_position_id(self) -> BleakGATTCharacteristic:
     def _get_characteristic_rotation(self) -> BleakGATTCharacteristic:
         return self._get_characteristic(CHARACTERISTIC_ROTATION_UUID)
 
-    async def send_speed(self, speed: int) -> None:
-        assert 0 <= speed <= 255
-        characteristic_speed = self._get_characteristic_motor_input()
-        await self._client.write_gatt_char(characteristic_speed, bytes([speed]))
-        logger.info("%s send speed %s", self, speed)
+    async def send_motor_input(self, motor_input: int) -> None:
+        assert isinstance(motor_input, int)
+        assert 0 <= motor_input <= 255
+        characteristic = self._get_characteristic_motor_input()
+        await self._client.write_gatt_char(characteristic, bytes([motor_input]))
+        logger.info("%s send motor input %s", self, motor_input)
 
     async def start_notify_position_id(self, callback: NotifyPositionIdCallback) -> None:
         def wrapped_callback(_characteristic: BleakGATTCharacteristic, data: bytearray):
@@ -68,8 +69,8 @@ def wrapped_callback(_characteristic: BleakGATTCharacteristic, data: bytearray):
             logger.info("%s notify position id %s", self, position_id)
             callback(self, position_id)
 
-        characteristic_position_id = self._get_characteristic_position_id()
-        await self._client.start_notify(characteristic_position_id, wrapped_callback)
+        characteristic = self._get_characteristic_position_id()
+        await self._client.start_notify(characteristic, wrapped_callback)
 
     async def start_notify_rotation(self, callback: NotifyRotationCallback) -> None:
         def wrapped_callback(_characteristic: BleakGATTCharacteristic, data: bytearray):
@@ -78,5 +79,5 @@ def wrapped_callback(_characteristic: BleakGATTCharacteristic, data: bytearray):
             logger.info("%s notify rotation %s", self, 1)
             callback(self, 1)
 
-        characteristic_rotation = self._get_characteristic_rotation()
-        await self._client.start_notify(characteristic_rotation, wrapped_callback)
+        characteristic = self._get_characteristic_rotation()
+        await self._client.start_notify(characteristic, wrapped_callback)

ptcs/ptcs_bridge/train_simulator.py

@@ -71,8 +71,7 @@ async def disconnect(self) -> None:
         self._task = None
         logger.info("%s disconnected", self)
 
-    async def send_speed(self, speed: int) -> None:
-        assert 0 <= speed <= 255
+    async def send_speed(self, speed: float) -> None:
         self._target_speed_cm_s = speed * self.INPUT_TO_CENTIMETERS_PER_SECOND
         logger.info("%s send speed %s", self, speed)
 

ptcs/ptcs_bridge/wire_pole_client.py

@@ -0,0 +1,49 @@
+import logging
+from typing import Callable
+from uuid import UUID
+
+from bleak import BleakClient
+from bleak.backends.characteristic import BleakGATTCharacteristic
+
+NotifyCollapseCallback = Callable[["WirePoleClient", bool], None]
+
+
+SERVICE_WIRE_POLE_UUID = UUID("62dd9b52-2995-7978-82e2-6abf1ae56555")
+CHARACTERISTIC_COLLAPSE_UUID = UUID("79fe0b5c-754c-3fe0-941f-3dc191cf09bf")
+
+
+logger = logging.getLogger(__name__)
+
+
+class WirePoleClient:
+    id: str
+    _client: BleakClient
+
+    def __init__(self, id: str, address: str) -> None:
+        self.id = id
+        self._client = BleakClient(address)
+
+    def __str__(self) -> str:
+        return f"WirePoleClient({self.id}, {self._client.address})"
+
+    async def connect(self) -> None:
+        await self._client.connect()
+        logger.info("%s connected", self)
+
+    async def disconnect(self) -> None:
+        await self._client.disconnect()
+        logger.info("%s disconnected", self)
+
+    async def start_notify_collapse(self, callback: NotifyCollapseCallback) -> None:
+        def wrapped_callback(_characteristic: BleakGATTCharacteristic, data: bytearray):
+            assert len(data) == 1
+            is_collapsed = bool(data[0])
+            logger.info("%s notify collapse %s", self, is_collapsed)
+            callback(self, is_collapsed)
+
+        service = self._client.services.get_service(SERVICE_WIRE_POLE_UUID)
+        assert service is not None
+        characteristic = service.get_characteristic(CHARACTERISTIC_COLLAPSE_UUID)
+        assert characteristic is not None
+
+        await self._client.start_notify(characteristic, wrapped_callback)

ptcs/ptcs_control/mft2023.py

@@ -13,6 +13,7 @@
     OUTER_CURVE_RAIL,
     STRAIGHT_1_2_RAIL,
     STRAIGHT_1_4_RAIL,
+    STRAIGHT_1_6_RAIL,
     STRAIGHT_RAIL,
     WATARI_RAIL_A,
     WATARI_RAIL_B,
@@ -194,55 +195,35 @@ def create_control(logger: logging.Logger | None = None) -> Control:
     stop_0 = Stop(
         id="stop_0",
         position=DirectedPosition(
-            section=s0,
+            section=s3,
             target_junction=j1,
-            mileage=WATARI_RAIL_B * 1 + STRAIGHT_RAIL * 4.5,
+            mileage=WATARI_RAIL_A + STRAIGHT_RAIL - STRAIGHT_1_6_RAIL,
         ),
     )
     stop_1 = Stop(
         id="stop_1",
-        position=DirectedPosition(
-            section=s0,
-            target_junction=j1,
-            mileage=WATARI_RAIL_B * 1 + STRAIGHT_RAIL * 10.0 + CURVE_RAIL * 8 + STRAIGHT_1_4_RAIL * 1,
-        ),
-    )
-    stop_2 = Stop(
-        id="stop_2",
-        position=DirectedPosition(
-            section=s1,
-            target_junction=j1,
-            mileage=WATARI_RAIL_B * 1 + STRAIGHT_RAIL * 1.5,
-        ),
-    )
-    stop_3 = Stop(
-        id="stop_3",
-        position=DirectedPosition(
-            section=s1,
-            target_junction=j3,
-            mileage=WATARI_RAIL_B * 1 + STRAIGHT_RAIL * 1.5,
-        ),
-    )
-    stop_4 = Stop(
-        id="stop_4",
         position=DirectedPosition(
             section=s3,
-            target_junction=j0,
-            mileage=WATARI_RAIL_A * 1 + STRAIGHT_RAIL * 1.5,
+            target_junction=j1,
+            mileage=WATARI_RAIL_A
+            + STRAIGHT_RAIL * 2
+            + OUTER_CURVE_RAIL * 2
+            + STRAIGHT_RAIL
+            + STRAIGHT_1_2_RAIL
+            + OUTER_CURVE_RAIL * 2
+            + STRAIGHT_RAIL * 2
+            - STRAIGHT_1_6_RAIL,
         ),
     )
 
-    # control.add_stop(stop_0)
-    # control.add_stop(stop_1)
-    # control.add_stop(stop_2)
-    # control.add_stop(stop_3)
-    # control.add_stop(stop_4)
+    control.add_stop(stop_0)
+    control.add_stop(stop_1)
 
-    station_0 = Station(id="station_0", stops=[stop_0, stop_1])
-    station_1 = Station(id="station_1", stops=[stop_2, stop_3, stop_4])
+    station_0 = Station(id="station_0", stops=[stop_0])
+    station_1 = Station(id="station_1", stops=[stop_1])
 
-    # control.add_station(station_0)
-    # control.add_station(station_1)
+    control.add_station(station_0)
+    control.add_station(station_1)
 
     position_173 = SensorPosition(
         id="position_173",
@@ -278,9 +259,9 @@ def create_control(logger: logging.Logger | None = None) -> Control:
         id="obstacle_0",
         position=UndirectedPosition(
             section=s3,
-            mileage=WATARI_RAIL_A + STRAIGHT_RAIL,
+            mileage=WATARI_RAIL_A + STRAIGHT_RAIL * 1.5,
         ),
-        is_detected=True,
+        is_detected=False,
     )
 
     control.add_obstacle(obstacle_0)

ptcs/ptcs_server/mft2023.py

@@ -0,0 +1,33 @@
+import platform
+
+from ptcs_bridge.bridge2 import Bridge2
+from ptcs_bridge.train_client import TrainClient
+from ptcs_bridge.train_simulator import TrainSimulator
+from ptcs_bridge.wire_pole_client import WirePoleClient
+
+if platform.system() == "Windows":
+    ADDRESS_T0 = "e0:5a:1b:e2:7a:f2"
+    ADDRESS_T1 = "94:b5:55:84:15:42"
+    ADDRESS_T2 = "e0:5a:1b:e2:7b:1e"
+    ADDRESS_T3 = "1c:9d:c2:66:84:32"
+    ADDRESS_T4 = "24:4c:ab:f5:c6:3e"
+elif platform.system() == "Darwin":
+    ADDRESS_T0 = "00B55AE6-34AA-23C2-8C7B-8C11E6998E12"
+    ADDRESS_T1 = "F2158243-18BB-D34C-88BC-F8F193CAD15E"
+    ADDRESS_T2 = "EB57E065-90A0-B6D0-98BA-81096FA5765E"
+    ADDRESS_T3 = "4AA3AAE5-A039-8484-013C-32AD94F50BE0"
+    ADDRESS_T4 = "FC44FB3F-CF7D-084C-EA29-7AFD10C47A57"
+else:
+    raise Exception(f"{platform.system()} not supported")
+
+
+def create_bridge() -> Bridge2:
+    bridge = Bridge2()
+    bridge.add_train(TrainSimulator("t0"))
+    # bridge.add_train(TrainClient("t0", ADDRESS_T0))
+    bridge.add_train(TrainSimulator("t1"))
+    bridge.add_train(TrainSimulator("t2"))
+    bridge.add_train(TrainSimulator("t3"))
+    bridge.add_train(TrainSimulator("t4"))
+    # bridge.add_obstacle(WirePoleClient("obstacle_0", "24:62:AB:E3:67:9A"))
+    return bridge