予算3万円でつくる BLDC 2足歩行ロボット 第4号
ー2足歩行ロボット完成ー
お父ちゃん
2025.9.19
0
0
前号では股関節ユニットを完成させて、機体傾きによる制御を楽しみました。
本号ではこれまでに製作した左足と股関節ユニットに加えて右足を追加製作してロボットを完成させます。
目次
はじめに
ここからいよいよ2足歩行ロボットを完成させます。
まずは右足を製作します。
その後にこれまで製作した左足と股関節を結合させてロボットを組みます。
本号では完成させたロボットのモータをPS4コントローラで動かすまでを紹介いたします。
ロボット基本構成
本ロボットのCAN通信システム構成は以下の通りです。
本ロボの中枢コントローラとなるATOM Matrix で股関節のモータ角度と各足の位置座標を指定します。
さらに足からはヒザモータのトルクをATOMに送信します。
PS4コントローラとATOMをペアリングしてロボ操作を実施します。
右足製作
まずは残りのパーツとなる右足を組み立てます。
必要部品
- BLDCコントローラ MKS ESP32 FOC V2.0 ：1個
- BLDC 5010 360KV：2個
- 磁気エンコーダ AS5600：2個
- その他
10mm以上のねじスペーサ：4個
M2ナット：6個
M2 12mm なべネジ：4本
M2 14mm なべネジ：2本
M2 20mm なべネジ：6本
M2 25mm なべネジ：2本
M3ナイロンネジ：2本
M3ナット：2個
M3 5mm なべネジ：8本
M3 6mm なべネジ：16本
M3 14mm なべネジ：2本
M3 18mm なべネジ：1本
M3 20mm なべネジ：1本
M3 25mm なべネジ：5本
M3 30mm なべネジ：1本
Arduinoコード
右足用のコードです。あらかじめMKS ESP32 FOC V2.0に書き込みます。
足座標からIKでモータ回転位置を算出してFOCで制御します。
ブラウザアプリでパラメータ調整し、CAN通信で足座標を受信・トルクデータ送信します。
環境バージョンは前回と同様です。
Arduino IDEバージョン：ver. 1.8.19
ESP32ライブラリ：ver. 2.0.13
SimpleFOCライブラリ：ver. 2.3.3
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 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 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 |
#include <SimpleFOC.h> #include <WiFi.h> #include <WebServer.h> #include <Preferences.h> #include "driver/twai.h" float angle0 = 0.0, angle1 = 0.0; twai_message_t rx_msg, tx_msg; WebServer server(80); const char ssid[] = "BLDC-R"; // SSID const char pass[] = "password"; // password const IPAddress ip(192, 168, 22, 92); // IPアドレス const IPAddress subnet(255, 255, 255, 0); // サブネットマスク Preferences preferences; float current_magnitude0, current_magnitude1; //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 xx = 0.0, 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; //CAN送信 トルク void sendI(float i) { tx_msg.identifier = 0x212; tx_msg.extd = 0; // 標準ID（11bit） tx_msg.data_length_code = 4; // floatをバイト配列にコピー memcpy(&tx_msg.data[0], &i, 4); // 送信 twai_transmit(&tx_msg, 0); } //ブラウザ表示 void handleRoot() { String temp = "<!DOCTYPE html> \n<html lang=\"ja\">"; temp += "<head>"; temp += "<meta charset=\"utf-8\">"; temp += "<title>BLDC-R</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-R</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(-xx, height); } handleRoot(); } void angleOffset0P() { if (angleOffset0 < 360.0) { angleOffset0 += 1.0; preferences.putFloat("angleOffset0", angleOffset0); ik(-xx, height); } handleRoot(); } void angleOffset1M() { if (angleOffset1 >= -360.0) { angleOffset1 -= 1.0; preferences.putFloat("angleOffset1", angleOffset1); ik(-xx, height); } handleRoot(); } void angleOffset1P() { if (angleOffset1 < 360.0) { angleOffset1 += 1.0; preferences.putFloat("angleOffset1", angleOffset1); ik(-xx, 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) { twai_general_config_t g_config = { .mode = TWAI_MODE_NORMAL, .tx_io = (gpio_num_t)17, .rx_io = (gpio_num_t)16, .clkout_io = TWAI_IO_UNUSED, .bus_off_io = TWAI_IO_UNUSED, .tx_queue_len = 5, .rx_queue_len = 5, .alerts_enabled = TWAI_ALERT_NONE, .clkout_divider = 0, .intr_flags = ESP_INTR_FLAG_LEVEL1 }; twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS(); // フィルタ設定（受信マスク） twai_filter_config_t f_config = { .acceptance_code = (0x122 << 21), // IDは左に21ビットシフトする必要がある（11bit標準ID用） .acceptance_mask = ~(0x7FF << 21), // マスクで0x122以外全部除外（0x7FF=11bit全部1） .single_filter = true // 単一フィルタ使う }; // ドライバインストール if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) { Serial.println("TWAI driver installed"); } else { Serial.println("TWAI driver install failed"); while (1); } // 通信開始 if (twai_start() == ESP_OK) { Serial.println("TWAI started"); } else { Serial.println("TWAI start failed"); while (1); } 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(-xx, height); for (;;) { server.handleClient(); if (twai_receive(&rx_msg, 0) == ESP_OK) { if (rx_msg.identifier == 0x122 && rx_msg.data_length_code == 8) { memcpy(&xx, &rx_msg.data[0], 4); memcpy(&height, &rx_msg.data[4], 4); ik(-xx, height); } } sendI(current_magnitude0); 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(¤t_sense0); current_sense0.linkDriver(&driver0); // current sense init and linking current_sense1.init(); motor1.linkCurrentSense(¤t_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); current_magnitude0 = current_sense0.getDCCurrent() * 1000; //mA //current_magnitude1 = current_sense1.getDCCurrent() * 1000; //mA } //逆運動学の計算 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回転位置制御であり第2号のコードとほぼ同様です。
ここではPS4コントローラ制御を廃止して外部からCAN通信で足座標を指定してるの主な差分となるCANのコードを中心に説明します。
L. 5 ESP32のTWAIドライバでCAN通信を実施します。
L. 57～68 CAN送信関数
ID 0x212でモータのトルク(負荷電流)を送信
L. 333～422 core0動作
ESP32 デュアルコアのcore0でWiFI、CAN通信を実行します。
通信のIOはMKS ESP32 FOC V2.0の外部コネクタの一つを使用します
TX：IO 17、RX：IO 16とします (L. 336, 337)。
伝送速度は 1Mbps固定 (L. 348)
受信は角度指定値のID 0x122のみ通すようにフィルタリング (L. 350～355)
CANで足座標(x, z)を受信 データID：0x122 (L. 409～416)
CANでモータトルクを送信 (L. 418)
L. 542～547 SimpleFOCモータ初期化動作
エンコーダオフセット(zero_electric_angle)とモータ回転方向(sensor_direction)をコメントアウトしてます。
モータ接続後にオフセット値と回転方向を確認してコメントアウトを外して再度書き込んでください。
L. 579, 588 モータ回転
モータはCAN通信で指定された足座標からIK換算された角度 (angel)とブラウザアプリによる微調整 (angleOffset)で回転します。
下腿ヒザモータ(M0)のトルクを検知 (L. 586)
3Dモデル
右足は左足のミラー対象で構成は全く同じです。
非対称部品でまだ公開していないデータを以下で販売しています。
zipファイルでダウンロードされますので解凍してご利用ください。
データには3個のstlファイルがあります。 1個づつ印刷してください。
右足組み立てに必要な3Dプリントパーツを以下にまとめます。
| STL | 数量 | 掲載号 |
| cover | 1 | 第2号 |
| motorFix | 1 | 創刊号 |
| boardFix | 1 | 第2号 |
| gearFix | 2 | 創刊号 |
| gear10 | 2 | 創刊号 |
| gear56-70-R | 1 | 創刊号 |
| gear56-R | 1 | 第4号 |
| leg1 | 1 | 創刊号 |
| leg2-R | 1 | 第4号 |
| leg3 | 1 | 第2号 |
| leg4-R | 1 | 第4号 |
| leg5 | 1 | 第2号 |
| leg6 | 1 | 第2号 |
| leg7 | 2 | 第2号 |
| leg8 | 1 | 第2号 |
| leg9 | 1 | 第2号 |
| leg10 | 1 | 第2号 |
| leg11 | 1 | 第2号 |
| foot | 1 | 第2号 |
| M2-52 | 10 | 創刊号 |
| M2-102 | 2 | 第2号 |
| M2-149 | 1 | 第2号 |
| M2-152 | 1 | 第2号 |
| M3-55 | 1 | 第2号 |
| M3-102 | 1 | 第2号 |
| M3-155 | 2 | 第2号 |
| M3-178 | 1 | 第2号 |
上記パーツで左足とミラー対象となるように創刊号、第２号を参照して組んでください 。
これまでと同様にモータのエンコーダオフセット処理や初期位置を考慮したギア固定＆微調整を実施してください。
左足用Arduinoコード
左足もCAN通信で足座標を受信・トルクデータ送信するよう以下を書き込みます。
エンコーダオフセット値は前回コードの値を引きついで書き込んでください (L. 543, 546)。
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 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 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 |
#include <SimpleFOC.h> #include <WiFi.h> #include <WebServer.h> #include <Preferences.h> #include "driver/twai.h" float angle0 = 0.0, angle1 = 0.0; twai_message_t rx_msg, tx_msg; 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; float current_magnitude0, current_magnitude1; //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 xx = 0.0, 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; //CAN送信 トルク void sendI(float i) { tx_msg.identifier = 0x211; tx_msg.extd = 0; // 標準ID（11bit） tx_msg.data_length_code = 4; // floatをバイト配列にコピー memcpy(&tx_msg.data[0], &i, 4); // 送信 twai_transmit(&tx_msg, 0); } //ブラウザ表示 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(-xx, height); } handleRoot(); } void angleOffset0P() { if (angleOffset0 < 360.0) { angleOffset0 += 1.0; preferences.putFloat("angleOffset0", angleOffset0); ik(-xx, height); } handleRoot(); } void angleOffset1M() { if (angleOffset1 >= -360.0) { angleOffset1 -= 1.0; preferences.putFloat("angleOffset1", angleOffset1); ik(-xx, height); } handleRoot(); } void angleOffset1P() { if (angleOffset1 < 360.0) { angleOffset1 += 1.0; preferences.putFloat("angleOffset1", angleOffset1); ik(-xx, 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) { twai_general_config_t g_config = { .mode = TWAI_MODE_NORMAL, .tx_io = (gpio_num_t)17, .rx_io = (gpio_num_t)16, .clkout_io = TWAI_IO_UNUSED, .bus_off_io = TWAI_IO_UNUSED, .tx_queue_len = 5, .rx_queue_len = 5, .alerts_enabled = TWAI_ALERT_NONE, .clkout_divider = 0, .intr_flags = ESP_INTR_FLAG_LEVEL1 }; twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS(); // フィルタ設定（受信マスク） twai_filter_config_t f_config = { .acceptance_code = (0x121 << 21), // IDは左に21ビットシフトする必要がある（11bit標準ID用） .acceptance_mask = ~(0x7FF << 21), // マスクで0x121以外全部除外（0x7FF=11bit全部1） .single_filter = true // 単一フィルタ使う }; // ドライバインストール if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) { Serial.println("TWAI driver installed"); } else { Serial.println("TWAI driver install failed"); while (1); } // 通信開始 if (twai_start() == ESP_OK) { Serial.println("TWAI started"); } else { Serial.println("TWAI start failed"); while (1); } 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(-xx, height); for (;;) { server.handleClient(); if (twai_receive(&rx_msg, 0) == ESP_OK) { if (rx_msg.identifier == 0x121 && rx_msg.data_length_code == 8) { memcpy(&xx, &rx_msg.data[0], 4); memcpy(&height, &rx_msg.data[4], 4); ik(-xx, height); } } sendI(current_magnitude0); 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(¤t_sense0); current_sense0.linkDriver(&driver0); // current sense init and linking current_sense1.init(); motor1.linkCurrentSense(¤t_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); current_magnitude0 = current_sense0.getDCCurrent() * 1000; //mA //current_magnitude1 = current_sense1.getDCCurrent() * 1000; //mA } //逆運動学の計算 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; } |
ほぼ右足のコードと同様です。
CAN ID：0x211で下腿モータのトルク(負荷電流)を送信
CAN ID：0x121で足座標(x, z)を受信
CANボードを足に実装
足にCANコントローラボードを載せます。
CANコントローラボードを加工します。
4ピン GHコネクタケーブルをMKS ESP32 FOC V2.0のコネクタに対応するようにCANコントローラボードに配線します。
以下の図のように右足、左足にCANコントローラボードを両面テープで固定してコネクタをMKS ESP32 FOC V2.0につけます。
股関節加工
股関節のMKS ESP32 FOC V2.0のコードは前回の第3号のまま使用します。
ロボ完成に向けて股関節の加工を実施します。
必要部品
- LiPoバッテリ用配線コネクタ
- 電源用スライドスイッチ
- ねじ
M3 6mm なべネジ：2本
M2 8mm タッピングネジ：4本
電源周り加工
LiPoバッテリからスイッチを介して股関節・足のMKS ESP32 FOC V2.0 に給電します。
以下のように配線加工
モータには大きめの電流が流れるので太めの配線材を使用しました。
電源線は股関節用は10cm、足用は20cmくらいにしました。
atomCover.stlに加工したスイッチをねじ止め (M3 6mm なべネジ：2本)。
atomCover.stlに革などで取っ手を付けると便利です。
ここではタッピングネジ (M2 8mm：4本)でねじ止め
atomCover.stlをユニットに戻して、股関節用電源配線をMKS ESP32 FOC V2.0に接続して加工完了です。
ロボ組立て
足と股関節を組み合わせてロボットを完成させます。
必要部品
- LiPoバッテリ 3セル 11.1V：1個
- 足底用クッション シールフレックスモールド 5X20mmX2m巻 SFM-009
- ねじ
M3 20mm なべネジ：12本
組立て
股関節ユニットのbody3.stlを取り外して、足にねじ止めします (M3 20mm なべネジ：6本ずつ)。
足をbody3.stlを介して股関節に取り付けます。
電源配線をそれぞれの足のMKS ESP32 FOC V2.0に接続します。
足の底にクッションとしてシールフレックスモールドを貼り付けます。
LiPoバッテリをスペーサで作った空間にはめて両面テープで固定します。
固定後バッテリ用コネクタに接続 (スイッチがOFFになっていることを確認してください)。
足のCANコントローラボードを以下のように結線します。
CANH, CANLピン同士を配線接続します。
CANネットワークでは両端のみに終端抵抗120Ωを載せる必要がありますが、ここでは特にケアせずにすべてのボードに終端抵抗を載せたままにしています。
気になる方はケアしてください。
以上で組み立て完了です。
ATOM用Arduinoコード
ATOM Matrixにロボット用コードを書き込みます。
前号のIMUによる股関節制御に加えてPS4コントローラでも足や股関節を操作できるようにします。
環境バージョンはこれまでと同様で以下の通りです。
Arduino IDEバージョン：ver. 1.8.19
ESP32ライブラリ：ver. 2.0.13
M5Atomライブラリ：ver. 0.1.3
Kalman Filter Library ：ver. 1.0.2
PS4Controllerライブラリ：ver. 1.1.0
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 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 557 558 559 560 561 |
#include <M5Atom.h> #include <Kalman.h> #include <WiFi.h> #include <WebServer.h> #include <Preferences.h> #include <PS4Controller.h> #include "driver/twai.h" twai_message_t rx_msg; WebServer server(80); const char ssid[] = "ATOM"; // SSID const char pass[] = "password"; // password const IPAddress ip(192, 168,22, 90); // IPアドレス const IPAddress subnet(255, 255, 255, 0); // サブネットマスク int Ope = 0; float xZero = 10.0, zZero = 99.0; float offsetX = 0.0, offsetY = 0.0; int lastLStickX = 0, lastLStickY = 0, lastRStickX = 0, lastRStickY = 0; unsigned long oldTime = 0, loopTime, nowTime, startTime; float dt; float accX = 0, accY = 0, accZ = 0; float gyroX = 0, gyroY = 0, gyroZ = 0; float theta_X = 0.0, theta_Y = 0.0; float theta_Xdot = 0.0, theta_Ydot = 0.0; Kalman kalmanX, kalmanY; float kalAngleX, kalAngleDotX, kalAngleY, kalAngleDotY; float Kp = 0.0, Kd = 0.0; float rollVal = 0.0, rollCtrlL = 0.0, rollCtrlR = 0.0; Preferences preferences; //加速度センサから傾きデータ取得 [deg] void get_theta() { M5.IMU.getAccelData(&accX,&accY,&accZ); //傾斜角導出 単位はdeg theta_X = atan2(accY, -accZ) * 57.29578f + offsetX; theta_Y = -atan2(-accX, -accZ) * 57.29578f + offsetY; } //角速度取得 void get_gyro_data() { M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ); theta_Xdot = -gyroX; theta_Ydot = gyroY; } //CAN送信 L void sendL(float xL, float hL) { twai_message_t msg = {}; msg.identifier = 0x121; msg.extd = 0; // 標準ID（11bit） msg.data_length_code = 8; // floatをバイト配列にコピー memcpy(&msg.data[0], &xL, 4); memcpy(&msg.data[4], &hL, 4); // 送信 twai_transmit(&msg, 0); } //CAN送信 R void sendR(float xR, float hR) { twai_message_t msg = {}; msg.identifier = 0x122; msg.extd = 0; // 標準ID（11bit） msg.data_length_code = 8; // floatをバイト配列にコピー memcpy(&msg.data[0], &xR, 4); memcpy(&msg.data[4], &hR, 4); // 送信 twai_transmit(&msg, 0); } //CAN送信 Roll void sendRoll(float angL, float angR) { twai_message_t msg = {}; msg.identifier = 0x123; msg.extd = 0; // 標準ID（11bit） msg.data_length_code = 8; // floatをバイト配列にコピー memcpy(&msg.data[0], &angL, 4); memcpy(&msg.data[4], &angR, 4); // 送信 twai_transmit(&msg, 0); } //Core0 void display(void *pvParameters) { M5.IMU.Init(); //フルスケールレンジ M5.IMU.SetAccelFsr(M5.IMU.AFS_2G); M5.IMU.SetGyroFsr(M5.IMU.GFS_250DPS); get_theta(); kalmanX.setAngle(theta_X); kalmanY.setAngle(theta_Y); M5.dis.setBrightness(20); 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("/x0M", x0M); server.on("/x0P", x0P); server.on("/z0M", z0M); server.on("/z0P", z0P); server.on("/KpM", KpM); server.on("/KpP", KpP); server.on("/KdM", KdM); server.on("/KdP", KdP); server.on("/offsetXP", offsetXP); server.on("/offsetXM", offsetXM); server.on("/offsetYP", offsetYP); server.on("/offsetYM", offsetYM); server.begin(); twai_general_config_t g_config = { .mode = TWAI_MODE_NORMAL, .tx_io = (gpio_num_t)19, .rx_io = (gpio_num_t)22, .clkout_io = TWAI_IO_UNUSED, .bus_off_io = TWAI_IO_UNUSED, .tx_queue_len = 5, .rx_queue_len = 5, .alerts_enabled = TWAI_ALERT_NONE, .clkout_divider = 0, .intr_flags = ESP_INTR_FLAG_LEVEL1 }; //twai_timing_config_t t_config = TWAI_TIMING_CONFIG_500KBITS(); twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS(); twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL(); if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) { Serial.println("TWAI Driver installed"); } else { Serial.println("TWAI Driver install failed"); } if (twai_start() == ESP_OK) { Serial.println("TWAI Started"); } else { Serial.println("TWAI Start failed"); } for (;;){ server.handleClient(); //LED表示 M5.dis.clear(); if(kalAngleX > 20.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i, 0xff0000); } }else if(kalAngleX <= 20.0 && kalAngleX > 12.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i, 0x0000ff); } }else if(kalAngleX <= 12.0 && kalAngleX > 4.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i + 5, 0x0000ff); } }else if(kalAngleX <= 4.0 && kalAngleX > 1.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i + 10, 0x0000ff); } }else if(abs(kalAngleX) <= 1.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i + 10, 0x00ff00); } }else if(kalAngleX >= -4.0 && kalAngleX < -1.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i + 10, 0x0000ff); } }else if(kalAngleX >= -12.0 && kalAngleX < -4.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i + 15, 0x0000ff); } }else if(kalAngleX >= -20.0 && kalAngleX < -12.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i + 20, 0x0000ff); } }else if(kalAngleX < -20.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i + 20, 0xff0000); } } if(kalAngleY > 20.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i * 5, 0xff0000); } }else if(kalAngleY <= 20.0 && kalAngleY > 12.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i * 5, 0x0000ff); } }else if(kalAngleY <= 12.0 && kalAngleY > 4.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i * 5 + 1, 0x0000ff); } }else if(kalAngleY <= 4.0 && kalAngleY > 1.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i * 5 + 2, 0x0000ff); } }else if(abs(kalAngleY) <= 1.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i * 5 + 2, 0x00ff00); } }else if(kalAngleY >= -4.0 && kalAngleY < -1.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i * 5 + 2, 0x0000ff); } }else if(kalAngleY >= -12.0 && kalAngleY < -4.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i * 5 + 3, 0x0000ff); } }else if(kalAngleY >= -20.0 && kalAngleY < -12.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i * 5 + 4, 0x0000ff); } }else if(kalAngleY < -20.0){ for(int i = 0; i < 5; i++){ M5.dis.drawpix(i * 5 + 4, 0xff0000); } } nowTime = micros(); loopTime = nowTime - oldTime; oldTime = nowTime; dt = (float)loopTime / 1000000.0; //sec get_theta(); get_gyro_data(); //カルマンフィルタ 姿勢 傾き kalAngleX = kalmanX.getAngle(theta_X, theta_Xdot, dt); kalAngleY = kalmanY.getAngle(theta_Y, theta_Ydot, dt); //カルマンフィルタ 姿勢 角速度 kalAngleDotX = kalmanX.getRate(); kalAngleDotY = kalmanY.getRate(); //ロール軸send rollVal = Kp * kalAngleY + Kd * kalAngleDotY; sendRoll(rollVal - rollCtrlL, rollVal - rollCtrlR); disableCore0WDT(); } } //ブラウザ表示 void handleRoot() { String temp ="<!DOCTYPE html> \n<html lang=\"ja\">"; temp +="<head>"; temp +="<meta charset=\"utf-8\">"; temp +="<title>ATOM</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 +="button.on{ background:lime; color:white; }"; temp +=".column-3{ max-width:330px; margin:auto; text-align:center; display:flex; justify-content:space-between; flex-wrap:wrap; }"; temp +="</style>"; temp +="</head>"; temp +="<body>"; temp +="<div class=\"container\">"; temp +="<h3>ATOM</h3>"; //初期姿勢x temp +="x0 <br>"; temp +="<a class=\"pm\" href=\"/x0M\">-</a>"; temp +="<span>" + String(xZero) + "</span>"; temp +="<a class=\"pm\" href=\"/x0P\">+</a><br>"; //初期姿勢z temp +="z0 <br>"; temp +="<a class=\"pm\" href=\"/z0M\">-</a>"; temp +="<span>" + String(zZero) + "</span>"; temp +="<a class=\"pm\" href=\"/z0P\">+</a><br>"; //Kp temp +="Kp<br>"; temp +="<a class=\"pm\" href=\"/KpM\">-</a>"; temp +="<span>" + String(Kp) + "</span>"; temp +="<a class=\"pm\" href=\"/KpP\">+</a><br>"; //Kd temp +="Kd<br>"; temp +="<a class=\"pm\" href=\"/KdM\">-</a>"; temp +="<span>" + String(Kd) + "</span>"; temp +="<a class=\"pm\" href=\"/KdP\">+</a><br>"; //offsetX temp += "offsetX<br>"; temp += "<a class=\"pm\" href=\"/offsetXM\">-</a>"; temp += "<span>" + String(offsetX) + "</span>"; temp += "<a class=\"pm\" href=\"/offsetXP\">+</a><br>"; //offsetY temp += "offsetY<br>"; temp += "<a class=\"pm\" href=\"/offsetYM\">-</a>"; temp += "<span>" + String(offsetY) + "</span>"; temp += "<a class=\"pm\" href=\"/offsetYP\">+</a><br>"; temp +="</div>"; temp +="</body>"; server.send(200, "text/HTML", temp); } void x0M() { if(xZero > -150.0){ xZero -= 1.0; preferences.putFloat("xZero", xZero ); Ope = 0; } handleRoot(); } void x0P() { if(xZero < 150.0){ xZero += 1.0; preferences.putFloat("xZero", xZero ); Ope = 0; } handleRoot(); } void z0M() { if(zZero > 0.0){ zZero -= 1.0; preferences.putFloat("zZero", zZero ); Ope = 0; } handleRoot(); } void z0P() { if(zZero < 150.0){ zZero += 1.0; preferences.putFloat("zZero", zZero ); Ope = 0; } handleRoot(); } void KpM() { if(Kp > -10.0){ Kp -= 0.1; preferences.putFloat("Kp", Kp); } handleRoot(); } void KpP() { if(Kp <= 10.0){ Kp += 0.1; preferences.putFloat("Kp", Kp); } handleRoot(); } void KdM() { if(Kd > -10.0){ Kd -= 0.01; preferences.putFloat("Kd", Kd); } handleRoot(); } void KdP() { if(Kd <= 10.0){ Kd += 0.01; preferences.putFloat("Kd", Kd); } handleRoot(); } void offsetXM() { if (offsetX >= -160) { offsetX -= 0.1; preferences.putFloat("offsetX", offsetX); } handleRoot(); } void offsetXP() { if (offsetX <= 160) { offsetX += 0.1; preferences.putFloat("offsetX", offsetX); } handleRoot(); } void offsetYM() { if (offsetY >= -160) { offsetY -= 0.1; preferences.putFloat("offsetY", offsetY); } handleRoot(); } void offsetYP() { if (offsetY <= 160) { offsetY += 0.1; preferences.putFloat("offsetY", offsetY); } handleRoot(); } void setup() { M5.begin(true, true, true); //SerialEnable, bool I2CEnable, DisplayEnable 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]); PS4.begin(); //パラメータ初期値取得 preferences.begin("parameter", false); Kp = preferences.getFloat("Kp", Kp); Kd = preferences.getFloat("Kd", Kd); xZero = preferences.getFloat("xZero", xZero); zZero = preferences.getFloat("zZero", zZero); offsetX = preferences.getFloat("offsetX", offsetX); offsetY = preferences.getFloat("offsetY", offsetY); //core0 タスク xTaskCreatePinnedToCore( display , "display" // 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); } void loop(){ if (PS4.isConnected()) { //PS4コントローラ int currentLStickX = PS4.LStickX(); int currentLStickY = PS4.LStickY(); int currentRStickX = PS4.RStickX(); int currentRStickY = PS4.RStickY(); if (currentLStickX != lastLStickX || currentLStickY != lastLStickY) { float Lx = PS4.LStickX() / 4.0; float Ly = PS4.LStickY() / 3.0; sendL(-Lx + xZero, Ly + zZero); sendR(Lx + xZero, Ly + zZero); lastLStickX = currentLStickX; lastLStickY = currentLStickY; }else if (currentRStickX != lastRStickX || currentRStickY != lastRStickY) { rollCtrlL = PS4.RStickX() / 10.0; rollCtrlR = rollCtrlL; lastRStickX = currentRStickX; lastRStickY = currentRStickY; } } if(Ope == 0){ //初期姿勢 rollCtrlL = 0.0; rollCtrlR = 0.0; sendL(xZero, zZero); sendR(xZero, zZero); delay(10); rollCtrlL = 0.0; rollCtrlR = 0.0; sendL(xZero, zZero); sendR(xZero, zZero); delay(10); rollCtrlL = 0.0; rollCtrlR = 0.0; sendL(xZero, zZero); sendR(xZero, zZero); Ope = 1; } } |
L. 25 ロボの足の初期座標を指定
前後 x0 = 10mm、高さz0 = 99mm
重心の兼ね合いでx0 = 10mmとして少し前に出しています。
足の初期位置調整の際は x0 = 0mm、z0 = 99mmにして実施してください。
L. 66～109 CAN送信関数
ID 0x121で左足座標を送信 (L. 66～79)
ID 0x122で左足座標を送信 (L. 81～94)
ID 0x123で2個のモータの回転位置角度を送信 (L. 96～109)
L. 112～290 core0動作
ATOM Matrix デュアルコアのcore0でLED表示と傾斜角検知、股関節ロール軸CAN送信を実行します。
L. 292～478 ブラウザアプリ表示設定
APモード接続時の各パラメータ設定用の表示を記述しています。
足の初期座標 (x0, z0)と股関節ロール軸PDパラメータ、IMU傾斜角オフセット調整を実施
L. 482～516 setup関数
PS4コントローラ接続のために Bluetooth Mac Addressをシリアル出力しています。 (L. 485～488)
L. 519～561 loop関数
PS4コントローラでロボの足を操作
左スティック 左右：足前後 左右で逆に動かして体をひねります
左スティック 上下：足高さ
右スティック 左右：股関節ロール
書き込み
ATOM Matrixに書き込む際にはモータに給電しないようにATOMの電源ラインは外してください。
ボードは”M5Stack-ATOM”を選択
PS4Controllerライブラリのサイズが大きいのでPartition Schemeは”No OTA (Large APP)”を選択
2足歩行ロボット HM-01 完成
ついにロボが完成しました。
今更ですがこのロボの名前は”HM-01″と申します。
スイッチをONすると初期化後にモータが初期位置に設定されます。
予算3万円でつくる BLDC 2足歩行ロボット
印加帝国 pic.twitter.com/HJGuknWxr7— HomeMadeGarbage (@H0meMadeGarbage) September 9, 2025
モータが初期位置 (-π : -180°)から大きくズレていると 一周回った状態で初期化して過電流が流れることがあるので、極力初期位置に近い状態で起動するようにしてください。
HM-01
全長：約 25 cm
重さ：訳 1.1 kg
ブラウザアプリ
各コントローラ (ATOM Matrix, MKS ESP32 FOC V2.0)はWiFiでブラウザからもろもろ調整できます。
PS4コントローラ
PS4コントローラとATOM Matrix をペアリングすることでコントローラのアナログスティックでロボットを動作させます。
ATOM Matrix 起動時に Bluetooth Mac Addressをシリアル出力させています。
アドレスを控えてコントローラと接続します。
PS4コントローラとESP32のペアリング方法の詳細は以下を参照ください。
PS4ペアリング後にはアプリによるWiFi接続ができませんので、両方実施したい場合はWiFiアプリ接続後にBluetoothコントローラペアリングしてください。
動作
PS4コントローラで足を動かします。
予算3万円でつくる BLDC 2足歩行ロボット
2足歩行ロボット HM-01 完成 pic.twitter.com/Cds24QeerR— HomeMadeGarbage (@H0meMadeGarbage) September 7, 2025
予算3万円でつくる BLDC 2足歩行ロボット
2足歩行ロボット HM-01 の足 pic.twitter.com/sLoSo8auf9— HomeMadeGarbage (@H0meMadeGarbage) September 7, 2025
演習問題
1-パラメータ調整
ブラウザアプリやコードを修正するなどして足座標や各PIDパラメータ値を変更して動作にどう影響するのか体験してみよう。
パラメータ調整で過制動などで発振すると過電流でMKS ESP32 FOC V2.0のパワートランジスタが壊れる可能性がありますので十分ご注意ください。
2-動作検討
ここではPS4コントローラのジョイスティックでロボの足動作を楽しみました。
次回は足踏みや歩行動作を紹介しますが、ぜひ前もって考えてみてください。
ATOM Matrixの足座標やロール軸 CAN通信送信データを組み合わせて動作を構築します。
おわりに
本号ではついに2足歩行ロボット HM-01 を完成させました。
PS4コントローラによる足動作でBLDCの応答性の良さやポテンシャルを感じ取っていただけたと存じます。
次回はロボットの歩行動作を楽しみたいと思います。
