前号では記念すべき創刊ということでSimpleFOCによるBLDC制御についてもろもろ紹介させていただきました。

予算3万円でつくる BLDC 2足歩行ロボット創刊号ーSimpleFOCでBLDC動作を楽しむー

本号ではロボットの左足となる足を1本完成させます。
足を上下前後に移動するための逆運動学についても紹介いたします。

こうかな pic.twitter.com/DozASWVB1A

— HomeMadeGarbage (@H0meMadeGarbage) August 24, 2025

はじめに

ここからは足の完成に向けて組み立て手順通りに記載します。

作業前の前提としましては創刊号の FOC回転位置制御が書き込まれ、モータ回転方向やエンコーダオフセット初期値設定がさせているものとします。

ギア等は外しておいてください。

また裏面のスペーサも必要ないので外してください。

本号で必要な部品

主な必要部品及び数量は創刊号と全く一緒です。
ねじ等は足組み立てのために各種必要となります。

BLDCコントローラ MKS ESP32 FOC V2.0 ：1個

　　
BLDC 5010 360KV：2個

　
磁気エンコーダ AS5600：2個

　
LiPoバッテリ 3セル 11.1V：1個
本号では安定化電源 (12V)の使用を想定していますが、完成ロボはLiPoバッテリを使用します。

　
PS4コントローラ
本ロボは基本的にはPS4コントローラでの操作となります。必須ではないですがあると便利です。

　
その他
10mm以上のねじスペーサ：4個
M2ナット：6個
M2 12mm なべネジ：4本
M2 14mm なべネジ：2本
M2 20mm なべネジ：6本
M2 25mm なべネジ：2本
M3ナイロンネジ：2本
M3ナット：2個
M3 14mm なべネジ：2本
M3 18mm なべネジ：1本
M3 20mm なべネジ：1本
M3 25mm なべネジ：5本
M3 30mm なべネジ：1本

ロボット完成に必要な主な部品

AliExpressでの部品購入は到着までに日数がかかるので、あらかじめロボ完成までの部品を購入しておきたい方のために主な必要部品をまとめておきます。

#	品目	数量
1	MKS ESP32 FOC V2.0	3
2	BLDC 5010 360KV	6
3	磁気エンコーダ AS5600	6
4	ATOM Matrix	1
5	CANコントローラボード	4
6	4ピン GHコネクタケーブル 150mm	4
7	LiPoバッテリ 3セル 11.1V	1

3Dモデル

今回使用する追加の筐体の3Dモデルデータは以下で販売しております。
zipファイルでダウンロードされますので解凍してご利用ください。

https://shop.homemadegarbage.com/product/bldcrobo3d02/

データには21個のstlファイルがあり leg7.stl, M2-102.stl,
M3-155.stlは2個づつプリントし、その他は1個づつ印刷してください。

足の組み立てには一部を除いて創刊号のモデルも使用します。
M2-52.stlにつきましては合計10個使用しますので追加でプリントしてください。

足上部組立て

boardFix.stlの面4つ角の穴をM3ネジでねじ切りして10mmスペーサを付けます。

以下のようにboardFix.stlをmotorFix.stlに2か所ねじ止めします (M3 14mm なべネジ：2本)。

以下のように MKS ESP32 FOC V2.0 をスペーサにM3 ナイロンネジで2か所止めます (必須ではありません)。
モータやエンコーダのコネクタを配線後に cover.stl をかぶせてM3 18mm、M3 25mm なべネジで固定します。

MKS ESP32 FOC V2.0のネジ穴周辺にスペースがないので、以下のようにナイロンネジの頭を切って小型化して使用しています。

足フレーム組立て

3Dプリント品を組んでリンク機構の足を作ります。

leg8.stlとleg11.stlのくぼみにM3ナットをはめ込んで、foot.stlを挟んでM3-155を通してねじ止め (M3 25mm なべネジ：2本)。

leg8.stlとleg11.stlの上図右写真の赤〇の穴はあらかじめM2ネジサイズでねじ切りしてください (反対側も)。

gear56-70-L.stl (創刊号付属モデル)の下部の穴にleg9.stlの突起穴を通して、leg8.stlの穴にねじ止め (M2 20mm なべネジ)。
M2-149.stlをleg9.stlとleg10.stlの穴に通してねじ止め (M2 25mm なべネジ)。

leg4-L.stl の下部の2つ穴にM2-52.stlを通して、leg11.stlの穴にねじ止め (M2 20mm なべネジ：2本)。
leg5.stlの穴にM2-152とM2-52.stlをとおしてM2ナットをはめてgear56-70-Lstlとleg4-Lstlにねじ止め (それぞれ M2 25mm, 12mm なべネジ)。

leg6.stl の下部の2つ穴にM2-52.stlを通して、leg11.stlの中段穴にねじ止め (M2 20mm なべネジ：2本)。
leg7.stlの穴にM2-102とM2-52.stlをとおしてM2ナットをはめてleg4-Lstlとleg6.stlにねじ止め (それぞれ M2 20mm, 12mm なべネジ)。

leg4-L.stl の上部の穴にM3-178.stlを通して、leg2-L .stlとgear56-L.stlを通す。
leg6.stlの穴にM2-52.stlをとおしてM2ナットをはめてgear56-L.stlにねじ止め (M2 12mm なべネジ)。
leg7.stlの穴にM2-102とM2-52.stlをとおしてleg4-Lstlとleg6.stlにねじ止め (それぞれ M2 12mm, 20mm なべネジ)。
leg6.stlの中段の穴はM2ネジサイズであらかじめねじ切りしてください

以上で左足フレームが完成です。

予算3万円でつくる BLDC 2足歩行ロボット
足フレーム pic.twitter.com/OEtbcfeFau

— HomeMadeGarbage (@H0meMadeGarbage) August 31, 2025

足完成

足フレームを足上部に取り付け足を完成させます。

leg1.stl (創刊号付属モデル)の突起をgear56-70-L.stlの穴に通して、motorFix .stlにねじ止め (M3 25mm なべネジ)。
M3-178 .stlの穴にM3 30mm なべネジをとおして、motorFix .stlにねじ止め。

leg4-L .stlの穴にM3-55.stlをとおして、motorFix .stlにねじ止め (M3 20mm なべネジ)。
leg3 .stlの穴にM3-105.stlをとおして、motorFix .stlにねじ止め (M3 25mm なべネジ)。

gear56-70-L.stl、gear56-L.stlのアームがだいたい45°になる位置でモータのgearFix.stlにgear10.stlをはめます。
この際にモータは初期位置であることをご確認ください。

45°の目安としてgear56-70とgear56-L.stlにマーキングを入れてあります。
逆三角マークの下頂点がそれぞれleg1.stlとleg2-L.stlの上辺にかかる点が傾き45°となります。
ここではだいたい45°で問題ありません。ブラウザアプリのangleOffsetで微調整できます。

穴にM2-52.stlを通してM2 14mm なべネジでギアを固定します。

以上で足が完成です。

動作

創刊号のFOC回転位置制御Arduinoコードで足を動かします。
あらかじめブラウザアプリのangleOffsetで足位置の微調整を実施しておいてください。

予算3万円でつくる BLDC 2足歩行ロボット
FOC回転位置制御 pic.twitter.com/bdycyDjLFI

— HomeMadeGarbage (@H0meMadeGarbage) September 2, 2025

PS4コントローラのジョイスティックでモータを回転させることでリンク機構の足が上下に動きます。

前回のコードではモータの電圧・電流を3V, 2Aと低出力にしているため (L. 46, 47) トルクが低く上の動画のように足を浮かさないと動きません。

逆運動学 (IK)

足が完成し、動作が確認できました。
ここからは逆運動学 (Inverse Kinematics：IK)でリンク機構の足先の位置を指定してモータの回転を制御します。

座標は以下のように定義しました。

motor1で駆動するギアの回転軸を原点として、足先 (足首前方)の前後をx軸、高さをz軸とします。

motor1で大腿の角度、motor0で下腿の角度をギア比5.6の歯車を介して制御します。
大腿、下腿は同一の長さで70mmです。

逆運動学算出

足先の座標 (x, z)から大腿、下腿の角度$θ_1$, $θ_2$を算出します。

角度$θ_1$は余弦定理を用いて導出します。ちなみに$L$は大腿、下腿の長さ(70mm)。

$L^2 = L^2 + {l_d}^2 – 2{L}{l_d}\cos{θ_1}’ $

${θ_1}’ = \cos^{-1} \left(\frac{l_d}{2{L}} \right)$

$$θ_1 = φ + {θ_1}’ = φ +\cos^{-1} \left(\frac{l_d}{2{L}} \right) $$

$$　　　　　　但し、φ = \tan^{-1} \left(\frac{x}{z} \right)$$

$$　　　　　　　l_d = \sqrt{x^2 + z^2}$$

後方サーボの角度$θ_2$も余弦定理を用いて導出します。

${l_d}^2 = L^2 + L^2 – 2{L}{L}\cos{θ_2}’ = 2L^2 – 2L^2\cos{θ_2}’$

${θ_2}’ = \cos^{-1} \left(\frac{2L^2-{l_d}^2}{2{L}^2} \right)$

$$θ_2 =\pi-θ_1-{θ_2}’ = \pi-θ_1-\cos^{-1} \left(\frac{2L^2-{l_d}^2}{2{L}^2} \right)$$

以上より、足先の座標(x, z)を指定することで大腿、下腿の角度$θ_1$, $θ_2$を導出できるようになりました。

$θ_1$, $θ_2$にギア比5.6をかけることでモータの回転位置となります。

Arduinoコード

創刊号のFOC回転位置制御Arduinoコードを改良して、IKで足を制御できるようにします。

環境バージョンは前回と同様です。
　Arduino IDEバージョン：ver. 1.8.19
　ESP32ライブラリ：ver. 2.0.13
　SimpleFOCライブラリ：ver. 2.3.3
　PS4Controllerライブラリ：ver. 1.1.0

エンコーダオフセット値 (zero_electric_angle)は前回測定した値を入力してください。

#include <SimpleFOC.h>
#include <WiFi.h>
#include <WebServer.h>
#include <Preferences.h>
#include <PS4Controller.h>

float angle0 = 0.0, angle1 = 0.0;

int lastLStickX = 0, lastLStickY = 0;

WebServer server(80);

const char ssid[] = "BLDC-L";  // SSID
const char pass[] = "password";   // password

const IPAddress ip(192, 168, 22, 91);      // IPアドレス
const IPAddress subnet(255, 255, 255, 0); // サブネットマスク

Preferences preferences;


//Motor Instance
BLDCMotor motor0 = BLDCMotor(7);
BLDCDriver3PWM driver0 = BLDCDriver3PWM(32, 33, 25, 12);

BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(26, 27, 14, 12);

//Encoder Instance
MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Cone = TwoWire(0);
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Ctwo = TwoWire(1);

// Inline Current sensing Instance
InlineCurrentSense current_sense0 = InlineCurrentSense(0.01f, 50.0f, 39, 36);
InlineCurrentSense current_sense1 = InlineCurrentSense(0.01f, 50.0f, 35, 34);

float L = 70.0;
float height = 99.0;

float angleOffset0 = 0.0, angleOffset1 = 0.0;

float angleP = 60.0, angleI = 1.0, angleD = 7.0;
float velP = 0.05, velI = 2.0;
float velLimit = 200.0;
float Vlimit = 3.0;
float Ilimit = 6.0;

float currentP = 1;
float currentI = 500;
float currentTf = 2;


//ブラウザ表示
void handleRoot() {
  String temp = "<!DOCTYPE html> \n<html lang=\"ja\">";
  temp += "<head>";
  temp += "<meta charset=\"utf-8\">";
  temp += "<title>BLDC-L</title>";
  temp += "<meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">";
  temp += "<style>";
  temp += ".container{";
  temp += "  max-width: 500px;";
  temp += "  margin: auto;";
  temp += "  text-align: center;";
  temp += "  font-size: 1.2rem;";
  temp += "}";
  temp += "span,.pm{";
  temp += "  display: inline-block;";
  temp += "  border: 1px solid #ccc;";
  temp += "  width: 50px;";
  temp += "  height: 30px;";
  temp += "  vertical-align: middle;";
  temp += "  margin-bottom: 20px;";
  temp += "}";
  temp += "span{";
  temp += "  width: 120px;";
  temp += "}";
  temp += "button{";
  temp += "  width: 100px;";
  temp += "  height: 40px;";
  temp += "  font-weight: bold;";
  temp += "  margin-bottom: 20px;";
  temp += "}";
  temp += "</style>";
  temp += "</head>";

  temp += "<body>";
  temp += "<div class=\"container\">";
  temp += "<h3>BLDC-L</h3>";



  //angleOffset0
  temp += "angleOffset0<br>";
  temp += "<a class=\"pm\" href=\"/angleOffset0M\">-</a>";
  temp += "<span>" + String(angleOffset0) + "</span>";
  temp += "<a class=\"pm\" href=\"/angleOffset0P\">+</a><br>";

  //angleOffset1
  temp += "angleOffset1<br>";
  temp += "<a class=\"pm\" href=\"/angleOffset1M\">-</a>";
  temp += "<span>" + String(angleOffset1) + "</span>";
  temp += "<a class=\"pm\" href=\"/angleOffset1P\">+</a><br>";


  //angleP
  temp += "angleP<br>";
  temp += "<a class=\"pm\" href=\"/anglePM\">-</a>";
  temp += "<span>" + String(angleP) + "</span>";
  temp += "<a class=\"pm\" href=\"/anglePP\">+</a><br>";

  //angleI
  temp += "angleI<br>";
  temp += "<a class=\"pm\" href=\"/angleIM\">-</a>";
  temp += "<span>" + String(angleI) + "</span>";
  temp += "<a class=\"pm\" href=\"/angleIP\">+</a><br>";

  //angleD
  temp += "angleD<br>";
  temp += "<a class=\"pm\" href=\"/angleDM\">-</a>";
  temp += "<span>" + String(angleD) + "</span>";
  temp += "<a class=\"pm\" href=\"/angleDP\">+</a><br>";


  //velP
  temp += "velP<br>";
  temp += "<a class=\"pm\" href=\"/velPM\">-</a>";
  temp += "<span>" + String(velP) + "</span>";
  temp += "<a class=\"pm\" href=\"/velPP\">+</a><br>";

  //velI
  temp += "velI<br>";
  temp += "<a class=\"pm\" href=\"/velIM\">-</a>";
  temp += "<span>" + String(velI) + "</span>";
  temp += "<a class=\"pm\" href=\"/velIP\">+</a><br>";


  //velLimit
  temp += "velLimit<br>";
  temp += "<a class=\"pm\" href=\"/velLimitM\">-</a>";
  temp += "<span>" + String(velLimit) + "</span>";
  temp += "<a class=\"pm\" href=\"/velLimitP\">+</a><br>";


  temp += "</div>";
  temp += "</body>";
  server.send(200, "text/HTML", temp);
}


void angleOffset0M() {
  if (angleOffset0 >= -360.0) {
    angleOffset0 -= 1.0;
    preferences.putFloat("angleOffset0", angleOffset0);
    ik(0.0, height);
  }
  handleRoot();
}

void angleOffset0P() {
  if (angleOffset0 < 360.0) {
    angleOffset0 += 1.0;
    preferences.putFloat("angleOffset0", angleOffset0);
    ik(0.0, height);
  }
  handleRoot();
}


void angleOffset1M() {
  if (angleOffset1 >= -360.0) {
    angleOffset1 -= 1.0;
    preferences.putFloat("angleOffset1", angleOffset1);
    ik(0.0, height);
  }
  handleRoot();
}

void angleOffset1P() {
  if (angleOffset1 < 360.0) {
    angleOffset1 += 1.0;
    preferences.putFloat("angleOffset1", angleOffset1);
    ik(0.0, height);
  }
  handleRoot();
}


void velPM() {
  if (velP >= 0.0) {
    velP -= 0.01;
    preferences.putFloat("velP", velP);
    motor0.PID_velocity.P = velP / 10.0;
    motor1.PID_velocity.P = velP / 10.0;
  }
  handleRoot();
}

void velPP() {
  if (velP < 3.0) {
    velP += 0.01;
    preferences.putFloat("velP", velP);
    motor0.PID_velocity.P = velP / 10.0;
    motor1.PID_velocity.P = velP / 10.0;
  }
  handleRoot();
}


void velIM() {
  if (velI >= 0.0) {
    velI -= 0.1;
    preferences.putFloat("velI", velI);
    motor0.PID_velocity.I = velI;
    motor1.PID_velocity.I = velI;
  }
  handleRoot();
}

void velIP() {
  if (velI < 20.0) {
    velI += 0.1;
    preferences.putFloat("velI", velI);
    motor0.PID_velocity.I = velI;
    motor1.PID_velocity.I = velI;
  }
  handleRoot();
}


void anglePM() {
  if (angleP >= 0.0) {
    angleP -= 1.0;
    preferences.putFloat("angleP", angleP);
    motor0.P_angle.P = angleP;
    motor1.P_angle.P = angleP;
  }
  handleRoot();
}

void anglePP() {
  if (angleP < 400.0) {
    angleP += 1.0;
    preferences.putFloat("angleP", angleP);
    motor0.P_angle.P = angleP;
    motor1.P_angle.P = angleP;
  }
  handleRoot();
}


void angleIM() {
  if (angleI >= 0.0) {
    angleI -= 0.1;
    preferences.putFloat("angleI", angleI);
    motor0.P_angle.I = angleI;
    motor1.P_angle.I = angleI;
  }
  handleRoot();
}

void angleIP() {
  if (angleI < 400.0) {
    angleI += 0.1;
    preferences.putFloat("angleI", angleI);
    motor0.P_angle.I = angleI;
    motor1.P_angle.I = angleI;
  }
  handleRoot();
}


void angleDM() {
  if (angleD >= 0.0) {
    angleD -= 0.1;
    preferences.putFloat("angleD", angleD);
    motor0.P_angle.D = angleD;
    motor1.P_angle.D = angleD;
  }
  handleRoot();
}

void angleDP() {
  if (angleD < 400.0) {
    angleD += 0.1;
    preferences.putFloat("angleD", angleD);
    motor0.P_angle.D = angleD;
    motor1.P_angle.D = angleD;
  }
  handleRoot();
}


void velLimitM() {
  if (velLimit >= 0.0) {
    velLimit -= 10.0;
    preferences.putFloat("velLimit", velLimit);
    motor0.velocity_limit = velLimit;
    motor1.velocity_limit = velLimit;
  }
  handleRoot();
}

void velLimitP() {
  if (velLimit < 500.0) {
    velLimit += 10.0;
    preferences.putFloat("velLimit", velLimit);
    motor0.velocity_limit = velLimit;
    motor1.velocity_limit = velLimit;
  }
  handleRoot();
}


//Core0
void web(void *pvParameters) {
  PS4.begin();

  WiFi.softAP(ssid, pass);           // SSIDとパスの設定
  delay(100);                        // 追記：このdelayを入れないと失敗する場合がある
  WiFi.softAPConfig(ip, ip, subnet); // IPアドレス、ゲートウェイ、サブネットマスクの設定

  IPAddress myIP = WiFi.softAPIP();  // WiFi.softAPIP()でWiFi起動

  server.on("/", handleRoot);

  server.on("/angleOffset0P", angleOffset0P);
  server.on("/angleOffset0M", angleOffset0M);
  server.on("/angleOffset1P", angleOffset1P);
  server.on("/angleOffset1M", angleOffset1M);

  server.on("/velPP", velPP);
  server.on("/velPM", velPM);
  server.on("/velIP", velIP);
  server.on("/velIM", velIM);
  server.on("/velLimitP", velLimitP);
  server.on("/velLimitM", velLimitM);

  server.on("/anglePP", anglePP);
  server.on("/anglePM", anglePM);
  server.on("/angleIP", angleIP);
  server.on("/angleIM", angleIM);
  server.on("/angleDP", angleDP);
  server.on("/angleDM", angleDM);

  server.begin();

  ik(0.0, height);


  for (;;) {
    server.handleClient();

    if (PS4.isConnected()) { //PS4コントローラ
      int currentLStickX = PS4.LStickX(); 
      int currentLStickY = PS4.LStickY();
        
      if (currentLStickX != lastLStickX || currentLStickY != lastLStickY) {
        float Lx = PS4.LStickX() / 4.0;
        float Ly = PS4.LStickY() / 3.0;
        
        ik(-Lx, height + Ly);
  
        lastLStickX = currentLStickX;
        lastLStickY = currentLStickY;
      }
    }

    disableCore0WDT();
  }
}


void setup() {
  pinMode(32, INPUT_PULLUP);
  pinMode(33, INPUT_PULLUP);
  pinMode(25, INPUT_PULLUP);
  pinMode(26, INPUT_PULLUP);
  pinMode(27, INPUT_PULLUP);
  pinMode(14, INPUT_PULLUP);


  Serial.begin(115200);

  Serial.println("-----------------------------");
  uint8_t btmac[6];
  esp_read_mac(btmac, ESP_MAC_BT);
  Serial.printf("[Bluetooth] Mac Address = %02X:%02X:%02X:%02X:%02X:%02X\r\n", btmac[0], btmac[1], btmac[2], btmac[3], btmac[4], btmac[5]);


  //パラメータ初期値取得
  preferences.begin("parameter", false);

  angleOffset0 = preferences.getFloat("angleOffset0", angleOffset0);
  angleOffset1 = preferences.getFloat("angleOffset1", angleOffset1);

  velP = preferences.getFloat("velP", velP);
  velI = preferences.getFloat("velI", velI);
  velLimit = preferences.getFloat("velLimit", velLimit);

  angleP = preferences.getFloat("angleP", angleP);
  angleI = preferences.getFloat("angleI", angleI);
  angleD = preferences.getFloat("angleD", angleD);


  // Encoder Settings
  I2Cone.begin(19, 18, 400000UL); // AS5600_M0
  I2Ctwo.begin(23, 5, 400000UL); // AS5600_M1
  sensor0.init(&I2Cone);
  sensor1.init(&I2Ctwo);


  //Connect the motor object and the sensor object
  motor0.linkSensor(&sensor0);
  motor1.linkSensor(&sensor1);

  // Drive Settings
  driver0.voltage_power_supply = 12;
  driver0.init();
  motor0.linkDriver(&driver0);
  driver1.voltage_power_supply = 12;
  driver1.init();
  motor1.linkDriver(&driver1);

  // Current Limitation
  motor0.current_limit = Ilimit;
  motor1.current_limit = Ilimit;

  // Voltage Limitation
  motor0.voltage_limit = Vlimit;
  motor1.voltage_limit = Vlimit;


  // Current Sensing
  current_sense0.init();
  motor0.linkCurrentSense(&current_sense0);
  current_sense0.linkDriver(&driver0);

  // current sense init and linking
  current_sense1.init();
  motor1.linkCurrentSense(&current_sense1);
  current_sense1.linkDriver(&driver1);

  // Control loop
  // Other modes TorqueControlType::voltage TorqueControlType::dc_current
  motor0.torque_controller = TorqueControlType::foc_current;
  motor0.controller = MotionControlType::angle;
  motor1.torque_controller = TorqueControlType::foc_current;
  motor1.controller = MotionControlType::angle;


  // FOC current control PID parameters
  motor0.PID_current_q.P = currentP;
  motor0.PID_current_q.I = currentI;
  motor0.PID_current_d.P = currentP;
  motor0.PID_current_d.I = currentI;
  motor0.LPF_current_q.Tf = currentTf / 1000.0;  // 1ms default
  motor0.LPF_current_d.Tf = currentTf / 1000.0;  // 1ms default

  motor1.PID_current_q.P = currentP;
  motor1.PID_current_q.I = currentI;
  motor1.PID_current_d.P = currentP;
  motor1.PID_current_d.I = currentI;
  motor1.LPF_current_q.Tf = currentTf / 1000.0;  // 1ms default
  motor1.LPF_current_d.Tf = currentTf / 1000.0;  // 1ms default

  // Speed loop PID parameters
  motor0.PID_velocity.P = velP / 10.0;
  motor0.PID_velocity.I = velI;
  motor0.PID_velocity.D = 0;
  motor1.PID_velocity.P = velP / 10.0;
  motor1.PID_velocity.I = velI;
  motor1.PID_velocity.D = 0;

  motor0.LPF_velocity.Tf = 0.0;
  motor1.LPF_velocity.Tf = 0.0;

  //Angle PID Setting
  motor0.P_angle.P = angleP;
  motor0.P_angle.I = angleI;
  motor0.P_angle.D = angleD;
  motor1.P_angle.P = angleP;
  motor1.P_angle.I = angleI;
  motor1.P_angle.D = angleD;

  // Speed Limit
  motor0.velocity_limit = velLimit;
  motor1.velocity_limit = velLimit;


  motor0.voltage_sensor_align = 0.5;
  motor0.zero_electric_angle = 2.14; 
  motor0.sensor_direction = Direction::CCW; // CW or CCW
  motor1.voltage_sensor_align = 0.5;
  motor1.zero_electric_angle = 2.94; 
  motor1.sensor_direction = Direction::CCW; // CW or CCW


  // monitor Interface Settings
  // comment out if not needed
  motor0.useMonitoring(Serial);
  motor1.useMonitoring(Serial);


  //core0
  xTaskCreatePinnedToCore(
    web
    ,  "web"   // A name just for humans
    ,  4096  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  1  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  NULL
    ,  0);


  //Motor initialization
  motor0.init();
  // align encoder and start FOC
  motor0.initFOC();

  motor1.init();
  // align encoder and start FOC
  motor1.initFOC();

}


void loop() {
  motor0.loopFOC();
  motor1.loopFOC();

  motor0.move(-PI + (angle0 + angleOffset0) / 180.0 * PI);
  motor1.move(-PI - (angle1 + angleOffset1) / 180.0 * PI);
}


//逆運動学の計算
void ik(float x, float z) {
  float th1, th2, th1d, th2d, ld, phi;
  //z = constrain(z, 0.0, 140.0);
  ld = sqrt(x * x + z * z);

  th1d = acos(ld / (2 * L));
  th2d = acos((2 * L * L - ld * ld) / (2 * L * L));
  phi =  atan2(x, z);

  th1 = (phi + th1d) * 180.0 / PI;
  th2 = (PI - th2d) * 180.0 / PI  - th1;
  th1 = constrain(-(th1 - 45.0) * 5.6, -80.0, 120.0);
  th2 = constrain(-(th2 - 45.0) * 5.6, -80.0, 120.0);

  if (isnan(th1) != 1) angle0 = th1;
  if (isnan(th2) != 1) angle1 = th2;
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

#include <SimpleFOC.h>

#include <WiFi.h>

#include <WebServer.h>

#include <Preferences.h>

#include <PS4Controller.h>

float angle0 = 0.0, angle1 = 0.0;

int lastLStickX = 0, lastLStickY = 0;

WebServer server(80);

const char ssid[] = "BLDC-L"; // SSID

const char pass[] = "password"; // password

const IPAddress ip(192, 168, 22, 91); // IPアドレス

const IPAddress subnet(255, 255, 255, 0); // サブネットマスク

Preferences preferences;

//Motor Instance

BLDCMotor motor0 = BLDCMotor(7);

BLDCDriver3PWM driver0 = BLDCDriver3PWM(32, 33, 25, 12);

BLDCMotor motor1 = BLDCMotor(7);

BLDCDriver3PWM driver1 = BLDCDriver3PWM(26, 27, 14, 12);

//Encoder Instance

MagneticSensorI2C sensor0 = MagneticSensorI2C(AS5600_I2C);

TwoWire I2Cone = TwoWire(0);

MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);

TwoWire I2Ctwo = TwoWire(1);

// Inline Current sensing Instance

InlineCurrentSense current_sense0 = InlineCurrentSense(0.01f, 50.0f, 39, 36);

InlineCurrentSense current_sense1 = InlineCurrentSense(0.01f, 50.0f, 35, 34);

float L = 70.0;

float height = 99.0;

float angleOffset0 = 0.0, angleOffset1 = 0.0;

float angleP = 60.0, angleI = 1.0, angleD = 7.0;

float velP = 0.05, velI = 2.0;

float velLimit = 200.0;

float Vlimit = 3.0;

float Ilimit = 6.0;

float currentP = 1;

float currentI = 500;

float currentTf = 2;

//ブラウザ表示

void handleRoot() {

String temp = "<!DOCTYPE html> \n<html lang=\"ja\">";

temp += "<head>";

temp += "<meta charset=\"utf-8\">";

temp += "<title>BLDC-L</title>";

temp += "<meta name=\"viewport\" content=\"width=device-width, initial-scale=1\">";

temp += "<style>";

temp += ".container{";

temp += " max-width: 500px;";

temp += " margin: auto;";

temp += " text-align: center;";

temp += " font-size: 1.2rem;";

temp += "}";

temp += "span,.pm{";

temp += " display: inline-block;";

temp += " border: 1px solid #ccc;";

temp += " width: 50px;";

temp += " height: 30px;";

temp += " vertical-align: middle;";

temp += " margin-bottom: 20px;";

temp += "}";

temp += "span{";

temp += " width: 120px;";

temp += "}";

temp += "button{";

temp += " width: 100px;";

temp += " height: 40px;";

temp += " font-weight: bold;";

temp += " margin-bottom: 20px;";

temp += "}";

temp += "</style>";

temp += "</head>";

temp += "<body>";

temp += "<div class=\"container\">";

temp += "<h3>BLDC-L</h3>";

//angleOffset0

temp += "angleOffset0 ";

temp += "<a class=\"pm\" href=\"/angleOffset0M\">-</a>";

temp += "" + String(angleOffset0) + "";

temp += "<a class=\"pm\" href=\"/angleOffset0P\">+</a> ";

//angleOffset1

temp += "angleOffset1 ";

temp += "<a class=\"pm\" href=\"/angleOffset1M\">-</a>";

temp += "" + String(angleOffset1) + "";

temp += "<a class=\"pm\" href=\"/angleOffset1P\">+</a> ";

//angleP

temp += "angleP ";

temp += "<a class=\"pm\" href=\"/anglePM\">-</a>";

temp += "" + String(angleP) + "";

temp += "<a class=\"pm\" href=\"/anglePP\">+</a> ";

//angleI

temp += "angleI ";

temp += "<a class=\"pm\" href=\"/angleIM\">-</a>";

temp += "" + String(angleI) + "";

temp += "<a class=\"pm\" href=\"/angleIP\">+</a> ";

//angleD

temp += "angleD ";

temp += "<a class=\"pm\" href=\"/angleDM\">-</a>";

temp += "" + String(angleD) + "";

temp += "<a class=\"pm\" href=\"/angleDP\">+</a> ";

//velP

temp += "velP ";

temp += "<a class=\"pm\" href=\"/velPM\">-</a>";

temp += "" + String(velP) + "";

temp += "<a class=\"pm\" href=\"/velPP\">+</a> ";

//velI

temp += "velI ";

temp += "<a class=\"pm\" href=\"/velIM\">-</a>";

temp += "" + String(velI) + "";

temp += "<a class=\"pm\" href=\"/velIP\">+</a> ";

//velLimit

temp += "velLimit ";

temp += "<a class=\"pm\" href=\"/velLimitM\">-</a>";

temp += "" + String(velLimit) + "";

temp += "<a class=\"pm\" href=\"/velLimitP\">+</a> ";

temp += "</div>";

temp += "</body>";

server.send(200, "text/HTML", temp);

}

void angleOffset0M() {

if (angleOffset0 >= -360.0) {

angleOffset0 -= 1.0;

preferences.putFloat("angleOffset0", angleOffset0);

ik(0.0, height);

}

handleRoot();

}

void angleOffset0P() {

if (angleOffset0 < 360.0) {

angleOffset0 += 1.0;

preferences.putFloat("angleOffset0", angleOffset0);

ik(0.0, height);

}

handleRoot();

}

void angleOffset1M() {

if (angleOffset1 >= -360.0) {

angleOffset1 -= 1.0;

preferences.putFloat("angleOffset1", angleOffset1);

ik(0.0, height);

}

handleRoot();

}

void angleOffset1P() {

if (angleOffset1 < 360.0) {

angleOffset1 += 1.0;

preferences.putFloat("angleOffset1", angleOffset1);

ik(0.0, height);

}

handleRoot();

}

void velPM() {

if (velP >= 0.0) {

velP -= 0.01;

preferences.putFloat("velP", velP);

motor0.PID_velocity.P = velP / 10.0;

motor1.PID_velocity.P = velP / 10.0;

}

handleRoot();

}

void velPP() {

if (velP < 3.0) {

velP += 0.01;

preferences.putFloat("velP", velP);

motor0.PID_velocity.P = velP / 10.0;

motor1.PID_velocity.P = velP / 10.0;

}

handleRoot();

}

void velIM() {

if (velI >= 0.0) {

velI -= 0.1;

preferences.putFloat("velI", velI);

motor0.PID_velocity.I = velI;

motor1.PID_velocity.I = velI;

}

handleRoot();

}

void velIP() {

if (velI < 20.0) {

velI += 0.1;

preferences.putFloat("velI", velI);

motor0.PID_velocity.I = velI;

motor1.PID_velocity.I = velI;

}

handleRoot();

}

void anglePM() {

if (angleP >= 0.0) {

angleP -= 1.0;

preferences.putFloat("angleP", angleP);

motor0.P_angle.P = angleP;

motor1.P_angle.P = angleP;

}

handleRoot();

}

void anglePP() {

if (angleP < 400.0) {

angleP += 1.0;

preferences.putFloat("angleP", angleP);

motor0.P_angle.P = angleP;

motor1.P_angle.P = angleP;

}

handleRoot();

}

void angleIM() {

if (angleI >= 0.0) {

angleI -= 0.1;

preferences.putFloat("angleI", angleI);

motor0.P_angle.I = angleI;

motor1.P_angle.I = angleI;

}

handleRoot();

}

void angleIP() {

if (angleI < 400.0) {

angleI += 0.1;

preferences.putFloat("angleI", angleI);

motor0.P_angle.I = angleI;

motor1.P_angle.I = angleI;

}

handleRoot();

}

void angleDM() {

if (angleD >= 0.0) {

angleD -= 0.1;

preferences.putFloat("angleD", angleD);

motor0.P_angle.D = angleD;

motor1.P_angle.D = angleD;

}

handleRoot();

}

void angleDP() {

if (angleD < 400.0) {

angleD += 0.1;

preferences.putFloat("angleD", angleD);

motor0.P_angle.D = angleD;

motor1.P_angle.D = angleD;

}

handleRoot();

}

void velLimitM() {

if (velLimit >= 0.0) {

velLimit -= 10.0;

preferences.putFloat("velLimit", velLimit);

motor0.velocity_limit = velLimit;

motor1.velocity_limit = velLimit;

}

handleRoot();

}

void velLimitP() {

if (velLimit < 500.0) {

velLimit += 10.0;

preferences.putFloat("velLimit", velLimit);

motor0.velocity_limit = velLimit;

motor1.velocity_limit = velLimit;

}

handleRoot();

}

//Core0

void web(void *pvParameters) {

PS4.begin();

WiFi.softAP(ssid, pass); // SSIDとパスの設定

delay(100); // 追記：このdelayを入れないと失敗する場合がある

WiFi.softAPConfig(ip, ip, subnet); // IPアドレス、ゲートウェイ、サブネットマスクの設定

IPAddress myIP = WiFi.softAPIP(); // WiFi.softAPIP()でWiFi起動

server.on("/", handleRoot);

server.on("/angleOffset0P", angleOffset0P);

server.on("/angleOffset0M", angleOffset0M);

server.on("/angleOffset1P", angleOffset1P);

server.on("/angleOffset1M", angleOffset1M);

server.on("/velPP", velPP);

server.on("/velPM", velPM);

server.on("/velIP", velIP);

server.on("/velIM", velIM);

server.on("/velLimitP", velLimitP);

server.on("/velLimitM", velLimitM);

server.on("/anglePP", anglePP);

server.on("/anglePM", anglePM);

server.on("/angleIP", angleIP);

server.on("/angleIM", angleIM);

server.on("/angleDP", angleDP);

server.on("/angleDM", angleDM);

server.begin();

ik(0.0, height);

for (;;) {

server.handleClient();

if (PS4.isConnected()) { //PS4コントローラ

int currentLStickX = PS4.LStickX();

int currentLStickY = PS4.LStickY();

if (currentLStickX != lastLStickX || currentLStickY != lastLStickY) {

float Lx = PS4.LStickX() / 4.0;

float Ly = PS4.LStickY() / 3.0;

ik(-Lx, height + Ly);

lastLStickX = currentLStickX;

lastLStickY = currentLStickY;

}

disableCore0WDT();

}

void setup() {

pinMode(32, INPUT_PULLUP);

pinMode(33, INPUT_PULLUP);

pinMode(25, INPUT_PULLUP);

pinMode(26, INPUT_PULLUP);

pinMode(27, INPUT_PULLUP);

pinMode(14, INPUT_PULLUP);

Serial.begin(115200);

Serial.println("-----------------------------");

uint8_t btmac[6];

esp_read_mac(btmac, ESP_MAC_BT);

Serial.printf("[Bluetooth] Mac Address = %02X:%02X:%02X:%02X:%02X:%02X\r\n", btmac[0], btmac[1], btmac[2], btmac[3], btmac[4], btmac[5]);

//パラメータ初期値取得

preferences.begin("parameter", false);

angleOffset0 = preferences.getFloat("angleOffset0", angleOffset0);

angleOffset1 = preferences.getFloat("angleOffset1", angleOffset1);

velP = preferences.getFloat("velP", velP);

velI = preferences.getFloat("velI", velI);

velLimit = preferences.getFloat("velLimit", velLimit);

angleP = preferences.getFloat("angleP", angleP);

angleI = preferences.getFloat("angleI", angleI);

angleD = preferences.getFloat("angleD", angleD);

// Encoder Settings

I2Cone.begin(19, 18, 400000UL); // AS5600_M0

I2Ctwo.begin(23, 5, 400000UL); // AS5600_M1

sensor0.init(&I2Cone);

sensor1.init(&I2Ctwo);

//Connect the motor object and the sensor object

motor0.linkSensor(&sensor0);

motor1.linkSensor(&sensor1);

// Drive Settings

driver0.voltage_power_supply = 12;

driver0.init();

motor0.linkDriver(&driver0);

driver1.voltage_power_supply = 12;

driver1.init();

motor1.linkDriver(&driver1);

// Current Limitation

motor0.current_limit = Ilimit;

motor1.current_limit = Ilimit;

// Voltage Limitation

motor0.voltage_limit = Vlimit;

motor1.voltage_limit = Vlimit;

// Current Sensing

current_sense0.init();

motor0.linkCurrentSense(&current_sense0);

current_sense0.linkDriver(&driver0);

// current sense init and linking

current_sense1.init();

motor1.linkCurrentSense(&current_sense1);

current_sense1.linkDriver(&driver1);

// Control loop

// Other modes TorqueControlType::voltage TorqueControlType::dc_current

motor0.torque_controller = TorqueControlType::foc_current;

motor0.controller = MotionControlType::angle;

motor1.torque_controller = TorqueControlType::foc_current;

motor1.controller = MotionControlType::angle;

// FOC current control PID parameters

motor0.PID_current_q.P = currentP;

motor0.PID_current_q.I = currentI;

motor0.PID_current_d.P = currentP;

motor0.PID_current_d.I = currentI;

motor0.LPF_current_q.Tf = currentTf / 1000.0; // 1ms default

motor0.LPF_current_d.Tf = currentTf / 1000.0; // 1ms default

motor1.PID_current_q.P = currentP;

motor1.PID_current_q.I = currentI;

motor1.PID_current_d.P = currentP;

motor1.PID_current_d.I = currentI;

motor1.LPF_current_q.Tf = currentTf / 1000.0; // 1ms default

motor1.LPF_current_d.Tf = currentTf / 1000.0; // 1ms default

// Speed loop PID parameters

motor0.PID_velocity.P = velP / 10.0;

motor0.PID_velocity.I = velI;

motor0.PID_velocity.D = 0;

motor1.PID_velocity.P = velP / 10.0;

motor1.PID_velocity.I = velI;

motor1.PID_velocity.D = 0;

motor0.LPF_velocity.Tf = 0.0;

motor1.LPF_velocity.Tf = 0.0;

//Angle PID Setting

motor0.P_angle.P = angleP;

motor0.P_angle.I = angleI;

motor0.P_angle.D = angleD;

motor1.P_angle.P = angleP;

motor1.P_angle.I = angleI;

motor1.P_angle.D = angleD;

// Speed Limit

motor0.velocity_limit = velLimit;

motor1.velocity_limit = velLimit;

motor0.voltage_sensor_align = 0.5;

motor0.zero_electric_angle = 2.14;

motor0.sensor_direction = Direction::CCW; // CW or CCW

motor1.voltage_sensor_align = 0.5;

motor1.zero_electric_angle = 2.94;

motor1.sensor_direction = Direction::CCW; // CW or CCW

// monitor Interface Settings

// comment out if not needed

motor0.useMonitoring(Serial);

motor1.useMonitoring(Serial);

//core0

xTaskCreatePinnedToCore(

web

, "web" // A name just for humans

, 4096 // This stack size can be checked & adjusted by reading the Stack Highwater

, NULL

, 1 // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.

, NULL

, 0);

//Motor initialization

motor0.init();

// align encoder and start FOC

motor0.initFOC();

motor1.init();

// align encoder and start FOC

motor1.initFOC();

}

void loop() {

motor0.loopFOC();

motor1.loopFOC();

motor0.move(-PI + (angle0 + angleOffset0) / 180.0 * PI);

motor1.move(-PI - (angle1 + angleOffset1) / 180.0 * PI);

}

//逆運動学の計算

void ik(float x, float z) {

float th1, th2, th1d, th2d, ld, phi;

//z = constrain(z, 0.0, 140.0);

ld = sqrt(x * x + z * z);

th1d = acos(ld / (2 * L));

th2d = acos((2 * L * L - ld * ld) / (2 * L * L));

phi = atan2(x, z);

th1 = (phi + th1d) * 180.0 / PI;

th2 = (PI - th2d) * 180.0 / PI - th1;

th1 = constrain(-(th1 - 45.0) * 5.6, -80.0, 120.0);

th2 = constrain(-(th2 - 45.0) * 5.6, -80.0, 120.0);

if (isnan(th1) != 1) angle0 = th1;

if (isnan(th2) != 1) angle1 = th2;

}

創刊号のFOC回転位置制御Arduinoコードとの差分のみ説明します。

L. 39　L = 70.0 下腿、大腿の長さを指定 (70.0mm)
L. 40　足先の初期高さを99mmと指定
　初期の足の角度を45°とするため ($\sqrt{2} × 70 \approx 99$)。

L. 48　電流リミットを2Aから6Aにしてモータトルクを向上させます。
　電源は十分電流能力があるものを使用ください

L. 356～369　PS4コントローラ左ジョイスティック動作
　上下に加えて前後も指定してIKで足を動かします。

L. 493～498　モータ初期設定
　モータ回転方向とエンコーダオフセット値は前回の測定値を入力してください。

motor0.voltage_sensor_align = 0.5;
motor0.zero_electric_angle = 2.14; 
motor0.sensor_direction = Direction::CCW; // CW or CCW
motor1.voltage_sensor_align = 0.5;
motor1.zero_electric_angle = 2.94; 
motor1.sensor_direction = Direction::CCW; // CW or CCW

motor0.voltage_sensor_align = 0.5;

motor0.zero_electric_angle = 2.14;

motor0.sensor_direction = Direction::CCW; // CW or CCW

motor1.voltage_sensor_align = 0.5;

motor1.zero_electric_angle = 2.94;

motor1.sensor_direction = Direction::CCW; // CW or CCW

　
L. 539～556　逆運動学の計算 ik(x, z)
　足先の前後:xと高さzを指定してモータの角度を算出しています。
　L. 551, 552　下腿や大腿の初期角度を45°として(ギア比5.6で換算)、
　　　　　　　モータは-80°～120°までの回転で制限しています (干渉による過電流防止)。

ブラウザアプリ動作やPS4コントローラ接続については創刊号と全く同じです。

動作

Arduinoコードを書き込んでPS4コントローラと接続すると、左ジョイスティックで前後上下に足が動きます。

予算3万円でつくる BLDC 2足歩行ロボット
足のIKを楽しむ pic.twitter.com/6gKO0acqjX

— HomeMadeGarbage (@H0meMadeGarbage) September 2, 2025

以上でIKによる足制御ができるようになりました。

演習問題

1-パラメータ調整

IKのコードでブラウザアプリやコードを修正するなどして各PIDパラメータ値を変更して動作にどう影響するのか体験してみよう。

電流リミットは6A以上にすると過電流でMKS ESP32 FOC V2.0のパワートランジスタが壊れる可能性がありますのでご注意ください。

2-ジャンプ動作

ここではPS4コントローラのジョイスティックでのIKを楽しみましたが
コードを改良してボタンを押すとジャンプするようにしてみましょう。

ik(x, z)関数でジャンプ動作のシーケンスを組んでみましょう。

おわりに

本号ではロボットの足を完成させて、IKによる制御を楽しみました。
モータ角度ではなく足の位置での制御が可能になるとより豊かな動作が実現できるようになります。

次回はロボットの股関節を製作し動作を楽しみたいと思います。

コメントはこちらからコメントをキャンセル

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください。

はじめに

本号で必要な部品

ロボット完成に必要な主な部品

3Dモデル

足上部組立て

足フレーム組立て

足完成

動作

逆運動学 (IK)

逆運動学 算出

Arduinoコード

動作

演習問題

1-パラメータ調整

2-ジャンプ動作

おわりに

関連記事

コメントはこちらから コメントをキャンセル

逆運動学算出

コメントはこちらからコメントをキャンセル