// センサレスBLDCモーター駆動 20230705_BLDCmotorDrive-4
// パワー調整、回転数表示、極数を2-16から選択可能。CPU内蔵コンパレーターで回転数検知
// 2023/7/5 ラジオペンチ http://radiopench.blog96.fc2.com/

#include <Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>  // OLEDを使用

#define SCREEN_WIDTH 128       // OLED display width, in pixels
#define SCREEN_HEIGHT 32       // OLED display height, in pixels
#define OLED_RESET    -1       // Reset pin # (or -1 if sharing Arduino reset pin)

#define MAX_SIG     2000       // PWM信号の最大のパルス幅[単位：us]
#define MIN_SIG     1000       // PWM信号の最小のパルス幅
#define MAX_SETTING 2005       // 最大出力設定(100%が出せるようちょっと大きめの値にした)
#define MIN_SETTING 1050       // 最小出力（ゼロにならないよう5%残した）
#define ESC_PIN 9              // ESCへのPWM信号出力ピン

#define cbi(addr, bit) addr &= ~(1 << bit)  // addrのbit目を'0'にする
#define sbi(addr, bit) addr |=  (1 << bit)  // addrのbit目を'1'にする

int poleN;                // モーターの極数
int pulseWidth = 0;       // ESCへの送信パルス幅
int x;
char cBuff[22];           // 文字列操作バッファ
volatile long tp;         // パルス周期（mag周期）
// long rpm;                 // 6万回転超えることもあるのでlong
long k1 = 1000000UL * 60; // 回転数(rpm)計算定数

Servo esc;                //Servoでescオブジェクトを作成

Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

ISR(ANALOG_COMP_vect) {       // アナログコンパレーター割込みハンドラ
  static long tLast = 0;
  static long tLastIrq = 0;
  static int magCycle = 0;
  long x;
  //  digitalWrite(13, HIGH); // 割込み状態観察
  x = micros();
  if ( x > tLastIrq + 100) {  // 前回割り込みから100us以上経過してた有効な信号と見なす（ノイズ対策)
    digitalWrite(13, HIGH);   // 割込み状態観察
    magCycle++;
    if (magCycle >= poleN) {  // 磁気角1回転で2回割り込みが入るので結局はpoleNのまま
      tp = x - tLast;         // 1回転に要した時間(us単位)を計算
      tLast = x;
      magCycle = 0;
    }
  }
  tLastIrq = x;               // 次回のチェック用に値を保存
  digitalWrite(13, LOW);
}

void setup() {
  Serial.begin(115200);
  pinMode(2, INPUT_PULLUP);  // 極数設定ピン
  pinMode(3, INPUT_PULLUP);  // 〃
  pinMode(4, INPUT_PULLUP);  // 〃
  pinMode(13, OUTPUT);

  poleN = readPoleN();       // ジャンパーを読んでモーターの極数を設定
  Serial.println(poleN);

  // アナログコンパレーターの設定
  cbi(ADCSRB, ACME);  // －入力はAIN1(D7)、平均コイル電圧を入力。コンパレータの入力マルチプレクサ不使用
  cbi(ACSR,   ACBG);  // ＋入力はAIN0(D6)、コイル電圧信号を入力。
  DIDR1 |= 0x03;      // D6,D7ピンからのデジタル入力禁止（下位2ビットに1)
  cbi(ACSR, ACIE);    // AC割込み停止（止めないと下記の設定で割込みが発生することがある）
  cbi(ACSR, ACIS1);   // 両側エッジで割り込み発生
  cbi(ACSR, ACIS0);   // 同上
  //  sbi(ACSR, ACIE);    // コンパレーター割込み許可

  // OLED 初期化
  oled.begin(SSD1306_SWITCHCAPVCC, 0x3C);        // OLED初期化
  oled.clearDisplay();
  oled.setTextColor(WHITE);                      // 白文字で表示

  esc.attach(ESC_PIN);  //ESCへの出力ピンをアタッチ
  escSetup();   // ESCを初期化
}

void loop() {
  x = analogRead(A0);
  pulseWidth = map(x, 0, 1023, MIN_SETTING, MAX_SETTING);  // パルス幅をスケーリング
  esc.writeMicroseconds(pulseWidth);                       // パルス幅 `pulseWidth` のPWM信号を送信する
  dispStatus();
  sbi(ACSR, ACIE);    // コンパレーター割込み許可(回転開始後に割り込み許可）
  delay(250);
}

int readPoleN() {  // ジャンパーを読んで極数を設定
  int n;
  n = (1 + !digitalRead(2) + !digitalRead(3) * 2 + !digitalRead(4) * 4) * 2;
  return n;
}

void escSetup() {                  // ESCの初期設定（電源投入直後に実施）
  oled.clearDisplay();
  oled.setCursor(0, 0);            // 表示位置指定
  oled.print("setting ESC");       // メッセージ表示
  oled.setCursor(0, 10);           // 表示位置指定
  oled.print("MagPole = ");        
  oled.print(poleN);               // 極数を表示
  oled.display();
  esc.writeMicroseconds(MAX_SIG);  //ESCへ最大のパルス幅を送信
  delay(2000);
  esc.writeMicroseconds(MIN_SIG);  //ESCへ最小のパルス幅を送信
  delay(2000);
}

void dispStatus() {             // OLEDに状態表示
  long s;
  int ss;
  s = k1 / tp;                  // 回転数(rpm)を計算
  s = (s / 10) * 10;            // 10で丸める

  oled.clearDisplay();
  sprintf(cBuff, "%5ldrpm", s); // 回転数を5桁の整数 + "rpm" の文字列に変換
  oled.setCursor(20, 0);        // 開始位置
  oled.setTextSize(2);          // 文字サイズを倍角に設定
  oled.print(cBuff);            // 回転数表示

  oled.setCursor(16, 16);       // 開始位置
  oled.print("P=");
  ss = (pulseWidth - MIN_SIG) * 100 / (MAX_SIG - MIN_SIG);  // パワーの値を計算
  sprintf(cBuff, "%3d%%", ss);  // パワーを3桁の整数 + "%" の文字列に変換
  oled.setCursor(44, 16);
  oled.print(cBuff);

  oled.display();               // OLEDに表示
}