前号ではロボットの足を1本完成させて、IKによる制御を楽しみました。

予算3万円でつくる BLDC 2足歩行ロボット第2号ー足のIKを楽しむー

本号ではロボの股関節を製作します。

はじめに

ここからは股関節部の組み立てを手順通りに記載します。

外部コントローラ ATOM Matrixを用いてIMUによる姿勢検知で股関節の制御を楽しみます。

予算3万円でつくる BLDC 2足歩行ロボット
股関節 IMU姿勢検知してCAN通信でBLDC制御 pic.twitter.com/JcuEJWVkf0

— HomeMadeGarbage (@H0meMadeGarbage) August 28, 2025

ATOM Matrix内蔵のIMUの姿勢角データを股関節のコントローラ(MKS ESP32 FOC V2.0)に送信してBLDCモータを制御します。
通信にはCAN通信を使用します。

本号で必要な部品

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

　　
BLDC 5010 360KV：2個

　
磁気エンコーダ AS5600：2個

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

　　
CANコントローラボード：2個

　
ATOM Matrix：1個

　
ATOM Mate：1個
ATOM Matrixの機体固定に使用します。

ATOM Mate
スイッチサイエンス

　
4ピン GHコネクタケーブル 150mm：2本
MKS ESP32 FOC V2.0の外部配線として使用

　
その他
10mm M3ねじスペーサ：4個
25mm M3ねじスペーサ：4個
M2 12mm タッピングネジ：2本
M2 14mm なべネジ：2本
M3 5mm なべネジ：8本
M3 6mm なべネジ：18本
M3 10mm なべネジ：4本
M3 14mm なべネジ：8本
M3 25mm なべネジ：2本
M3 30mm なべネジ：4本
M3 40mm なべネジ：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

3Dモデル

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

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

データには8個のstlファイルがあり atomCover.stl, boardFix2.stl,
LiPoBoard.stlは1個づつプリントし、その他は2個づつ印刷してください。

創刊号のモータギアモデルgearFix.stl、gear10.stl、M2-52.stlも2個づつ使用しますのでご用意ください。

股関節組立て

股関節を組んでいきます

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

以下の赤丸で示す穴をM3サイズでねじ切りしてください。
下の写真の上部2か所もM3サイズでねじ切りします（スペーサ用）。

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

5010 360KV をbody1.stlにねじ止めします (M3 6mm なべネジ：4本ずつ)。

モータにはgearFix.stl (創刊号付属モデル)をねじ止めしています (M3 5mm なべネジ：4本ずつ)。

足の時と同様に磁気エンコーダAS5600に MKS ESP32 FOC V2.0付属の5ピンコネクタラインを加工して配線します。
MKS ESP32 FOC V2.0とAS5600が以下のようにつながるよう配線してください。
1線余るので切断なりで除去
　3.3V－VCC
　GND－GND
　SDA－SDA
　SCL－SCL

コネクタ配線した磁気エンコーダをbody1.stlにねじ止めします (M3 6mm なべネジ：4本ずつ)。

組んだ body1.stl 2組をM3 14mm なべネジ (4本) で合わせて両サイドから固定します。

上部にM3ネジ10mm高さスペーサ (4本)をはめます。

ここでは金属のスペーサを使用しました。

LiPoBoard.stlをスペーサにねじ止めします (M3 10mm なべネジ：4本)。

LiPoBoard.stlの4つ角の穴はM3ネジサイズでねじ切りして、25mm高さねじスペーサ (4本)をはめます。
このスペースにLiPoバッテリを配置します。

ここでは10mmと15mmのナイロンねじスペーサを使用しました。

boardFix2.stlをスペーサにねじ止めします (M3 14mm なべネジ：4本)。
赤丸の筒穴はM3ネジサイズでねじ切りしてください。

M2 12mm タッピングネジ (2本ずつ)でgear70.stlとbody3.stlをねじ止め。

body2.stl の突起をgear70.stlの穴に通して、body1.stlにねじ止め (M3 25mm なべネジ)。
body3 .stlの穴にM3-78を通して、body1.stlにねじ止め (M3 40mm なべネジ)。

次はMKS ESP32 FOC V2.0 を実装となりますが、その前にコードを書き込みます。

Arduinoコード

MKS ESP32 FOC V2.0 実装前に股関節用のコードを書き込みます。

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

このコードはFOC(ベクトル制御)によってモータの回転位置を指定して制御します。
回転位置は微調整用ブラウザアプリからとCAN通信によってコントロールします。

#include <M5Atom.h>
#include <Kalman.h>
#include <WiFi.h>
#include <WebServer.h>
#include <Preferences.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); // サブネットマスク


float offsetX = 0.0, offsetY = 0.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.3, Kd = 0.02;

float rollVal = 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;
}

//Y軸 角速度取得
void get_gyro_data() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  theta_Xdot = -gyroX;
  theta_Ydot = gyroY;
}


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

    /*
    Serial.print(kalAngleY);
    Serial.print(", ");
    Serial.println(kalAngleDotY);
    */

    //ロール軸send
    rollVal = Kp * kalAngleY + Kd * kalAngleDotY; 
    sendRoll(rollVal, rollVal);
    
    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>";


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


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

  Kp = preferences.getFloat("Kp", Kp);
  Kd = preferences.getFloat("Kd", Kd);

  offsetX = preferences.getFloat("offsetX", offsetX);
  offsetY = preferences.getFloat("offsetY", offsetY);
  

  //回転位置検出 タスク
  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(){
  
}

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

#include <M5Atom.h>

#include <Kalman.h>

#include <WiFi.h>

#include <WebServer.h>

#include <Preferences.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); // サブネットマスク

float offsetX = 0.0, offsetY = 0.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.3, Kd = 0.02;

float rollVal = 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;

}

//Y軸角速度取得

void get_gyro_data() {

M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);

theta_Xdot = -gyroX;

theta_Ydot = gyroY;

}

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

Serial.print(kalAngleY);

Serial.print(", ");

Serial.println(kalAngleDotY);

//ロール軸send

rollVal = Kp * kalAngleY + Kd * kalAngleDotY;

sendRoll(rollVal, rollVal);

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

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

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

preferences.begin("parameter", false);

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

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

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

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

//回転位置検出タスク

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

}

モータの基本制御はFOC回転位置制御であり足の時とほぼ同様です。
股関節では外部からCAN通信で回転角度指定をするので足のコードとの差分となるCANのコードを中心に説明します。

L. 5　ESP32のTWAIドライバでCAN通信を実施します。

L. 44, 45　モータ駆動の電圧制限と電流制限はそれぞれ 3V, 6Aとしています。

L. 310～350　ESP32デュアルコアのcore0でCAN通信を設定・ドライバ実行します。
　通信のIOはMKS ESP32 FOC V2.0の外部コネクタの一つを使用します
　　TX：IO 17、RX：IO 16とします (L. 312, 313)。

　伝送速度は 1Mbps固定 (L. 325)
　受信は角度指定値のID 0x123のみ通すようにフィルタリング (L. 327～332)

　
L. 385～394　CANで回転角度を受信（ID 0x123）
　M0、M1のモータ2個分の角度を受信して±40°で制限

　
L. 553, 554 モータ回転
　モータはCAN通信での指定 (angel)とブラウザアプリによる微調整 (angleOffset)で回転します。

　股関節のギア比は7.0なのでangelによる角度はギア比で換算しています。

MKS ESP32 FOC V2.0 実装

boardFix2.stlにコードを書き込んだMKS ESP32 FOC V2.0 を載せてモータとエンコーダを配線します。
下の写真のように前方を矢印の方向として、前方のモータがmotor0（M0）、後方がmotor1（M1）となるように結線します。

MKS ESP32 FOC V2.0は頭を小さくカットしたナイロンねじで固定しましたが後ほどatomCover.stlでねじ止めするので、ここでは仮止めとなります (必須ではありません)。

モータ配線は足と同様に左から黄色、黒、赤の順で接続します。

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

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

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

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

motor0.voltage_sensor_align = 0.5;

motor0.zero_electric_angle = 2.14;

motor0.sensor_direction = Direction::CCW;

motor1.voltage_sensor_align = 0.5;

motor1.zero_electric_angle = 2.94;

motor1.sensor_direction = Direction::CCW;

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

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

gear70.stlのアームがだいたい水平になる位置でモータのgearFix.stlにgear10.stl（創刊号付属モデル）をはめます。
この際にモータは初期位置であることをご確認ください。
body2.stl穴にM2-52.stl（創刊号付属モデル）を通してM2 14mm なべネジでギアを固定します。

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

ブラウザアプリで微調整

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

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

angleOffset0とangleOffset1の値 [°]を変更することでモータ回転位置をそれぞれ制御できます。
アームが水平になるように微調整してください。

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

以上で概ね股関節が完成しました。

CAN通信

続いてCAN通信でモータを制御するための加工を進めます。

以下のようにATOM MatrixのIMUセンサによる姿勢角でモータをCAN通信で制御します。

CAN信号受信用のコントローラボードを加工します。
4ピン GHコネクタケーブルをMKS ESP32 FOC V2.0のコネクタに対応するようにCANコントローラボードに配線します。

以下の図のようにboardFix2.stlにCANコントローラボードを両面テープで固定してコネクタをMKS ESP32 FOC V2.0につけます。

　
ちなみに両面テープは以下を使用してます。

ATOM Matrix Arduinoコード

ATOM Matrixにコードを書き込みます。
IMUで傾きを検知してモータの制御角度をCANで送信します。

環境バージョンはこれまでと同様で以下の通りです。
　Arduino IDEバージョン：ver. 1.8.19
　ESP32ライブラリ：ver. 2.0.13
　M5Atomライブラリ：ver. 0.1.3
　Kalman Filter Library ：ver. 1.0.2

#include <M5Atom.h>
#include <Kalman.h>
#include <WiFi.h>
#include <WebServer.h>
#include <Preferences.h>
#include "driver/twai.h"

twai_message_t rx_msg;
float torqueL = 0.0, torqueR = 0.0;
float preTorque = 0.0, currentTorque = 0.0;

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); // サブネットマスク


float offsetX = 0.0, offsetY = 0.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.3, Kd = 0.02;

float rollVal = 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送信 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("/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();

    /*
    Serial.print(kalAngleY);
    Serial.print(", ");
    Serial.println(kalAngleDotY);
    */

    //ロール軸send
    rollVal = Kp * kalAngleY + Kd * kalAngleDotY; 
    sendRoll(rollVal, rollVal);
    
    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>";


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


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

  Kp = preferences.getFloat("Kp", Kp);
  Kd = preferences.getFloat("Kd", Kd);

  offsetX = preferences.getFloat("offsetX", offsetX);
  offsetY = preferences.getFloat("offsetY", offsetY);
  

  //回転位置検出 タスク
  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(){
  
}

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

#include <M5Atom.h>

#include <Kalman.h>

#include <WiFi.h>

#include <WebServer.h>

#include <Preferences.h>

#include "driver/twai.h"

twai_message_t rx_msg;

float torqueL = 0.0, torqueR = 0.0;

float preTorque = 0.0, currentTorque = 0.0;

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); // サブネットマスク

float offsetX = 0.0, offsetY = 0.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.3, Kd = 0.02;

float rollVal = 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送信 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("/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();

Serial.print(kalAngleY);

Serial.print(", ");

Serial.println(kalAngleDotY);

//ロール軸send

rollVal = Kp * kalAngleY + Kd * kalAngleDotY;

sendRoll(rollVal, rollVal);

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

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

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

preferences.begin("parameter", false);

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

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

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

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

//回転位置検出タスク

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

}

L. 8　受信側と同様にESP32のTWAIドライバでCAN通信を実施します。

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

L. 41～47　加速度センサから傾斜角を取得する関数
　ATOM Matrix内蔵のIMUセンサ(MPU6886)で検出した加速度からX軸とY軸の傾斜角を算出

　
L. 49～54　角速度取得関数
　IMUのX軸とY軸の角速度を使用

L. 57～70　CAN送信関数
　ID 0x123で2個のモータの回転位置角度を送信

L. 74～252　core0動作
　ATOM Matrix デュアルコアのcore0でLED表示と傾斜角検知、CAN送信を実行します。

　慣性センサMPU6886のフルスケールレンジ指定 (L. 79, 80)
　・加速度：16bit　±2 g
　・ジャイロ(角速度)：16bit ±250 deg/sec

　WiFiとブラウザアプリ用のハンドラを設定 (L. 89～107)

　CAN通信を設定・ドライバ実行 (L. 110～137)。
　　TX：IO 19、RX：IO 22
　　伝送速度は 1Mbps固定

　ATOM のX軸とY軸の角度を以下の通りLEDインジケータ表示しています (L. 142～219)。
　・±1°以下：中心で緑表示
　・±20°以上：赤表示
　・その他：青表示

　カルマンフィルタライブラリの関数を用いてセンサの姿勢角と角速度を算出 (L. 222~238)

　ATOMのY軸 (機体ロール軸)の角度と角速度のPD制御でモータ制御角度算出してCAN通信で送信 (L. 247, 248)

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

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

L. 417～419　loop関数
　ATOM Matrix デュアルコアのcore1 今回は未使用

書き込み

ボードはESP32ライブラリの”M5Stack-ATOM”を選択して書き込み

動作

コードを書き込むとIMUによる角度に準じてLEDマトリクスにインジケータが表示されます。

予算3万円でつくる BLDC 2足歩行ロボット
ATOM Matrix pic.twitter.com/rYJkB1tP9a

— HomeMadeGarbage (@H0meMadeGarbage) September 6, 2025

ブラウザアプリ

ATOMのAPモードでWiFi接続してブラウザからパラメータ調整できます。

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

Kp, KdでIMUのロール角からのモータ駆動角度制御パラメータを調整

offsetでIMUの初期角度調整が可能です。
ATOM MatrixのIMUセンサ値に大きな誤差オフセットがみられる場合は調整してください。

ATOM実装

ATOM MatrixはMKS ESP32 FOC V2.0の外部コネクタの3.3Vピンから給電します。
さらにモータ角度送信用のCANコントローラボードも接続します。

ここでは下の図のように ATOM Mate を使用して配線しました。

　
CANコントローラボードとATOMは以下のように配線します。
　3.3V － 3V3
　GND － G
　RX － IO 22
　TX － IO 19

4ピン GHコネクタケーブルを加工してMKS ESP32 FOC V2.0の外部コネクタの3.3VピンとGNDピンからATOMに給電します。

ATOM MatrixをATOM Mate に載せてatomCover.stlに固定します。
atomCover.stlは下の図の赤丸をあらかじめM3ネジサイズでねじ切りしときます。

atomCover.stlにATOMをねじ止め (M3 6mm なべネジ：2本)
CANコントローラボードは両面テープで固定

atomCover.stlを股関節ユニットにねじ止め (M3 30mm なべネジ：4本)
ATOMの電源コネクタをMKS ESP32 FOC V2.0の外部コネクタに接続

CANコントローラボードのCANH, CANLピン同士を配線接続します。

以上で股関節完成です。

動作

傾けることで連動してアームが動きます。

予算3万円でつくる BLDC 2足歩行ロボット
股関節を楽しむ pic.twitter.com/BjRt0swcCI

— HomeMadeGarbage (@H0meMadeGarbage) September 6, 2025

演習問題

パラメータ調整

ブラウザアプリやコードを修正するなどしてMKS ESP32 FOC V2.0の各PIDパラメータやATOMのIMU角度PDパラメータ値を変更して動作にどう影響するのか体験してみよう。

過制動などで動作が制御不能となり過電流が流れることもございますのでご注意ください。

おわりに

本号では股関節ユニットを完成させて、機体傾きによる制御を楽しみました。
IMU搭載のATOM Matrixで傾きを検知してCAN通信でBLDCコントローラ側に回転角度をデータ送信しました。

次回は足と股関節を組み合わせてロボットを完成させ、基本動作を楽しみたいと思います。

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

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

はじめに

本号で必要な部品

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

3Dモデル

股関節組立て

Arduinoコード

MKS ESP32 FOC V2.0 実装

ブラウザアプリで微調整

CAN通信

ATOM Matrix Arduinoコード

書き込み

動作

ブラウザアプリ

ATOM実装

動作

演習問題

パラメータ調整

おわりに

関連記事

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

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