ロボット制作⑱ 頭をつくってみる
ロボットヘッドをアニマトロニクス制御で動かしてみた。

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

// PWMコントローラー
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);

// サーボモータの定数値
#define SERVO_FREQ      50        // 50Hz
#define SG90_SERVOMIN   500       // SG90の最小パルス幅（μs）
#define SG90_SERVOMAX   2500      // SG90の最大パルス幅（μs）
#define SG90_SERVOMID   1500      // SG90の中間パルス幅（μs）
#define MG995_SERVOMIN  500       // MG995の最大パルス幅（μs）
#define MG995_SERVOMAX  2500      // MG995の最小パルス幅（μs）
#define MG995_SERVOMID  1500      // MG995の中間パルス幅（μs）
// ジョイスティックの定数値
#define JS_MIN          0
#define JS_MID          512
#define JS_MAX          1023
// デバッグ用
#define SKIP_COUNT      200
#define STOP_COUNT      0
int loop_count          = 0;

// 各ジョイスティック、スイッチの接続先ピン
int RX_PIN    = A0;   // 右ジョイスティックＸ軸
int RY_PIN    = A1;   // 右ジョイスティックＹ軸
int RSW_PIN   = 3;    // 右ジョイスティックボタン
int LX_PIN    = A2;   // 左ジョイスティックＸ軸
int LY_PIN    = A3;   // 左ジョイスティックＹ軸
int LSW_PIN   = 4;    // 左ジョイスティックボタン
int RTSW_PIN  = 5;    // 右タクトスイッチ
int LTSW_PIN  = 6;    // 左タクトスイッチ

// 各ジョイスティック、スイッチの位置
int RX_pos;       // 右目Ｘ軸のジョイスティック位置
int RY_pos;       // 右目Ｙ軸のジョイスティック位置
int RSW_flg;      // 右目ボタン（1が押してないとき、0が押された時）
int RZ_pos;       // 右瞼のジョイスティック位置（仮想）
int LX_pos;       // 左目Ｘ軸のジョイスティック位置
int LY_pos;       // 左目Ｙ軸のジョイスティック位置
int LSW_flg;      // 左目ボタン（1が押してないとき、0が押された時）
int LZ_pos;       // 左瞼のジョイスティック位置（仮想）
int RTSW_flg;     // 右タクトスイッチ（0が押してないとき、1が押された時）
int LTSW_flg;     // 左タクトスイッチ（0が押してないとき、1が押された時）
int NECK_pos;     // 首のジョイスティック位置（仮想）

// 各モーターのピン
int SERVO_L1    = 0;    // 左目の上下
int SERVO_L2    = 1;    // 左目の左右
int SERVO_L3    = 2;    // 左目の瞼開閉
int SERVO_R1    = 3;    // 右目の上下
int SERVO_R2    = 4;    // 右目の左右
int SERVO_R3    = 5;    // 右目の瞼開閉
int SERVO_N1    = 6;    // 首の左右回転

// サーボモーターをスムーズに動かすための変数
double L1_Scaled, L1_Smoothed, L1_SmoothedPrev;
double L2_Scaled, L2_Smoothed, L2_SmoothedPrev;
double L3_Scaled, L3_Smoothed, L3_SmoothedPrev;
double R1_Scaled, R1_Smoothed, R1_SmoothedPrev;
double R2_Scaled, R2_Smoothed, R2_SmoothedPrev;
double R3_Scaled, R3_Smoothed, R3_SmoothedPrev;
double N1_Scaled, N1_Smoothed, N1_SmoothedPrev;

void setup() {
  // 入力ピンのセットアップ
  pinMode(RX_PIN, INPUT);
  pinMode(RY_PIN, INPUT);
  pinMode(RSW_PIN, INPUT);
  pinMode(LX_PIN, INPUT);
  pinMode(LY_PIN, INPUT);
  pinMode(LSW_PIN, INPUT);
  digitalWrite(RSW_PIN, HIGH);
  digitalWrite(LSW_PIN, HIGH);
  pinMode(RTSW_PIN, INPUT);
  pinMode(LTSW_PIN, INPUT);
  // サーボモータを初期化する。
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);
  initServoPos();
  // シリアルモニタの初期化
  Serial.begin(115200);
  delay(500);
}

/***********************************************************
 * サーボモータの位置の初期化
 ***********************************************************/
void initServoPos() {
  pwm.writeMicroseconds(SERVO_L1, MG995_SERVOMID);
  pwm.writeMicroseconds(SERVO_L2, SG90_SERVOMID);
  pwm.writeMicroseconds(SERVO_L3, SG90_SERVOMID);
  pwm.writeMicroseconds(SERVO_R1, MG995_SERVOMID);
  pwm.writeMicroseconds(SERVO_R2, SG90_SERVOMID);
  pwm.writeMicroseconds(SERVO_R3, SG90_SERVOMID);
  pwm.writeMicroseconds(SERVO_N1, MG995_SERVOMID);
}

void loop() {
  // ループカウントアップ
  loop_count++;
  // 入力値の取得
  RY_pos = analogRead(RY_PIN);
  RX_pos = analogRead(RX_PIN);
  RSW_flg = digitalRead(RSW_PIN);
  if (RSW_flg) { RZ_pos = JS_MID; } else { RZ_pos = JS_MIN; }
  LY_pos = analogRead(LY_PIN);
  LX_pos = analogRead(LX_PIN);
  LSW_flg = digitalRead(LSW_PIN);
  if (LSW_flg) { LZ_pos = JS_MID; } else { LZ_pos = JS_MAX; }
  RTSW_flg = digitalRead(RTSW_PIN);
  LTSW_flg = digitalRead(LTSW_PIN);
  if (RTSW_flg && !LTSW_flg) { NECK_pos = JS_MAX; }
  else if (!RTSW_flg && LTSW_flg) { NECK_pos = JS_MIN; }
  else { NECK_pos = JS_MID; }
  // スケーリング
  R1_Scaled = ((RY_pos - JS_MID) * -1.5) + MG995_SERVOMID;
  R2_Scaled = ((RX_pos - JS_MID) *  1.8) + SG90_SERVOMID;
  R3_Scaled = ((RZ_pos - JS_MID) *  1.2) + SG90_SERVOMID;
  L1_Scaled = ((LY_pos - JS_MID) *  1.5) + MG995_SERVOMID;
  L2_Scaled = ((LX_pos - JS_MID) *  1.8) + SG90_SERVOMID;
  L3_Scaled = ((LZ_pos - JS_MID) *  1.2) + SG90_SERVOMID;
  N1_Scaled = ((NECK_pos - JS_MID) * -1.6) + MG995_SERVOMID;
  // スムーズ補正
  R1_Smoothed = (R1_Scaled * 0.02) + (R1_SmoothedPrev * 0.98);
  R2_Smoothed = (R2_Scaled * 0.02) + (R2_SmoothedPrev * 0.98);
  R3_Smoothed = (R3_Scaled * 0.05) + (R3_SmoothedPrev * 0.95);
  L1_Smoothed = (L1_Scaled * 0.02) + (L1_SmoothedPrev * 0.98);
  L2_Smoothed = (L2_Scaled * 0.02) + (L2_SmoothedPrev * 0.98);
  L3_Smoothed = (L3_Scaled * 0.05) + (L3_SmoothedPrev * 0.95);
  N1_Smoothed = (N1_Scaled * 0.02) + (N1_SmoothedPrev * 0.98);
  // スムーズ補正を保存する（次回使うために）
  R1_SmoothedPrev = R1_Smoothed;
  R2_SmoothedPrev = R2_Smoothed;
  R3_SmoothedPrev = R3_Smoothed;
  L1_SmoothedPrev = L1_Smoothed;
  L2_SmoothedPrev = L2_Smoothed;
  L3_SmoothedPrev = L3_Smoothed;
  N1_SmoothedPrev = N1_Smoothed;
  // シリアルプリント
  Serial.print(R1_Smoothed);
  Serial.print(" , ");
  Serial.print(R2_Smoothed);
  Serial.print(" , ");
  Serial.print(R3_Smoothed);
  Serial.print(" , ");
  Serial.print(L1_Smoothed);
  Serial.print(" , ");
  Serial.print(L2_Smoothed);
  Serial.print(" , ");
  Serial.print(L3_Smoothed);
  Serial.print(" , ");
  Serial.println(N1_Smoothed);
  // 起動直後は、不安定なためモーター駆動はスキップする
  if (SKIP_COUNT > loop_count) {
    return;
  } else if  (SKIP_COUNT == loop_count) {
    Serial.println("Start the servo motor operation.");
  }
  // サーボモーターへ出力
  pwm.writeMicroseconds(SERVO_R1, (int)R1_Smoothed);
  pwm.writeMicroseconds(SERVO_R2, (int)R2_Smoothed);
  pwm.writeMicroseconds(SERVO_R3, (int)R3_Smoothed);
  pwm.writeMicroseconds(SERVO_L1, (int)L1_Smoothed);
  pwm.writeMicroseconds(SERVO_L2, (int)L2_Smoothed);
  pwm.writeMicroseconds(SERVO_L3, (int)L3_Smoothed);
  pwm.writeMicroseconds(SERVO_N1, (int)N1_Smoothed);
  delay(5);

  if (STOP_COUNT > 0 && loop_count > STOP_COUNT) {
    exit(0);
  }
}