Merge pull request #11 from plarailers/feature/ptcs-update-4

PTCS Update 4

point_switching/point_switching/point_switching.ino

@@ -10,9 +10,9 @@ VarSpeedServo servo[NUM_SERVO];
 const int SERVO_SPEED = 100;
 
 // サーボを直進にする際の角度。適宜いじってください。
-const int SERVO_ANGLE_STRAIGHT[NUM_SERVO] = {25, 25, 25, 25};
+const int SERVO_ANGLE_STRAIGHT[NUM_SERVO] = {25, 25, 50, 70};
 // サーボを曲げる際の角度。適宜いじってください。
-const int SERVO_ANGLE_CURVE[NUM_SERVO] = {75, 75, 75, 75};
+const int SERVO_ANGLE_CURVE[NUM_SERVO] = {75, 75, 70, 50};
 // サーボをアタッチするピンの指定。適宜いじってください。
 const int SERVO_ATTACH_PIN[NUM_SERVO] = {3, 5, 9, 10};
 const byte STRAIGHT = 0;
@@ -54,4 +54,17 @@ void loop() {
         byte servo_state = Serial.read();
         servo_change(servo_id, servo_state);
     }
+    // if (Serial.available() > 0) { 
+    //     // シリアルで受け取った信号をもとにサーボを動かす
+    //     Serial.print("ID: ");
+    //     byte servo_id = Serial.parseInt();
+    //     Serial.print(servo_id);
+    //     Serial.println();
+    //     Serial.print("state: ");
+    //     byte servo_state = Serial.parseInt();
+    //     Serial.print(servo_state);
+    //     Serial.println();
+    //     // servo_change(servo_id, servo_state);
+    //     servo[servo_id].write(servo_state, SERVO_SPEED, true);
+    // }
 }

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

@@ -14,7 +14,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

@@ -12,7 +12,7 @@
     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")
@@ -41,13 +41,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
@@ -64,11 +64,12 @@ def _get_characteristic_rotation(self) -> BleakGATTCharacteristic:
     def _get_characteristic_voltage(self) -> BleakGATTCharacteristic:
         return self._get_characteristic(CHARACTERISTIC_VOLTAGE_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, f"{motor_input}".encode())
+        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):
@@ -77,18 +78,18 @@ 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):
             assert len(data) == 1
             assert data[0] == 1
-            logger.info("%s notify rotation %s", self, 1)
+            # 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)
 
     async def start_notify_voltage(self, callback: NotifyVoltageCallback) -> None:
         def wrapped_callback(_characteristic: BleakGATTCharacteristic, data: bytearray):
@@ -97,5 +98,5 @@ def wrapped_callback(_characteristic: BleakGATTCharacteristic, data: bytearray):
             logger.info("%s notify voltage %s mV", self, voltage)
             callback(self, voltage)
 
-        characteristic_voltage = self._get_characteristic_voltage()
-        await self._client.start_notify(characteristic_voltage, wrapped_callback)
+        characteristic = self._get_characteristic_voltage()
+        await self._client.start_notify(characteristic, wrapped_callback)

ptcs/ptcs_bridge/train_simulator.py

@@ -59,7 +59,7 @@ async def _loop(self):
 
             if self._notify_rotation_callback is not None:
                 for _ in range(rotation):
-                    logger.info("%s notify rotation %s", self, 1)
+                    # logger.info("%s notify rotation %s", self, 1)
                     self._notify_rotation_callback(self, 1)
 
     async def connect(self) -> None:
@@ -76,8 +76,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)