パパ日記

PS3コントローラを接続できたので、サーボを動かしてみる。
ESP32は16個までのサーボが制御可能な様だが、そんなにいらないので6ch仕様にする。4ch分はアナログスティックを使用するが、残り2ch分はトリガーを使用することにした。
トリガーが返す値は0〜255でニュートラルがないので、ショルダーとの組み合わせでアナログスティックと同様に-128〜127を得る（ショルダープレスでマイナス、トリガーリリースでニュートラルとなる）様にする。
ESP32でサーボを制御するには、ESP32Servoを使用するらしい。Servo::writeを使うと分解能が1度になってしまいもったいないので、Servo::writeMicrosecondsを使用する。パルス幅は使用するサーボの諸元に応じて定義する。
後々、ニュートラルや舵角調整、反転を調整可能としたいが、取り敢えず初期値で定義しておく。
アナログスティックはニュートラルが安定しないので、マージンを取るようにした。
ESP32にサーボを適切に接続して、以下のスケッチを実行してPS3コントローラと接続すると、6ch分のサーボ制御が出来た。

/*******************************************************************************
  デバグ用マクロ
*******************************************************************************/

// DEBUGマクロを定義するとデバグ情報をシリアルポートへ出力する
#define DEBUG

#if defined(DEBUG)
#define DSTART() Serial.begin(115200)
#define DPRINTF(...) Serial.printf(__VA_ARGS__)
#else
#define DSTART()
#define DPRINTF(...)
#endif

/*******************************************************************************
  使用ライブラリのヘッダファイル
*******************************************************************************/

#include <Ps3Controller.h>
#include <ESP32Servo.h>

/*******************************************************************************
  定数定義
*******************************************************************************/

// 最大チャンネル数
const int MAX_SERVO_NUM = 16;

// PS3コントローラのアナログコントロールスティック／トリガーとサーボチャンネルの対応
enum SERVO_CH {LX_CH, LY_CH, RX_CH, RY_CH, L1L2_CH, R1R2_CH};

/*******************************************************************************
  型定義
*******************************************************************************/

// ピンアサイン
typedef struct {
  uint8_t servo[MAX_SERVO_NUM];
} pin_asign_t;

// サーボ諸元
typedef struct {
  uint16_t min_pulse_width;
  uint16_t max_pulse_width;
  int8_t neutral_deg;
} servo_factor_t;

// 全体構成
typedef struct {
  servo_factor_t servo_factor;
  uint8_t neutral_margin;
  uint8_t servo_num;
  pin_asign_t pin;
} conf_t;

// サーボ調整
typedef struct {
  int8_t neutral[MAX_SERVO_NUM];
  uint8_t range[MAX_SERVO_NUM];
  uint16_t reverse; // （ビット配列）
} servo_trim_t;

/*******************************************************************************
  グローバル変数定義
*******************************************************************************/

// 全体構成
conf_t conf = {{500, 2500, 90} /* （SG92Rの場合） */, 20, 6,
  {{32, 33, 25, 26, 27, 14}}
};

// サーボ調整
servo_trim_t trim = {{0, 0, 0, 0, 0, 0}, {45, 45, 45, 45, 45, 45}, 0};

// サーボインスタンス、ショルダーボタン状態（ビット配列）
Servo servo[MAX_SERVO_NUM];
uint8_t lr1_bn = 0;

/*******************************************************************************
  サーボ制御
*******************************************************************************/

void servo_op(int ch, int val)
{
  // リバースが必要な場合値を反転する
  if (bitRead(trim.reverse, ch)) val = -val;

  // ニュートラル調整と舵角調整で設定された範囲のパルス幅を求める
  int neutral = conf.servo_factor.neutral_deg + trim.neutral[ch];
  int pwm_min = map(neutral - trim.range[ch], 0,
                    conf.servo_factor.neutral_deg * 2,
                    conf.servo_factor.min_pulse_width,
                    conf.servo_factor.max_pulse_width);
  int pwm_max = map(neutral + trim.range[ch], 0,
                    conf.servo_factor.neutral_deg * 2,
                    conf.servo_factor.min_pulse_width,
                    conf.servo_factor.max_pulse_width);

  // コントローラのニュートラル付近にマージンを取る
  if (abs(val) < conf.neutral_margin) val = 0;
  else val -= (val >= conf.neutral_margin) ?
                conf.neutral_margin : -conf.neutral_margin;

  // マージン除外後の範囲に再マッピングしてサーボを動作させる
  servo[ch].writeMicroseconds(
    map(val, -128 + conf.neutral_margin,
        127 + conf.neutral_margin, pwm_min, pwm_max));
}

/*******************************************************************************
  サーボ初期化
*******************************************************************************/

void servo_init()
{
  // 全サーボをアタッチしてニュートラルにする
  for (int ch = 0; ch < conf.servo_num; ++ch) {
    servo[ch].attach(conf.pin.servo[ch]);
    servo_op(ch, 0);
  }
}

/*******************************************************************************
  PS3コントローライベントハンドラ
*******************************************************************************/

void notify()
{
  // アナログスティックイベント
  // アナログスティックLXチェンジ（LX_CH）
  if (Ps3.event.analog_changed.stick.lx)
    servo_op(LX_CH, Ps3.data.analog.stick.lx);
  // アナログスティックLYチェンジ（LY_CH）
  if (Ps3.event.analog_changed.stick.ly)
    servo_op(LY_CH, Ps3.data.analog.stick.ly);
  // アナログスティックRXチェンジ（RX_CH）
  if (Ps3.event.analog_changed.stick.rx)
    servo_op(RX_CH, Ps3.data.analog.stick.rx);
  // アナログスティックRYチェンジ（RY_CH）
  if (Ps3.event.analog_changed.stick.ry)
    servo_op(RY_CH, Ps3.data.analog.stick.ry);

  // ショルダー／トリガーボタンイベント
  // ショルダーL1プレスとトリガーL2チェンジの組み合わせ（L1L2_CH）
  if (Ps3.event.button_down.l1) bitSet(lr1_bn, 0);
  if (Ps3.event.button_up.l1) bitClear(lr1_bn, 0);
  if (Ps3.event.analog_changed.button.l2)
    servo_op(L1L2_CH, Ps3.data.analog.button.l2
             / (bitRead(lr1_bn, 0) ? -2 : 2));
  // ショルダーR1プレスとトリガーR2チェンジの組み合わせ（R1R2_CH）
  if (Ps3.event.button_down.r1) bitSet(lr1_bn, 1);
  if (Ps3.event.button_up.r1) bitClear(lr1_bn, 1);
  if (Ps3.event.analog_changed.button.r2)
    servo_op(R1R2_CH, Ps3.data.analog.button.r2
             / (bitRead(lr1_bn, 1) ? -2 : 2));
}

/*******************************************************************************
  PS3コントローラ接続完了イベント
*******************************************************************************/

void onConnect()
{
  DPRINTF("Connected!\n");
}

/*******************************************************************************
  PS3コントローラ接続
*******************************************************************************/

void connect_controller()
{
  // ESP32のMACアドレス（バイナリ、文字列）
  uint8_t btmac[6];
  char btmac_str[20];

  // PS3コントローラを接続する
  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  esp_read_mac(btmac, ESP_MAC_BT);
  sprintf(btmac_str, "%02x:%02x:%02x:%02x:%02x:%02x",
          btmac[0], btmac[1], btmac[2], btmac[3], btmac[4], btmac[5]);
  DPRINTF("MAC: %s\n", btmac_str);
  Ps3.begin(btmac_str);
  DPRINTF("Ready...\n");
}

/*******************************************************************************
  セットアップ
*******************************************************************************/

void setup()
{
  // デバグ情報を有効化する
  DSTART();

  // サーボを初期化する
  servo_init();

  // PS3コントローラを接続する
  connect_controller();
}

/*******************************************************************************
  メインループ
*******************************************************************************/

void loop()
{
}