気軽に作れるブラシレスモータ(BLDC)による2足歩行ロボットを紹介したく、この度本主題で創刊をさせていただきました。

近頃ドローン向けのBLDCが低価格で流通されるようになり、ギアで減速すればロボにも活用できることを確認しました。

卓上で楽しめるブラシレスモータによる2足歩行ロボットは大変珍しいと自負しております。

私はエレクトロニクスエンジニアなので油圧は考えたことがありません。 pic.twitter.com/U9hs9hOdg1

— HomeMadeGarbage (@H0meMadeGarbage) May 12, 2025

まだまたロボット用のアクチュエータは高額ですので、本格ロボ製作の前段階の学習として本連載を活用いただければ幸いです。

全6号を予定しており4号まででロボが完成する予定です。
ロボ筐体は3Dプリント品で構成され、毎号必要なモデルデータを販売させていただきます
(低額で販売させていただきますので何卒宜しくお願い致します子がお腹を空かせているもので….)。

はじめに

初回の今回はBLDCの駆動に使用しているSimpleFOCライブラリについて紹介をさせていただきます。

ブラシレスモータのベクトル制御 (FOC) を自身で実装となるとそれだけでも大変でモータを回すだけで疲弊してしまいます。

ESP32でベクトル制御　ーブラシレスモータ駆動への道9ー

　
そこでSimpleFOCライブラリを活用して、サクッとFOC駆動で応用していきましょう。
SimpleFOCライブラリは各種駆動モードが用意されており、ここでSimpleFOCを体に入れておけば、本ロボット以外の他のプロジェクトにも利用できると思います。

本号で必要な部品

本号では2足歩行ロボットの足の駆動に必要な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個
M3 6mm なべネジ：16本
M3 5mm なべネジ：8本
M3 25mm なべネジ：2本
M2 14mm なべネジ：2本

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

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

MKS ESP32 FOC V2.0

ブラシレスモータのコントローラとして MKS ESP32 FOC V2.0 を使用します。
ESP32が搭載され2個のBLDCを駆動できます。

V1.0からV2.0で変更点がいくつかありました。

MKS ESP32 FOC V2.0入手
外部インターフェースのコネクタがなじみのうすい型で
困る

KH-A1252RS-4Phttps://t.co/omvOe0YJlP pic.twitter.com/jenxfuT4pk

— HomeMadeGarbage (@H0meMadeGarbage) January 9, 2025

USBがCコネクタとなり、電流センサに変更がありました。

USBシリアル変換ICとしてCH340Kが採用されWinPCの場合はドライバのインストールが必要です。
以下を参考にドライバインストールしてください。

V1.0はモータ電源を供給しないとUSBシリアル書き込みができませんでしたが、
V2.0はUSB給電で書き込みができるようになったので不便さは改善されています。
しかし外部電源供給前にUSBを接続するとモータ駆動で過電流が流れる可能性があるので注意が必要です。

コード書き込み前にモータを接続するとプリドライバの入力が不定で貫通電流が流れ壊れる可能性があるため、モータ接続前にプリドライバの入力を確定したコードを書き込む必要があります。

GitHub
　https://github.com/makerbase-motor/MKS-ESP32FOC

3Dモデル

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

[予算3万円でつくる BLDC 2足歩行ロボット] 創刊号用 3Dモデルデータ

データには7つのstlファイルがあり motorFix.stlとgear56-70-L.stl, gear56-70-R.stlは1個プリントし、その他は2個づつ印刷してください。

gear10.stlとgearFix.stlは小型で強度が求められる部品のため、硬いPETGフィラメントで充填率100％で出力することをおすすめします。
その他の部品はPLAなどで通常通りのプリントで問題ありません。

FOC回転位置制御

ここからはいよいよSimpleFOCによるBLDC回転位置制御実施に向けて手順通り記載いたします。

筐体組み立て

motorFix.stlにモータやエンコーダを固定します。
M3ネジで固定するので固定穴をねじ切りします。

以下の図で示す穴をM3サイズでねじ切りしてください。

穴の数が多いので電動ドリルでの実施がおすすめです。手作業だと腱鞘炎なるで。。。

6角軸タップ M3×0.5https://t.co/Q8ozPICWEQ pic.twitter.com/8vSZaHgJqp

— HomeMadeGarbage (@H0meMadeGarbage) August 26, 2025

モータ動作試験が実施しやすくなるのでスペーサをつけることをおすすめします。

BLDC 5010 360KVの回転軸裏面に磁気エンコーダ付属の磁石を接着します。ここでは瞬間接着剤を使用しました。

5010 360KV 2個を筐体にねじ止めします (M3 6mm なべネジ：8本)。

磁気エンコーダAS5600に MKS ESP32 FOC V2.0付属の5ピンコネクタラインを加工して配線します。

MKS ESP32 FOC V2.0とAS5600が以下のようにつながるよう配線してください。
1線余るので切断なりで除去
　3.3V－VCC
　GND－GND
　SDA－SDA
　SCL－SCL

コネクタを付けた磁気エンコーダを筐体にねじ止めします (M3 6mm なべネジ：8本)。

MKS ESP32 FOC V2.0

モータ接続前にFOC回転位置制御用のコードを書き込みます。

Arduinoコード

今回の環境バージョンは以下の通りです。
　Arduino IDEバージョン：ver. 1.8.19
　ESP32ライブラリ：ver. 2.0.13
　SimpleFOCライブラリ：ver. 2.3.3
　PS4Controllerライブラリ：ver. 1.1.0

このコードはFOC(ベクトル制御)によってモータの回転位置を指定して制御します。
回転位置はWiFiを介してブラウザからもしくはPS4コントローラで指定します。

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

float angle0 = 0.0, angle1 = 0.0;


int 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 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 = 190.0;
float Vlimit = 3.0;
float Ilimit = 2.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);
  }
  handleRoot();
}

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


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

void angleOffset1P() {
  if (angleOffset1 < 360.0) {
    angleOffset1 += 1.0;
    preferences.putFloat("angleOffset1", angleOffset1);
  }
  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();


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

    if (PS4.isConnected()) { //PS4コントローラ
      int currentLStickY = PS4.LStickY();
        
      if (currentLStickY != lastLStickY) {
        float Ly = PS4.LStickY();
        
        angle0 = Ly;
        angle1 = Ly;

        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 = 0.0; 
  //motor0.sensor_direction = Direction::CCW; // CW or CCW
  motor1.voltage_sensor_align = 0.5;
  //motor1.zero_electric_angle = 0.0; 
  //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);
}

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

#include <SimpleFOC.h>

#include <WiFi.h>

#include <WebServer.h>

#include <Preferences.h>

#include <PS4Controller.h>

float angle0 = 0.0, angle1 = 0.0;

int 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 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 = 190.0;

float Vlimit = 3.0;

float Ilimit = 2.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);

}

handleRoot();

}

void angleOffset0P() {

if (angleOffset0 < 360.0) {

angleOffset0 += 1.0;

preferences.putFloat("angleOffset0", angleOffset0);

}

handleRoot();

}

void angleOffset1M() {

if (angleOffset1 >= -360.0) {

angleOffset1 -= 1.0;

preferences.putFloat("angleOffset1", angleOffset1);

}

handleRoot();

}

void angleOffset1P() {

if (angleOffset1 < 360.0) {

angleOffset1 += 1.0;

preferences.putFloat("angleOffset1", angleOffset1);

}

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();

for (;;) {

server.handleClient();

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

int currentLStickY = PS4.LStickY();

if (currentLStickY != lastLStickY) {

float Ly = PS4.LStickY();

angle0 = Ly;

angle1 = Ly;

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 = 0.0;

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

motor1.voltage_sensor_align = 0.5;

//motor1.zero_electric_angle = 0.0;

//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);

}

L. 12～18　WiFi APモード用設定。SSIDやパスワード、IPを指定 (それぞれ任意)。

L. 20　Preferencesライブラリを使用して各設定値をフラッシュに記憶して電源OFF後も保持します。

L. 23～51　モータやエンコーダ、電流センサおよび各パラメータの設定
　ちなみにBLDC 5010 360KVは14極の7ペアです。

L. 55～309　ブラウザアプリ表示設定
　APモード接続時の各パラメータ設定用の表示を記述しています。　

L. 312～364　core0動作
　ESP32 デュアルコアのcore0でブラウザ表示とPS4コントローラ処理を実行します。

L. 367～518　setup関数
　冒頭でモータのプリドライバの入力が不定にならないようにプルアップしで貫通電流防止

　PS4コントローラ接続のために Bluetooth Mac Addressをシリアル出力しています。 (L. 378～381)

　アプリで設定した各設定値はPreferences機能でフラッシュに記録します。
　電源起動時にフラッシュに記録されたオフセット値を読み出します (L. 384～396)

　モータ駆動の設定 (L. 399～495, 509～516)
　モータの電圧と電流はここではそれぞれ3Vと2Aで制限しています。
　今回はベクトル制御 (FOC)の回転位置制御としています

motor0.torque_controller = TorqueControlType::foc_current;
motor0.controller = MotionControlType::angle;
motor1.torque_controller = TorqueControlType::foc_current;
motor1.controller = MotionControlType::angle;

motor0.torque_controller = TorqueControlType::foc_current;

motor0.controller = MotionControlType::angle;

motor1.torque_controller = TorqueControlType::foc_current;

motor1.controller = MotionControlType::angle;

　制御ブロックは以下のとおり

　モータ回転位置(角度)と回転速度がPID制御され、さらにトルク制御ループ内で電流値もPID制御されます。

L. 521～527　loop関数
　モータ角度を指定します。
　初期位置は-π (-180°)で angleはPS4コントローラでangleOffsetはブラウザで指定して回転させます。

書き込み

ボードは”ESP32 Dev Module”を選択
PS4Controllerライブラリのサイズが大きいのでPartition Schemeは”Huge APP”を選択

モータ、エンコーダ接続

以下の図のように右側がmotor0 (M0)、左側がmotor1 (M1)となるようにモータ配線と磁気エンコーダのコネクタをMKS ESP32 FOC V2.0に接続します。

モータ配線はここでは左から黄色、黒、赤の順で接続します。

動作

MKS ESP32 FOC V2.0の電源コネクタに安定化電源 (12V)や3セルLiPoバッテリを接続して給電するとモータの初期化動作が実行されます。

予算3万円でつくる BLDC 2足歩行ロボット
SimpleFOC モータ初期化動作 pic.twitter.com/JSU1o8WXKK

— HomeMadeGarbage (@H0meMadeGarbage) August 27, 2025

初期化動作時にモータの回転方向や電気角オフセットの同定がなされます。

初期化動作時にモータに大きな電圧がかかると過電流が流れてしまうため0.5Vに指定しています (Arduinoコード L. 484, 487)。

motor0.voltage_sensor_align = 0.5;
motor1.voltage_sensor_align = 0.5;

1 2	motor0.voltage_sensor_align = 0.5; motor1.voltage_sensor_align = 0.5;

初期化動作がフェイルなしに終了するとモータが初期位置の-π (-180°)で停止し回転位置制御モードとなります。

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

— HomeMadeGarbage (@H0meMadeGarbage) August 29, 2025

ブラウザやPS4コントローラで回転位置を指定できます。

ブラウザ指定

ESP32のAPモードでWiFi接続してブラウザから回転位置を指定します。

スマホもしくはPCのWiFi接続設定でSSID ”BLDC-L”に接続
　パスワード：password
ブラウザで”192.168.22.91”にアクセス

angleOffset0とangleOffset1の値 [°]を変更することでモータ回転位置をそれぞれ制御できます。

制御ループの角度PIDや速度PIDパラメータも調整できるのでぜひご自身で調整して動きをご確認ください (以下に演習あり)。

PS4コントローラ指定

PS4コントローラとペアリングすることでコントローラのアナログスティックでモータを回転させます。

Lスティックの上下でモータを±128°回します (Arduinoコード L. 353～356)。

float Ly = PS4.LStickY();
        
angle0 = Ly;
angle1 = Ly;

float Ly = PS4.LStickY();

angle0 = Ly;

angle1 = Ly;

MKS ESP32 FOC V2.0の起動時に Bluetooth Mac Addressをシリアル出力させています。
アドレスを控えてコントローラと接続します。

PS4コントローラとESP32のペアリング方法の詳細は以下を参照ください。

・ESP32 (M5StickC) で PS4コントローラを使ってみた : Skyzoo ヨッシーの備忘録

Skyzoo ヨッシーの備忘録

PS4ペアリング後にはアプリによるWiFi接続ができませんので、両方実施したい場合はWiFiアプリ接続後にBluetoothコントローラペアリングしてください。

ギアを楽しむ

FOC回転位置制御ができるようになったのでロボ足に近づくべくギアをつけて回転を確認します。

エンコーダオフセット初期値設定

ギアを導入する前に電源投入のたびに初期化動作でモータが回転してしまうのが具合が悪いので回転方向とエンコーダオフセット値をあらかじめ指定して初期化時の回転チェックをパスします。

初期化動作時にチェック結果はシリアル出力されるので値を控えます。

Arduinoコードの初期化動作設定部 (L. 484～489)にシリアル出力で得たオフセット値と回転方向を記載しコメントアウトを解除して書き込みます。

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

これで初期化動作時のモータ回転はなくなります。

モータは初期位置の-π (-180°)で停止しますので、位置をわかりやすくするために以下のようにマーキングすることをおすすめします。

ギア接続

gearFix.stlの中心穴をM2ネジサイズでねじ切りします。

ねじ切りしたgearFix.stl 2個をモータにねじ止めします (M3 5mm なべネジ：8本)。

以下のようにgear56-70-L.stl、gear56-70-R.stlをleg1.stlの筒穴に通してM3 25mm なべネジでmotorFix.stlにねじ止めします。

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

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

gear10.stlをはめたのちにleg1.stlの穴にM2-52.stlを通してM2 14mm なべネジでギアを固定します

動作

まずはWiFIブラウザアプリでギアのアーム角度を刻印されたマーキングを目安に45°に調整します。

予算3万円でつくる BLDC 2足歩行ロボット
ギア位置調整 pic.twitter.com/rueUdeTZ04

— HomeMadeGarbage (@H0meMadeGarbage) August 30, 2025

アプリのangleOffsetで”-“, “+”ボタンをクリックして調整ください。

調整後にPS4コントローラを接続してLスティックの上下でモータを動かします。

予算3万円でつくる BLDC 2足歩行ロボット
FOC回転位置制御ギアバージョン pic.twitter.com/NztiYkGuiH

— HomeMadeGarbage (@H0meMadeGarbage) August 30, 2025

接続したギアはモータのギアの歯数が10でアームのほうが56となり、ギア比が5.6となります。

PS4コントローラでモータを±128°動かすとアームは±128 / 5.6 = 23° 動きます。
動きが狭くなった分トルクは5.6倍となっておりますのでぜひ実際に体感ください。

モータ初期位置やギア固定位置を誤るとギアが上限以上に動いて過電流が流れる可能性があるので十分に気を付けてください。

FOCトルク制御

ここまででSimpleFOCによるFOC回転位置制御を実施いただきました。
製作する2足歩行ロボットの足動作は回転位置制御で実施するので、準備はOKなのですが、せっかくなのでさらにSimpleFOCを楽しんでいただくべくここではトルク制御を紹介いたします。

FOCトルク制御のシステム図は以下の通りです。

モータ電流 (トルク)を検知して2相変換された電流の誤差で制御となるためシステム自体は非常にすっきりします。

Arduinoコード

#include <SimpleFOC.h>


//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 Vlimit = 3.0;
float Ilimit = 2.0;

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



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);


  // 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::torque;
  motor1.torque_controller = TorqueControlType::foc_current;
  motor1.controller = MotionControlType::torque;


  // 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


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


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


  //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(3.0 * (motor1.shaft_angle - motor0.shaft_angle));
  motor1.move(3.0 * (motor0.shaft_angle - motor1.shaft_angle));
}

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

#include <SimpleFOC.h>

//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 Vlimit = 3.0;

float Ilimit = 2.0;

float currentP = 1;

float currentI = 500;

float currentTf = 2;

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);

// 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::torque;

motor1.torque_controller = TorqueControlType::foc_current;

motor1.controller = MotionControlType::torque;

// 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

motor0.voltage_sensor_align = 0.5;

//motor0.zero_electric_angle = 0.0;

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

motor1.voltage_sensor_align = 0.5;

//motor1.zero_electric_angle = 0.0;

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

// monitor Interface Settings

// comment out if not needed

motor0.useMonitoring(Serial);

motor1.useMonitoring(Serial);

//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(3.0 * (motor1.shaft_angle - motor0.shaft_angle));

motor1.move(3.0 * (motor0.shaft_angle - motor1.shaft_angle));

}

MotionControlTypeをtorqueと指定しています(L. 84, 86)。

PID制御パラメータも2相変換された電流値のもののみになります(L. 89～102)。

loop関数内のmoveでトルクを指定してモータを動かします(L. 135, 136)。
双方のモータの位置をshaft_angleで検知してその差分の3倍をトルクとしています。
したがってモータは互いの回転位置に追従して動きます。

初期化のモータの回転方向やオフセット値をコメントアウトしているので
ギアは外してコードを書き込んでください。

動作

予算3万円でつくる BLDC 2足歩行ロボット
FOCトルク制御DJ pic.twitter.com/svGUcNGhiE

— HomeMadeGarbage (@H0meMadeGarbage) August 27, 2025

演習問題

さらにさらにSimpleFOCを堪能していただきたく以下に演習問題を挙げますので是非トライしてみてください。

1-パラメータ調整

FOC回転位置制御でアプリやコードを修正するなどして各PIDパラメータ値を変更すると動作にどう影響するのか体験してみよう。

2-ほかの動作モードに挑戦

ここではFOCによる回転位置制御とトルク制御を紹介しましたが是非ほかの動作モードも試してみてください。

たとえばモータ回転スピードを指定する制御など。

　
以下に大変に参考になるリンクを挙げます、ご参照ください。

おわりに

記念すべき予算3万円でつくる BLDC 2足歩行ロボット創刊号としてSimpleFOCについて紹介させていただきました。

simpleFOCライブラリは大変充実しており、BLDCを活用する際の大きな助けになると考えております。
本号が読者様のBLDC応用のきっかけとなればうれしく思います。

次回はここでのFOC回転位置制御を発展させてロボットの足のIK(逆運動学)を楽しみたいと思います。

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

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

はじめに

本号で必要な部品

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

MKS ESP32 FOC V2.0

3Dモデル

FOC回転位置制御

筐体組み立て

MKS ESP32 FOC V2.0

Arduinoコード

書き込み

モータ、エンコーダ接続

動作

ブラウザ指定

PS4コントローラ指定

ギアを楽しむ

エンコーダオフセット初期値設定

ギア接続

動作

FOCトルク制御

Arduinoコード

動作

演習問題

1-パラメータ調整

2-ほかの動作モードに挑戦

おわりに

関連記事

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

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