ロータリエンコーダ付きステッピングモータをArduinoで制御して角度を取得する

// ステッピングモータ 制御コード

#include <TimerOne.h>

// ピンはどこでもOK
#define PIN_MTR_DIR 5
#define PIN_MTR_CK 6
#define PIN_MTR_CD 7

// 特性図
// https://www.orientalmotor.co.jp/file_addon/products/st/image/tj_pkp214u06[].gif
#define RPM_PER_PULSE_KHZ 300

bool pulseIsHigh = false;

void initSteppingMotor() {
  pinMode(PIN_MTR_DIR, OUTPUT);
  pinMode(PIN_MTR_CK, OUTPUT);
  pinMode(PIN_MTR_CD, OUTPUT);

  digitalWrite(PIN_MTR_DIR, LOW);
  digitalWrite(PIN_MTR_CK, LOW);
  digitalWrite(PIN_MTR_CD, HIGH);

  Timer1.initialize(1000*1000);
}

void stopPulse() {
  Timer1.detachInterrupt();
}
void startPulse() {
  Timer1.attachInterrupt(switchPulse);
}
void switchPulse() {
  pulseIsHigh = !pulseIsHigh;
  digitalWrite(PIN_MTR_CK, pulseIsHigh ? HIGH : LOW);
}

void setPulseLengthMillis(float pulseMillis) {
  int pulseMicros = pulseMillis * 1000;
  if (pulseMicros < 1) pulseMicros = 1;
  Timer1.setPeriod(pulseMicros);
}
void setPulseLengthByKHz(float khz) {
  float pulseMillis = 1.0 / khz;
  setPulseLengthMillis(pulseMillis);
}
void setPulseLengthByRPM(float rpm) {
  float pulseKHz = rpm / RPM_PER_PULSE_KHZ;
  setPulseLengthByKHz(pulseKHz);
}

void disableCurrentDown() {
  digitalWrite(PIN_MTR_CD, LOW);
}
void enableCurrentDown() {
  digitalWrite(PIN_MTR_CD, HIGH);
}

void setCW() {
  digitalWrite(PIN_MTR_DIR, LOW);
}
void setCCW() {
  digitalWrite(PIN_MTR_DIR, HIGH);
}

// ステッピングモータ 実行コード

int prevPhase = -1;
bool isClockwise = true;

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

  initSteppingMotor();
}

void loop() {
  long now = millis();
  int interval = 1000;
  int phase = (now / interval) % 6;


  if (phase != prevPhase) {
    Serial.print("[");
    Serial.print(now);
    Serial.print("] ");
    if (isClockwise) Serial.print("(C) ");
    else             Serial.print("(A) ");

    if (phase == 0) {
      Serial.println("stop");
      stopPulse();
      enableCurrentDown();
    }
    else if (phase == 1) {
      Serial.println("30rpm");

      disableCurrentDown();

      if (isClockwise) setCW();
      else             setCCW();
      isClockwise = !isClockwise;

      setPulseLengthByRPM(30);
      startPulse();
    }
    else if (phase == 2) {
      Serial.println("300rpm");
      setPulseLengthByRPM(300);
    }
    else if (phase == 3) {
      Serial.println("1000rpm");
      setPulseLengthByRPM(1000);
    }
    else if (phase == 4) {
      Serial.println("300rpm");
      setPulseLengthByRPM(300);
    }
    else if (phase == 5) {
      Serial.println("30rpm");
      setPulseLengthByRPM(30);
    }
    else {
      Serial.println("nope");
    }
  }

  prevPhase = phase;
}

// ロータリエンコーダ 計算コード

#define PPR 200 // p/r, pulse per revolution

#define PIN_RTE_A 2
#define PIN_RTE_B 3
#define PIN_RTE_Z 4

#define NONE 0
#define CW 1
#define CCW 2

volatile char direction = NONE; // NONE, CW, CCW
volatile int steps = 0;

volatile bool prevPinA = false;
volatile bool prevPinB = false;
volatile bool prevPinZ = false;

void initRotaryEncoder() {
  pinMode(PIN_RTE_A, INPUT_PULLUP);
  pinMode(PIN_RTE_B, INPUT_PULLUP);
  pinMode(PIN_RTE_Z, INPUT_PULLUP);

  attachInterrupt(0, onRotaryEncoderPulse, CHANGE); // pin2
  attachInterrupt(1, onRotaryEncoderPulse, CHANGE); // pin3
}

int getRotaryEncoderStep() {
  return steps;
}
float getRotaryEncoderAngleRate() {
  return (float)steps / PPR;
}
float getRotaryEncoderAngleDegrees() {
  return getRotaryEncoderAngleRate() * 360.0f;
}
float getRotaryEncoderAngleRadians() {
  return getRotaryEncoderAngleRate() * 2 * PI;
}

void onRotaryEncoderPulse() {
  bool pinZ = digitalRead(PIN_RTE_Z);
  if (pinZ && prevPinZ) steps = 0;

  bool pinA = !digitalRead(PIN_RTE_A);
  bool pinB = !digitalRead(PIN_RTE_B);

  if (pinA != prevPinA || pinB != prevPinB) {
    if (direction == NONE) {
      if (pinA) direction = CW;
      if (pinB) direction = CCW;
    }
    else {
      if (!pinA && !pinB) {
        if      (direction == CW  && prevPinB) steps++;
        else if (direction == CCW && prevPinA) steps--;

        if (steps < 0)    steps += PPR;
        if (PPR <= steps) steps -= PPR;

        direction = NONE;
      }
    }
  }

  prevPinA = pinA;
  prevPinB = pinB;
  prevPinZ = pinZ;
}

// ロータリエンコーダ 実行コード

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

  initRotaryEncoder();
}

void loop() {
  Serial.println(getRotaryEncoderAngleDegrees());
  delay(10);
}

// Requirements:
// #include <ArduinoJson.h>

String serialBuffer = "";

bool nextSerialLine(String *serialLine) {
  *serialLine = "";

  while (Serial.available() > 0) {
    int ch = Serial.read();
    if (ch != -1) {
      if (ch == '\n') {
        *serialLine = serialBuffer;
        serialBuffer = "";
        return true;
      }

      serialBuffer += (char) ch;
    }
  }

  return false;
}

bool nextSerialJson(JsonDocument *serialJson, bool *jsonError) {
  String serialLine = "";

  *jsonError = false;
  serialJson->clear();

  if (nextSerialLine(&serialLine)) {
    DeserializationError error;
    error = deserializeJson(*serialJson, serialLine);

    if (error == DeserializationError::Ok) {
      return true;
    }

    *jsonError = true;
    serialJson->clear();
  }

  return false;
}

{"rpm": 300}

#include <ArduinoJson.h>

#define MIN_RPM 30
#define MAX_RPM 3000
float rpm = 0;

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

  initRotaryEncoder();
  initSteppingMotor();
}

void loop() {
  float angleDegrees = getRotaryEncoderAngleDegrees();

  if (rpm < MIN_RPM) {
    rpm = 0;
    stopPulse();
    enableCurrentDown();
  }
  else {
    setPulseLengthByRPM(rpm);
    startPulse();
  }

  StaticJsonDocument<255> msg;
  msg["angle"] = angleDegrees;
  msg["rpm"] = rpm;

  serializeJson(msg, Serial); Serial.println();

// serialEvent is now deprecated and not working on some Arduino devices (e.g. Arduino Nano Every).
// https://github.com/arduino/ArduinoCore-avr/issues/206#issuecomment-532133626
// https://forum.arduino.cc/t/nano-every-serialevent-does-not-get-called/690025
//
// }
//
// void serialEvent() {
//   StaticJsonDocument<255> msg;
  bool jsonError = false;

  if (nextSerialJson(&msg, &jsonError)) {
    float _rpm = msg["rpm"];
    if (_rpm < MIN_RPM) _rpm = 0; // current down
    if (MAX_RPM < _rpm) _rpm = MAX_RPM;

    rpm = _rpm;
  }
  else if (jsonError) {
    // StaticJsonDocument<255> response;
    // serializeJson(response, Serial); Serial.println();
  }
}