パパ日記

前回の記事の構想に基づいて実装してみた。
ウインカーはハンドル切らないと点滅しないのではなんだかなぁなので、ステアリングとは連動しないで十字ボタンの左右で制御することにした。ブレーキはESCが勝手に制御するのでESP32からは制動中であることが判らない。R1プレス中に点灯することで妥協する。
ブザーとLEDによる操作反応は結構イケている。でもバッテリーの消費が早すぎる気がするが、ESP32が多喰らいなのと電源監視のADCに常時加圧しているのが原因かな。この辺は改善の余地あり。

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

// 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>
#include <Preferences.h>

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

// このプログラムの名前空間と最大チャンネル数
const char NAME_SPACE[] = "ESP32RCPS3BT6CH";
const int MAX_SERVO_NUM = 16;
const int   MAX_LED_NUM = 16;

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

// ライトとLEDチャンネルの対応
enum LED_CH {HEAD_CH, TAIL_CH, BRAKE_CH, FOG_CH, LWINK_CH, RWINK_CH};

// ステアリング／スロットルとPS3コントローラの対応
const int STEERING_CH = LX_CH;
const int THROTTLE_CH = R1R2_CH;

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

// ピンアサイン
typedef struct {
  uint8_t adc;
  uint8_t buzzer;
  uint8_t servo[MAX_SERVO_NUM];
  uint8_t led[MAX_LED_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;
  uint32_t led_interval;
  float min_voltage;
  uint8_t servo_num;
  uint8_t led_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}, 20, 100000, 4.5, 6, 6,
  {34, 13, {32, 33, 25, 26, 27, 14}, {19, 18, 5, 17, 16, 4}}
};

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

Preferences pref;

// サーボインスタンス、サーボ調整修飾ボタン（□△○×）状態、サーボ調整対象チャンネル、緊急処理中
Servo servo[MAX_SERVO_NUM];
uint8_t servo_trim_bn = 0;
uint8_t lr1_bn = 0;
uint8_t channel = 0;
uint8_t isEmargency = 0;

// LED状態初期値（タイマー割込同期用ミューテックス,ON/OFF、フラッシュ対象）
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
uint8_t led_val[MAX_LED_NUM] = {LOW, LOW, LOW, LOW};
uint16_t led_isFlash = bit(LWINK_CH) + bit(RWINK_CH);

/*******************************************************************************
  ブザー制御
*******************************************************************************/

void buzzer_op(int count, int t1, int t2)
{
  // 回数、有音長、無音長に従ってブザーを鳴らす
  while (--count > 0) {
    digitalWrite(conf.pin.buzzer, HIGH);
    delay(t1);
    digitalWrite(conf.pin.buzzer, LOW);
    delay(t2);
  }
  digitalWrite(conf.pin.buzzer, HIGH);
  delay(t1);
  digitalWrite(conf.pin.buzzer, LOW);
}

/*******************************************************************************
  ブザー初期化
*******************************************************************************/

void buzzer_init()
{
  // ブザー用ピン割当
  pinMode(conf.pin.buzzer, OUTPUT);
}

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

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_trim_neutral(int dir)
{
  // 増減フラグ（非０：増、０：減）
  int isUp = (dir + 1) & 2;

  // ニュートラル補正の絶対値は90度を越えない
  if (!isUp && trim.neutral[channel] > -(int)conf.servo_factor.neutral_deg
      || isUp && trim.neutral[channel] < conf.servo_factor.neutral_deg) {

    // Dパッドボタンの右或いは上ボタンなら増加、左或いは下ボタンなら減少
    trim.neutral[channel] += isUp ? 1 : -1;

    // 舵角とニュートラル補正の絶対値の和が90度を越える場合は舵角を減少
    trim.range[channel] -=
      abs(trim.neutral[channel]) + trim.range[channel]
      > conf.servo_factor.neutral_deg;

    // 設定結果を反映する
    servo_op(channel, 0);
  }

  // 限界ならブザーを２回鳴らす
  else buzzer_op(2, 20, 50);
}

/*******************************************************************************
  サーボ舵角調整
*******************************************************************************/

void servo_trim_range(int dir)
{
  // 増減フラグ（非０：増、０：減）
  int isUp = (dir + 1) & 2;

  // Dパッドボタンの右或いは上ボタン（増加）
  // かつ角幅とニュートラル補正の絶対値の和が90度を越えない場合のみ増加
  if (isUp && abs(trim.neutral[channel]) +
      trim.range[channel] < conf.servo_factor.neutral_deg)
    ++trim.range[channel];

  // Dパッドボタンの左或いは下ボタン（減少）かつ舵角が0度未満にならない場合のみ減少
  else if (!isUp && trim.range[channel] > 0)
    --trim.range[channel];

  // 限界ならブザーを２回鳴らす
  else buzzer_op(2, 20, 50);
}

/*******************************************************************************
  サーボ調整チェック
*******************************************************************************/

void servo_check()
{
  // 動作確認用
  const int check_val[5] = {0, -128, 0, 127, 0};

  // １秒間隔で５段階動作させる
  for (int i = 0; i < 5; ++i) {
    for (int ch = 0; ch < conf.servo_num; ++ch) servo_op(ch, check_val[i]);
    delay(1000);
  }
}

/*******************************************************************************
  サーボ調整
*******************************************************************************/

void servo_trim(int dir)
{
  // 短いブザーを１回鳴らす
  buzzer_op(1, 10, 0);

  // □ボタンが押されている場合はニュートラル調整
  if (bitRead(servo_trim_bn, 0)) servo_trim_neutral(dir);

  // △ボタンが押されている場合は舵角調整
  else if (bitRead(servo_trim_bn, 1)) servo_trim_range(dir);

  // ○ボタンが押されている場合はリバース切替
  else if (bitRead(servo_trim_bn, 2))
    bitWrite(trim.reverse, channel, !bitRead(trim.reverse, channel));

  // デバグ情報
  DPRINTF("Neutral: ");
  for (int i = 0; i < conf.servo_num; ++i)
    DPRINTF("%5d ", trim.neutral[i]);
  DPRINTF("\n");
  DPRINTF("  Range: ");
  for (int i = 0; i < conf.servo_num; ++i)
    DPRINTF("%5d ", trim.range[i]);
  DPRINTF("\n");
  DPRINTF("Reverse: %02x\n", trim.reverse);
}

/*******************************************************************************
  サーボ調整チャンネル選択
*******************************************************************************/

int select_ch(int ch)
{
  // セレクトボタンでサーボ調整用チャンネルを切り替える
  if (ch < conf.servo_num - 1) {

    // 短いブザーを１回鳴らす
    buzzer_op(1, 10, 0);
    ++ch;
  } else {

    // 一周したら短いブザーを２回鳴らす
    buzzer_op(2, 10, 100);
    ch = 0;
  }

  // 現在のチャンネルをコントローラのLEDで表示する（2進数表現で0〜15）
  DPRINTF("CHANNEL = %d\n", ch);
  ps3_cmd_t cmd = {0};
  *(((uint8_t *)&cmd) + 4) = ch;
  ps3Cmd(cmd);
  return ch;
}

/*******************************************************************************
  サーボ調整情報入力
*******************************************************************************/

void read_trim()
{
  // サーボ調整情報を入力する（無ければ初期値を温存）
  pref.getBytes("trim", &trim, sizeof(trim));
}

/*******************************************************************************
  サーボ調整情報出力
*******************************************************************************/

void write_trim()
{
  // 現在のサーボ調整情報を保存する
  pref.putBytes("trim", &trim, sizeof(trim));

  // ブザーを３回鳴らす
  buzzer_op(3, 20, 100);
}

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

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

/*******************************************************************************
  LED点滅
*******************************************************************************/

void led_flash()
{
  // トグルスイッチ
  static byte toggle = HIGH;

  // フラッシュ有効かつON状態のLEDを点滅させる
  portENTER_CRITICAL(&timerMux);
  for (int ch = 0; ch < conf.led_num; ++ch)
    if (led_val[ch] && bitRead(led_isFlash, ch))
      digitalWrite(conf.pin.led[ch], toggle);
    else digitalWrite(conf.pin.led[ch], led_val[ch]);
  toggle = !toggle;
  portEXIT_CRITICAL(&timerMux);
}

/*******************************************************************************
  LED制御
*******************************************************************************/

void led_op(int ch)
{
  // 押されたボタンに対応するチャネルのON/OFF状態を反転して点灯／消灯する
  portENTER_CRITICAL(&timerMux);
  digitalWrite(conf.pin.led[ch], led_val[ch] = !led_val[ch]);
  portEXIT_CRITICAL(&timerMux);
}

/*******************************************************************************
  ウインカー制御
*******************************************************************************/

void winker_op(int ch)
{
  // 排他連動するチャネル
  int exch = (ch == LWINK_CH) ? RWINK_CH : LWINK_CH;

  // LWINK_CHとRWINK_CHを排他連動させる
  if (led_val[LWINK_CH] == led_val[RWINK_CH])
    led_op((led_val[exch] ? exch : ch));
  else {
    if (led_val[exch]) led_op(exch);
    led_op(ch);
  }
}

/*******************************************************************************
  ハザード制御
*******************************************************************************/

void hazard_op()
{
  // LWINK_CHとRWINK_CHを同期連動させる
  if (led_val[LWINK_CH] == led_val[RWINK_CH]) {
    led_op(LWINK_CH);
    led_op(RWINK_CH);
  }
  else led_op(led_val[LWINK_CH] == HIGH ? RWINK_CH : LWINK_CH);
}

/*******************************************************************************
  LED初期化
*******************************************************************************/

void led_init()
{
  // 全LEDをピンモード設定して初期化（消灯）する
  for (int ch = 0; ch < conf.led_num; ++ch) {
    pinMode(conf.pin.led[ch], OUTPUT);
    digitalWrite(conf.pin.led[ch], led_val[ch]);
  }

  // タイマー割り込みを有効にする
  // タイマー番号=0、分周比=80:80MHz/80=1マイクロ秒、カウントアップ、エッジタイプ、自動再起動
  hw_timer_t *flash_timer = timerBegin(0, 80, true);
  timerAttachInterrupt(flash_timer, &led_flash, true);
  timerAlarmWrite(flash_timer, conf.led_interval, true);
  timerAlarmEnable(flash_timer);
}

/*******************************************************************************
  緊急制御
*******************************************************************************/

void emergency_op()
{
  // 緊急処理中
  isEmargency = 1;

  // 全サーボをニュートラルに戻す
  for (int ch = 0; ch < conf.servo_num; ++ch) servo_op(ch, 0);

  // 全LEDを点滅する
  for (int ch = 0; ch < conf.led_num; ++ch) {
    led_val[ch] = HIGH;
    led_isFlash = 0xffff;
  }

  // ブザーを10回鳴らす
  buzzer_op(10, 20, 100);
  DPRINTF("shutdown!!!\n");

  // 念の為もう一度全サーボをニュートラルに戻す
  for (int ch = 0; ch < conf.servo_num; ++ch) servo_op(ch, 0);

  // スリープする
  delay(1000);
  esp_deep_sleep_start();
}

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

void notify()
{
  // 緊急処理中はイベントを無視する
  if (isEmargency) return;

  // アナログスティックイベント
  // アナログスティック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 (THROTTLE_CH == L1L2_CH) led_op(BRAKE_CH);
  }
  if (Ps3.event.button_up.l1) {
    bitClear(lr1_bn, 0);
    // ブレーキランプ
    if (THROTTLE_CH == L1L2_CH) led_op(BRAKE_CH);
  }
  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 (THROTTLE_CH == R1R2_CH) led_op(BRAKE_CH);
  }
  if (Ps3.event.button_up.r1) {
    bitClear(lr1_bn, 1);
    // ブレーキランプ
    if (THROTTLE_CH == R1R2_CH) led_op(BRAKE_CH);
  }
  if (Ps3.event.analog_changed.button.r2)
    servo_op(R1R2_CH, Ps3.data.analog.button.r2
             / (bitRead(lr1_bn, 1) ? -2 : 2));

  // セレクト／スタート／PSボタンイベント
  // PSボタンプレス（サーボ調整チェック）
  if (Ps3.event.button_down.ps) servo_check();
  // セレクトボタンプレス（サーボ調整用チャンネル選択）
  if (Ps3.event.button_down.select) channel = select_ch(channel);
  // スタートボタンプレス（ハザード）
  if (Ps3.event.button_down.start) hazard_op();

  // サーボ調整修飾用ボタンイベント（0:□、1:△、2:○、3:☓）
  // □ボタンプレス／リリース（ニュートラル調整用）
  if (Ps3.event.button_down.square) bitSet(servo_trim_bn, 0);
  if (Ps3.event.button_up.square) bitClear(servo_trim_bn, 0);
  // △ボタンプレス／リリース（舵角調整用）
  if (Ps3.event.button_down.triangle) bitSet(servo_trim_bn, 1);
  if (Ps3.event.button_up.triangle) bitClear(servo_trim_bn, 1);
  // ○ボタンプレス／リリース（反転用）
  if (Ps3.event.button_down.circle) bitSet(servo_trim_bn, 2);
  if (Ps3.event.button_up.circle) bitClear(servo_trim_bn, 2);
  // ☓ボタンプレス／リリース（未使用）
  if (Ps3.event.button_down.cross) bitSet(servo_trim_bn, 3);
  if (Ps3.event.button_up.cross) bitClear(servo_trim_bn, 3);

  // Dパッドボタンイベント（0:左、1:上、2:右、3:下）
  // サーボ調整情操作
  if (servo_trim_bn) {
    // 下げる
    if (Ps3.event.button_down.left) servo_trim(0);
    else if (Ps3.event.button_down.down) servo_trim(3);
    // 上げる
    else if (Ps3.event.button_down.up) servo_trim(1);
    else if (Ps3.event.button_down.right) servo_trim(2);
  }

  // LED操作
  else {
    // ヘッドライトとテールライト
    if (Ps3.event.button_down.up) {
      led_op(HEAD_CH);
      led_op(TAIL_CH);
    }
    // フォグライト
    else if (Ps3.event.button_down.down) led_op(FOG_CH);
    // 左ウインカー
    else if (Ps3.event.button_down.left) winker_op(LWINK_CH);
    // 右ウインカー
    else if (Ps3.event.button_down.right) winker_op(RWINK_CH);
  }

  // バッテリーイベント（PS3コントローラのバッテリー切れ時の緊急停止）
  if (Ps3.data.status.battery == ps3_status_battery_dying
      || Ps3.data.status.battery == ps3_status_battery_shutdown)
    emergency_op();

  // L3ボタン押下イベント（サーボ調整情報保存）
  if (Ps3.event.button_down.l3) write_trim();
}

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

void onConnect()
{
  // ブザーを２回鳴らしてハザード解除する
  buzzer_op(2, 20, 100);
  hazard_op();

  // サーボ調整用チャンネルを初期化する
  select_ch(-1);
  DPRINTF("Connected!\n");
}

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

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

  // PS3コントローラを接続する
  Ps3.attach(notify);
  Ps3.attachOnConnect(onConnect);
  Ps3.attachOnDisconnect(emergency_op);
  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");

  // ハザード点滅してブザーを１回鳴らす
  hazard_op();
  buzzer_op(1, 20, 0);
}

/*******************************************************************************
  電源チェック
*******************************************************************************/

void power_check()
{
  static int count = 5;

  // 電源監視
  int value = analogRead(conf.pin.adc);
  float voltage = value * 2 * (3.6 / 4096);
  DPRINTF("value = %4d, voltage = %3.2f count = %d\n", value, voltage, count);
  if (voltage < conf.min_voltage)
    // 突発的な低下は5回まで見逃してやる
    if (--count <= 0) emergency_op();
}

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

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

  // トリム情報を入力して初期化する
  pref.begin(NAME_SPACE, false);
  read_trim();

  // ブザー、サーボ、LEDを初期化する
  buzzer_init();
  servo_init();
  led_init();

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

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

void loop()
{
  // 電源チェック
  if (Ps3.isConnected()) power_check();
  delay(1000);
}