はじめに

SHISEIGYO-3 DC は小型ドローンに使用されるコアレスモータを採用した１辺82mmの小型3軸姿勢制御モジュールです。コントローラとしてIMU内蔵のATOMS3を使用し点倒立を実現します。

SHISEIGYO-3 DC
coming soon… pic.twitter.com/DoIUnrOsoB

— HomeMadeGarbage (@H0meMadeGarbage) October 13, 2024

本作例ではSHISEIGYO-3 DC をアクセスポイントとしてスマホ等でWiFi接続して専用ブラウザアプリで倒立パラメータの調整や回転制御も可能です。

構成

マイコンにはIMU内蔵のATOMS3を使用します。
1セルのLiPoバッテリを使用してマイコンとモータに電源供給しています。
モータはモータドライバDRV8835で駆動

回路図

部品

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

ATOMS3
　スイッチサイエンス：https://www.switch-science.com/products/8670
　
モータドライバ DRV8835：3個
　秋月電子：https://akizukidenshi.com/catalog/g/g109848/
　
コアレスモータ 8520A：3個
　Amazon：https://amzn.to/4fciWWs
　
1セル 3.7V LiPoバッテリ
　Amazon：https://amzn.to/3NvHYDY
　
LiPoバッテリ用配線コネクタ　51006 molex オス
　Amazon：https://amzn.to/3A9mHNo
電源スイッチ SS12D00G3
　Amazon：https://amzn.to/4f6AS4s
逆起電力防止用ダイオード IN5819
　Amazon：https://amzn.to/3YuP41L
　
ATOMS3用ピンヘッダ (4ピン、5ピン)
　スイッチサイエンス：https://www.switch-science.com/catalog/92/
ATOMS3用4ピンコネクタケーブル
　マルツ：https://www.marutsu.co.jp/pc/i/829426/
　
モータ用 PHコネクタ：3セット
　秋月電子；
　　https://akizukidenshi.com/catalog/g/g112802/
　　https://akizukidenshi.com/catalog/g/g112795/
　　https://akizukidenshi.com/catalog/g/g112809/

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

M2×6mmネジ：1本　(ATOMと基板固定用)
M3×6mmネジ：4本　(基板固定用)
M3×5mmネジとナット：24セット　(フライホイール重り用)
M2×5mmネジ：6本　(フライホイールシャフト固定用)

専用基板

以下で専用基板を販売しております。
もちろんユニバーサル基板などでの自作も可能です。

[電子工作キット] SHISEIGYO-3 DC 専用基板

基板のサイズは以下のとおりで、筐体３Dモデルもこの基板サイズを元に製作しています。

3Dモデル

3Dモデルは以下よりダウンロードできます。zipファイルがダウンロードされます。
　筐体モデルデータ

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

筐体は2パーツ構成とシンプルにしました。

ネジを使用しないスナップフィットです。

ケースをスナップフィット pic.twitter.com/cjpYw9MANd

— HomeMadeGarbage (@H0meMadeGarbage) August 19, 2024

筐体組み立てキット

3Dプリント品も販売しております。
よろしければご検討ください。

[電子工作キット] SHISEIGYO-3 DC 筐体組み立てキット

製作

SHISEIGYO-3 DC を組み立てます。ここでは専用基板を使用した製法を記載します。

コントローラ作製

専用基板オモテ面に逆起電力防止用ダイオードと電源スイッチ、ATOMS3用ピンヘッダ、ATOMS3用4ピンコネクタケーブルをはんだ付けします。

4ピンコネクタケーブルはATOMS3のIO1とIO2への配線のために使用します。
残りの電源・GND配線は使用しません。
基板表記のIO1とIO2がATOMS3のGPIOに対応するように配線してください。

専用基板ウラ面にモータドライバDRV8835、LiPoバッテリ用配線コネクタ、モータ接続用コネクタをはんだ付けします。

DRV8835は付属のピンヘッダを介して実装します。ICの向きは基板表記に従ってください。
モータ接続用コネクタを使用せずにモータ配線を直接はんだ接続しても問題ありません。

基板にATOMS3をスペーサ (spacer.stl) を挟んでM2×6mmで固定します。

モータ実装

モータに接続用の2ピンPHコネクタを接続します。
配線が赤青モータの場合は赤:1pin/青2pin、配線が黒白の場合は黒:1pin/白2pinとします。

物によっては配線方向が異なる場合がありますので、後述するモータ回転方向確認プログラミングでご確認ください。

以下でコネクタ配線し動作確認済みのモータ3個セットの販売もしております。

[電子工作キット] SHISEIGYO-3 DC 用コアレスモータ

　
モータを筐体case1.stlにはめ込みます。緩い場合は瞬間接着剤点付けなどで固定。
モータをコネクタを使用せずにはんだ付けする場合にも先にモータを筐体に固定します。

コントローラ実装

筐体case1.stlのネジ穴をM3のタップでねじ切りして、モータコネクタを接続して基板を実装します。

　
モータコネクタは正面をCo、左をLo、右をRoに接続します。

バッテリ、フライホイール実装

LiPoバッテリをモータ固定部の底面に両面テープで固定し基板のコネクタと接続。

ホイールフレーム (wheel.stl) のモータシャフト固定部の穴２か所をM2のタップでねじ切りしておきます。

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

ホイールをモータシャフトにはめて軸のネジ穴２か所をM2×5mmネジで固定します。
モータシャフトに入らない場合はホイールの穴を1.2㎜ピンバイスで拡張してください。

バッテリなどに干渉しないか各ホイールのまわり具合をよく確認してください。

完成

筐体case2.stlをかぶせて組み立て完了です。

モータ回転確認コード

以下のコードを書きこんでモータの回転方向の確認を実施します。
モータ接続に不安がない場合はスキップしてください。

Arduinoやライブラリのバージョンは次節を確認ください。

#define Lch1 5
#define Lch2 4
#define Rch1 3
#define Rch2 2
#define Cch1 1
#define Cch2 0

#define Lin1 5
#define Lin2 6
#define Rin1 2
#define Rin2 1
#define Cin1 7
#define Cin2 8


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


  ledcSetup(Lch1, 20000, 10);
  ledcAttachPin(Lin1, Lch1);
  ledcSetup(Lch2, 20000, 10);
  ledcAttachPin(Lin2, Lch2);

  ledcSetup(Rch1, 20000, 10);
  ledcAttachPin(Rin1, Rch1);
  ledcSetup(Rch2, 20000, 10);
  ledcAttachPin(Rin2, Rch2);

  ledcSetup(Cch1, 20000, 10);
  ledcAttachPin(Cin1, Cch1);
  ledcSetup(Cch2, 20000, 10);
  ledcAttachPin(Cin2, Cch2);

  ledcWrite(Lch1, 1023);
  ledcWrite(Lch2, 1023);
  ledcWrite(Rch1, 1023);
  ledcWrite(Rch2, 1023);
  ledcWrite(Cch1, 1023);
  ledcWrite(Cch2, 1023);
}


void loop() {
  ledcWrite(Cch1, 1023);
  ledcWrite(Cch2, 1008);

  ledcWrite(Lch1, 1023);
  ledcWrite(Lch2, 1008);

  ledcWrite(Rch1, 1023);
  ledcWrite(Rch2, 1008);
}

#define Lch1 5

#define Lch2 4

#define Rch1 3

#define Rch2 2

#define Cch1 1

#define Cch2 0

#define Lin1 5

#define Lin2 6

#define Rin1 2

#define Rin2 1

#define Cin1 7

#define Cin2 8

void setup() {

Serial.begin(115200);

ledcSetup(Lch1, 20000, 10);

ledcAttachPin(Lin1, Lch1);

ledcSetup(Lch2, 20000, 10);

ledcAttachPin(Lin2, Lch2);

ledcSetup(Rch1, 20000, 10);

ledcAttachPin(Rin1, Rch1);

ledcSetup(Rch2, 20000, 10);

ledcAttachPin(Rin2, Rch2);

ledcSetup(Cch1, 20000, 10);

ledcAttachPin(Cin1, Cch1);

ledcSetup(Cch2, 20000, 10);

ledcAttachPin(Cin2, Cch2);

ledcWrite(Lch1, 1023);

ledcWrite(Lch2, 1023);

ledcWrite(Rch1, 1023);

ledcWrite(Rch2, 1023);

ledcWrite(Cch1, 1023);

ledcWrite(Cch2, 1023);

}

void loop() {

ledcWrite(Cch1, 1023);

ledcWrite(Cch2, 1008);

ledcWrite(Lch1, 1023);

ledcWrite(Lch2, 1008);

ledcWrite(Rch1, 1023);

ledcWrite(Rch2, 1008);

}

回転確認

全てのホイールが正面から見て時計回りに回っていたら問題ありません。

反転しているホイールがある場合は配線を逆にするか、コード(L. 8～13)の各モータドライバへの入力ピン(Lin, Rin Cin)の1と2を逆にしてください。

モーターライズ pic.twitter.com/G0pG2NwOCm

— HomeMadeGarbage (@H0meMadeGarbage) October 13, 2024

Arduinoコード

倒立動作用のコードです。以下よりダウンロードしてください。
ATOMS3のIMUライブラリの自由度が低いためM5StickのMPU6886ライブラリをATOMS3用にカスタマイズして使用しています。

コードダウンロードリンク (Zipファイル)

使用ライブラリ

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

・Arduino IDEバージョン：1.8.19
・ESP32ボードマネージャ：2.0.17
・M5GFXライブラリ：0.1.17
・Kalman Filter Library：1.0.2

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

コード

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

M5GFX lcd;

WebServer server(80);

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

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

#define Lch1 5
#define Lch2 4
#define Rch1 3
#define Rch2 2
#define Cch1 1
#define Cch2 0

#define Lin1 5
#define Lin2 6
#define Rin1 2
#define Rin2 1
#define Cin1 7
#define Cin2 8

MPU6886 IMU;

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

float omegaZ = 0.0, varW = 0.0;
int omZon = 0;

float Prot = 0.1;
float Kp = 50;
float Kd = 70;
float Kw = 2.0;
float IDRS = 2.5;
int delayTime = 0;

int pwmDutyC, pwmDutyL, pwmDutyR;
int GetUP = 0;
int GetUpCnt = 0;
float MtC, MtL, MtR, MtX, MtY, MpreX = 0.0, MpreY = 0.0;
float AjX = 0.0, AjY = 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;

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

float offsetX = 0.0, offsetY = 0.0;

Preferences preferences;


//加速度センサから傾きデータ取得 [deg]
void get_theta_acc() {
  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() {
  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>SHISEIGYO-3 DC</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-3 DC</h3>";

  //omegaZ
  temp += "回転<br>";
  temp += "<a class=\"pm\" href=\"/omegaZm\">-</a>";
  temp += "<span>" + String(omegaZ) + "</span>";
  temp += "<a class=\"pm\" href=\"/omegaZp\">+</a><br>";

  //Prot
  temp += "回転パラメータ<br>";
  temp += "<a class=\"pm\" href=\"/ProtM\">-</a>";
  temp += "<span>" + String(Prot) + "</span>";
  temp += "<a class=\"pm\" href=\"/ProtP\">+</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>";

  

  //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 omegaZm() {
  if (omegaZ >= -50) {
    omegaZ -= 5;
  }
  handleRoot();
}

void omegaZp() {
  if (omegaZ <= 50) {
    omegaZ += 5;
  }
  handleRoot();
}

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

void ProtP() {
  if (Prot <= 100) {
    Prot += 0.1;
    preferences.putFloat("Prot", Prot);
  }
  handleRoot();
}


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.1) {
    IDRS -= 0.1;
    preferences.putFloat("IDRS", IDRS);
  }
  handleRoot();
}

void IDRSp() {
  if (IDRS <= 10) {
    IDRS += 0.1;
    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 display(void *pvParameters) {
  lcd.begin();
  lcd.setRotation(2);
  lcd.setTextColor(0xFFFFFFU, 0x000000U); //１つ目の引数が文字色、２つ目の引数が背景色


  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("/omegaZm", omegaZm);
  server.on("/omegaZp", omegaZp);
  server.on("/ProtM", ProtM);
  server.on("/ProtP", ProtP);
  
  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();

    lcd.setTextFont(4);
    lcd.setCursor(0, 1);
    lcd.printf("%+05.1f", kalAngleX);
    //lcd.printf("%+07.5f", varW);
    lcd.setCursor(64, 1);
    lcd.printf("%+05.1f", kalAngleY);

    int xx = map(-kalAngleX, 20, -20, 25, 127);
    xx = constrain(xx, 25, 127);
    uint32_t colorX;

    if (fabs(kalAngleX) <= 1.0) {
      colorX = 0x00FF00U;
    } else if (fabs(kalAngleX) <= 15) {
      colorX = 0x0000FFU;
    } else {
      colorX = 0xFF0000U;
    }
    lcd.fillRect(0, xx - 2, 128, 5, colorX);


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

    /*
      USBSerial.print(rotEst);
      USBSerial.print(", ");
      USBSerial.println(rotSpeed);
    */

    disableCore0WDT();
  }
}



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

  IMU.Init();

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

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

  ledcSetup(Lch1, 20000, 10);
  ledcAttachPin(Lin1, Lch1);
  ledcSetup(Lch2, 20000, 10);
  ledcAttachPin(Lin2, Lch2);

  ledcSetup(Rch1, 20000, 10);
  ledcAttachPin(Rin1, Rch1);
  ledcSetup(Rch2, 20000, 10);
  ledcAttachPin(Rin2, Rch2);

  ledcSetup(Cch1, 20000, 10);
  ledcAttachPin(Cin1, Cch1);
  ledcSetup(Cch2, 20000, 10);
  ledcAttachPin(Cin2, Cch2);

  ledcWrite(Lch1, 1023);
  ledcWrite(Lch2, 1023);
  ledcWrite(Rch1, 1023);
  ledcWrite(Rch2, 1023);
  ledcWrite(Cch1, 1023);
  ledcWrite(Cch2, 1023);

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

  Prot = preferences.getFloat("Prot", Prot);
  
  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(
    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() {
  nowTime = micros();
  loopTime = nowTime - oldTime;
  oldTime = nowTime;

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


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


  if (fabs(kalAngleX) < 1 && fabs(kalAngleY) < 1 && GetUP == 0) {
    GetUP = 80;
  }


  if (GetUP == 80) {
    //ブレーキ
    if (fabs(kalAngleX) > 15.0 || fabs(kalAngleY) > 15.0) {
      ledcWrite(Lch1, 1023);
      ledcWrite(Lch2, 1023);
      ledcWrite(Rch1, 1023);
      ledcWrite(Rch2, 1023);
      ledcWrite(Cch1, 1023);
      ledcWrite(Cch2, 1023);
      AjX = 0.0;
      AjY = 0.0;
      varW = 0.0;
      GetUP = 0;
    }

    //モータ回転
    if (GetUP == 80) {
      AjY +=  IDRS * MpreY / 100.0;
      MtY = Kp * (kalAngleY + AjY) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * MpreY / 10.0;

      AjX +=  IDRS * MpreX / 100.0;
      MtX = Kp * (kalAngleX + AjX) / 90.0 + Kd * kalAngleDotX / 500.0 + Kw * MpreX / 10.0;
    }


    //回転速度追従ON
    if(omegaZ != 0.0){
      omZon = 1;
      varW = Prot / 100.0 * (omegaZ - gyroZ);
    }

    //回転速度追従OFF
    if(fabs(varW) < 0.000005 && omZon == 1 && fabs(MtR) < 0.001){
      omZon = 0;
      varW = 0.0;
    }


    MtC = constrain(MtY + varW , -1.0, 1.0);
    MpreY = MtC;
    pwmDutyC = 1023 * (1.0 - fabs(MtC));
    if (pwmDutyC < 300) pwmDutyC = 300;
    
    
    MtL = constrain(float(0.5 * MtY - 0.866 * MtX), -1.0, 1.0);
    pwmDutyL = 1023 * (1.0 - fabs(MtL));
    if (pwmDutyL < 300) pwmDutyL = 300;

    
    MtR = constrain(float(0.5 * MtY + 0.866 * MtX), -1.0, 1.0);
    MpreX = (MtR - MtL) * 0.5;
    pwmDutyR = 1023 * (1.0 - fabs(MtR));
    if (pwmDutyR < 300) pwmDutyR = 300;

    
    
    //回転方向
    if (MtC > 0.0) {
      ledcWrite(Cch1, pwmDutyC);
      ledcWrite(Cch2, 1023);
    } else {
      ledcWrite(Cch1, 1023);
      ledcWrite(Cch2, pwmDutyC);
    }
    
    if (MtL < 0.0) {
      ledcWrite(Lch1, pwmDutyL);
      ledcWrite(Lch2, 1023);
    } else {
      ledcWrite(Lch1, 1023);
      ledcWrite(Lch2, pwmDutyL);
    }

    if (MtR < 0.0) {
      ledcWrite(Rch1, pwmDutyR);
      ledcWrite(Rch2, 1023);
    } else {
      ledcWrite(Rch1, 1023);
      ledcWrite(Rch2, pwmDutyR);
    }
  } else if (GetUP == 0) {
    ledcWrite(Lch1, 1023);
    ledcWrite(Lch2, 1023);
    ledcWrite(Rch1, 1023);
    ledcWrite(Rch2, 1023);
    ledcWrite(Cch1, 1023);
    ledcWrite(Cch2, 1023);
  }

  delay(delayTime);
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

557

558

559

560

561

562

563

564

565

566

567

568

569

570

571

572

573

574

575

576

577

578

579

580

581

582

583

584

585

586

587

588

589

590

591

592

593

594

595

596

597

598

599

600

601

#include "MPU6886.h"

#include <Kalman.h>

#include <WiFi.h>

#include <WebServer.h>

#include <Preferences.h>

#include <M5GFX.h>

M5GFX lcd;

WebServer server(80);

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

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

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

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

#define Lch1 5

#define Lch2 4

#define Rch1 3

#define Rch2 2

#define Cch1 1

#define Cch2 0

#define Lin1 5

#define Lin2 6

#define Rin1 2

#define Rin2 1

#define Cin1 7

#define Cin2 8

MPU6886 IMU;

unsigned long oldTime = 0, loopTime, nowTime;

float dt;

float omegaZ = 0.0, varW = 0.0;

int omZon = 0;

float Prot = 0.1;

float Kp = 50;

float Kd = 70;

float Kw = 2.0;

float IDRS = 2.5;

int delayTime = 0;

int pwmDutyC, pwmDutyL, pwmDutyR;

int GetUP = 0;

int GetUpCnt = 0;

float MtC, MtL, MtR, MtX, MtY, MpreX = 0.0, MpreY = 0.0;

float AjX = 0.0, AjY = 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;

Kalman kalmanX, kalmanY;

float kalAngleX, kalAngleDotX, kalAngleY, kalAngleDotY;

float offsetX = 0.0, offsetY = 0.0;

Preferences preferences;

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

void get_theta_acc() {

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

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>SHISEIGYO-3 DC</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-3 DC</h3>";

//omegaZ

temp += "回転 ";

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

temp += "" + String(omegaZ) + "";

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

//Prot

temp += "回転パラメータ ";

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

temp += "" + String(Prot) + "";

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

//offsetX

temp += "offsetX ";

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

temp += "" + String(offsetX) + "";

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

//offsetY

temp += "offsetY ";

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

temp += "" + String(offsetY) + "";

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

//Kp

temp += "Kp ";

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

temp += "" + String(Kp) + "";

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

//Kd

temp += "Kd ";

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

temp += "" + String(Kd) + "";

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

//Kw

temp += "Kw ";

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

temp += "" + String(Kw) + "";

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

//IDRS

temp += "IDRS ";

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

temp += "" + String(IDRS) + "";

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

//delayTime

temp += "delayTime ";

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

temp += "" + String(delayTime) + "";

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

temp += "</div>";

temp += "</body>";

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

}

void omegaZm() {

if (omegaZ >= -50) {

omegaZ -= 5;

}

handleRoot();

}

void omegaZp() {

if (omegaZ <= 50) {

omegaZ += 5;

}

handleRoot();

}

void ProtM() {

if (Prot >= 0) {

Prot -= 0.1;

preferences.putFloat("Prot", Prot);

}

handleRoot();

}

void ProtP() {

if (Prot <= 100) {

Prot += 0.1;

preferences.putFloat("Prot", Prot);

}

handleRoot();

}

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.1) {

IDRS -= 0.1;

preferences.putFloat("IDRS", IDRS);

}

handleRoot();

}

void IDRSp() {

if (IDRS <= 10) {

IDRS += 0.1;

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 display(void *pvParameters) {

lcd.begin();

lcd.setRotation(2);

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

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

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

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

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

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

lcd.setTextFont(4);

lcd.setCursor(0, 1);

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

//lcd.printf("%+07.5f", varW);

lcd.setCursor(64, 1);

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

int xx = map(-kalAngleX, 20, -20, 25, 127);

xx = constrain(xx, 25, 127);

uint32_t colorX;

if (fabs(kalAngleX) <= 1.0) {

colorX = 0x00FF00U;

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

colorX = 0x0000FFU;

} else {

colorX = 0xFF0000U;

}

lcd.fillRect(0, xx - 2, 128, 5, colorX);

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

USBSerial.print(rotEst);

USBSerial.print(", ");

USBSerial.println(rotSpeed);

disableCore0WDT();

}

void setup() {

Serial.begin(115200);

IMU.Init();

//フルスケールレンジ

IMU.SetAccelFsr(IMU.AFS_2G);

IMU.SetGyroFsr(IMU.GFS_250DPS);

get_theta_acc();

kalmanX.setAngle(theta_X);

kalmanY.setAngle(theta_Y);

ledcSetup(Lch1, 20000, 10);

ledcAttachPin(Lin1, Lch1);

ledcSetup(Lch2, 20000, 10);

ledcAttachPin(Lin2, Lch2);

ledcSetup(Rch1, 20000, 10);

ledcAttachPin(Rin1, Rch1);

ledcSetup(Rch2, 20000, 10);

ledcAttachPin(Rin2, Rch2);

ledcSetup(Cch1, 20000, 10);

ledcAttachPin(Cin1, Cch1);

ledcSetup(Cch2, 20000, 10);

ledcAttachPin(Cin2, Cch2);

ledcWrite(Lch1, 1023);

ledcWrite(Lch2, 1023);

ledcWrite(Rch1, 1023);

ledcWrite(Rch2, 1023);

ledcWrite(Cch1, 1023);

ledcWrite(Cch2, 1023);

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

preferences.begin("parameter", false);

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

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(

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

nowTime = micros();

loopTime = nowTime - oldTime;

oldTime = nowTime;

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

get_theta_acc();

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

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

GetUP = 80;

}

if (GetUP == 80) {

//ブレーキ

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

ledcWrite(Lch1, 1023);

ledcWrite(Lch2, 1023);

ledcWrite(Rch1, 1023);

ledcWrite(Rch2, 1023);

ledcWrite(Cch1, 1023);

ledcWrite(Cch2, 1023);

AjX = 0.0;

AjY = 0.0;

varW = 0.0;

GetUP = 0;

}

//モータ回転

if (GetUP == 80) {

AjY += IDRS * MpreY / 100.0;

MtY = Kp * (kalAngleY + AjY) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * MpreY / 10.0;

AjX += IDRS * MpreX / 100.0;

MtX = Kp * (kalAngleX + AjX) / 90.0 + Kd * kalAngleDotX / 500.0 + Kw * MpreX / 10.0;

}

//回転速度追従ON

if(omegaZ != 0.0){

omZon = 1;

varW = Prot / 100.0 * (omegaZ - gyroZ);

}

//回転速度追従OFF

if(fabs(varW) < 0.000005 && omZon == 1 && fabs(MtR) < 0.001){

omZon = 0;

varW = 0.0;

}

MtC = constrain(MtY + varW , -1.0, 1.0);

MpreY = MtC;

pwmDutyC = 1023 * (1.0 - fabs(MtC));

if (pwmDutyC < 300) pwmDutyC = 300;

MtL = constrain(float(0.5 * MtY - 0.866 * MtX), -1.0, 1.0);

pwmDutyL = 1023 * (1.0 - fabs(MtL));

if (pwmDutyL < 300) pwmDutyL = 300;

MtR = constrain(float(0.5 * MtY + 0.866 * MtX), -1.0, 1.0);

MpreX = (MtR - MtL) * 0.5;

pwmDutyR = 1023 * (1.0 - fabs(MtR));

if (pwmDutyR < 300) pwmDutyR = 300;

//回転方向

if (MtC > 0.0) {

ledcWrite(Cch1, pwmDutyC);

ledcWrite(Cch2, 1023);

} else {

ledcWrite(Cch1, 1023);

ledcWrite(Cch2, pwmDutyC);

}

if (MtL < 0.0) {

ledcWrite(Lch1, pwmDutyL);

ledcWrite(Lch2, 1023);

} else {

ledcWrite(Lch1, 1023);

ledcWrite(Lch2, pwmDutyL);

}

if (MtR < 0.0) {

ledcWrite(Rch1, pwmDutyR);

ledcWrite(Rch2, 1023);

} else {

ledcWrite(Rch1, 1023);

ledcWrite(Rch2, pwmDutyR);

}

} else if (GetUP == 0) {

ledcWrite(Lch1, 1023);

ledcWrite(Lch2, 1023);

ledcWrite(Rch1, 1023);

ledcWrite(Rch2, 1023);

ledcWrite(Cch1, 1023);

ledcWrite(Cch2, 1023);

}

delay(delayTime);

}

コード説明

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

L. 18～30　PWM出力Ch、GPIO指定
　前節のモータ回転確認時にIOピンの反転がある際はここで実施ください。

L. 40～45　パラメータ値指定
　これらを調整して点倒立動作を実現させる。

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

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

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

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

　LCDにはのX軸とY軸の角度の値と以下の通りインジケータ表示しています。
　・±1°以内：緑表示
　・±15°以上：赤表示
　・その他：青表示

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

　　
　モータドライバへのPWM信号設定 (L. 437～457)
　・周波数： 20kHz
　・分解能： 10bit 0～1023
　

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

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

　倒立動作 (L. 506～600)
　　センサのX軸、Y軸が共に角度±1°以下になるとモータを起動します (L. 506～508)。

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

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

　倒立自転制御 (L. 536～546)
　　IMUセンサのZ軸の角速度との差でP制御してモジュールを自転させます。
　　パラメータ omegaZで回転速度 [deg/sec]を指定します。
　　P制御パラメータはProt(L. 40)で調整。
　　omegaZが0指定となりP制御値とモータ回転(ここでは右のモータの回転)が小さくなると自転回転動作は自動でOFFします。

　先ほど算出したモジュールのX, Y軸の必要トルクに基づいてドライバに印可するPWMのデューティ値を導出して各モータを回転させます (L. 549~591)。

　　真ん中のモータCはY軸上に配置しているのでY軸のトルク(MtY)で回転させます。
　　また自転用のオフセット(varW)も加算してモジュールを倒立自転させます (L. 549)。
　　モータの回転速度(MpreY)はセンサがないためここで算出したデューティ値で代用しています (L. 550）。
　　ここではデューティ値(0~1023)が小さいほどモータ回転が速くなり300いかにならないように制限しています (L. 552)

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

　　真ん中のモータと同様に速度制限をして(L. 557, 563)、モータ速度は左右のモータの平均MpreXをフィードバックしています (L. 561)。

　算出したモータのデューティで回転実行 (L. 567~590)。

　ループディレイ (L. 600)
　　delayTime (L. 45) で制御のループ時間を調整します。

アプリ画面

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

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

動作

スイッチをONして機体を優しく起き上げて点倒立させます。
アプリの回転で速度[deg/sec]を指定すると自転します。

SHISEIGYO-3 DC
スイッチ追加 pic.twitter.com/ExSxTnRnQb

— HomeMadeGarbage (@H0meMadeGarbage) October 20, 2024

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

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

— HomeMadeGarbage (@H0meMadeGarbage) October 21, 2024

センサ自体のオフセットが大きい場合には倒立ができない場合があります。
ATOMS3単体を平らなところに置いて起動し、LCDディスプレイに表示される角度が0に近いかご確認ください。

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

おわりに

ここではコアレスモータによる3軸姿勢制御モジュールの製法を紹介いたしました。

軽量で小型で安価な点倒立モジュールを提案できたかと思います。
是非製作してみてご感想をお聞かせください。

以下でATOMS3とLiPoバッテリ以外をまとめた準完成キットの販売もしております。ご検討ください。　

[電子工作キット] SHISEIGYO-3 DC 準完成キット

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

This site uses Akismet to reduce spam. Learn how your comment data is processed.

はじめに

構成

回路図

部品

専用基板

3Dモデル

筐体組み立てキット

製作

コントローラ作製

モータ実装

コントローラ実装

バッテリ、フライホイール実装

完成

モータ回転確認コード

回転確認

Arduinoコード

使用ライブラリ

コード

コード説明

アプリ画面

動作

関連記事

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

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