Arduinoでエンコーダ付きDCモータの回転速度、回転角を制御する方法

// PWMを変化させPRMを測定する

// ----- ピン定義 -----
const int PIN_ENCODER_A = 2;    // 右モータ エンコーダ A相 (INT0)
const int PIN_ENCODER_B = 12;   // 右モータ エンコーダ B相 (配線表の左モータ表記箇所)
const int PIN_AIN1      = 4;    // TB6612 AIN1 (回転方向制御 1)
const int PIN_AIN2      = 5;    // TB6612 AIN2 (回転方向制御 2)
const int PIN_PWMA      = 6;    // TB6612 PWMA (速度制御 PWM)
const int PIN_STBY      = 11;   // TB6612 STBY (スタンバイ)
const int PIN_IR        = A0;   // IRセンサ (入力設定のみ)
const int PIN_LED       = A1;   // ステータス用 LED

// ----- 定数・変数定義 -----
const float PULSES_PER_REV = 495.0; // 1回転あたりのエンコーダパルス数
const unsigned long MEASURE_TIME_MS = 10000; // 測定時間 (10秒)

volatile long encoderCount = 0; // パルスカウント用 (加減算対応)

// パルスカウント用割り込みサービスルーチン (ISR)
// A相の立ち上がり時にB相の状態を見て回転方向を判別
void countEncoder() {
  if (digitalRead(PIN_ENCODER_B) == LOW) {
    encoderCount++; // 時計方向（正転）：加算
  } else {
    encoderCount--; // 反時計方向（逆転）：減算
  }
}

// モータ駆動ヘルパー関数
// speed: -255 〜 255 (正: 時計方向, 負: 反時計方向, 0: 停止)
void setMotorSpeed(int speed) {
  if (speed > 0) {
    digitalWrite(PIN_AIN1, HIGH);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, speed);
  } else if (speed < 0) {
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, HIGH);
    analogWrite(PIN_PWMA, -speed);
  } else {
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, 0);
  }
}

// 測定関数
void runTestSequence(const char* directionName, bool isClockwise) {
  Serial.print("=== テスト開始: ");
  Serial.print(directionName);
  Serial.println(" ===");
  Serial.println("PWM\tカウント値\tRPM");

  // 32ステップ刻みでPWMを変化 (0, 32, 64, 96, 128, 160, 192, 224, 255)
  for (int step = 0; step <= 8; step++) {
    int pwmVal = step * 32;
    if (pwmVal > 255) pwmVal = 255;

    // LED点灯 (測定中表示)
    digitalWrite(PIN_LED, HIGH);

    // エンコーダカウント初期化
    noInterrupts();
    encoderCount = 0;
    interrupts();

    // モータ回転開始
    int motorCmd = isClockwise ? pwmVal : -pwmVal;
    setMotorSpeed(motorCmd);

    // 10秒間計測
    delay(MEASURE_TIME_MS);

    // モータ一時停止 & カウント取得
    setMotorSpeed(0);
    noInterrupts();
    long currentCount = encoderCount;
    interrupts();

    // LED消灯
    digitalWrite(PIN_LED, LOW);

    // RPMの計算（カウント値の正負に対応するため abs を使用して絶対値換算）
    // (カウント数 / 495) = 回転数(10秒間)
    // 10秒間の回転数 * 6 = 1分あたりの回転数(RPM)
    float rpm = (float(labs(currentCount)) / PULSES_PER_REV) * 6.0;

    // シリアル出力 (カウント値は負の値のまま表示)
    Serial.print(pwmVal);
    Serial.print("\t");
    Serial.print(currentCount);
    Serial.print("\t\t");
    Serial.println(rpm, 2); // 小数点以下2桁表示

    // ステップ間のインターバル (1秒)
    delay(1000);
  }
  Serial.println();
}

void setup() {
  Serial.begin(115200);

  // ピンモード設定
  pinMode(PIN_ENCODER_A, INPUT_PULLUP);
  pinMode(PIN_ENCODER_B, INPUT_PULLUP);
  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_PWMA, OUTPUT);
  pinMode(PIN_STBY, OUTPUT);
  pinMode(PIN_IR, INPUT);
  pinMode(PIN_LED, OUTPUT);

  // モータドライバを動作許可状態にする
  digitalWrite(PIN_STBY, HIGH);
  digitalWrite(PIN_LED, LOW);

  // 外部割り込みの有効化 (D2ピンの立ち上がりエッジで割込発生)
  attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_A), countEncoder, RISING);

  Serial.println("SGM25 Test スケッチが起動しました。");
  delay(2000);

  // --- 1) 時計方向 (CW) のテスト ---
  runTestSequence("時計方向 (CW)", true);

  // 方向転換前のインターバル (2秒)
  delay(2000);

  // --- 2) 反時計方向 (CCW) のテスト ---
  runTestSequence("反時計方向 (CCW)", false);

  Serial.println("すべてのテストが完了しました。");
}

void loop() {
  // テスト完了後は待機状態（LED低速点滅）
  digitalWrite(PIN_LED, HIGH);
  delay(500);
  digitalWrite(PIN_LED, LOW);
  delay(500);
}

// PWMをsin()変化させ回転角度を測定する

#include <Arduino.h>

// ----- ピン定義 -----
const int PIN_ENCODER_A = 2;    // 右モータ エンコーダ A相 (INT0)
const int PIN_ENCODER_B = 12;   // 右モータ エンコーダ B相
const int PIN_AIN1      = 4;    // TB6612 AIN1 (回転方向制御 1)
const int PIN_AIN2      = 5;    // TB6612 AIN2 (回転方向制御 2)
const int PIN_PWMA      = 6;    // TB6612 PWMA (速度制御 PWM)
const int PIN_STBY      = 11;   // TB6612 STBY (スタンバイ)
const int PIN_IR        = A0;   // IRセンサ (入力設定)
const int PIN_LED       = A1;   // ステータス用 LED

// ----- 定数・設定変数 -----
const float PULSES_PER_REV = 495.0; // 1回転あたりのエンコーダパルス数
const float SIN_PERIOD_MS  = 10000.0; // sin波の周期 (10秒 = 10000ms)
const int   MAX_PWM_AMPLITUDE = 128;   // PWMの最大振幅

// シリアルプロッタ出力の間隔 (ms) ※変数で変更可能
const unsigned long PLOT_INTERVAL_MS = 500; 

// ----- 内部変数 -----
volatile long encoderCount = 0; // パルスカウント用 (正転: +, 逆転: -)
unsigned long startTime = 0;
unsigned long lastPlotTime = 0;

// エンコーダパルスカウント用割り込みサービスルーチン (ISR)
void countEncoder() {
  if (digitalRead(PIN_ENCODER_B) == LOW) {
    encoderCount++; // 正転（時計方向）：加算
  } else {
    encoderCount--; // 逆転（反時計方向）：減算
  }
}

// モータ駆動関数
// speed: -128 〜 128 (正: 時計方向, 負: 反時計方向, 0: 停止)
void setMotorSpeed(int speed) {
  if (speed > 0) {
    digitalWrite(PIN_AIN1, HIGH);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, speed);
  } else if (speed < 0) {
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, HIGH);
    analogWrite(PIN_PWMA, -speed);
  } else {
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, 0);
  }
}

void setup() {
  // ボーレート 115200 bps
  Serial.begin(115200);

  // ピンモード設定
  pinMode(PIN_ENCODER_A, INPUT_PULLUP);
  pinMode(PIN_ENCODER_B, INPUT_PULLUP);
  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_PWMA, OUTPUT);
  pinMode(PIN_STBY, OUTPUT);
  pinMode(PIN_IR, INPUT);
  pinMode(PIN_LED, OUTPUT);

  // モータドライバ有効化 & ステータスLED点灯
  digitalWrite(PIN_STBY, HIGH);
  digitalWrite(PIN_LED, HIGH);

  // 外部割り込み有効化 (D2ピンの立ち上がりエッジで割込発生)
  attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_A), countEncoder, RISING);

  // シリアルプロッタ用ラベル出力 (必要に応じて自動スケール用)
  Serial.println("PWM_Cmd,Encoder_Count,Angle_Deg");

  startTime = millis();
}

void loop() {
  unsigned long currentTime = millis();
  unsigned long elapsedTime = currentTime - startTime;

  // 1) 10秒周期の sin波で PWM指令値を計算 (振幅: -128 〜 +128)
  // 経過時間 (ms) から角度 rad を求める: (elapsedTime / 10000.0) * 2 * PI
  float radiansVal = (float)elapsedTime / SIN_PERIOD_MS * 2.0 * PI;
  int pwmCmd = -round(sin(radiansVal) * MAX_PWM_AMPLITUDE);

  // モータに出力
  setMotorSpeed(pwmCmd);

  // 2) 指定間隔 (PLOT_INTERVAL_MS) ごとにシリアルプロッタ/モニタに出力
  if (currentTime - lastPlotTime >= PLOT_INTERVAL_MS) {
    lastPlotTime = currentTime;

    // 割込保護してカウント値を取得
    noInterrupts();
    long currentCount = encoderCount;
    interrupts();

    // 回転角度の計算 (+/- 表示)
    float angleDeg = (float)currentCount / PULSES_PER_REV * 360.0;

// シリアルプロッタ用出力フォーマット (ラベル名:値)
    Serial.print("PWM_Cmd:");
    Serial.print(pwmCmd);
    Serial.print(",");
    Serial.print("Encoder_Count:");
    Serial.print(currentCount);
    Serial.print(",");
    Serial.print("Angle_Deg:");
    Serial.println(angleDeg, 2);
  }
}

// RPM値を目標設定しPID制御で収束させる 

#include <Arduino.h>
#include <IRremote.hpp> // IRremoteライブラリ (v3.0以降対応)

// ----- ピン定義 -----
const int PIN_ENCODER_A = 2;    // 右モータ エンコーダ A相 (INT0)
const int PIN_ENCODER_B = 12;   // 右モータ エンコーダ B相
const int PIN_AIN1      = 4;    // TB6612 AIN1 (回転方向制御 1)
const int PIN_AIN2      = 5;    // TB6612 AIN2 (回転方向制御 2)
const int PIN_PWMA      = 6;    // TB6612 PWMA (速度制御 PWM)
const int PIN_STBY      = 11;   // TB6612 STBY (スタンバイ)
const int PIN_IR        = A0;   // IRセンサ
const int PIN_LED       = A1;   // ステータス用 LED

// ----- 赤外線リモコンのキーコード定義 -----
#define IR_KEY_2 0x18  // RPM +10
#define IR_KEY_8 0x52  // RPM -10
#define IR_KEY_5 0x1C  // RPM 0 (即時停止)

// ----- エンコーダ & 物理定数 -----
const float PULSES_PER_REV = 495.0; // 立ち上がりのみ(RISING)割込用

// ----- PID制御ゲインパラメータ -----
float Kp = 1.2;   // 比例ゲイン
float Ki = 0.8;   // 積分ゲイン
float Kd = 0.0;  // 微分ゲイン

// ----- 制御用変数 -----
volatile long encoderCount = 0; // 累積パルスカウント
long lastEncoderCount = 0;

float targetRPM = 0.0;   // 目標RPM (-80 〜 +80)
float currentRPM = 0.0;  // 現在の測定RPM (回転速度測定値)
int   currentPWM = 0;    // モータへの出力PWM値 (-255 〜 255)

float errorSum = 0.0;
float lastError = 0.0;

// ----- 基準時間変数 -----
unsigned long pidLastTime = 0;
unsigned long plotLastTime = 0;
unsigned long pwmSetTime = 0; 

// 出力間隔 (ms)
const unsigned long PLOT_INTERVAL_MS = 100;
const unsigned long PID_INTERVAL_MS = 50;

// エンコーダパルスカウント用割り込みサービスルーチン (ISR: 立ち上がりのみ)
void countEncoder() {
  if (digitalRead(PIN_ENCODER_B) == LOW) {
    encoderCount++; // 正転
  } else {
    encoderCount--; // 逆転
  }
}

// モータ駆動ヘルパー関数
void setMotorSpeed(int speed) {
  speed = constrain(speed, -255, 255);
  
  if (speed > 0) {
    digitalWrite(PIN_AIN1, HIGH);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, speed);
  } else if (speed < 0) {
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, HIGH);
    analogWrite(PIN_PWMA, -speed);
  } else {
    // 停止（両ピンLOW & PWM 0）
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, 0);
  }
}

void setup() {
  Serial.begin(115200);

  // ピン設定
  pinMode(PIN_ENCODER_A, INPUT_PULLUP);
  pinMode(PIN_ENCODER_B, INPUT_PULLUP);
  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_PWMA, OUTPUT);
  pinMode(PIN_STBY, OUTPUT);
  pinMode(PIN_LED, OUTPUT);

  // モータドライバ有効化
  digitalWrite(PIN_STBY, HIGH);
  digitalWrite(PIN_LED, HIGH);

  // 赤外線受信開始
  IrReceiver.begin(PIN_IR, DISABLE_LED_FEEDBACK);

  // 外部割り込み有効化 (RISING)
  attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_A), countEncoder, RISING);

  pwmSetTime = millis();
  pidLastTime = millis();
  plotLastTime = millis();
}

void loop() {
  unsigned long now = millis();

  // --------------------------------------------------
  // 1) 赤外線リモコン受信処理
  // --------------------------------------------------
  if (IrReceiver.decode()) {
    if (!(IrReceiver.decodedIRData.flags & IRDATA_FLAGS_IS_REPEAT)) {
      uint16_t command = IrReceiver.decodedIRData.command;

      if (command == IR_KEY_2) {
        targetRPM += 10.0;
        if (targetRPM > 80.0) targetRPM = 80.0;
        pwmSetTime = now;
      } else if (command == IR_KEY_8) {
        targetRPM -= 10.0;
        if (targetRPM < -80.0) targetRPM = -80.0;
        pwmSetTime = now;
      } else if (command == IR_KEY_5) {
        // ボタン5：回転即時停止処理
        targetRPM = 0.0;
        currentPWM = 0;
        errorSum = 0.0;    // 積分項のリセット
        lastError = 0.0;   // 微分項のリセット
        setMotorSpeed(0);  // 即時停止
        pwmSetTime = now;
      }
    }
    IrReceiver.resume();
  }

// --------------------------------------------------
  // 2) PID制御演算 (周期: PID_INTERVAL_MS)
  // --------------------------------------------------
  if (now - pidLastTime >= PID_INTERVAL_MS) {
    float dt = (now - pidLastTime) / 1000.0; // 秒単位
    pidLastTime = now;

    // 現在のカウントを取得 (割込保護)
    noInterrupts();
    long currentCount = encoderCount;
    interrupts();

    // 周期内でのパルス変化量から RPM を計算
    long deltaCount = currentCount - lastEncoderCount;
    lastEncoderCount = currentCount;

    // 生のRPM計算
    float rawRPM = ((float)deltaCount / PULSES_PER_REV) * (60.0 / dt);
    
    // 一次ローパスフィルタ (前回70% + 今回30%)
    currentRPM = (currentRPM * 0.7) + (rawRPM * 0.3);

    if (targetRPM == 0.0) {
      currentPWM = 0;
      errorSum = 0.0;
      lastError = 0.0;
      setMotorSpeed(0);
    } else {
      // 偏差の計算
      float error = targetRPM - currentRPM;

      // 積分項の加算 (Ki を 0.5〜1.0 に上げて速やかに追従させる)
      errorSum += error * dt;

      // errorSum 自体ではなく、積分出力(Ki * errorSum)の段階で制限をかける
      float pTerm = Kp * error;
      float iTerm = Ki * errorSum;
      iTerm = constrain(iTerm, -255.0, 255.0); // 積分出力をPWM全域許可
      
      float dError = (error - lastError) / dt;
      float dTerm = Kd * dError;
      lastError = error;

      // PID出力計算
      float output = pTerm + iTerm + dTerm;
      
      // 出力PWMの制限 (-255 〜 255)
      currentPWM = constrain(round(output), -255, 255);

      // モータにPWM出力
      setMotorSpeed(currentPWM);
    }
  }

  // --------------------------------------------------
  // 3) シリアルプロッタ / モニタ出力 (間引き出力)
  // --------------------------------------------------
  if (now - plotLastTime >= PLOT_INTERVAL_MS) {
    plotLastTime = now;

    unsigned long elapsedTime = now - pwmSetTime;

    // シリアルプロッタ用KeyValueフォーマット出力
    Serial.print("ElapsedTime_ms:");
    Serial.print(elapsedTime);
    Serial.print(",");
    Serial.print("Target_RPM:");
    Serial.print(targetRPM);
    Serial.print(",");
    Serial.print("PWM_Cmd:");
    Serial.print(currentPWM);
    Serial.print(",");
    Serial.print("Measured_RPM:");
    Serial.println(currentRPM, 2); // 回転角度のかわりに回転速度測定値を表示
  }
}

// 回転角度を目標設定しPID制御で収束させる

#include <Arduino.h>
#include <IRremote.hpp> // IRremoteライブラリ (v3.0以降対応)

// ----- ピン定義 -----
const int PIN_ENCODER_A = 2;    // 右モータ エンコーダ A相 (INT0)
const int PIN_ENCODER_B = 12;   // 右モータ エンコーダ B相
const int PIN_AIN1      = 4;    // TB6612 AIN1 (回転方向制御 1)
const int PIN_AIN2      = 5;    // TB6612 AIN2 (回転方向制御 2)
const int PIN_PWMA      = 6;    // TB6612 PWMA (速度制御 PWM)
const int PIN_STBY      = 11;   // TB6612 STBY (スタンバイ)
const int PIN_IR        = A0;   // IRセンサ
const int PIN_LED       = A1;   // ステータス用 LED

// ----- 赤外線リモコンのキーコード定義 (お使いのリモコンのコードに差し替えてください) -----
#define IR_KEY_2 0x18  // 目標角度 +30 deg
#define IR_KEY_8 0x52  // 目標角度 -30 deg
#define IR_KEY_5 0x1C  // 目標角度 0 deg (即時停止)

// ----- エンコーダ & 物理定数 -----
// 立ち上がりのみ(RISING)割込のため 495 パルス/回転
const float PULSES_PER_REV = 495.0; 

// ----- PID制御ゲインパラメータ (位置制御用調整値) -----
float Kp = 3.5;   // 比例ゲイン（角度差に応じて強く回す）
float Ki = 0.05;  // 積分ゲイン（静止前の微妙な隙間・摩擦を補正）
float Kd = 0.0;   // 微分ゲイン（目標直前でブレーキをかけてオーバーシュートを防止）

// ----- 制御用変数 -----
volatile long encoderCount = 0; // 累積パルスカウント

float targetAngle = 0.0;   // 目標回転角度 (-360.0 〜 +360.0 deg)
float currentAngle = 0.0;  // 現在の測定回転角度 (deg)
int   currentPWM = 0;    // モータへの出力PWM値 (-255 〜 255)

float errorSum = 0.0;
float lastError = 0.0;

// ----- 基準時間変数 -----
unsigned long pidLastTime = 0;
unsigned long plotLastTime = 0;
unsigned long pwmSetTime = 0; // 目標角度が変更されてからの経過時間計算用

// シリアルプロッタ出力の間隔 (ms)
const unsigned long PLOT_INTERVAL_MS = 100;
// PID演算の割り込み周期 (ms) - 位置制御のため20ms周期で高速演算
const unsigned long PID_INTERVAL_MS = 20;

// エンコーダパルスカウント用割り込みサービスルーチン (ISR: 立ち上がりのみ)
void countEncoder() {
  if (digitalRead(PIN_ENCODER_B) == LOW) {
    encoderCount++; // 正転（時計方向）：加算
  } else {
    encoderCount--; // 逆転（反時計方向）：減算
  }
}

// モータ駆動ヘルパー関数
void setMotorSpeed(int speed) {
  speed = constrain(speed, -255, 255);
  
  if (speed > 0) {
    digitalWrite(PIN_AIN1, HIGH);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, speed);
  } else if (speed < 0) {
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, HIGH);
    analogWrite(PIN_PWMA, -speed);
  } else {
    // 強制停止（両ピンLOW & PWM 0 でブレーキ）
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, 0);
  }
}

void setup() {
  Serial.begin(115200);

  // ピン設定
  pinMode(PIN_ENCODER_A, INPUT_PULLUP);
  pinMode(PIN_ENCODER_B, INPUT_PULLUP);
  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_PWMA, OUTPUT);
  pinMode(PIN_STBY, OUTPUT);
  pinMode(PIN_LED, OUTPUT);

  // モータドライバ有効化
  digitalWrite(PIN_STBY, HIGH);
  digitalWrite(PIN_LED, HIGH);

  // 赤外線受信開始
  IrReceiver.begin(PIN_IR, DISABLE_LED_FEEDBACK);

  // 外部割り込み有効化 (立ち上がりのみ: RISING)
  attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_A), countEncoder, RISING);

  pwmSetTime = millis();
  pidLastTime = millis();
  plotLastTime = millis();
}

void loop() {
  unsigned long now = millis();

  // --------------------------------------------------
  // 1) 赤外線リモコン受信処理
  // --------------------------------------------------
  if (IrReceiver.decode()) {
    if (!(IrReceiver.decodedIRData.flags & IRDATA_FLAGS_IS_REPEAT)) {
      uint16_t command = IrReceiver.decodedIRData.command;

      if (command == IR_KEY_2) {
        targetAngle += 30.0;
        if (targetAngle > 360.0) targetAngle = 360.0;
        pwmSetTime = now;
      } else if (command == IR_KEY_8) {
        targetAngle -= 30.0;
        if (targetAngle < -360.0) targetAngle = -360.0;
        pwmSetTime = now;
      } else if (command == IR_KEY_5) {
        // --- ボタン5：目標角度を0にし回転を強制停止 ---
        targetAngle = 0.0;
        currentPWM = 0;
        errorSum = 0.0;    // 積分値リセット
        lastError = 0.0;   // 微分値リセット
        setMotorSpeed(0);  // 即時ブレーキ出力
        pwmSetTime = now;
      }
    }
    IrReceiver.resume();
  }

  // --------------------------------------------------
  // 2) 位置PID制御演算 (周期: PID_INTERVAL_MS)
  // --------------------------------------------------
  if (now - pidLastTime >= PID_INTERVAL_MS) {
    float dt = (now - pidLastTime) / 1000.0; // 秒単位
    pidLastTime = now;

    // 現在のカウントを取得 (割込保護)
    noInterrupts();
    long currentCount = encoderCount;
    interrupts();

    // カウント数から現在の回転角度 (deg) を算出
    currentAngle = ((float)currentCount / PULSES_PER_REV) * 360.0;

    // 目標が0で静止指示が出ている場合
    if (targetAngle == 0.0 && abs(currentAngle) < 1.0) {
      currentPWM = 0;
      setMotorSpeed(0);
    } else {
      // 角度偏差の計算 (目標角度 - 現在角度)
      float error = targetAngle - currentAngle;

      // 積分項の計算 (静止間際の摩擦補正用)
      errorSum += error * dt;
      errorSum = constrain(errorSum, -500.0, 500.0);

      // 微分項の計算 (行き過ぎを抑えるダンパー効果)
      float dError = (error - lastError) / dt;
      lastError = error;

      // PID出力計算
      float pTerm = Kp * error;
      float iTerm = Ki * errorSum;
      float dTerm = Kd * dError;

      float output = pTerm + iTerm + dTerm;
      
      // PWM出力制限 (-255 〜 255)
      currentPWM = constrain(round(output), -255, 255);

      // モータにPWM出力
      setMotorSpeed(currentPWM);
    }
  }

  // --------------------------------------------------
  // 3) シリアルプロッタ / モニタ出力 (間引き出力: PLOT_INTERVAL_MS)
  // --------------------------------------------------
  if (now - plotLastTime >= PLOT_INTERVAL_MS) {
    plotLastTime = now;

    unsigned long elapsedTime = now - pwmSetTime;

    // シリアルプロッタ用 Output (KeyValueフォーマット)
    Serial.print("ElapsedTime_ms:");
    Serial.print(elapsedTime);
    Serial.print(",");
    Serial.print("Target_Angle:");
    Serial.print(targetAngle, 2);
    Serial.print(",");
    Serial.print("PWM_Cmd:");
    Serial.print(currentPWM);
    Serial.print(",");
    Serial.print("Measured_Angle:");
    Serial.println(currentAngle, 2);
  }
}

// 目標回転角度をsin()で変化させPID制御で追従させる

#include <Arduino.h>
#include <IRremote.hpp> // IRremoteライブラリ (v3.0以降対応)

// ----- ピン定義 -----
const int PIN_ENCODER_A = 2;    // 右モータ エンコーダ A相 (INT0)
const int PIN_ENCODER_B = 12;   // 右モータ エンコーダ B相
const int PIN_AIN1      = 4;    // TB6612 AIN1 (回転方向制御 1)
const int PIN_AIN2      = 5;    // TB6612 AIN2 (回転方向制御 2)
const int PIN_PWMA      = 6;    // TB6612 PWMA (速度制御 PWM)
const int PIN_STBY      = 11;   // TB6612 STBY (スタンバイ)
const int PIN_IR        = A0;   // IRセンサ
const int PIN_LED       = A1;   // ステータス用 LED

// ----- 赤外線リモコンのキーコード定義 (お使いのリモコンに合わせて調整してください) -----
#define IR_KEY_2 0x18  // 時計方向側から sin 追従スタート
#define IR_KEY_8 0x52  // 反時計方向側から sin 追従スタート
#define IR_KEY_5 0x1C  // 目標角度 0deg で強制停止

// ----- エンコーダ & 物理定数 -----
// 立ち上がりのみ(RISING)割込のため 495 パルス/回転
const float PULSES_PER_REV = 495.0; 

// ----- PID制御ゲインパラメータ (正弦波追従用) -----
float Kp = 4.0;   // 比例ゲイン（動的追従のため少し高めに設定）
float Ki = 0.1;   // 積分ゲイン
float Kd = 0.0;   // 微分ゲイン（目標値の変化速度に対するブレーキ・応答性向上面）

// ----- 制御用変数 -----
volatile long encoderCount = 0; // 累積パルスカウント

bool isRunning = false;    // sin追従動作中フラグ
float phaseDirection = 1.0; // 1.0: 時計方向スタート, -1.0: 反時計方向スタート

float targetAngle = 0.0;   // 目標回転角度 (deg)
float currentAngle = 0.0;  // 現在の測定回転角度 (deg)
int   currentPWM = 0;    // モータへの出力PWM値 (-255 〜 255)

float errorSum = 0.0;
float lastError = 0.0;

// ----- 基準時間変数 -----
unsigned long pidLastTime = 0;
unsigned long plotLastTime = 0;
unsigned long pwmSetTime = 0; // 目標波形がスタートしてからの経過時間計算用

// シリアルプロッタ出力の間隔 (ms)
const unsigned long PLOT_INTERVAL_MS = 500;
// PID演算の割り込み周期 (ms) - 連続追従のため20ms周期
const unsigned long PID_INTERVAL_MS = 20;

// エンコーダパルスカウント用割り込みサービスルーチン (ISR: 立ち上がりのみ)
void countEncoder() {
  if (digitalRead(PIN_ENCODER_B) == LOW) {
    encoderCount++; // 正転（時計方向）：加算
  } else {
    encoderCount--; // 逆転（反時計方向）：減算
  }
}

// モータ駆動ヘルパー関数
void setMotorSpeed(int speed) {
  speed = constrain(speed, -255, 255);
  
  if (speed > 0) {
    digitalWrite(PIN_AIN1, HIGH);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, speed);
  } else if (speed < 0) {
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, HIGH);
    analogWrite(PIN_PWMA, -speed);
  } else {
    // 強制停止（両ピンLOW & PWM 0）
    digitalWrite(PIN_AIN1, LOW);
    digitalWrite(PIN_AIN2, LOW);
    analogWrite(PIN_PWMA, 0);
  }
}

void setup() {
  Serial.begin(115200);

  // ピン設定
  pinMode(PIN_ENCODER_A, INPUT_PULLUP);
  pinMode(PIN_ENCODER_B, INPUT_PULLUP);
  pinMode(PIN_AIN1, OUTPUT);
  pinMode(PIN_AIN2, OUTPUT);
  pinMode(PIN_PWMA, OUTPUT);
  pinMode(PIN_STBY, OUTPUT);
  pinMode(PIN_LED, OUTPUT);

  // モータドライバ有効化
  digitalWrite(PIN_STBY, HIGH);
  digitalWrite(PIN_LED, HIGH);

  // 赤外線受信開始
  IrReceiver.begin(PIN_IR, DISABLE_LED_FEEDBACK);

  // 外部割り込み有効化 (立ち上がりのみ: RISING)
  attachInterrupt(digitalPinToInterrupt(PIN_ENCODER_A), countEncoder, RISING);

  pwmSetTime = millis();
  pidLastTime = millis();
  plotLastTime = millis();
}

void loop() {
  unsigned long now = millis();

  // --------------------------------------------------
  // 1) 赤外線リモコン受信処理
  // --------------------------------------------------
  if (IrReceiver.decode()) {
    if (!(IrReceiver.decodedIRData.flags & IRDATA_FLAGS_IS_REPEAT)) {
      uint16_t command = IrReceiver.decodedIRData.command;

      if (command == IR_KEY_2) {
        // 時計方向側から開始 (+180 deg に最初向かう sin 波)
        isRunning = true;
        phaseDirection = 1.0;
        pwmSetTime = now;
        errorSum = 0.0;
        lastError = 0.0;
      } else if (command == IR_KEY_8) {
        // 反時計方向側から開始 (-180 deg に最初向かう sin 波)
        isRunning = true;
        phaseDirection = -1.0;
        pwmSetTime = now;
        errorSum = 0.0;
        lastError = 0.0;
      } else if (command == IR_KEY_5) {
        // 強制停止処理
        isRunning = false;
        targetAngle = 0.0;
        currentPWM = 0;
        errorSum = 0.0;
        lastError = 0.0;
        setMotorSpeed(0);
        pwmSetTime = now;
      }
    }
    IrReceiver.resume();
  }

  // --------------------------------------------------
  // 2) 位置PID制御演算 (周期: PID_INTERVAL_MS)
  // --------------------------------------------------
  if (now - pidLastTime >= PID_INTERVAL_MS) {
    float dt = (now - pidLastTime) / 1000.0; // 秒単位
    pidLastTime = now;

    // 現在のカウントを取得 (割込保護)
    noInterrupts();
    long currentCount = encoderCount;
    interrupts();

    // カウント数から現在の回転角度 (deg) を算出
    currentAngle = ((float)currentCount / PULSES_PER_REV) * 360.0;

    if (isRunning) {
      // 経過時間 (秒)
      float elapsedSec = (now - pwmSetTime) / 1000.0;
      
      // 周期 10 秒の正弦波目標角度を生成 (±180度)
      // omega = 2 * PI / 10.0
      float omega = (2.0 * PI) / 10.0;
      targetAngle = phaseDirection * 180.0 * sin(omega * elapsedSec);

      // 角度偏差の計算 (目標角度 - 現在角度)
      float error = targetAngle - currentAngle;

      // 積分項
      errorSum += error * dt;
      errorSum = constrain(errorSum, -300.0, 300.0);

      // 微分項
      float dError = (error - lastError) / dt;
      lastError = error;

      // PID出力計算
      float pTerm = Kp * error;
      float iTerm = Ki * errorSum;
      float dTerm = Kd * dError;

      float output = pTerm + iTerm + dTerm;
      
      // PWM出力制限 (-255 〜 255)
      currentPWM = constrain(round(output), -255, 255);

      // モータにPWM出力
      setMotorSpeed(currentPWM);
    } else {
      // 停止モード時は PWM 0
      currentPWM = 0;
      setMotorSpeed(0);
    }
  }

  // --------------------------------------------------
  // 3) シリアルプロッタ / モニタ出力 (間引き出力: PLOT_INTERVAL_MS)
  // --------------------------------------------------
  if (now - plotLastTime >= PLOT_INTERVAL_MS) {
    plotLastTime = now;

    unsigned long elapsedTime = now - pwmSetTime;

    // シリアルプロッタ用 Output (KeyValueフォーマット)
    Serial.print("ElapsedTime_ms:");
    Serial.print(elapsedTime);
    Serial.print(",");
    Serial.print("Target_Angle:");
    Serial.print(targetAngle, 2);
    Serial.print(",");
    Serial.print("PWM_Cmd:");
    Serial.print(currentPWM);
    Serial.print(",");
    Serial.print("Measured_Angle:");
    Serial.println(currentAngle, 2);
  }
}