パパ日記

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

// 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);
}