先日 ATOMS3 という製品がM5Stack社から発売されました。

m5-docs

docs.m5stack.com

M5ATOM MatrixのLEDマトリクスがLCD (128×128ピクセル)に変更され、コントローラとしてESP32-S3が採用されています。

この度 ATOMS3 を入手し姿勢制御モジュールを製作しましたので報告させていただきます。

ATOMS3

待ちわびた製品が到着。白くてカワイイ

ATOMS3
北海道なので少し遅れて到着
白くて綺麗 pic.twitter.com/ro1skDwpeV

— HomeMadeGarbage (@H0meMadeGarbage) December 28, 2022

コントローラがESP32-S3に変わったのでピン番号が変わっていますが、電源・GNDやIMUのI2Cピンなど構成はMatrixと同様です。

姿勢制御モジュールで味見

試しにM5ATOM Matrix を使って製作したSHISEIGYO-1 Jr. をATOMS3で試してみました。

ATOMS3で姿勢制御モジュール

M5AtomS3.hが使えなかったのでSHISEIGYO-1 用に用意したMPU6886ライブラリで動作

IMU軸が変わっていた。ATOM MatrixはLED基板の裏に実装されていたけど、今回は恐らく本体基板に普通に実装されてる。

GPIOは番号変えるだけで動いた。

ディスプレイはまだ動かせていない pic.twitter.com/GmdPGw5cdM

— HomeMadeGarbage (@H0meMadeGarbage) December 28, 2022

ATOMS3のArduinoライブラリが用意されていましたが、この時点 (2022/12/28)ではコンパイルエラーで使用できませんでした。

ATOMS3ライブラリは使用せずにSHISEIGYO-1 Jr. のコードをそのまま使用し、対応するGPIOの番号を変えるのみで動作しました。
Arduino IDEでのボードは”ESP32S3 Dev Module”を選択。
ディスプレイはまだ駆動できていません。

もう１点 Matrixと比較してIMUセンサMPU6886の軸が異なっていました。
ATOM MatrixはLED基板の裏に実装されていましたが、ATOMS3は恐らく本体基板に普通に実装されてるようです (中みていないので確定ではないですが)。

傾きを算出する際に注意が必要となります。

ディスプレイ表示

ATOMS3には128×128のLCDが搭載されております。ピンアサインはパッケージに記載があるのですがドライバが不明で良くわからず。。
しかも専用ライブラリは現状使えないので途方に暮れていたのですが、Twitter上でM5GFXライブラリで表示可能との情報を得ました。

GitHub - m5stack/M5GFX: Graphics library for M5Stack series

GitHub

早速表示実験

スプライトとか必要かなと思ってたけど
普通の描画でもキレイにできた pic.twitter.com/NaUbDJ96J4

— HomeMadeGarbage (@H0meMadeGarbage) December 28, 2022

M5GFXライブラリのおかげで何の設定もなしに簡単に表示できました。ありがたや。
小さいディスプレイですがコントラストも良く視野角も広くて素晴らしいです。

SHISEIGYO-1 Jr. S3

ディスプレイ表示ができるようになったので、姿勢制御モジュールを仕上げました。

SHISEIGYO-1 Jr. S3 爆誕
ということで ATOMS3 の白いボディに合わせて真っ白に仕上げてみました。
ディスプレイ表示もいい感じです♪

Arduino IDEコード

SHISEIGYO-1 Jr. のサンプルコードをベースにコーディングしました。

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

M5GFX lcd;

WebServer server(80);

const char ssid[] = "SHISEIGYO-1 Jr.";  // SSID
const char pass[] = "password";   // password

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

#define ENC_A 5
#define ENC_B 6
#define brake 7
#define rote_pin 1
#define PWM_pin 2
#define button 41

MPU6886 IMU;

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

volatile byte pos;
volatile int  enc_count = 0;

float Kp = 1.8;
float Kd = 2;
float Kw = 0.4;
float IDRS = 0.6;

float getupRange = 0.1;
float injectRatio = 14;
int rotMaxL = 720;
int rotMaxR = 790;

int delayTime = 2;

int pwmDuty;
int GetUP = 0;
int GetUpCnt = 0;

float M;
float Aj = 0.0;

float accX = 0, accY = 0, accZ = 0;
float gyroX = 0, gyroY = 0, gyroZ = 0;
float temp = 0;

float theta_acc = 0.0;
float theta_dot = 0.0;

Kalman kalmanY;
float kalAngleY, kalAngleDotY;

Preferences preferences;


//加速度センサから傾きデータ取得 [deg]
float get_theta_acc() {
  IMU.getAccelData(&accX,&accY,&accZ);
  //傾斜角導出 単位はdeg
  theta_acc  = atan(-1.0 * accX / accZ) * 57.29578f;
  return theta_acc;
}

//Y軸 角速度取得
float get_gyro_data() {
  IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  theta_dot = gyroY;
  return theta_dot;
}

//起き上がり
void getup(){
  digitalWrite(brake, HIGH);
  int rotMax;
  //回転方向
  if(kalAngleY < 0.0){
    rotMax = rotMaxL;
    digitalWrite(rote_pin, LOW);
    GetUP = 1;
  }else{
    rotMax = rotMaxR;
    digitalWrite(rote_pin, HIGH);
    GetUP = 2;
  }

  for(int i = 1023; i >= rotMax; i--){
    ledcWrite(0, i);
    delay(5);
  }
  ledcWrite(0, rotMax);
  delay(300);
  
  if(kalAngleY > 0.0){
    digitalWrite(rote_pin, LOW);
  }else{
    digitalWrite(rote_pin, HIGH);
  }
}


//Core0
void display(void *pvParameters) {
  for (;;){
    lcd.setTextFont(4);
    lcd.setCursor(10, 2);
    lcd.printf("%+05.1f", kalAngleY);

    int yy = map(kalAngleY, 20, -20, 0, 127);
    yy = constrain(yy, 0, 127);
    uint32_t colorY;

    if(fabs(kalAngleY) <= 1.0){
      colorY = 0x00FF00U;
    }else if(fabs(kalAngleY) <= 15){
      colorY = 0x0000FFU;
    }else{
      colorY = 0xFF0000U;
    }
    lcd.fillRect(yy - 2, 25, 5, 128, colorY);
    delay(33);
    lcd.clear();

    disableCore0WDT();
  }
}

//ブラウザ表示
void handleRoot() {
  String temp ="<!DOCTYPE html> \n<html lang=\"ja\">";
  temp +="<head>";
  temp +="<meta charset=\"utf-8\">";
  temp +="<title>SHISEIGYO-1 Jr.</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>SHISEIGYO-1 Jr.</h3>";
  
  //起き上がりボタン
  temp +="<button type=\"button\" ><a href=\"/GetUp\">GetUp</a></button><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>";

  //Rot Max L
  temp +="Rot Max L<br>";
  temp +="<a class=\"pm\" href=\"/rotMaxLm\">-</a>";
  temp +="<span>" + String(rotMaxL) + "</span>";
  temp +="<a class=\"pm\" href=\"/rotMaxLp\">+</a><br>";

  //Rot Max R
  temp +="Rot Max R<br>";
  temp +="<a class=\"pm\" href=\"/rotMaxRm\">-</a>";
  temp +="<span>" + String(rotMaxR) + "</span>";
  temp +="<a class=\"pm\" href=\"/rotMaxRp\">+</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>";
  
  temp +="</div>";
  temp +="</body>";
  server.send(200, "text/HTML", temp);
}

void handleGetUp() {
  handleRoot();
  if(GetUP == 0){
    getup();
  }
}

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

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

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

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

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

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

void handleRotMaxLm() {
  if(rotMaxL >= 10){
    rotMaxL -= 10;
    preferences.putInt("rotMaxL", rotMaxL);
  }
  handleRoot();
}

void handleRotMaxLp() {
  if(rotMaxL <= 1010){
    rotMaxL += 10;
    preferences.putInt("rotMaxL", rotMaxL);
  }
  handleRoot();
}

void handleRotMaxRm() {
  if(rotMaxR >= 10){
    rotMaxR -= 10;
    preferences.putInt("rotMaxR", rotMaxR);
  }
  handleRoot();
}

void handleRotMaxRp() {
  if(rotMaxR <= 1010){
    rotMaxR += 10;
    preferences.putInt("rotMaxR", rotMaxR);
  }
  handleRoot();
}

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

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

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

  lcd.begin();
  lcd.setTextColor(0xFFFFFFU, 0x000000U); //１つ目の引数が文字色、２つ目の引数が背景色
  
  
  pinMode(ENC_A, INPUT);
  pinMode(ENC_B, INPUT);
  pinMode(brake, OUTPUT);
  pinMode(button, INPUT);
 
  attachInterrupt(ENC_A, ENC_READ, CHANGE);
  attachInterrupt(ENC_B, ENC_READ, CHANGE);

  IMU.Init();

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

  kalmanY.setAngle(get_theta_acc());

  ledcSetup(0, 20000, 10);
  ledcAttachPin(PWM_pin, 0);
  
  pinMode(rote_pin, OUTPUT);
  digitalWrite(brake, LOW);

  preferences.begin("parameter", false);

  //パラメータ初期値取得
  rotMaxL = preferences.getInt("rotMaxL", rotMaxL);
  rotMaxR = preferences.getInt("rotMaxR", rotMaxR);
  Kp = preferences.getFloat("Kp", Kp);
  Kd = preferences.getFloat("Kd", Kd);
  Kw = preferences.getFloat("Kw", Kw);
  IDRS = preferences.getFloat("IDRS", IDRS);
 

  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("/GetUp", handleGetUp);
  
  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("/rotMaxLm", handleRotMaxLm);
  server.on("/rotMaxLp", handleRotMaxLp);
  server.on("/rotMaxRm", handleRotMaxRm);
  server.on("/rotMaxRp", handleRotMaxRp);

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

  //回転位置検出 タスク
  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() {
  server.handleClient();
  
  //オフセット再計算&起き上がり
  if (digitalRead(button) == 0){
    getup();
  }
  
  nowTime = micros();
  loopTime = nowTime - oldTime;
  oldTime = nowTime;
  
  dt = (float)loopTime / 1000000.0; //sec
  
  //モータの角速度算出
  float theta_dotWheel = -1.0 * float(enc_count) * 3.6 / dt;
  enc_count = 0;
  
  //カルマンフィルタ 姿勢 傾き
  kalAngleY = kalmanY.getAngle(get_theta_acc(), get_gyro_data(), dt);
  
  //カルマンフィルタ 姿勢 角速度
  kalAngleDotY = kalmanY.getRate();


  if(GetUP == 1 || GetUP == 2){
    if(GetUP == 1 && kalAngleY >= getupRange){
      digitalWrite(brake, LOW);
      GetUP = 99;
    }else if(GetUP == 2 && kalAngleY <= -getupRange){
      digitalWrite(brake, LOW);
      GetUP = 99;
    }else{
      if(GetUP == 1 && kalAngleY < 0 || GetUP == 2 && kalAngleY > 0){
        ledcWrite(0, max(1023 - int(injectRatio * fabs(kalAngleY)), 0));
      }else {
        ledcWrite(0,511);
      }
    }
  }else {
    if (fabs(kalAngleY) < 1 && GetUP == 0){
      GetUP = 80;
    }

    /*
    Serial.print("Kp: ");
    Serial.print(Kp);
    Serial.print(", Kd: ");
    Serial.print(Kd,3);
    Serial.print(", Kw: ");
    Serial.print(Kw, 3);
    Serial.print(", kalAngleY: ");
    Serial.print(kalAngleY);
    */
      
    
    if(GetUP == 99 || GetUP == 80){
      //ブレーキ
      if(fabs(kalAngleY) > 15.0){
        digitalWrite(brake, LOW);
        Aj = 0.0;
        GetUP = 0;
      }else{
        digitalWrite(brake, HIGH);
      }
        
      //モータ回転
      if(GetUP == 80){
        M = Kp * kalAngleY / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 20000.0;
        GetUpCnt++;
        if(GetUpCnt > 100){
          GetUpCnt = 0;
          GetUP = 99;
        }
      }else{
        Aj +=  IDRS * theta_dotWheel / 1000000.0;
        M = Kp * (kalAngleY + Aj) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 10000.0;
      }
      M = max(-1.0f, min(1.0f, M));
      pwmDuty = 1023 * (1.0 - fabs(M));
      
        
      //回転方向
      if(M > 0.0){
        digitalWrite(rote_pin, LOW);
        ledcWrite(0, pwmDuty);
      }else{
        digitalWrite(rote_pin, HIGH);
        ledcWrite(0, pwmDuty);
      }
    }
      
    
    Serial.print(", loopTime: ");
    Serial.print((float)loopTime / 1000.0);
    
    
    delay(delayTime);
      
    Serial.println("");
  }
}

//ブラシレスモータエンコーダ出力 割り込み処理
//参考：https://jumbleat.com/2016/12/17/encoder_1/
void ENC_READ() {
  byte cur = (!digitalRead(ENC_B) << 1) + !digitalRead(ENC_A);
  byte old = pos & B00000011;
  byte dir = (pos & B00110000) >> 4;
 
  if (cur == 3) cur = 2;
  else if (cur == 2) cur = 3;
 
  if (cur != old)
  {
    if (dir == 0)
    {
      if (cur == 1 || cur == 3) dir = cur;
    } else {
      if (cur == 0)
      {
        if (dir == 1 && old == 3) enc_count--;
        else if (dir == 3 && old == 1) enc_count++;
        dir = 0;
      }
    }
 
    bool rote = 0;
    if (cur == 3 && old == 0) rote = 0;
    else if (cur == 0 && old == 3) rote = 1;
    else if (cur > old) rote = 1;
 
    pos = (dir << 4) + (old << 2) + cur;
  }
}

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

#include "MPU6886.h"

#include <Kalman.h>

#include "FastLED.h"

#include <WiFi.h>

#include <WebServer.h>

#include <Preferences.h>

#include <M5GFX.h>

M5GFX lcd;

WebServer server(80);

const char ssid[] = "SHISEIGYO-1 Jr."; // SSID

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

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

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

#define ENC_A 5

#define ENC_B 6

#define brake 7

#define rote_pin 1

#define PWM_pin 2

#define button 41

MPU6886 IMU;

unsigned long oldTime = 0, loopTime, nowTime;

float dt;

volatile byte pos;

volatile int enc_count = 0;

float Kp = 1.8;

float Kd = 2;

float Kw = 0.4;

float IDRS = 0.6;

float getupRange = 0.1;

float injectRatio = 14;

int rotMaxL = 720;

int rotMaxR = 790;

int delayTime = 2;

int pwmDuty;

int GetUP = 0;

int GetUpCnt = 0;

float M;

float Aj = 0.0;

float accX = 0, accY = 0, accZ = 0;

float gyroX = 0, gyroY = 0, gyroZ = 0;

float temp = 0;

float theta_acc = 0.0;

float theta_dot = 0.0;

Kalman kalmanY;

float kalAngleY, kalAngleDotY;

Preferences preferences;

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

float get_theta_acc() {

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

//傾斜角導出単位はdeg

theta_acc = atan(-1.0 * accX / accZ) * 57.29578f;

return theta_acc;

}

//Y軸角速度取得

float get_gyro_data() {

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

theta_dot = gyroY;

return theta_dot;

}

//起き上がり

void getup(){

digitalWrite(brake, HIGH);

int rotMax;

//回転方向

if(kalAngleY < 0.0){

rotMax = rotMaxL;

digitalWrite(rote_pin, LOW);

GetUP = 1;

}else{

rotMax = rotMaxR;

digitalWrite(rote_pin, HIGH);

GetUP = 2;

}

for(int i = 1023; i >= rotMax; i--){

ledcWrite(0, i);

delay(5);

}

ledcWrite(0, rotMax);

delay(300);

if(kalAngleY > 0.0){

digitalWrite(rote_pin, LOW);

}else{

digitalWrite(rote_pin, HIGH);

}

//Core0

void display(void *pvParameters) {

for (;;){

lcd.setTextFont(4);

lcd.setCursor(10, 2);

lcd.printf("%+05.1f", kalAngleY);

int yy = map(kalAngleY, 20, -20, 0, 127);

yy = constrain(yy, 0, 127);

uint32_t colorY;

if(fabs(kalAngleY) <= 1.0){

colorY = 0x00FF00U;

}else if(fabs(kalAngleY) <= 15){

colorY = 0x0000FFU;

}else{

colorY = 0xFF0000U;

}

lcd.fillRect(yy - 2, 25, 5, 128, colorY);

delay(33);

lcd.clear();

disableCore0WDT();

}

//ブラウザ表示

void handleRoot() {

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

temp +="<head>";

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

temp +="<title>SHISEIGYO-1 Jr.</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>SHISEIGYO-1 Jr.</h3>";

//起き上がりボタン

temp +="<button type=\"button\" ><a href=\"/GetUp\">GetUp</a></button><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>";

//Rot Max L

temp +="Rot Max L<br>";

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

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

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

//Rot Max R

temp +="Rot Max R<br>";

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

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

temp +="<a class=\"pm\" href=\"/rotMaxRp\">+</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>";

temp +="</div>";

temp +="</body>";

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

}

void handleGetUp() {

handleRoot();

if(GetUP == 0){

getup();

}

void KpM() {

if(Kp >= 0.1){

Kp -= 0.1;

preferences.putFloat("Kp", Kp);

}

handleRoot();

}

void KpP() {

if(Kp <= 30){

Kp += 0.1;

preferences.putFloat("Kp", Kp);

}

handleRoot();

}

void KdM() {

if(Kd >= 0.1){

Kd -= 0.1;

preferences.putFloat("Kd", Kd);

}

handleRoot();

}

void KdP() {

if(Kd <= 30){

Kd += 0.1;

preferences.putFloat("Kd", Kd);

}

handleRoot();

}

void KwM() {

if(Kw >= 0.1){

Kw -= 0.1;

preferences.putFloat("Kw", Kw);

}

handleRoot();

}

void KwP() {

if(Kw <= 30){

Kw += 0.1;

preferences.putFloat("Kw", Kw);

}

handleRoot();

}

void handleRotMaxLm() {

if(rotMaxL >= 10){

rotMaxL -= 10;

preferences.putInt("rotMaxL", rotMaxL);

}

handleRoot();

}

void handleRotMaxLp() {

if(rotMaxL <= 1010){

rotMaxL += 10;

preferences.putInt("rotMaxL", rotMaxL);

}

handleRoot();

}

void handleRotMaxRm() {

if(rotMaxR >= 10){

rotMaxR -= 10;

preferences.putInt("rotMaxR", rotMaxR);

}

handleRoot();

}

void handleRotMaxRp() {

if(rotMaxR <= 1010){

rotMaxR += 10;

preferences.putInt("rotMaxR", rotMaxR);

}

handleRoot();

}

void IDRSm() {

if(IDRS >= 0.1){

IDRS -= 0.1;

preferences.putFloat("IDRS", IDRS);

}

handleRoot();

}

void IDRSp() {

if(IDRS <= 10){

IDRS += 0.1;

preferences.putFloat("IDRS", IDRS);

}

handleRoot();

}

void setup() {

Serial.begin(115200);

lcd.begin();

lcd.setTextColor(0xFFFFFFU, 0x000000U); //１つ目の引数が文字色、２つ目の引数が背景色

pinMode(ENC_A, INPUT);

pinMode(ENC_B, INPUT);

pinMode(brake, OUTPUT);

pinMode(button, INPUT);

attachInterrupt(ENC_A, ENC_READ, CHANGE);

attachInterrupt(ENC_B, ENC_READ, CHANGE);

IMU.Init();

//フルスケールレンジ

IMU.SetAccelFsr(IMU.AFS_2G);

IMU.SetGyroFsr(IMU.GFS_250DPS);

kalmanY.setAngle(get_theta_acc());

ledcSetup(0, 20000, 10);

ledcAttachPin(PWM_pin, 0);

pinMode(rote_pin, OUTPUT);

digitalWrite(brake, LOW);

preferences.begin("parameter", false);

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

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

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

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

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

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

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

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

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

server.on("/rotMaxLp", handleRotMaxLp);

server.on("/rotMaxRm", handleRotMaxRm);

server.on("/rotMaxRp", handleRotMaxRp);

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

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

server.begin();

//回転位置検出タスク

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

server.handleClient();

//オフセット再計算&起き上がり

if (digitalRead(button) == 0){

getup();

}

nowTime = micros();

loopTime = nowTime - oldTime;

oldTime = nowTime;

dt = (float)loopTime / 1000000.0; //sec

//モータの角速度算出

float theta_dotWheel = -1.0 * float(enc_count) * 3.6 / dt;

enc_count = 0;

//カルマンフィルタ姿勢傾き

kalAngleY = kalmanY.getAngle(get_theta_acc(), get_gyro_data(), dt);

//カルマンフィルタ姿勢角速度

kalAngleDotY = kalmanY.getRate();

if(GetUP == 1 || GetUP == 2){

if(GetUP == 1 && kalAngleY >= getupRange){

digitalWrite(brake, LOW);

GetUP = 99;

}else if(GetUP == 2 && kalAngleY <= -getupRange){

digitalWrite(brake, LOW);

GetUP = 99;

}else{

if(GetUP == 1 && kalAngleY < 0 || GetUP == 2 && kalAngleY > 0){

ledcWrite(0, max(1023 - int(injectRatio * fabs(kalAngleY)), 0));

}else {

ledcWrite(0,511);

}

}else {

if (fabs(kalAngleY) < 1 && GetUP == 0){

GetUP = 80;

}

Serial.print("Kp: ");

Serial.print(Kp);

Serial.print(", Kd: ");

Serial.print(Kd,3);

Serial.print(", Kw: ");

Serial.print(Kw, 3);

Serial.print(", kalAngleY: ");

Serial.print(kalAngleY);

if(GetUP == 99 || GetUP == 80){

//ブレーキ

if(fabs(kalAngleY) > 15.0){

digitalWrite(brake, LOW);

Aj = 0.0;

GetUP = 0;

}else{

digitalWrite(brake, HIGH);

}

//モータ回転

if(GetUP == 80){

M = Kp * kalAngleY / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 20000.0;

GetUpCnt++;

if(GetUpCnt > 100){

GetUpCnt = 0;

GetUP = 99;

}

}else{

Aj += IDRS * theta_dotWheel / 1000000.0;

M = Kp * (kalAngleY + Aj) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 10000.0;

}

M = max(-1.0f, min(1.0f, M));

pwmDuty = 1023 * (1.0 - fabs(M));

//回転方向

if(M > 0.0){

digitalWrite(rote_pin, LOW);

ledcWrite(0, pwmDuty);

}else{

digitalWrite(rote_pin, HIGH);

ledcWrite(0, pwmDuty);

}

Serial.print(", loopTime: ");

Serial.print((float)loopTime / 1000.0);

delay(delayTime);

Serial.println("");

}

//ブラシレスモータエンコーダ出力割り込み処理

//参考：https://jumbleat.com/2016/12/17/encoder_1/

void ENC_READ() {

byte cur = (!digitalRead(ENC_B) << 1) + !digitalRead(ENC_A);

byte old = pos & B00000011;

byte dir = (pos & B00110000) >> 4;

if (cur == 3) cur = 2;

else if (cur == 2) cur = 3;

if (cur != old)

{

if (dir == 0)

{

if (cur == 1 || cur == 3) dir = cur;

} else {

if (cur == 0)

{

if (dir == 1 && old == 3) enc_count--;

else if (dir == 3 && old == 1) enc_count++;

dir = 0;

}

bool rote = 0;

if (cur == 3 && old == 0) rote = 0;

else if (cur == 0 && old == 3) rote = 1;

else if (cur > old) rote = 1;

pos = (dir << 4) + (old << 2) + cur;

}

ESP32-S3対応のためにGPIO番号を修正しています。
MPU6886.cppのI2Cピン番号も変更が必要です。 [ Wire1.begin(38,39,100000); ]

カルマンフィルタで誤差算出をするので、事前のIMUのオフセットの導出は必要ないことに気づいたので廃止しました。

ディスプレイはデュアルコアにしてcore0で表示させています。

モジュールの各変数はAPモードでESP32-S3とつなげて、ブラウザから調整できるようにしています。
調整した値は電源OFF後もフラッシュで記憶されます。
起き上がり動作ボタンも実装しています。

コードでは “SHISEIGYO-1 Jr.” にWiFi接続して (パスワード：”password”)、192.168.22.10にブラウザアクセスで設定画面が表示されます。

おわりに

ATOMS3を用いて姿勢制御モジュールの製作を楽しみました。
小型ですがきれいなディスプレイもついてしかも低価格なのでATOMS3は非常におすすめの製品です。

まだライブラリが整ていないようで、ツギハギのコードになってしまいましたが動けばこっちのもんです。

しかしこれまでのSHISEIGYO-1 もATOM Matrixのライブラリが成熟する前に製作したので、今度どこかのタイミングで最新ライブラリでコードをきれいにまとめたいなぁなんて思っています。

センサレス姿勢制御モジュール完成　ーリアクションホイールへの道54ー

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

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

ATOMS3 で姿勢制御モジュール　ーリアクションホイールへの道53ー

ATOMS3

姿勢制御モジュールで味見

ディスプレイ表示

SHISEIGYO-1 Jr. S3

Arduino IDEコード

おわりに

次の記事

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

ATOMS3

姿勢制御モジュールで味見

ディスプレイ表示

SHISEIGYO-1 Jr. S3

Arduino IDEコード

おわりに

次の記事

関連記事

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

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