ploclockをステッピングモーターで車輪の再開発をしてみた件について

plotclockってご存じですか？
2つのサーボでアームの先のペンを動かして時間を描く時計です

Plotclock by joo - Thingiverse
https://www.thingiverse.com/thing:248009

ググると作例がいっぱい出てくるんですが、
なんとなくアームの構成を見てたら円の交点を計算すればペン動かせるんじゃね？
ってことに気が付いたので自分でも作ってみることにしました
最初はサーボで作ってたんですが、
サーボの分解能が低くて線がガタガタになるのが納得いかなくて放棄しちゃいました

plotclockの描画改善できたけどこれ以上は設計の見直し(ペンの長さに合わせた台の角度)とかサーボの分解能とか工作精度とか見直さないとダメなんじゃないかなー
左右下はサーボの関係上歪むので使わなければ良さそう

あ、サーボの角度は整数だけど少数に直せるな・・・(´・ω・`)#M5Atom pic.twitter.com/SgqNmIJx1f

— もけ@ムギ㌠ (@coppercele) November 2, 2020

サーボがダメならステッピングモーターで作ったらいいんじゃね？
というわけで作り始めました

まずオリジナルの設計図を流用してモーターのホルダーをステッピングモーターの形にモデリングして構成しました

逆運動学？

ハードウェアを作ったらスケッチを書く前にGoogle Spreadsheetを開いてペン先の座標とモーターの角度を計算する方法を検討します

冒頭で言ってた「円の交点を計算を計算すればいいんじゃね？」を図示するとこうなります

アームの各関節が青点として、
ペンとモーターの基部の円の交点を算出してモーター基部から交点への角度を出せばいいというわけです

必要な要素を書きだすとこのようになります

モーターからpointへの角度をθとすると
アークタンジェントを使って

  theta = atan2(pointY - servoLy, pointX - servoLx);

左モーター基部とpointの距離をlengthとしアームL1とpointがなす角をαとすると
lengthはピタゴラスの定理を、
αは余弦定理を使って計算できます

余弦定理 - Wikipedia
https://ja.wikipedia.org/wiki/%E4%BD%99%E5%BC%A6%E5%AE%9A%E7%90%86

  float length = sqrt(pow(pointX - servoLx, 2) + pow(pointY - servoLy, 2));
  float alpha = acos((pow(length, 2) + pow(l1, 2) - pow(l2, 2)) / (2 * length * l1));

α+θをラジアンから度にして最終的な角度が計算できます
右側はθ-αです

  float rad = theta + alpha - 0.5 * PI; // 上方向を0度にするため90度分引く 
  degL = (float)((rad * 180) / PI); // degreeに変換する

全体的な関数はこのようになります
ちなみにアームが到達しない座標を与えると余弦定理の結果がnanになって物故割れますので対処したほうがいいです(´・ω・`)

getDeg()

// servoとL1~L4を変更することで大きさを変えられる
float servoLx = 0;
float servoLy = 0;
float servoRx = 45;
float servoRy = 0;

void getDeg(float pointX, float pointY, float &degL, float &degR) {
  float theta = 0;

  float l1 = 40;
  float l2 = 40;
  float l3 = 40;
  float l4 = 40;

  theta = atan2(pointY - servoLy, pointX - servoLx);
  float length = sqrt(pow(pointX - servoLx, 2) + pow(pointY - servoLy, 2));
  float alpha =
      acos((pow(length, 2) + pow(l1, 2) - pow(l2, 2)) / (2 * length * l1));
  float rad = theta + alpha - 0.5 * PI;

  // Serial.printf("theta=%f\n", theta);
  // Serial.printf("length=%f\n", length);
  // Serial.printf("alpha=%f\n", alpha);
  // Serial.printf("rad=%f\n", rad);

  float thetaR = atan2(pointY - servoRy, pointX - servoRx);
  float lengthR = sqrt(pow(pointX - servoRx, 2) + pow(pointY - servoRy, 2));
  float alphaR =
      acos((pow(lengthR, 2) + pow(l3, 2) - pow(l4, 2)) / (2 * lengthR * l3));
  float radR = thetaR - alphaR - 0.5 * PI;

  // Serial.printf("thetaR=%f\n", thetaR);
  // Serial.printf("lengthR=%f\n", lengthR);
  // Serial.printf("alphaR=%f\n", alphaR);
  // Serial.printf("radR=%f\n", radR);

  degL = (float)((rad * 180) / PI);
  degR = (float)((radR * 180) / PI);

  // Serial.printf("degL=%f,degR=%f\n", degL, degR);
}

  float toDegL;
  float toDegR;

 getDeg(22.5, 60, toDegL, toDegR);

}

これ全部組みあがってから気づいたんですけど逆運動学の考え方なんですよね(多分)

ステッピングモーターを動かす

ペンの座標に対してモーターの角度が分かるようになったのでそれに合わせてステッピングモーターを動かします

サーボで作る場合は絶対角度が分かるのでtoDegL,toDegRをそのままライブラリに渡してやればいいと思います

ライブラリはunistep2を採用しました

GitHub - reven/Unistep2: A non-blocking Arduino library for controlling 28BYJ-48 stepper motors
https://github.com/reven/Unistep2

複数モーター対応、速度の変更、現在のステップ数を取得できるライブラリでもっといい物があったら教えてください(´・ω・`)

安物のステッピングモーターをM5Stackに接続します

GPIO18,19がSD,LCDと被っているのでSDとLCDは使えません
GPIO22がI2Cと被るので左側のGPIO26を使用します
GPIO25に繋ぐとスピーカーから爆音が出て鼓膜が破壊されるので注意しましょう(´・ω・`)

#include <M5Stack.h>
#include <Unistep2.h>
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
Unistep2 stepperL(16, 17, 2, 5, 4096, 1000);
Unistep2 stepperR(18, 19, 23, 26, 4096, 1000);

直上を0度としてステッピングモーターを制御します
isStrokeは線を引くときはtrueにしてサーボで降ろしたり降ろさなかったりします

// 前回移動の終点を記録しておく
float fromPointX = 22.5;
float fromPointY = 73;


// 終点X,Y,線を引くかどうか
void moveTo2(float toPointX, float toPointY, bool isStroke) {
  if (isStroke) {
    penDown();
  }
  else {
    penUp();
  }
  Serial.printf("from=%5.2f,%5.2f\n", fromPointX, fromPointY);
  Serial.printf("to  =%5.2f,%5.2f\n\n", toPointX, toPointY);

  float toDegL;
  float toDegR;
// ステッピングモーターの角度を取得する
  getDeg(toPointX, toPointY, toDegL, toDegR);

  // 45度=600ステップなので本来は13.3を掛けるべきだがマージンを取って少し小さめに動かしている
  stepperL.moveTo(toDegL * 11);
  stepperR.moveTo(toDegR * 11);

// ステッピングモーターが動き終わるまでwhileで待つ
// run()を実行することでstepsToGo()の戻り値が更新される
  while (stepperL.stepsToGo() != 0 || stepperR.stepsToGo() != 0) {
    stepperL.run();
    stepperR.run();
  }
  // 終点を現在地として記録する
  fromPointX = toPointX;
  fromPointY = toPointY;
}

// (0,0)へ移動する
moveTo(0, 0, false);
// 現在の座標から(65,45)へ線を引く
moveTo(65, 45, true);

これでステッピングモーターが好きな角度に動くようになったのですが、
斜めに動かした時に顕著なのですが左右のアームの速度が同じだと
左右の動きが対称じゃない時に困ったことになります

左右のモーターを同じ速度で動かすと斜めに動かした時に動きが小さい方が先に動き終わるので真っ直ぐになりません😂 pic.twitter.com/D1RXWMXoBK

— もけ@ムギ㌠ (@coppercele) February 4, 2022

なので動きが小さい方を遅くすることで同期を取ってやります

// ステッピングモーターの全体的な速度を調整する　
float STEPPER_SPEED_RATIO = 5.0;

void moveTo2(float toPointX, float toPointY, bool isStroke) {
  if (isStroke) {
    penDown();
  }
  else {
    penUp();
  }
  Serial.printf("from=%5.2f,%5.2f\n", fromPointX, fromPointY);
  Serial.printf("to  =%5.2f,%5.2f\n\n", toPointX, toPointY);

  float fromDegL;
  float fromDegR;
  float toDegL;
  float toDegR;

  getDeg(fromPointX, fromPointY, fromDegL, fromDegR);
  getDeg(toPointX, toPointY, toDegL, toDegR);

  // 左右のステッピングモーターの移動量
  float deltaL = toDegL - fromDegL;
  float deltaR = toDegR - fromDegR;

  Serial.printf("delta=%5.2f,%5.2f\n", deltaL, deltaR);

  // 右の移動量が大きいとき左を遅くする
  // setStepdelay()は1000が最速で数字を大きくすると遅くなる
  if (abs(deltaL) < abs(deltaR)) {
    stepperL.setStepdelay(
        (int)(1000 * (abs(deltaR) / abs(deltaL)) * STEPPER_SPEED_RATIO));
    stepperR.setStepdelay((int)(1000 * STEPPER_SPEED_RATIO));
  }
  else {
    stepperL.setStepdelay((int)(1000 * STEPPER_SPEED_RATIO));
    stepperR.setStepdelay(
        (int)(1000 * (abs(deltaL) / abs(deltaR)) * STEPPER_SPEED_RATIO));
  }
  stepperL.moveTo(toDegL * 11);
  stepperR.moveTo(toDegR * 11);
  while (stepperL.stepsToGo() != 0 || stepperR.stepsToGo() != 0) {
    stepperL.run();
    stepperR.run();
  }

  fromPointX = toPointX;
  fromPointY = toPointY;
}

これで左右のアーム移動が同時に終わるようになります

なので動きが小さい方を遅くして同期を取ってやるといい感じになります🤗 pic.twitter.com/Cbf1H2YSVI

— もけ@ムギ㌠ (@coppercele) February 4, 2022

サーボだとここまで細かい制御は出来ないのでストロークをメチャメチャ細かくするくらいしか対処法無いんじゃないですかね・・・(´・ω・`)

2つのアームでペンを動かす関係上どうしても線が弧を描くのでストロークが長いときは分割して描くようにします

void moveTo(float toPointX, float toPointY, bool isStroke) {

  float deltaX = toPointX - fromPointX;
  float deltaY = toPointY - fromPointY;
  Serial.printf("from=%5.2f,%5.2f\n", fromPointX, fromPointY);
  Serial.printf("to  =%5.2f,%5.2f\n\n", toPointX, toPointY);
  Serial.printf("deltaXY=%5.2f,%5.2f\n", deltaX, deltaY);
  // 移動距離を計算して10(mm)より大きければ5分割する
  float length = sqrtf(powf(deltaX, 2) + powf(deltaY, 2));
  Serial.printf("length=%5.2f,\n", length);
  if (length < 10.0) {
    moveTo2(toPointX, toPointY, isStroke);
  }
  else {
    for (int i = 1; i <= 5; i++) {
      moveTo2(fromPointX + (deltaX * 0.2), fromPointY + (deltaY * 0.2),
              isStroke);
    }
  }
}

この関数ではストロークを分割してチェインしてるだけですがだいぶ直線に近づくのでまぁこれくらいでいいかな～と思ってます

普通に動かすと直線が弧を描くのでストロークを5分割するとだいぶ直線に近くなります
これ以上望むなら全部作り直さないとダメなんじゃ無いかな〜 pic.twitter.com/vXy4l89bgy

— もけ@ムギ㌠ (@coppercele) February 7, 2022

ストロークフォントに対応する

これでペンを思い通りに動かせるようになったので文字が書けるようにします
以前ペンプロッタをつくったときと同様にストロークフォントに対応させます

こちらのサイトを参考にさせていただきました
ストロークフォントを使って PlotClock で文字を描く – しかるのち
https://shikarunochi.matrix.jp/?p=4276

この記事を書いた人はポインタがよくわからないのでポインタを使わないようにアレンジしました

M5Stackペンプロッタをストロークフォント対応にしてみた - Qiita
https://qiita.com/coppercele/items/803c423c59b6c5dee842

ストロークフォントは上3ビットが命令、下5ビットがデータになっていますのでそれに合わせて関数を作ります

const int TEXT_WIDTH = 10; // 文字幅を設定
float textRatio = 1.5; // 文字の大きさを調整する

// 文字データ,文字のバイト数,何文字目か
void drawFont(const int *fontData, int textWidth, int textIndex) {
  if (fontData == NULL) {
    return;
  }
  int index = 0;
  float x = 0;
  float y = 0;

  while (fontData[index] != 0x20) {
    uint16_t flag = fontData[index] & 0b11100000;
    uint16_t data = fontData[index] & 0b00011111;
    Serial.printf("flag:%02X data:%02X\n", flag, data);

    if (flag == 0x20) {

      if (data == 0x00) {
        // 0x20は終端なので何もしない
        return;
      }
      if (6 < data) {
        // 0x27が飛んでいる分引く
        data--;
      }
      // 0x21が0なので引く
      data--;

      Serial.printf("Move toX=%d\n", data);
      x = data / textRatio;
      // X軸移動
      moveTo(x + textIndex * TEXT_WIDTH, y + OFFSET_Y, false);
    }
    else if (flag == 0x40) {
      // 横移動のみで線を引く
      if (27 < data) {
        // 5Bから5Eまで飛んでいるので引く
        data -= 2;
      }
      Serial.printf("Draw from here toX=%d\n", data);
      x = data / textRatio;
      // 線を引くのでペンを降ろす
      moveTo(x + textIndex * TEXT_WIDTH, y + OFFSET_Y, true);
    }
    else if (flag == 0x60) {
      // 線を引く終点Xを指定
      Serial.printf("Set toX=%d\n", data);
      x = data / textRatio;
    }
    else if (flag == 0xA0 || fontData[index] == 0x7E) {
      // 0x60終点Xとして終点Yへ移動する
      // Move toY
      if (fontData[index] == 0x7E) {
        // 0x7Eの例外処理
        Serial.printf("Move toY=0\n");
        data = 0;
      }
      else {
        Serial.printf("Move toY=%d\n", data);
      }
      y = data / textRatio;
      // 移動するのでペンを上げる
      moveTo(x + textIndex * TEXT_WIDTH, y + OFFSET_Y, false);
    }
    else if (flag == 0xC0) {
      // 0x60を終点Xとして終点Yへ線を引く
      // Draw toY
      Serial.printf("Draw toY=%d\n", data);
      y = data / textRatio;
      moveTo(x + textIndex * TEXT_WIDTH, y + OFFSET_Y, true);
    }
    index++;
  }
}


std::string plotText = "Hello";
Serial.printf("plotText:%s\n", plotText.c_str());
Serial.printf("plotText.length():%d\n", plotText.length());
for (int j = 0; j < plotText.length();) {
  uint16_t strUTF16;
  // fontIndex[]のindexを取得して文字のバイト数を取得する
  int textBytes =
      efontUFT8toUTF16(&strUTF16, (char *)(plotText.substr(j).c_str()));
  Serial.printf("textBytes:%d\n", textBytes);
  Serial.printf("strUTF16:%04x\n", strUTF16);
  Serial.printf("plotText:%d:%s\n", j,
                plotText.substr(j, textBytes).c_str());
  for (int i = 0; i < sizeof(fontIndex) / sizeof(fontIndex[0]); i++) {
    if (strUTF16 == fontIndex[i]) {
      // マルチバイト文字の場合textBytes分だけ関数に渡す
      drawFont(fontDataIndex[i], textBytes, j);
      break;
    }
  }
  // 複数バイト文字の場合ポインタをずらす
  j += textBytes;
  // Serial.printf("\n");
}

このような感じで文字が書けるようになりました

ストロークフォントの複数文字に対応しました

文字が歪んでるのは筐体が3Dプリントで歪んでるのと安いステッピングモーターのバックラッシ(遊び)のせいなので全体を作り直してシリアルサーボに変えないとダメそう🤯 pic.twitter.com/pmHPWqHRwm

— もけ@ムギ㌠ (@coppercele) February 9, 2022

NTPに対応して時計にする

オリジナルのplotclockに敬意を払って時計にします

ルータのID,パスワードをソースに書きたくないのでWPSでネットワーク接続します

ESP32(M5StickC)でWPSを使うと設定ファイルも必要ないし便利ですよ - Qiita
https://qiita.com/coppercele/items/6789deea453826916725

NTP対応をします
Wi-Fi接続とNTPクライアントの実験
https://garretlab.web.fc2.com/arduino/lab/ntp/

NTPで時間を取得したら文字列をフォント描画関数に渡すことでplotclockの完成です！

struct tm timeInfo; //時刻を格納するオブジェクト

Serial.println("WiFi started");
WiFi.begin();
WiFi.setSleep(false);
while (WiFi.status() != WL_CONNECTED) {
  delay(500); // 500ms毎に.を表示
  Serial.print(".");
}
Serial.println("\nConnected");

configTime(9 * 3600L, 0, "ntp.nict.jp", "time.google.com",
           "ntp.jst.mfeed.ad.jp"); // NTPの設定

char s[20]; //文字格納用
getLocalTime(&timeInfo); // tmオブジェクトのtimeInfoに現在時刻を入れ込む
sprintf(s, "%02d:%02d", timeInfo.tm_hour,
        timeInfo.tm_min); //人間が読める形式に変換
Serial.println(s);        //時間をシリアルモニタへ出力
std::string plotText = s;

// 以下一つ上のサンプルと同様

NTPに対応してステッピングモーター版plotclock完成！(02:46)

歪んでるのは工作精度とステッピングモーターのバックラッシのせいなので誰かシリアルサーボください🥺

は冗談として作り方の記事書くのでもっとちゃんとしたの作ってください🤗#M5Stack pic.twitter.com/LCyOr937a9

— もけ@ムギ㌠ (@coppercele) February 10, 2022

最後に全自動で動作させるために消しゴムとペンを自動でピックアップできるようにします

void pickEraser() {
  penUp();
  STEPPER_SPEED_RATIO = 2.0;
  moveTo(65, 45, false);
  penDown();
  penUp();
  STEPPER_SPEED_RATIO = 5.0;
}
void pickPen() {
  penUp();
  STEPPER_SPEED_RATIO = 2.0;
  moveTo(64, 30, false);
  penDown();
  penUp();
  STEPPER_SPEED_RATIO = 5.0;
}

void sweep() {
  STEPPER_SPEED_RATIO = 2.0;
  moveTo(45, 55, false);
  moveTo(0, 55, true);
  moveTo(0, 50, true);
  moveTo(45, 50, true);
  moveTo(45, 45, true);
  moveTo(0, 45, true);
  STEPPER_SPEED_RATIO = 5.0;
}

文字盤を書いた後適当な時間を空けてpickEraser()→sweep()→pickpen()と実行することで盤面をリセットできます
アームの長さや端末の大きさなどで座標は変わるので各自いい感じにしてください(´・ω・`)

文字盤消しも実装出来たのでこれでplotclock完成とします(´・ω・`)#M5Stack pic.twitter.com/Kn7qJVpSVs

— もけ@ムギ㌠ (@coppercele) February 14, 2022

埋め込んだツイートに再三書いてありますが3Dプリントの部品が反ったり
ステッピングモーターのバックラッシのせいで描画の精度が出ていません(´・ω・)
これは今後の課題とします(´・ω・)

全体のコード

#include "Adafruit_PWMServoDriver.h"
#include "WiFi.h"
#include "fontData.h"
#include <M5Stack.h>
#include <Unistep2.h>
#include <Wire.h>

#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40, Wire);
// Define some steppers and the pins they will use
Unistep2 stepperL(16, 17, 2, 5, 4096, 1000);
Unistep2 stepperR(18, 19, 23, 26, 4096, 1000);

float servoLx = 0;
float servoLy = 0;
float servoRx = 45;
float servoRy = 0;

const int X_MIN = 0;
const int X_MAX = 45;
const int Y_min = 40;
const int Y_MAX = 60;
const int FADER_INPUT_L = 36;

void getDeg(float pointX, float pointY, float &degL, float &degR) {
  float theta = 0;

  float l1 = 40;
  float l2 = 40;
  float l3 = 40;
  float l4 = 40;

  theta = atan2(pointY - servoLy, pointX - servoLx);
  float length = sqrt(pow(pointX - servoLx, 2) + pow(pointY - servoLy, 2));
  float alpha =
      acos((pow(length, 2) + pow(l1, 2) - pow(l2, 2)) / (2 * length * l1));
  float rad = theta + alpha - 0.5 * PI;

  // Serial.printf("theta=%f\n", theta);
  // Serial.printf("length=%f\n", length);
  // Serial.printf("alpha=%f\n", alpha);
  // Serial.printf("rad=%f\n", rad);

  float thetaR = atan2(pointY - servoRy, pointX - servoRx);
  float lengthR = sqrt(pow(pointX - servoRx, 2) + pow(pointY - servoRy, 2));
  float alphaR =
      acos((pow(lengthR, 2) + pow(l3, 2) - pow(l4, 2)) / (2 * lengthR * l3));
  float radR = thetaR - alphaR - 0.5 * PI;

  // Serial.printf("thetaR=%f\n", thetaR);
  // Serial.printf("lengthR=%f\n", lengthR);
  // Serial.printf("alphaR=%f\n", alphaR);
  // Serial.printf("radR=%f\n", radR);

  degL = (float)((rad * 180) / PI);
  degR = (float)((radR * 180) / PI);

  // Serial.printf("degL=%f,degR=%f\n", degL, degR);
}

void servoAngle(int angle) { pwm.setPWM(0, 0, map(angle, 0, 180, 120, 480)); }

float OFFSET_Y = 45;
float fromPointX = 22.5;
float fromPointY = 73;

bool isPenDown = false;

void penUp() {
  if (isPenDown) {
    servoAngle(35);
    delay(500);
    isPenDown = false;
  }
}

void penDown() {
  if (!isPenDown) {
    servoAngle(20);
    delay(100);
    servoAngle(18);
    delay(100);
    servoAngle(16);
    delay(100);
    servoAngle(14);
    delay(100);
    servoAngle(10);
    delay(100);
    isPenDown = true;
  }
}

float STEPPER_SPEED_RATIO = 5.0;

void moveTo2(float toPointX, float toPointY, bool isStroke) {
  if (isStroke) {
    penDown();
  }
  else {
    penUp();
  }
  Serial.printf("from=%5.2f,%5.2f\n", fromPointX, fromPointY);
  Serial.printf("to  =%5.2f,%5.2f\n\n", toPointX, toPointY);

  float fromDegL;
  float fromDegR;
  float toDegL;
  float toDegR;

  getDeg(fromPointX, fromPointY, fromDegL, fromDegR);
  getDeg(toPointX, toPointY, toDegL, toDegR);

  float deltaL = toDegL - fromDegL;
  float deltaR = toDegR - fromDegR;

  Serial.printf("delta=%5.2f,%5.2f\n", deltaL, deltaR);

  if (abs(deltaL) < abs(deltaR)) {
    stepperL.setStepdelay(
        (int)(1000 * (abs(deltaR) / abs(deltaL)) * STEPPER_SPEED_RATIO));
    stepperR.setStepdelay((int)(1000 * STEPPER_SPEED_RATIO));
    Serial.printf(
        "stepdelayL=%d,stepdelayR=%d\n",
        (int)(1000 * (abs(deltaR) / abs(deltaL)) * STEPPER_SPEED_RATIO),
        (int)(1000 * STEPPER_SPEED_RATIO));
  }
  else {
    stepperL.setStepdelay((int)(1000 * STEPPER_SPEED_RATIO));
    stepperR.setStepdelay(
        (int)(1000 * (abs(deltaL) / abs(deltaR)) * STEPPER_SPEED_RATIO));
    Serial.printf(
        "stepdelayL=%d,stepdelayR=%d\n", (int)(1000 * STEPPER_SPEED_RATIO),
        (int)(1000 * (abs(deltaL) / abs(deltaR)) * STEPPER_SPEED_RATIO));
  }
  stepperL.moveTo(toDegL * 11);
  stepperR.moveTo(toDegR * 11);
  while (stepperL.stepsToGo() != 0 || stepperR.stepsToGo() != 0) {
    stepperL.run();
    stepperR.run();
  }

  fromPointX = toPointX;
  fromPointY = toPointY;
}

void moveTo(float toPointX, float toPointY, bool isStroke) {

  float deltaX = toPointX - fromPointX;
  float deltaY = toPointY - fromPointY;
  Serial.printf("from=%5.2f,%5.2f\n", fromPointX, fromPointY);
  Serial.printf("to  =%5.2f,%5.2f\n\n", toPointX, toPointY);
  Serial.printf("deltaXY=%5.2f,%5.2f\n", deltaX, deltaY);

  float length = sqrtf(powf(deltaX, 2) + powf(deltaY, 2));
  Serial.printf("length=%5.2f,\n", length);
  if (length < 10.0) {
    moveTo2(toPointX, toPointY, isStroke);
  }
  else {
    for (int i = 1; i <= 5; i++) {
      moveTo2(fromPointX + (deltaX * 0.2), fromPointY + (deltaY * 0.2),
              isStroke);
    }
  }
}

// 戻り値で文字のバイト数を返すように変更
int efontUFT8toUTF16(uint16_t *pUTF16, char *pUTF8) {
  // 1Byte Code
  if (pUTF8[0] < 0x80) {
    // < 0x80 : ASCII Code
    *pUTF16 = pUTF8[0];
    return 1;
  }

  // 2Byte Code
  if (pUTF8[0] < 0xE0) {
    *pUTF16 = ((pUTF8[0] & 0x1F) << 6) + (pUTF8[1] & 0x3F);
    return 2;
  }

  // 3Byte Code
  if (pUTF8[0] < 0xF0) {
    *pUTF16 = ((pUTF8[0] & 0x0F) << 12) + ((pUTF8[1] & 0x3F) << 6) +
              (pUTF8[2] & 0x3F);
    return 3;
  }
  else {
    // 4byte Code
    *pUTF16 = 0;
    return 4;
  }
}

int textOffset = 0;
const int TEXT_WIDTH = 10;
float textRatio = 1.5;

void drawFont(const int *fontData, int textWidth, int textIndex) {
  if (fontData == NULL) {
    return;
  }
  int index = 0;
  float x = 0;
  float y = 0;

  while (fontData[index] != 0x20) {
    uint16_t flag = fontData[index] & 0b11100000;
    uint16_t data = fontData[index] & 0b00011111;
    Serial.printf("flag:%02X data:%02X\n", flag, data);

    if (flag == 0x20) {

      if (data == 0x00) {
        // 0x20は終端なので何もしない
        return;
      }
      if (6 < data) {
        // 0x27が飛んでいる分引く
        data--;
      }
      // 0x21が0なので引く
      data--;

      Serial.printf("Move toX=%d\n", data);
      x = data / textRatio;
      // X軸移動
      moveTo(x + textIndex * TEXT_WIDTH, y + OFFSET_Y, false);
    }
    else if (flag == 0x40) {
      // 横移動のみで線を引く
      if (27 < data) {
        // 5Bから5Eまで飛んでいるので引く
        data -= 2;
      }
      Serial.printf("Draw from here toX=%d\n", data);
      x = data / textRatio;
      // 線を引くのでペンを降ろす
      moveTo(x + textIndex * TEXT_WIDTH, y + OFFSET_Y, true);
    }
    else if (flag == 0x60) {
      // 線を引く終点Xを指定
      Serial.printf("Set toX=%d\n", data);
      x = data / textRatio;
    }
    else if (flag == 0xA0 || fontData[index] == 0x7E) {
      // 0x60終点Xとして終点Yへ移動する
      // Move toY
      if (fontData[index] == 0x7E) {
        // 0x7Eの例外処理
        Serial.printf("Move toY=0\n");
        data = 0;
      }
      else {
        Serial.printf("Move toY=%d\n", data);
      }
      y = data / textRatio;
      // 移動するのでペンを上げる
      moveTo(x + textIndex * TEXT_WIDTH, y + OFFSET_Y, false);
    }
    else if (flag == 0xC0) {
      // 0x60を終点Xとして終点Yへ線を引く
      // Draw toY
      Serial.printf("Draw toY=%d\n", data);
      y = data / textRatio;
      moveTo(x + textIndex * TEXT_WIDTH, y + OFFSET_Y, true);
    }
    index++;
  }
  textOffset += TEXT_WIDTH;
}

struct tm timeInfo; //時刻を格納するオブジェクト

void pickEraser() {
  STEPPER_SPEED_RATIO = 2.0;
  moveTo(65, 45, false);
  penDown();
  penUp();
  STEPPER_SPEED_RATIO = 5.0;
}
void pickPen() {
  STEPPER_SPEED_RATIO = 2.0;
  moveTo(62, 22, false);
  penDown();
  penUp();
  STEPPER_SPEED_RATIO = 5.0;
}

void setup() {
  M5.begin(false, false, true, false);
  M5.Power.begin();
  Serial.printf("battery:%d\n", M5.Power.getBatteryLevel());
  pwm.begin();
  pwm.setPWMFreq(SERVO_FREQ);
  penUp();
  pinMode(36, INPUT);
}

int ind = 0;

int pastStep = 0;

void loop() {
  M5.update();
  // We need to call run() frequently during loop()
  stepperL.run();
  stepperR.run();

  if (M5.BtnA.wasPressed()) {
    penUp();

    moveTo(65, 45, false);
    penDown();
    penUp();
    moveTo(62, 22, false);
    penDown();
    penUp();
    moveTo(22.5, 73, false);

    // Serial.println("WiFi started");
    // WiFi.begin();
    // WiFi.setSleep(false);
    // while (WiFi.status() != WL_CONNECTED) {
    //   delay(500); // 500ms毎に.を表示
    //   Serial.print(".");
    // }
    // Serial.println("\nConnected");

    // configTime(9 * 3600L, 0, "ntp.nict.jp", "time.google.com",
    //            "ntp.jst.mfeed.ad.jp"); // NTPの設定
    // if (stepperL.stepsToGo() == 0 && stepperR.stepsToGo() == 0) {
    //   stepperL.setStepdelay(1000);
    //   stepperL.moveTo(200);
    //   stepperR.setStepdelay(1000);
    //   stepperR.moveTo(200);
    //   while (stepperL.stepsToGo() != 0 || stepperR.stepsToGo() != 0) {
    //     stepperL.run();
    //     stepperR.run();
    //   }
    //   Serial.println();
    //   stepperL.setStepdelay(1000);
    //   stepperL.moveTo(-200);
    //   stepperR.setStepdelay(1000);
    //   stepperR.moveTo(-200);
    //   while (stepperL.stepsToGo() != 0 || stepperR.stepsToGo() != 0) {
    //     stepperL.run();
    //     stepperR.run();
    //   }
    //   stepperL.setStepdelay(1000);
    //   stepperL.moveTo(0);
    //   stepperR.setStepdelay(1000);
    //   stepperR.moveTo(0);
    //   while (stepperL.stepsToGo() != 0 || stepperR.stepsToGo() != 0) {
    //     stepperL.run();
    //     stepperR.run();
    //   }
    //   stepperL.setStepdelay(1000);
    //   stepperL.moveTo(200);
    //   stepperR.setStepdelay(1000);
    //   stepperR.moveTo(-200);
    //   while (stepperL.stepsToGo() != 0 || stepperR.stepsToGo() != 0) {
    //     stepperL.run();
    //     stepperR.run();
    //   }
    //   stepperL.setStepdelay(1000);
    //   stepperL.moveTo(0);
    //   stepperR.setStepdelay(1000);
    //   stepperR.moveTo(0);
    // }
    // pwm.setPWM(0, 0, 4095);
  }
  if (M5.BtnB.wasPressed()) {
    ind = 0;
    // // pwm.setPWM(0, 0, 4095);

    stepperL.setStepdelay(1000);
    stepperL.moveTo(0);
    stepperR.setStepdelay(1000);
    stepperR.moveTo(0);
    penUp();

    fromPointX = 22.5;
    fromPointY = 73;
  }
  if (M5.BtnC.wasPressed()) {
    switch (ind) {
    case 0:
      moveTo(0, 45, false);
      ind++;
      break;
    case 1: {
      char s[20]; //文字格納用
      getLocalTime(&timeInfo); // tmオブジェクトのtimeInfoに現在時刻を入れ込む
      sprintf(s, "%02d:%02d", timeInfo.tm_hour,
              timeInfo.tm_min); //人間が読める形式に変換
      Serial.println(s);        //時間をシリアルモニタへ出力
      std::string plotText = s;
      // std::string plotText = "Hello";

      Serial.printf("plotText:%s\n", plotText.c_str());
      Serial.printf("plotText.length():%d\n", plotText.length());
      for (int j = 0; j < plotText.length();) {
        uint16_t strUTF16;
        // fontIndex[]のindexを取得して文字のバイト数を取得する
        int textBytes =
            efontUFT8toUTF16(&strUTF16, (char *)(plotText.substr(j).c_str()));
        Serial.printf("textBytes:%d\n", textBytes);
        Serial.printf("strUTF16:%04x\n", strUTF16);
        Serial.printf("plotText:%d:%s\n", j,
                      plotText.substr(j, textBytes).c_str());
        for (int i = 0; i < sizeof(fontIndex) / sizeof(fontIndex[0]); i++) {
          if (strUTF16 == fontIndex[i]) {
            // マルチバイト文字の場合textBytes分だけ関数に渡す
            drawFont(fontDataIndex[i], textBytes, j);
            break;
          }
        }
        // 複数バイト文字の場合ポインタをずらす
        j += textBytes;
        // Serial.printf("\n");
      }
    }
      ind = 0;
      break;

    default:
      break;
    }
  }
}