はじめに

SHISEIGYO-3 Roller はRoller485 Lite ユニットを用いた3軸姿勢制御モジュールです。
コントローラとしてIMU内蔵のATOM Matrixを使用し点倒立を実現します。

マイコン内蔵のRoller485 Lite ユニットを用いることで構成が非常にシンプルになり姿勢制御モジュールの構築が容易となります。

Roller485 Lite ユニット (STM32)

スイッチサイエンス

m5-docs

docs.m5stack.com

ダウンロードファイル

筐体3DmodelとArduinoコードを以下よりダウンロードできます。zipファイルがダウンロードされます。
　SHISEIGYO-3 Roller用DLファイル

部品

ここで使用した部品は以下のとおりです。
購入先はあくまで参考です。

ATOM Matrix
　スイッチサイエンス：https://www.switch-science.com/products/6260
　
Roller485 Lite ユニット：3個
　スイッチサイエンス：https://www.switch-science.com/products/9925
　
4ピンコネクタケーブル
　マルツ：https://www.marutsu.co.jp/pc/i/829426/
　　
2セル 7.4V LiPoバッテリ
　Amazon：https://amzn.to/4dRHNNX
　
LiPoバッテリ用配線コネクタ
　Amazon：https://amzn.to/3QgJw6u

　
使用したネジは以下のとおりです。

M2×4mmネジ：1本　(ATOMと台固定用)
タッピングなべネジ 2×10mm：3本 (ATOM台と筐体固定用)
M3×5mmネジとナット：72セット　(フライホイール重り用)
M3×6mmネジ：12本　(ホイールとモータ固定用)

3Dモデル

ダウンロードファイルに同梱されております。

ここではPLAフィラメントで出力しました。強度が求められるwheel.stlは充填密度100%で印刷し、残りのモデルは25%としました。

製作

SHISEIGYO-3 Roller を組み立てます。非常に簡単です。

まずは3Dプリントした筐体のサポートを外していざ！

発掘 pic.twitter.com/Si2vcxUxZK

— HomeMadeGarbage (@H0meMadeGarbage) October 22, 2024

モータ実装

筐体 body1.stl のモータ固定M3ネジ穴をタップでねじ切り(計12か所)。

筐体にRoller485 Lite ユニットをはめ込んで同梱の6角穴M3 12mmネジで固定します。

ロード pic.twitter.com/gqdwkjzvp5

— HomeMadeGarbage (@H0meMadeGarbage) October 22, 2024

Roller485はI2C通信モードでIDは以下のように、前方のモータを0x64、左右をそれぞれ0x65、0x66としました。

Roller485 Lite ユニット単体の使用法は以下を参照ください。

m5-docs

docs.m5stack.com

配線

モータ電源と４ピンコネクタラインを接続します。

モータ電源はRS485用オレンジコネクタの電源部にLiPoバッテリ用配線コネクタをつなげて３個のモータが並列につながるよう結線しました。

I2C通信用４ピンコネクタラインはATOM Matrixと3つのモータがつながるようにはんだ付け加工しました。
4ピンコネクタ用のハブユニットを使用すればハンダ加工は必要なく楽かもしれません。

結線も容易であっという間に電源・通信システム完成。

誉れ高き電源システム及び通信網の構築 pic.twitter.com/mxQfGZlDPR

— HomeMadeGarbage (@H0meMadeGarbage) October 22, 2024

マイコン、バッテリ実装

body2.stlにATOM Matrix をM2×4mmネジで固定。

2セルLiPoバッテリを底面に両面テープで固定。

コチラを筐体 body1.stl にタッピングなべネジ 2×10mmで固定します (3か所)。

ファントムメナス pic.twitter.com/uvc0Esdo4u

— HomeMadeGarbage (@H0meMadeGarbage) October 22, 2024

フライホイール実装

ホイールフレーム (wheel.stl) の穴に重りとして M3×5mmネジとナットを取り付けます。
ここではホイール1個の総重量は約26gになりました。ネジ長やナットの数を変えてホイール重量の調整も可能です。

ホイールをモータにM3×6mmネジで固定します。

フライホイール3つ pic.twitter.com/QokVJrieYx

— HomeMadeGarbage (@H0meMadeGarbage) October 22, 2024

Arduinoコード

倒立動作用のコードです。ダウンロードファイルに同梱されております。
ATOMライブラリとRoller485のI2Cライブラリの共存がうまく出来なかったのでM5StickのMPU6886ライブラリをATOM用にカスタマイズして使用しています。

使用ライブラリ

ここでは以下のバージョンを使用しています。各自の環境に合わせて適宜カスタマイズください。

・Arduino IDEバージョン：1.8.19
・ESP32ボードマネージャ：2.0.17
・FastLED ：3.4.0
・Kalman Filter Library：1.0.2
・Roller485

書き込みの設定は以下の通りです。

コード

#include "MPU6886.h"
#include <Kalman.h>
#include "FastLED.h"
#include <WiFi.h>
#include <WebServer.h>
#include <Preferences.h>
#include "unit_rolleri2c.hpp"


UnitRollerI2C RollerI2C0, RollerI2C1, RollerI2C2;  // Create a UNIT_ROLLERI2C object

#define led_pin 27
#define NUM_LEDS 25
CRGB leds[NUM_LEDS];

MPU6886 IMU;


WebServer server(80);

const char ssid[] = "S-3 Roller";  // SSID
const char pass[] = "password";   // password

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


unsigned long oldTime = 0, loopTime, nowTime;
float dt;

float Kp = 60;
float Kd = 65;
float Kw = 1.2;
float IDRS = 0.1;
int delayTime = 5;

float offsetX = 0.0, offsetY = 0.0;


int GetUP = 0;
float MX = 0.0, MY = 0.0, M0 = 0.0, M1 = 0.0, M2 = 0.0;
float AjX = 0.0, AjY = 0.0;
float velX = 0.0, velY = 0.0;


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

Kalman kalmanX, kalmanY;
float kalAngleX, kalAngleY, kalAngleDotX, kalAngleDotY;

Preferences preferences;


//加速度センサから傾きデータ取得 [deg]
void get_theta() {
  IMU.getAccelData(&accX,&accY,&accZ);
  //傾斜角導出 単位はdeg
  theta_X = atan2(accY , -accZ) * 180.0/PI  + offsetX;
  theta_Y = -atan2(-accX , -accZ) * 180.0/PI + offsetY;
}

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


//ブラウザ表示
void handleRoot() {
  String temp = "<!DOCTYPE html> \n<html lang=\"ja\">";
  temp += "<head>";
  temp += "<meta charset=\"utf-8\">";
  temp += "<title>S-3 Roller</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>S-3 Roller</h3>";

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

  

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

  //Kw
  temp += "Kw<br>";
  temp += "<a class=\"pm\" href=\"/KwM\">-</a>";
  temp += "<span>" + String(Kw) + "</span>";
  temp += "<a class=\"pm\" href=\"/KwP\">+</a><br>";

  //IDRS
  temp += "IDRS<br>";
  temp += "<a class=\"pm\" href=\"/IDRSm\">-</a>";
  temp += "<span>" + String(IDRS) + "</span>";
  temp += "<a class=\"pm\" href=\"/IDRSp\">+</a><br>";

  //delayTime
  temp += "delayTime<br>";
  temp += "<a class=\"pm\" href=\"/delayTimeM\">-</a>";
  temp += "<span>" + String(delayTime) + "</span>";
  temp += "<a class=\"pm\" href=\"/delayTimeP\">+</a><br>";


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


void offsetXm() {
  if (offsetX >= -10) {
    offsetX -= 0.1;
    preferences.putFloat("offsetX", offsetX);
  }
  handleRoot();
}

void offsetXp() {
  if (offsetX <= 10) {
    offsetX += 0.1;
    preferences.putFloat("offsetX", offsetX);
  }
  handleRoot();
}


void offsetYm() {
  if (offsetY >= -10) {
    offsetY -= 0.1;
    preferences.putFloat("offsetY", offsetY);
  }
  handleRoot();
}

void offsetYp() {
  if (offsetY <= 10) {
    offsetY += 0.1;
    preferences.putFloat("offsetY", offsetY);
  }
  handleRoot();
}



void KpM() {
  if (Kp >= 0.1) {
    Kp -= 1;
    preferences.putFloat("Kp", Kp);
  }
  handleRoot();
}

void KpP() {
  if (Kp <= 300) {
    Kp += 1;
    preferences.putFloat("Kp", Kp);
  }
  handleRoot();
}

void KdM() {
  if (Kd >= 0.1) {
    Kd -= 1;
    preferences.putFloat("Kd", Kd);
  }
  handleRoot();
}

void KdP() {
  if (Kd <= 300) {
    Kd += 1;
    preferences.putFloat("Kd", Kd);
  }
  handleRoot();
}

void KwM() {
  if (Kw >= 0) {
    Kw -= 0.1;
    preferences.putFloat("Kw", Kw);
  }
  handleRoot();
}

void KwP() {
  if (Kw <= 300) {
    Kw += 0.1;
    preferences.putFloat("Kw", Kw);
  }
  handleRoot();
}

void IDRSm() {
  if (IDRS >= 0.0) {
    IDRS -= 0.01;
    preferences.putFloat("IDRS", IDRS);
  }
  handleRoot();
}

void IDRSp() {
  if (IDRS <= 10) {
    IDRS += 0.01;
    preferences.putFloat("IDRS", IDRS);
  }
  handleRoot();
}


void delayTimeM() {
  if (delayTime > 0) {
    delayTime -= 1;
    preferences.putInt("delayTime", delayTime);
  }
  handleRoot();
}

void delayTimeP() {
  if (delayTime <= 30) {
    delayTime += 1;
    preferences.putInt("delayTime", delayTime);
  }
  handleRoot();
}


//Core0
void displayIMU(void *pvParameters) {
  FastLED.addLeds<WS2812B, led_pin, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(20);

  IMU.Init();

  //フルスケールレンジ
  IMU.SetAccelFsr(IMU.AFS_2G);
  IMU.SetGyroFsr(IMU.GFS_250DPS);

  get_theta();
  kalmanX.setAngle(theta_X);
  kalmanY.setAngle(theta_Y);


  delay(100); 


  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("/offsetXm", offsetXm);
  server.on("/offsetXp", offsetXp);
  server.on("/offsetYm", offsetYm);
  server.on("/offsetYp", offsetYp);
  

  server.on("/KpP", KpP);
  server.on("/KpM", KpM);
  server.on("/KdP", KdP);
  server.on("/KdM", KdM);
  server.on("/KwP", KwP);
  server.on("/KwM", KwM);
  server.on("/IDRSp", IDRSp);
  server.on("/IDRSm", IDRSm);
  server.on("/delayTimeP", delayTimeP);
  server.on("/delayTimeM", delayTimeM);

  server.begin();

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

    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(kalAngleX);
    Serial.print("\t");
    Serial.println(kalAngleY);


    //LED表示
    for(int i = 0; i < NUM_LEDS; i++){
      leds[i] = CRGB::Black;
    }


    if(kalAngleX > 20.0){
      for(int i = 0; i < 5; i++){
        leds[i] = CRGB::Red;
      }
    }else if(kalAngleX <= 20.0 && kalAngleX > 12.0){
      for(int i = 0; i < 5; i++){
        leds[i] = CRGB::Green;
      }
    }else if(kalAngleX <= 12.0 && kalAngleX > 4.0){
      for(int i = 0; i < 5; i++){
        leds[i + 5] = CRGB::Green;
      }
    }else if(kalAngleX <= 4.0 && kalAngleX > 1.0){
      for(int i = 0; i < 5; i++){
        leds[i + 10] = CRGB::Green;
      }
    }else if(abs(kalAngleX) <= 1.0){
      for(int i = 0; i < 5; i++){
        leds[i + 10] = CRGB::Blue;
      }
    }else if(kalAngleX >= -4.0 && kalAngleX < -1.0){
      for(int i = 0; i < 5; i++){
        leds[i + 10] = CRGB::Green;
      }
    }else if(kalAngleX >= -12.0 && kalAngleX < -4.0){
      for(int i = 0; i < 5; i++){
        leds[i + 15] = CRGB::Green;
      }
    }else if(kalAngleX >= -20.0 && kalAngleX < -12.0){
      for(int i = 0; i < 5; i++){
        leds[i + 20] = CRGB::Green;
      }
    }else if(kalAngleX < -20.0){
      for(int i = 0; i < 5; i++){
        leds[i + 20] = CRGB::Red;
      }
    }
    
      
    if(kalAngleY > 20.0){
      for(int i = 0; i < 5; i++){
        leds[i * 5] = CRGB::Red;
      }
    }else if(kalAngleY <= 20.0 && kalAngleY > 12.0){
      for(int i = 0; i < 5; i++){
        leds[i * 5] = CRGB::Green;
      }
    }else if(kalAngleY <= 12.0 && kalAngleY > 4.0){
      for(int i = 0; i < 5; i++){
        leds[i * 5 + 1] = CRGB::Green;
      }
    }else if(kalAngleY <= 4.0 && kalAngleY > 1.0){
      for(int i = 0; i < 5; i++){
        leds[i * 5 + 2] = CRGB::Green;
      }
    }else if(abs(kalAngleY) <= 1.0){
      for(int i = 0; i < 5; i++){
        leds[i * 5 + 2] = CRGB::Blue;
      }
    }else if(kalAngleY >= -4.0 && kalAngleY < -1.0){
      for(int i = 0; i < 5; i++){
        leds[i * 5 + 2] = CRGB::Green;
      }
    }else if(kalAngleY >= -12.0 && kalAngleY < -4.0){
      for(int i = 0; i < 5; i++){
        leds[i * 5 + 3] = CRGB::Green;
      }
    }else if(kalAngleY >= -20.0 && kalAngleY < -12.0){
      for(int i = 0; i < 5; i++){
        leds[i * 5 + 4] = CRGB::Green;
      }
    }else if(kalAngleY < -20.0){
      for(int i = 0; i < 5; i++){
        leds[i * 5 + 4] = CRGB::Red;
      }
    }
    
    FastLED.show();
        
    delay(delayTime);
    
    disableCore0WDT();
  }
}



void setup() {
  Serial.begin(115200);

  RollerI2C0.begin(&Wire, 0x64, 26, 32, 400000); 
  RollerI2C1.begin(&Wire, 0x65, 26, 32, 400000); 
  RollerI2C2.begin(&Wire, 0x66, 26, 32, 400000); 


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

  offsetX = preferences.getFloat("offsetX", offsetX);
  offsetY = preferences.getFloat("offsetY", offsetY);
  
  Kp = preferences.getFloat("Kp", Kp);
  Kd = preferences.getFloat("Kd", Kd);
  Kw = preferences.getFloat("Kw", Kw);
  IDRS = preferences.getFloat("IDRS", IDRS);
  delayTime = preferences.getInt("delayTime", delayTime);


  //回転位置検出 タスク
  xTaskCreatePinnedToCore(
    displayIMU
    ,  "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);


  delay(1000);

  RollerI2C0.setMode(ROLLER_MODE_CURRENT); 
  RollerI2C1.setMode(ROLLER_MODE_CURRENT); 
  RollerI2C2.setMode(ROLLER_MODE_CURRENT); 
  RollerI2C0.setOutput(1);
  RollerI2C1.setOutput(1);
  RollerI2C2.setOutput(1);
}


void loop() {
  if (fabs(kalAngleX) < 1.0 && fabs(kalAngleY) < 1.0 && GetUP == 0) {
    GetUP = 80;
  }
  

  if(GetUP == 80){
    //ブレーキ
    if(fabs(kalAngleX) > 20.0 || fabs(kalAngleY) > 20.0){
      RollerI2C0.setCurrent(0);
      RollerI2C1.setCurrent(0);
      RollerI2C2.setCurrent(0);
      AjX = 0.0;
      AjY = 0.0;
      GetUP = 0;
    }
        
    //モータ回転
    velY =  -RollerI2C0.getSpeedReadback() / 100.0;
    AjY +=  IDRS * velY / 10000.0;
    MY = (Kp * (kalAngleY + AjY) + Kd * kalAngleDotY / 10.0 + Kw * velY / 10.0) * 1000.0;

    velX =  0.5 / 100.0 * (-RollerI2C1.getSpeedReadback() + RollerI2C2.getSpeedReadback());
    AjX +=  IDRS * velX / 10000.0;
    MX = (Kp * (kalAngleX + AjX) + Kd * kalAngleDotX / 10.0 + Kw * velX / 10.0) * 1000.0;

    M0 = MY;
    M1 = -0.5 * MY + 0.866 * MX;
    M2 = 0.5 * MY + 0.866 * MX;
    
    RollerI2C0.setCurrent(-M0);
    RollerI2C1.setCurrent(-M1);
    RollerI2C2.setCurrent(M2);
    
  }else if(GetUP == 0){
    RollerI2C0.setCurrent(0);
    RollerI2C1.setCurrent(0);
    RollerI2C2.setCurrent(0);
  }
}

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

#include "MPU6886.h"

#include <Kalman.h>

#include "FastLED.h"

#include <WiFi.h>

#include <WebServer.h>

#include <Preferences.h>

#include "unit_rolleri2c.hpp"

UnitRollerI2C RollerI2C0, RollerI2C1, RollerI2C2; // Create a UNIT_ROLLERI2C object

#define led_pin 27

#define NUM_LEDS 25

CRGB leds[NUM_LEDS];

MPU6886 IMU;

WebServer server(80);

const char ssid[] = "S-3 Roller"; // SSID

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

const IPAddress ip(192, 168, 122, 133); // IPアドレス

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

unsigned long oldTime = 0, loopTime, nowTime;

float dt;

float Kp = 60;

float Kd = 65;

float Kw = 1.2;

float IDRS = 0.1;

int delayTime = 5;

float offsetX = 0.0, offsetY = 0.0;

int GetUP = 0;

float MX = 0.0, MY = 0.0, M0 = 0.0, M1 = 0.0, M2 = 0.0;

float AjX = 0.0, AjY = 0.0;

float velX = 0.0, velY = 0.0;

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

Kalman kalmanX, kalmanY;

float kalAngleX, kalAngleY, kalAngleDotX, kalAngleDotY;

Preferences preferences;

//加速度センサから傾きデータ取得 [deg]

void get_theta() {

IMU.getAccelData(&accX,&accY,&accZ);

//傾斜角導出単位はdeg

theta_X = atan2(accY , -accZ) * 180.0/PI + offsetX;

theta_Y = -atan2(-accX , -accZ) * 180.0/PI + offsetY;

}

//Y軸角速度取得

void get_gyro_data() {

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

theta_Xdot = -gyroX;

theta_Ydot = gyroY;

}

//ブラウザ表示

void handleRoot() {

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

temp += "<head>";

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

temp += "<title>S-3 Roller</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>S-3 Roller</h3>";

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

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

//Kw

temp += "Kw<br>";

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

temp += "<span>" + String(Kw) + "</span>";

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

//IDRS

temp += "IDRS<br>";

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

temp += "<span>" + String(IDRS) + "</span>";

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

//delayTime

temp += "delayTime<br>";

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

temp += "<span>" + String(delayTime) + "</span>";

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

temp += "</div>";

temp += "</body>";

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

}

void offsetXm() {

if (offsetX >= -10) {

offsetX -= 0.1;

preferences.putFloat("offsetX", offsetX);

}

handleRoot();

}

void offsetXp() {

if (offsetX <= 10) {

offsetX += 0.1;

preferences.putFloat("offsetX", offsetX);

}

handleRoot();

}

void offsetYm() {

if (offsetY >= -10) {

offsetY -= 0.1;

preferences.putFloat("offsetY", offsetY);

}

handleRoot();

}

void offsetYp() {

if (offsetY <= 10) {

offsetY += 0.1;

preferences.putFloat("offsetY", offsetY);

}

handleRoot();

}

void KpM() {

if (Kp >= 0.1) {

Kp -= 1;

preferences.putFloat("Kp", Kp);

}

handleRoot();

}

void KpP() {

if (Kp <= 300) {

Kp += 1;

preferences.putFloat("Kp", Kp);

}

handleRoot();

}

void KdM() {

if (Kd >= 0.1) {

Kd -= 1;

preferences.putFloat("Kd", Kd);

}

handleRoot();

}

void KdP() {

if (Kd <= 300) {

Kd += 1;

preferences.putFloat("Kd", Kd);

}

handleRoot();

}

void KwM() {

if (Kw >= 0) {

Kw -= 0.1;

preferences.putFloat("Kw", Kw);

}

handleRoot();

}

void KwP() {

if (Kw <= 300) {

Kw += 0.1;

preferences.putFloat("Kw", Kw);

}

handleRoot();

}

void IDRSm() {

if (IDRS >= 0.0) {

IDRS -= 0.01;

preferences.putFloat("IDRS", IDRS);

}

handleRoot();

}

void IDRSp() {

if (IDRS <= 10) {

IDRS += 0.01;

preferences.putFloat("IDRS", IDRS);

}

handleRoot();

}

void delayTimeM() {

if (delayTime > 0) {

delayTime -= 1;

preferences.putInt("delayTime", delayTime);

}

handleRoot();

}

void delayTimeP() {

if (delayTime <= 30) {

delayTime += 1;

preferences.putInt("delayTime", delayTime);

}

handleRoot();

}

//Core0

void displayIMU(void *pvParameters) {

FastLED.addLeds<WS2812B, led_pin, GRB>(leds, NUM_LEDS);

FastLED.setBrightness(20);

IMU.Init();

//フルスケールレンジ

IMU.SetAccelFsr(IMU.AFS_2G);

IMU.SetGyroFsr(IMU.GFS_250DPS);

get_theta();

kalmanX.setAngle(theta_X);

kalmanY.setAngle(theta_Y);

delay(100);

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

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

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

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

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

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

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

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

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

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

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

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

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

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

server.begin();

for (;;) {

server.handleClient();

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

Serial.print("\t");

Serial.println(kalAngleY);

//LED表示

for(int i = 0; i < NUM_LEDS; i++){

leds[i] = CRGB::Black;

}

if(kalAngleX > 20.0){

for(int i = 0; i < 5; i++){

leds[i] = CRGB::Red;

}

}else if(kalAngleX <= 20.0 && kalAngleX > 12.0){

for(int i = 0; i < 5; i++){

leds[i] = CRGB::Green;

}

}else if(kalAngleX <= 12.0 && kalAngleX > 4.0){

for(int i = 0; i < 5; i++){

leds[i + 5] = CRGB::Green;

}

}else if(kalAngleX <= 4.0 && kalAngleX > 1.0){

for(int i = 0; i < 5; i++){

leds[i + 10] = CRGB::Green;

}

}else if(abs(kalAngleX) <= 1.0){

for(int i = 0; i < 5; i++){

leds[i + 10] = CRGB::Blue;

}

}else if(kalAngleX >= -4.0 && kalAngleX < -1.0){

for(int i = 0; i < 5; i++){

leds[i + 10] = CRGB::Green;

}

}else if(kalAngleX >= -12.0 && kalAngleX < -4.0){

for(int i = 0; i < 5; i++){

leds[i + 15] = CRGB::Green;

}

}else if(kalAngleX >= -20.0 && kalAngleX < -12.0){

for(int i = 0; i < 5; i++){

leds[i + 20] = CRGB::Green;

}

}else if(kalAngleX < -20.0){

for(int i = 0; i < 5; i++){

leds[i + 20] = CRGB::Red;

}

if(kalAngleY > 20.0){

for(int i = 0; i < 5; i++){

leds[i * 5] = CRGB::Red;

}

}else if(kalAngleY <= 20.0 && kalAngleY > 12.0){

for(int i = 0; i < 5; i++){

leds[i * 5] = CRGB::Green;

}

}else if(kalAngleY <= 12.0 && kalAngleY > 4.0){

for(int i = 0; i < 5; i++){

leds[i * 5 + 1] = CRGB::Green;

}

}else if(kalAngleY <= 4.0 && kalAngleY > 1.0){

for(int i = 0; i < 5; i++){

leds[i * 5 + 2] = CRGB::Green;

}

}else if(abs(kalAngleY) <= 1.0){

for(int i = 0; i < 5; i++){

leds[i * 5 + 2] = CRGB::Blue;

}

}else if(kalAngleY >= -4.0 && kalAngleY < -1.0){

for(int i = 0; i < 5; i++){

leds[i * 5 + 2] = CRGB::Green;

}

}else if(kalAngleY >= -12.0 && kalAngleY < -4.0){

for(int i = 0; i < 5; i++){

leds[i * 5 + 3] = CRGB::Green;

}

}else if(kalAngleY >= -20.0 && kalAngleY < -12.0){

for(int i = 0; i < 5; i++){

leds[i * 5 + 4] = CRGB::Green;

}

}else if(kalAngleY < -20.0){

for(int i = 0; i < 5; i++){

leds[i * 5 + 4] = CRGB::Red;

}

FastLED.show();

delay(delayTime);

disableCore0WDT();

}

void setup() {

Serial.begin(115200);

RollerI2C0.begin(&Wire, 0x64, 26, 32, 400000);

RollerI2C1.begin(&Wire, 0x65, 26, 32, 400000);

RollerI2C2.begin(&Wire, 0x66, 26, 32, 400000);

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

preferences.begin("parameter", false);

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

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

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

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

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

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

delayTime = preferences.getInt("delayTime", delayTime);

//回転位置検出タスク

xTaskCreatePinnedToCore(

displayIMU

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

delay(1000);

RollerI2C0.setMode(ROLLER_MODE_CURRENT);

RollerI2C1.setMode(ROLLER_MODE_CURRENT);

RollerI2C2.setMode(ROLLER_MODE_CURRENT);

RollerI2C0.setOutput(1);

RollerI2C1.setOutput(1);

RollerI2C2.setOutput(1);

}

void loop() {

if (fabs(kalAngleX) < 1.0 && fabs(kalAngleY) < 1.0 && GetUP == 0) {

GetUP = 80;

}

if(GetUP == 80){

//ブレーキ

if(fabs(kalAngleX) > 20.0 || fabs(kalAngleY) > 20.0){

RollerI2C0.setCurrent(0);

RollerI2C1.setCurrent(0);

RollerI2C2.setCurrent(0);

AjX = 0.0;

AjY = 0.0;

GetUP = 0;

}

//モータ回転

velY = -RollerI2C0.getSpeedReadback() / 100.0;

AjY += IDRS * velY / 10000.0;

MY = (Kp * (kalAngleY + AjY) + Kd * kalAngleDotY / 10.0 + Kw * velY / 10.0) * 1000.0;

velX = 0.5 / 100.0 * (-RollerI2C1.getSpeedReadback() + RollerI2C2.getSpeedReadback());

AjX += IDRS * velX / 10000.0;

MX = (Kp * (kalAngleX + AjX) + Kd * kalAngleDotX / 10.0 + Kw * velX / 10.0) * 1000.0;

M0 = MY;

M1 = -0.5 * MY + 0.866 * MX;

M2 = 0.5 * MY + 0.866 * MX;

RollerI2C0.setCurrent(-M0);

RollerI2C1.setCurrent(-M1);

RollerI2C2.setCurrent(M2);

}else if(GetUP == 0){

RollerI2C0.setCurrent(0);

RollerI2C1.setCurrent(0);

RollerI2C2.setCurrent(0);

}

コード説明

L. 10　Roller485モータのオブジェクト生成。
　ここでは真ん中のモータをRollerI2C0、左をRollerI2C1、右をRollerI2C2としました。

L. 19～25　WiFi設定 SSID、パスワード、IPアドレスは適宜変えてください。

L. 31～37　パラメータ値指定
　これらを調整して点倒立動作を実現させる。後述のブラウザアプリでも変更できます。

L. 59～65　傾斜計算
　加速度センサからX軸とY軸の傾斜角を算出する関数
　コードやアプリで各軸のオフセット調整 (offsetX, offsetY)が可能です。

　
L. 67～72　角速度取得関数
　X軸とY軸の角速度を検出

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

L. 282～443　core0動作
　ATOM Matrix デュアルコアのcore0でLEDディスプレイ表示とIMUセンサの処理とブラウザ表示およびハンドラ処理を実行します。

　IMUセンサMPU6886のフルスケールレンジは以下で指定 (L. 290, 291)
　・加速度：16bit　±2 g
　・ジャイロ(角速度)：16bit ±250 deg/sec

　カルマンフィルタライブラリの関数を用いてセンサの姿勢角と角速度を算出 (L. 331~347)。
　センサのオフセット確認用に算出したX軸、Y軸の角度をシリアル出力しています (L. 349~351)。

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

　delayTime でcore0の制御ループ時間を調整します (L. 439)。
　

L. 447～487　setup関数
　Roller485 I2C設定 (L. 450～452)
　本プログラムではWire0でRoller485とWire1でIMUセンサとI2C通信します。

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

　Roller485をカレントモード (トルク指定駆動)にして起動 (L. 481～486)。

L. 490～529　loop関数
　センサのX軸、Y軸が共に角度±1°以下になるとモータを起動します (L. 491～493)。

　センサの角度が±20°以上になるとモータを停止させます (L. 498～505)。

　センサによるX軸とY軸の傾斜角と角速度とモータの回転速度から各軸の安定に必要なトルクを計算します (L. 508～514)。
　X軸のモータ回転速度は左右のモータの速度の平均をフィードバックしています (L. 512)。
　それぞれの値にかける係数Kp, Kd , Kwを調整して倒立を実現します。
　パラメータが極端に大きくなったり小さくなったりしないように各パラメータ値は割って正規化しております。
　またモータの回転速度をセンサ角度にフィードバックして安定性の向上を図っています(L. 509, 513)
　このフィードバックのかけ具合は係数IDRSで調整します。
　これによって機体にオフセットがあったり床が傾いてもモジュールの安定ターゲット角度が自動で調整されホイールの回転が抑制されます。
　必要ない場合はIDRS = 0.0としてください。

　先ほど算出したモジュールのX, Y軸の必要トルクに基づいて各モータのトルクを指定します (L. 516~522)。

　真ん中のモータM0はY軸上に配置しているのでY軸によるトルク(MY)で回転させます。
　左右のモータはセンサのX軸、Y軸に対してそれぞれ斜めに配置されているので、各軸の必要トルクを振り分けて回転デューティを算出します。
　左右モータの回転トルクは以下のように軸上に分解されて、Y軸の傾きによるトルクは左右で打ち消してX軸によるトルクにのみ寄与するように回転させます (L. 517, 518)。

　算出した各モータトルクで回転実行 (L. 520~522)。

アプリ画面

アプリを書き込んだATOMS3を起動して、スマホもしくはPCのWiFi接続設定でSSID ”S-3 Roller”に接続
　パスワード：password
ブラウザで”192.168.122.133”にアクセス

パラメータは “-“、”+”をクリックして値を増減させます。
値は電源OFF後も保持されます。

動作

筐体上蓋 body3.stl はスナップフィット的にカチャっとハメて固定します。

チャントハメタス pic.twitter.com/FwmmMByn2M

— HomeMadeGarbage (@H0meMadeGarbage) October 22, 2024

４ピンコネクタをATOMにも接続し、LiPoバッテリコネクタを接続すると起動します。
私は粗野な人間なのでバッテリ直結でON/OFFしていますが当然スイッチを挿入するのがジェントルです。
Roller485 Lite ユニットに5V DCDCが内蔵されており4ピンコネクタからATOMに5V供給されるので電源系も非常にシンプルです。

pic.twitter.com/D9Zx0FoS51

— HomeMadeGarbage (@H0meMadeGarbage) October 22, 2024

機体を起上てX軸とY軸の傾きが共に±1°以内になるとモータが回転し始めますので直ぐに手を放してください。
LEDのインジケータが助けになります。

起き上げ方 pic.twitter.com/tWj8lxjbqM

— HomeMadeGarbage (@H0meMadeGarbage) October 22, 2024

センサ自体のオフセットが大きい場合には倒立ができない場合があります。
ATOM単体を平らなところに置いて起動し、シリアル出力される角度が0に近いかご確認ください。

大きくズレがある場合にはアプリのOffsetX、offsetYで0に近い値にしてください。

おわりに

ここではRoller485 Lite ユニットを用いた3軸姿勢制御モジュールの製法を紹介いたしました。

Roller485が非常にスマートで使いやすいユニットのため機体構成はこの上なくシンプルになり製作も容易でした。

マジで世界一簡単に組める3軸姿勢制御モジュールになったのではないでしょうか？

スペーサとネジでステージ stage.stl を追加すれば、ちょっとした小机にもなります。

ガンダムMk-II pic.twitter.com/C1bxcpeF6p

— HomeMadeGarbage (@H0meMadeGarbage) October 19, 2024

是非点倒立の喜びを体感いただければと思います。

　
DCモータを使用した小型3軸姿勢制御モジュールの製法も以下で紹介しておりますのでよろしければご覧ください。

3軸姿勢制御モジュール SHISEIGYO-3 DC の製法

こちらからは以上です。

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

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

はじめに

ダウンロードファイル

部品

3Dモデル

製作

モータ実装

配線

マイコン、バッテリ実装

フライホイール実装

Arduinoコード

使用ライブラリ

コード

コード説明

アプリ画面

動作

おわりに

関連記事

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

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