Merge pull request #8 from plarailers/feature/ptcs-update-1

PTCS の更新

point_switching/switch_point.py

@@ -5,7 +5,7 @@
 from bleak import BleakClient, BleakScanner
 
 if platform.system() == "Windows":
-    ADDRESS_POINT0 = '3C:71:BF:99:36:84'
+    ADDRESS_POINT0 = '3c:71:bf:99:36:86'
     ADDRESS_POINT1 = '9c:9c:1f:cb:d9:f2'
 
 elif platform.system() == "Darwin":

ptcs/ptcs_bridge/wire_pole_client.py

@@ -52,3 +52,9 @@ def wrapped_callback(_characteristic: BleakGATTCharacteristic, data: bytearray):
 
         await self._client.start_notify(characteristic, wrapped_callback)
         logger.info("%s start notify collapse", self)
+
+        # 倒れたり起き上がったりした瞬間にしか notify が来ないので、
+        # 起動時にこちらから状態を読み取る
+        data = await self._client.read_gatt_char(characteristic)
+        wrapped_callback(characteristic, data)
+        logger.info("%s read collapse %s", self, data)

ptcs/ptcs_control/control.py

@@ -12,6 +12,7 @@
 from .components.station import Station
 from .components.stop import Stop
 from .components.train import Train
+from .constants import STRAIGHT_RAIL
 
 
 def create_empty_logger() -> logging.Logger:
@@ -106,7 +107,7 @@ def current_time(self) -> int:
 
     @current_time.setter
     def current_time(self, value) -> None:
-        self.logger.info(f"current_time = {value}")
+        # self.logger.info(f"current_time = {value}")
         self._current_time = value
 
     def tick(self, increment: int = 1) -> None:
@@ -323,10 +324,12 @@ def _calc_speed(self) -> None:
                 speed_command = stop_speed
 
             # [マスコン]
-            if train.manual_speed is not None:
-                speed_command = min(speed_command, train.manual_speed)
-
-            train.speed_command = speed_command
+            # 自動操縦ならATPとATO、
+            # 手動操縦なら手動速度とATPのみに従う
+            if train.manual_speed is None:
+                train.speed_command = speed_command
+            else:
+                train.speed_command = min(train.manual_speed, speedlimit)
 
             # print(
             #     train.id,
@@ -342,45 +345,50 @@ def _calc_stop(self) -> None:
         """
         列車の現在あるべき停止目標を割り出し、列車の状態として格納する。
         この情報は列車の速度を計算するのに使われる。
-
-        実際の挙動は「列車より手前にある停止目標を計算し、ちょっと待ってから格納する」であり、
-        これは以下の仮定をおいた上でうまく動作する。
-          - 列車は停止目標付近で停止する（= IPS 信号がしばらく送られなくなる）。
-          - 停止したときに停止目標の位置を過ぎている。
         """
 
         STOPPAGE_TIME: int = 50  # 列車の停止時間[フレーム] NOTE: 将来的にはパラメータとして定義
+        STOPPAGE_MERGIN: float = STRAIGHT_RAIL / 2  # 停止区間距離[cm]
 
         for train in self.trains.values():
             # 列車より手前にある停止目標を取得する
-            forward_stop, forward_stop_distance = train.find_forward_stop() or (None, 0)
-
-            # 停止目標がないままのとき（None → None）
-            # 停止目標を見つけたとき（None → not None）
-            if train.stop is None:
-                train.stop = forward_stop
-                if forward_stop:
+            forward_stop, forward_stop_distance = train.find_forward_stop() or (None, 0.0)
+
+            if train.departure_time is None:
+                # 「停止目標が変わらず、停止距離が区間外から区間内に変わる」のを検知することで駅の停止開始を判定する。
+                # これにより、以下の場合は停止する。
+                # - IPS により停止区間に踏み進んだ場合
+                #   - 停止目標は変わらず、停止距離が区間外から区間内に変わるのを検知する
+                #   - その後、同区間内の APS を踏んでも以下のどちらかになる
+                #     - 区間内で APS を踏む。このとき停止距離が区間内のままであるため無視
+                #     - APS により停止区間に踏み戻る。このとき無視
+                # - APS により停止区間に踏み進んだ場合
+                #   - 停止目標は変わらず、停止距離が区間外から区間内に変わるのを検知する
+                # 逆に、以下の場合は停止しない。
+                # - APS により停止区間に踏み戻った場合
+                #   - 停止目標が変わるので無視
+                #   - ただし、停止目標が次も同じになる (例: ループの中で駅がひとつしかない) ような路線ではないとする
+                # - ポイントが切り替わって停止目標が再計算された場合
+                #   - 停止目標が変わるような箇所はすべて区間外であるため無視
+                if train.stop == forward_stop and forward_stop_distance <= STOPPAGE_MERGIN < train.stop_distance:
                     train.stop_distance = forward_stop_distance
-                else:
-                    train.stop_distance = 0
-
-            # 停止目標を過ぎたとき（異なる）
-            # 停止目標を見失ったとき（not None → None）
-            # NOTE: セクションがblockされると停止目標を見失う場合がある。
-            # このときは駅に着いたと勘違いして止まってしまう現象が起きるが、
-            # 駅到着により見失う場合との区別が難しいので、無視する。
-            elif train.stop != forward_stop:
-                # 最初は発車時刻を設定
-                if train.departure_time is None:
                     train.departure_time = self.current_time + STOPPAGE_TIME
-                    train.stop_distance = 0
-
-                # 発車時刻になっていれば、次の停止目標に切り替える
-                elif self.current_time >= train.departure_time:
-                    train.departure_time = None
+                else:
                     train.stop = forward_stop
                     train.stop_distance = forward_stop_distance
-
-            # 停止目標が変わらないとき
             else:
-                train.stop_distance = forward_stop_distance
+                # すでに停止が開始されているとき、
+                # - まだ停止していてほしい場合
+                #   - 停止点を超えていない場合、距離を詰めていく
+                #   - 停止点を超えてしまった場合、強制停止
+                # - もう発車してほしい場合
+                #   - 停止点は超えていると仮定し、次の停止目標へ向かう
+                if self.current_time < train.departure_time:
+                    if train.stop == forward_stop:
+                        train.stop_distance = forward_stop_distance
+                    else:
+                        train.stop_distance = 0
+                else:
+                    train.stop = forward_stop
+                    train.stop_distance = forward_stop_distance
+                    train.departure_time = None

ptcs/ptcs_control/mft2023.py

@@ -109,24 +109,24 @@ def create_control(logger: logging.Logger | None = None) -> Control:
 
     t0 = Train(
         id="t0",
-        min_input=200,
-        max_input=250,
+        min_input=190,
+        max_input=240,
         max_speed=40.0,
         length=14.0,
-        delta_per_motor_rotation=0.4553 * 0.9 * 0.9,
+        delta_per_motor_rotation=0.4553,
         head_position=DirectedPosition(
             section=s3,
             target_junction=j1,
-            mileage=WATARI_RAIL_A + STRAIGHT_RAIL * 0.5,
+            mileage=WATARI_RAIL_A + STRAIGHT_RAIL,
         ),
     )
     t1 = Train(
         id="t1",
         min_input=150,
-        max_input=210,
+        max_input=190,
         max_speed=40.0,
         length=14.0,
-        delta_per_motor_rotation=0.4021,
+        delta_per_motor_rotation=0.4321,
         head_position=DirectedPosition(
             section=s3,
             target_junction=j1,
@@ -135,7 +135,7 @@ def create_control(logger: logging.Logger | None = None) -> Control:
     )
     t2 = Train(
         id="t2",
-        min_input=150,
+        min_input=180,
         max_input=230,
         max_speed=40.0,
         length=14.0,
@@ -154,11 +154,11 @@ def create_control(logger: logging.Logger | None = None) -> Control:
     )
     t3 = Train(
         id="t3",
-        min_input=190,
+        min_input=180,
         max_input=220,
         max_speed=40.0,
         length=14.0,
-        delta_per_motor_rotation=0.4208,
+        delta_per_motor_rotation=0.4508,
         head_position=DirectedPosition(
             section=s3,
             target_junction=j1,
@@ -178,26 +178,26 @@ def create_control(logger: logging.Logger | None = None) -> Control:
         max_input=230,
         max_speed=40.0,
         length=40.0,
-        delta_per_motor_rotation=0.4241 * 2.2,
+        delta_per_motor_rotation=0.4241,
         head_position=DirectedPosition(
             section=s1,
             target_junction=j0,
             mileage=WATARI_RAIL_B + STRAIGHT_RAIL * 4,
         ),
     )
 
-    # control.add_train(t0)
+    control.add_train(t0)
     control.add_train(t1)
-    control.add_train(t2)
+    # control.add_train(t2)
     control.add_train(t3)
-    # control.add_train(t4)
+    control.add_train(t4)
 
     stop_0 = Stop(
         id="stop_0",
         position=DirectedPosition(
             section=s3,
             target_junction=j1,
-            mileage=WATARI_RAIL_A + STRAIGHT_RAIL * 0.5,
+            mileage=WATARI_RAIL_A + STRAIGHT_RAIL * 0.8,
         ),
     )
     stop_1 = Stop(
@@ -211,7 +211,7 @@ def create_control(logger: logging.Logger | None = None) -> Control:
             + STRAIGHT_RAIL
             + STRAIGHT_1_2_RAIL
             + OUTER_CURVE_RAIL * 2
-            + STRAIGHT_RAIL * 1.5,
+            + STRAIGHT_RAIL * 1.8,
         ),
     )
 

ptcs/ptcs_server/mft2023.py

@@ -23,8 +23,8 @@
     ADDRESS_T2 = "EB57E065-90A0-B6D0-98BA-81096FA5765E"
     ADDRESS_T3 = "4AA3AAE5-A039-8484-013C-32AD94F50BE0"
     ADDRESS_T4 = "FC44FB3F-CF7D-084C-EA29-7AFD10C47A57"
-    ADDRESS_WIRE_POLE = ""
-    ADDRESS_MASTER_CONTROLLER = ""
+    ADDRESS_WIRE_POLE = "C6ED8F77-16C1-F870-C731-15AA663F0E29"
+    ADDRESS_MASTER_CONTROLLER = "E538DF32-E46A-3177-8196-3B9A8D4861A9"
     ADDRESS_POINT0 = "9FA4916E-AD02-6C9C-686A-1B97D9E3427A"
     ADDRESS_POINT1 = "90386433-4331-50CF-1637-EFFA587DD6DB"
 else:
@@ -33,18 +33,18 @@
 
 def create_bridge() -> Bridge2:
     bridge = Bridge2()
-    bridge.add_train(TrainSimulator("t0"))
-    # bridge.add_train(TrainClient("t0", ADDRESS_T0))
+    # bridge.add_train(TrainSimulator("t0"))
+    bridge.add_train(TrainClient("t0", ADDRESS_T0))
     # bridge.add_train(TrainSimulator("t1"))
     bridge.add_train(TrainClient("t1", ADDRESS_T1))
-    # bridge.add_train(TrainSimulator("t2"))
-    bridge.add_train(TrainClient("t2", ADDRESS_T2))
+    bridge.add_train(TrainSimulator("t2"))
+    # bridge.add_train(TrainClient("t2", ADDRESS_T2))
     # bridge.add_train(TrainSimulator("t3"))
     bridge.add_train(TrainClient("t3", ADDRESS_T3))
-    bridge.add_train(TrainSimulator("t4"))
-    # bridge.add_train(TrainClient("t4", ADDRESS_T4))
+    # bridge.add_train(TrainSimulator("t4"))
+    bridge.add_train(TrainClient("t4", ADDRESS_T4))
     bridge.add_obstacle(WirePoleClient("obstacle_0", ADDRESS_WIRE_POLE))
-    bridge.add_controller(MasterControllerClient("t3", ADDRESS_MASTER_CONTROLLER))
+    bridge.add_controller(MasterControllerClient("t4", ADDRESS_MASTER_CONTROLLER))
     bridge.add_point(PointClient("j0", ADDRESS_POINT1))
     bridge.add_point(PointClient("j2", ADDRESS_POINT0))
     return bridge

ptcs/ptcs_server/server.py

@@ -8,6 +8,7 @@
 from pydantic import BaseModel
 
 from ptcs_bridge.master_controller_client import MasterControllerClient
+from ptcs_bridge.point_client import PointClient
 from ptcs_bridge.train_base import TrainBase
 from ptcs_bridge.train_client import TrainClient
 from ptcs_bridge.train_simulator import TrainSimulator
@@ -72,27 +73,8 @@ async def control_loop():
     control_loop_task = asyncio.create_task(control_loop())
     app.state.control_loop_task = control_loop_task
 
-    async def bridge_loop():
-        await bridge.connect_all()
-
-        async def send_motor_input():
-            for train_id, train_control in control.trains.items():
-                train_client = bridge.trains.get(train_id)
-                if train_client is None:
-                    continue
-                match train_client:
-                    case TrainSimulator():
-                        await train_client.send_speed(train_control.speed_command)
-                    case TrainClient():
-                        motor_input = train_control.calc_input(train_control.speed_command)
-                        await train_client.send_motor_input(motor_input)
-
-        async def send_direction():
-            for junction_id, junction_control in control.junctions.items():
-                point_client = bridge.points.get(junction_id)
-                if point_client is None:
-                    continue
-                await point_client.send_direction(junction_control.current_direction)
+    async def train_loop(train_client: TrainBase):
+        await train_client.connect()
 
         def handle_notify_position_id(train_client: TrainBase, position_id: str):
             train_control = control.trains.get(train_client.id)
@@ -113,38 +95,74 @@ def handle_notify_voltage(train_client: TrainBase, _voltage_mV: int):
                 return
             train_control.voltage_mV = _voltage_mV
 
-        for train in bridge.trains.values():
-            match train:
+        await train_client.start_notify_rotation(handle_notify_rotation)
+        match train_client:
+            case TrainClient():
+                await train_client.start_notify_position_id(handle_notify_position_id)
+                await train_client.start_notify_voltage(handle_notify_voltage)
+
+        train_control = control.trains.get(train_client.id)
+        if train_control is None:
+            return
+
+        while True:
+            await asyncio.sleep(0.2)
+            match train_client:
+                case TrainSimulator():
+                    await train_client.send_speed(train_control.speed_command)
                 case TrainClient():
-                    await train.start_notify_position_id(handle_notify_position_id)
-                    await train.start_notify_voltage(handle_notify_voltage)
-            await train.start_notify_rotation(handle_notify_rotation)
+                    motor_input = train_control.calc_input(train_control.speed_command)
+                    await train_client.send_motor_input(motor_input)
+
+    app.state.train_loop_tasks = {}
+    for train_id, train_client in bridge.trains.items():
+        train_loop_task = asyncio.create_task(train_loop(train_client))
+        app.state.train_loop_tasks[train_id] = train_loop_task
+
+    async def point_loop(point_client: PointClient):
+        await point_client.connect()
+
+        junction_control = control.junctions.get(point_client.id)
+        if junction_control is None:
+            return
+
+        while True:
+            await asyncio.sleep(0.2)
+            await point_client.send_direction(junction_control.current_direction)
+
+    app.state.point_loop_tasks = {}
+    for point_id, point_client in bridge.points.items():
+        app.state.point_loop_tasks[point_id] = asyncio.create_task(point_loop(point_client))
+
+    async def obstacle_loop(obstacle_client: WirePoleClient):
+        await obstacle_client.connect()
 
         def handle_notify_collapse(obstacle_client: WirePoleClient, is_collapsed: bool):
             obstacle_control = control.obstacles.get(obstacle_client.id)
             if obstacle_control is None:
                 return
             obstacle_control.is_detected = is_collapsed
 
-        for obstacle in bridge.obstacles.values():
-            await obstacle.start_notify_collapse(handle_notify_collapse)
+        await obstacle_client.start_notify_collapse(handle_notify_collapse)
+
+    app.state.obstacle_loop_tasks = {}
+    for obstacle_id, obstacle_client in bridge.obstacles.items():
+        app.state.obstacle_loop_tasks[obstacle_id] = asyncio.create_task(obstacle_loop(obstacle_client))
+
+    async def controller_loop(controller_client: MasterControllerClient):
+        await controller_client.connect()
 
         def handle_notify_speed(controller_client: MasterControllerClient, speed: int):
             train_control = control.trains.get(controller_client.id)
             if train_control is None:
                 return
             train_control.manual_speed = speed / 255 * train_control.max_speed
 
-        for controller in bridge.controllers.values():
-            await controller.start_notify_speed(handle_notify_speed)
-
-        while True:
-            await asyncio.sleep(0.5)
-            await send_motor_input()
-            await send_direction()
+        await controller_client.start_notify_speed(handle_notify_speed)
 
-    bridge_loop_task = asyncio.create_task(bridge_loop())
-    app.state.bridge_loop_task = bridge_loop_task
+    app.state.controller_loop_tasks = {}
+    for controller_id, controller_client in bridge.controllers.items():
+        app.state.controller_loop_tasks[controller_id] = asyncio.create_task(controller_loop(controller_client))
 
     @app.on_event("shutdown")
     async def on_shutdown():