# joypad で cobotta の制御の方法

## 必要なプログラム

- [x_logger](https://github.com/ohara-lab-su/x_logger) / [doc](https://ohara-lab-su.github.io/x_logger)
- [cobotta2](https://github.com/ohara-lab-su/cobotta2) / [doc](https://ohara-lab-su.github.io/cobotta2)

## 自分のpadのボタン番号とプログラムの対応を確認する

現在プログラムでデフォルトで想定している番号

- L1: 4
- L2: 6
- R1: 5
- R2: 7
- start: 12

```python
# elecom joypad
STICK_LEFT_X = 0
STICK_LEFT_Y = 1
STICK_RIGHT_X = 2
STICK_RIGHT_Y = 3
BUTTON_L1 = 4
BUTTON_L2 = 6
BUTTON_R1 = 5
BUTTON_R2 = 7
BUTTON_START = 12
```

```python
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import pygame
import time


def main() -> None:
    pygame.init()
    pygame.joystick.init()

    if pygame.joystick.get_count() == 0:
        print("No joystick found")
        return

    js = pygame.joystick.Joystick(0)
    js.init()

    print("Joystick name:", js.get_name())
    print("num buttons:", js.get_numbuttons())
    print("Press any buttons. Ctrl+C to quit.")
    print("----")

    try:
        while True:
            pygame.event.pump()

            pressed = []
            i = 0
            while i < js.get_numbuttons():
                # ボタン
                if js.get_button(i):
                    pressed.append(i)
                i += 1

            if len(pressed) > 0:
                print("pressed buttons =", pressed)

            time.sleep(0.05)

    except KeyboardInterrupt:
        pass
    finally:
        js.quit()
        pygame.joystick.quit()
        pygame.quit()


if __name__ == "__main__":
    main()
```


## パッドの操作: P/J 切り替え

START

## パッドの操作: スピード制御

十字キーの上下

## パッドの操作: Pモード (xyz)
- L1 離してる(デフォルト): 位置（xyz）= 左スティックX→x, 左スティックY→y, 右スティックY→z
- L1 押してる: 回転（r*）= 左スティックX→rx, 左スティックY→ry, 右スティックY→rz

```{figure} fig/joypad_001.png
:width: 70%
:align: center
```

## パッドの操作: Jモード (J1〜J6)
- L1 離してる（J1–J4): 左X→J1, 左Y→J2, 右Y→J3, 右X→J4
- L1 押してる（J3–J6): 左X→J3, 左Y→J4, 右Y→J5, 右X→J6

```{figure} fig/joypad_002.png
:width: 70%
:align: center
```

## ハンド

- R1 押下（立ち上がり1回）: hand_open()
- R2 押下（立ち上がり1回）→ hand_close()


## Joypad プログラムを立ち上げる GUI を使わない方法

現在の cobotta モジュール内にあるプログラムだけで動かします。

### server script

cobotta と通信可能な権限があるマシン上で起動する

```python
#!/usr/bin/env python3
from cobotta2 import Config
from cobotta2.server_grpc import joypad_server
from x_logger import XLogger


if __name__ == "__main__":
    
    Config.load_yaml("config_cobotta3_server2.yaml")

    # ---- cobotta
    # BCAP_HOST = "10.170.10.37"
    BCAP_HOST = Config.COBOTTA_IP
    # BCAP_PORT = 5007
    BCAP_PORT = Config.BCAP_PORT
    # ---- joypad
    # GRPC_HOST = "10.170.10.38"
    GRPC_HOST = Config.SERVER_IP
    # GRPC_HOST = "0.0.0.0"
    # # GRPC_HOST = "localhost"
    # GRPC_PORT = 50051
    GRPC_PORT = Config.JOYPAD_PORT
    # JOYPAD_INTERVAL = 0.01
    JOYPAD_INTERVAL = Config.JOYPAD_INTERVAL

    LOG_LEVEL = "debug"

    logger = XLogger(
        log_level=LOG_LEVEL,
        logger_name=Config.JOYPAD_LOGGER_NAME,
        # log_mode="rotate",
        # log_mode="file",
        # log_name=f"c:/opt/log/{Config.JOYPAD_LOGGER_NAME}.log",
    )

    # JoyPad Server
    joypad_server(
        bcap_host=BCAP_HOST,
        bcap_port=BCAP_PORT,
        grpc_host=GRPC_HOST,
        grpc_port=GRPC_PORT,
        interval=JOYPAD_INTERVAL,
        logger=logger,
        log_level=LOG_LEVEL,
    )
```
設定ファイルを yaml にまとめたものを使う場合は
下記の yaml ファイルを用意する

```aiignore
# --- cobotta-server
SERVER_IP: "10.170.10.38"
SERVER_PORT: 8000
CLIENT_LOGGER_NAME: "cobotta1_client"
SERVER_LOGGER_NAME: "cobotta1_server"
TIMEOUT: 5
LOGGER_LEVEL: "debug"
# --- joypad-server
JOYPAD_PORT: 50051
JOYPAD_LOGGER_NAME: "cobotta1_joypad"
# --- cobotta
COBOTTA_IP: "10.170.10.37"
COBOTTA_CAMERA_IP: None
```

### joypad をつなげたPC 上で起動する

joypad-server と同じマシンでも違うマシンでも良い。
ただし、ネットワークの状況があまりよくないと
遅延が発生するかもしれないので注意

```python
#!/usr/bin/env python3
"""
step 型の 呼び出し
GUI 上で独立ループから呼び出す(mainスレッドとJoyPadスレッドを一緒にするため)

mac などでjoygame などのオブジェクトを main スレッド以外で使うとハマる問題(SDL問題)
の回避で、start/stop がたから、while: step 形に変更

Kengo NAKADA
kengo.nakada@mat.shimane-u.ac.jp
kengo.nakada@gmail.com
"""
from typing import Any, Dict, List, Optional, Union, Tuple, Callable, Sequence
import time

# import grpc

from cobotta2.server_grpc import JoyPadClient
from cobotta2.server_grpc import InputGrpcClient


def step(
    logger: Optional[Any] = None,
    log_level: str = "INFO",
) -> None:
    """
    step 型 client 起動（SDL 制約回避）
    """
    from cobotta2.pad import InputGrpcClient

    if logger is None:
        import logging

        logging.basicConfig(level=log_level.upper())
        logger = logging.getLogger(__name__)

    logger.info("[CLIENT] JoyPad gRPC client main start")

    client = JoyPadClient(
        server_ip="10.170.10.38",
        server_port=50051,
        logger=logger,
        log_level=log_level,
    )

    if not client.is_connected():
        logger.error("[CLIENT] gRPC connection failed")
        return

    logger.info("[CLIENT] connected")

    client.reset_error()
    client.motor_on()
    client.take_arm()

    client.joypad_set_scale(9.0)
    client.joypad_start()

    input_pad = InputGrpcClient(
        grpc_client=client,
        interval=0.02,
        logger=logger,
    )

    try:
        while True:
            input_pad.step()
            time.sleep(0.02)
    except KeyboardInterrupt:
        logger.info("[CLIENT] KeyboardInterrupt")
    finally:
        client.joypad_stop()
        client.give_arm()
        client.motor_off()
        logger.info("[CLIENT] JoyPad gRPC client main end")


if __name__ == "__main__":
    from x_logger import XLogger

    log_level_ = "INFO"
    logger_ = XLogger(log_level=log_level_, logger_name="JoyPadClient")
    step(log_level=log_level_, logger=logger_)

```


## GUI を使う

- [doc](https://ohara-lab-su.github.io/cobotta2_joypad/)
- [source](https://github.com/ohara-lab-su/cobotta2_joypad/)

![](fig/fig_jopad_main.png)

---
## 作者
- Kengo NAKADA