「電子工作」カテゴリーアーカイブ

3Dプリンタやマイコンを使ってロボットやリアクションホイール等、様々な電子工作品の製作過程をブログにしております。

DRV8876を採用してセンサレス姿勢制御モジュール ーリアクションホイールへの道55ー

前回は電流センサでモータ電流を検知して回転速度を推定し、磁気エンコーダなしでのセンサレス姿勢制御モジュールの倒立動作を実現しました。

SHISEIGYO-1 DC 完成!  ーリアクションホイールへの道47ー

 

ここでは電流センシング機能付きモータドライバを採用して更に部品点数の削減を試みました。

 

 

モータドライバ DRV8876

DRV8876は電流センシング機能付きのモータドライバです。
電流は以下のデータシートの図の通りドライバ下部のシャント抵抗で検知します。

DRV8876データシート

検知した電流をミラーしてIPROPIに流して外付け抵抗で電圧として測定できます。

 

ここではPololuのモジュールを購入しました。

Pololuのモジュールは電流センス用の抵抗やは逆起電力対策回路も内蔵で非常に使いやすかったです。

 

DRV8876による回転と電流検知

PololuのDRV8876モジュールで回転と電流検知を確認して観ます。

回転は以下のようにPhase / Enableモードで制御しました。

ENピンにPWM信号を入力し、PHピンで正転/反転を指示します。
CSピンの電圧で電流が検知できます。
 
 

ATOMS3をコントローラとして回転させました。
可変抵抗でENピンへのPWM信号のデューティ比を指定しています。
ボタンで正転/反転。

電流値も観測できました!
モータ電流の方向までは検知できないため、正転/反転信号で符号反転しました。

 

姿勢制御モジュール作製

DRV8876モジュールによる回転動作と電流センシングが確認できましたので、前回と同様にモータ電流を検知して回転速度を推定し、姿勢制御モジュールの倒立動作を目指します。

モータは普通のミニ四駆モータを使用します。
固定具を3Dプリントしました。

 

ATOMS3とDRV8876モジュールも配線。
モジュールに逆起電力対策回路や電源コンデンサもしっかり載ってるので非常に簡単に制御基板ができました。

 

動作

DRV8876によるセンサレス姿勢制御モジュールが完成しました!!
電流による推定回転速度でも力強い倒立が実現できております。

 

おわりに

これまではDCモータを用いた1軸 姿勢制御モジュールとしてSHISEIGYO-1 DCの製法レシピを販売しておりました。

SHISEIGYO-1 DC の製作レシピ

 
電流センサによって回転推定ができることがわかり磁気エンコーダが削減され、
モータもダブルシャフトではなく普通のモノが使用できるようになりました。
更にこの度モータドライバDRV8876を採用することで外付けの電流センサも必要がなくなり、部品点数の少ない1軸 DCモータ 姿勢制御モジュールが実現できました。

専用基板を設計して、より親しみやすいDCモータ 1軸 姿勢制御モジュールの製作レシピを完成できればと考えております。

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

先日は電流センサを用いて回転速度を推定するDCモータのセンサレス制御について学習しました。

SHISEIGYO-1 DC 完成!  ーリアクションホイールへの道47ー

 

ここではこのセンサレス回転速度制御を姿勢制御モジュールに応用してみます。

 

 

SHISEIGYO-1 DC 改造

1軸姿勢制御モジュール SHISEIGYO-1 DC をモータ電流をセンシングできるように改造して、回転速度を推定しての倒立を目指します。

SHISEIGYO-1 DC の製作レシピ

 

電流センサは以下を使用して、モータ配線間に挿入します。

[amazonjs asin=”B08QMCCYVZ” locale=”JP” title=”Hailege 2個セット ACS712 20A電流センサーモジュールACS712 20A電流検出範囲Arduino用”]

 
コントローラはセンサレス回転制御の検討時と同様にATOMS3を採用しました。

 

回転速度を推定するので磁気エンコーダは除去しました。
今回使用した基板はモータドライバ (DRV8835)を裏面に実装する古いものを使用しました。

 

回転速度推定

1軸姿勢制御モジュール SHISEIGYO-1 DC は以下のようにモジュールの姿勢角($θ_b$) とその角速度($\dotθ_b$) とモータの回転速度($\omega$) からモータに入力するべき電圧 (実際にはモータドライバに印可するPWM信号のデューティ比)を導出しています。
 $K_p$、$K_d$、$K_w$は調整パラデータ

$$V = K_p・θ_b+K_d・\dotθ_b+K_w・\omega (1)$$

ここではモータの回転速度($\omega$) を磁気エンコーダによるものから電流センサによる推定値に変更します。

モータは以下のようにモデル化できます。$E_m$は誘起電圧

上のモデルより回転速度($\omega$)は以下で導出できます。

$$\omega=\frac{V-(R+sL)I}{\Phi_m}\fallingdotseq\frac{V-RI}{\Phi_m}   (2)$$

インダクタンスの微分項は無視します。倒立時の応答はそれなりに早いので無視できない可能性ありますが簡単のために省略します!
Vは式(1)で算出された値、Iは電流センサで検出した値を用います。

モータの抵抗$R$と磁束密度$\Phi_m$は前回測定した値を使用します。

式(2)で得た推定回転速度を式(1)の$\omega$にフィードバックして姿勢制御回転をさせます。

参考文献

[amazonjs asin=”4789846342″ locale=”JP” title=”高トルク&高速応答! センサレス・モータ制御技術 (パワー・エレクトロニクス・シリーズ)”]

 

倒立動作

電流センサで回転速度を推定して倒立動作の検証を実施しました。

問題なく倒立動作が実現されました。
この変更に際して式(1)の$K_p$、$K_d$、$K_w$は再調整しました。

 

おわりに

ここでは電流を検知して回転速度を推定し、磁気エンコーダなしでの姿勢制御モジュールの倒立動作を実現しました。

センサレス姿勢制御モジュールの爆誕です!
(まぁ電流センサ使ってるんだけど。。)

DCモータはフライホイールと磁気エンコーダ用の円盤磁石を取り付けるためにダブルシャフトモータを使っておりました。

この度 磁気エンコーダの除去が実現されたので、普通のモータでも姿勢制御できそうです。
また、電流センシング機能付きモータドライバを採用することで更にコンパクトなシステムが実現できるかもしれません。

次の記事

DRV8876を採用してセンサレス姿勢制御モジュール ーリアクションホイールへの道55ー

DCモータのセンサレス回転速度制御

先日、現代制御理論の基礎を学習いたしました。

30日ではじめての現代制御理論に解きほぐされる俺 その3

[amazonjs asin=”4065301211″ locale=”JP” title=”はじめての現代制御理論 改訂第2版 (KS理工学専門書)”]

この学習を経て これまでの古典的な制御から徐々に脱却をはかりたいと思いまして、ここではDCモータの回転制御について考えます。

 

 

DCモータを考えるにあたり

以下を参考にDCモータの回転制御を考えます。

[amazonjs asin=”4789846342″ locale=”JP” title=”高トルク&高速応答! センサレス・モータ制御技術 (パワー・エレクトロニクス・シリーズ)”]

 
DCモータ回転システムはSHISEIGYO-1  DCをベースに構築します。

SHISEIGYO-1 DC 完成!  ーリアクションホイールへの道47ー

 

回転システム構築

まずはSHISEIGYO-1  DCの部品群をベースに回転システムを構築します。

電流センス

モータに流れる電流をはかるために電流センサも使用します。

[amazonjs asin=”B08QMCCYVZ” locale=”JP” title=”Hailege 2個セット ACS712 20A電流センサーモジュールACS712 20A電流検出範囲Arduino用”]

 

モータ配線間にセンサを挿入して電流値を確認しました。
若干ノイジー。。

可変抵抗でモータドライバ DRV8835に印可するPWMデューティ比を指定して回転速度を変えています。

回転センス

回転速度はSHISEIGYO-1  DCと同じ磁気エンコーダを使用して計測します。

エンコーダは1周 5パルス出力し、それを4逓倍して回転速度を算出しています。

 

まずは古典的にPI制御

基本的なモータ回転システムが完成しましたので、まずは古典的手法であるPI制御でモータ回転速度制御をやってみます。

可変抵抗で指定した回転速度とエンコーダで測定した回転速度を比較してPI制御しました。

いい感じに制御出来てるかと思います。

コントローラ変更

PI制御の際に制御ゲインを調整するためにWiFi通信を用いようとしたところ、ATOM Matrix (ESP32-pico)だとADC2ピンがWiFiと併用できことが発覚しました  (たびたびこのトラップにハマる。。)。
可変抵抗や電流センサでanalogReadピンが必要となりWiFi使用でI/Oピン不足になってしまいます。

そこでコントローラとしてATOMS3を採用することにしました。
ATOMS3にはESP32-S3が搭載されています。ESP32-S3のADC2がWiFiとの併用が可能であるかは不明ですが (いや調べろよ) ATOMS3ではADC2ピンは外に出ていないため安心です。

問題なくWiFiとの併用でき、ディスプレイ表示もできて便利です。

 

モータ定数測定

PI制御で十分なモータ回転制御はできましたが、ここではより現代的な制御を目指します。
参考書籍にならってDCモータを数理的にモデル化して制御するためにここではモータの定数を測定します。

抵抗 R

モータの抵抗はテスタで測定

R = 0.8 Ω

インダクタンス L

モータのインダクタンスは NanoVNA で測定

L = 107 uH

発電定数 (磁束密度) $\Phi_m$

同じモータを連結させて速度をはかりながら回転させて、他方のモータの電圧を測定

$\Phi_m$ = 0.135 mV/rpm

慣性モーメントJ

慣性モーメントを測定するためにまず電流制御器を構成して電流を指定してモータを回してみます。
ブロック図は以下の通り(詳細は参考書籍。本当に勉強になりました)。
電流制御器内のR, Lは先ほど測定したモータの値を使用します。

 
可変抵抗で電流値を指定して回転させてみた。

 

次に目標電流値を0から0.6Aのステップで指定して回転速度変化を測りました。

モータ起動の傾きから間接的に慣性モーメントJを換算します。

$\frac{\Phi_m}{J} = 2910$

 

センサあり回転速度制御

モータの定数がわかったので、先ほどの電流制御器によるフィードバックに速度制御器を追加してモータを回してみます。

ブロック図は以下の通り(繰り返しますが 詳細は参考書籍。本当に本当に勉強になりました)。
速度制御器内の$j / \Phi_m$は先ほど測定した値を使用します。

 

可変抵抗で回転速度を指定して回転させてみました。

なかなか良い追従です。
測定回転速度が1周20パルスのエンコーダによるもので荒いことと、電流センサのノイズケアをしていないので心配しておりましたが、多重フィードバックによる制御が実現できました。

 

センサレス回転速度制御

センサありの時は測定回転速度をフィードバックして指定回転速度と比較しましたが、ここでは回転速度を推定するセンサレス制御を目指します。

推定回転速度はモータの電気回路モデルから以下で得られます(何度も繰り返しますが 詳細は参考書籍。本当に本当に本当に勉強になりました)。

$$\omega_{est} = \frac{V + \Delta V – R I_{meas}}{\Phi_m}$$

ここでVは電流制御器で算出された電圧で、$\Delta V $は電圧のオフセット値で実測とのずれをこの値で調整しました。

まずは上の式で得られる推定回転速度を実システムで観てみました。
まだ回転フィードバックはエンコーダによる測定値を使用しています。

推定回転速度は少し応答性が良くないですが測定回転速度とよく一致しています。
絶対値が一致するように上式の$\Delta V $を調整しました。

 

いよいよ推定回転速度をフィードバックして指定回転速度と比較するセンサレス制御を実施します。

センサレスでもよく指定回転速度に追従し、推定回転速度は測定回転速度とも一致しています。
ついにDCモータのセンサレス回転速度制御が実現できました。

Arduinoコード

コードのコア部のみ掲載しておきます。

void loop() {
  nowTime = micros();
  loopTime = nowTime - oldTime;
  oldTime = nowTime;
    
  Ts = (float)loopTime / 1000000.0; //sec

  //電流センサ https://amzn.to/3euX3Id VOUT=2.5+0.185*I
  Imeas = -(analogReadMilliVolts(ImeasPin) - offset) / 185.0; //[A]
    
  //回転推定 + 10msフィルタ
  rotEst += ((V + Voffset - R * fabs(Imeas)) / Phi - rotEst) * 0.01;
   

  //速度制御
  roterr = rotSpeedTarget - fabs(rotEst);

  float KpASR = WcASR * JpPhi;
  SiASR += 0.2 * KpASR * WcASR * Ts * roterr;
  SiASR = constrain(SiASR, 0.0, 1.0);
   
  I = SiASR + KpASR * roterr;
  I = constrain(Ipi, 0.0, 1.0);


  //電流制御
  Ierr = I - fabs(Imeas);

  SiACR += WcACR * R * Ts * Ierr;
  SiACR = constrain(SiACR, 0.0, 2.0);
   
  V = SiACR + WcACR * L * Ierr;
  V = constrain(Vpi, 0.0, 2.0);

  
  //モータ回転
  pwm = 1023 - (1023 * V / 5.0);
  pwm = constrain(pwm, 800, 1023);
}

 

演算周期が一定でないので毎度 制御サンプル周期Tsを測って制御器で使用しています (L. 2-6)。

電流センサ値は起動の冒頭 (0A)で値を測っておいてoffsetとして電流計測時に引いています (L. 9)。

推定回転速度は10msecのフィルタをかけています (参考書籍 を参考 L. 12)。

電流制御で導出された電圧Vをpwmに変換してモータドライバに印可します (L. 37, 38)。

 

おわりに

ここではDCモータの現代的制御をめざしてセンサレス回転制御について勉強しました。

モータの物理モデル、電気回路モデルを用いたフィードバック制御によってセンサレス回転制御できることを確認でき感動いたしました。

エンコーダを使用しないモータ回転速度制御ができるようなったことは大きな前進と言え、色々応用したいと考えております。

しかし改めてPI制御の手軽さも痛感しました。
モータの電流検知って結構大変だし、モータ定数を測るのも結構めんどくさいからなぁ。。。

次の記事

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

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

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

[bc url=”https://docs.m5stack.com/en/core/AtomS3″]

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

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

 

 

ATOMS3

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

 

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

 

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

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

 

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

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

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

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

 

ディスプレイ表示

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

[bc url=”https://github.com/m5stack/M5GFX”]

 

早速 表示実験

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); //1つ目の引数が文字色、2つ目の引数が背景色
  
  
  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のライブラリが成熟する前に製作したので、今度どこかのタイミングで最新ライブラリでコードをきれいにまとめたいなぁなんて思っています。

次の記事

SHISEIGYO-1 DC 完成!  ーリアクションホイールへの道47ー

正弦波駆動の応用 ーブラシレスモータ駆動への道11ー

HomeMadeGarbage Advent Calendar 2022 |16日目

前回はクローズドループ正弦波駆動によるブラシレスモータの制御に再挑戦しました。

クローズドループ正弦波駆動 その2 ーブラシレスモータ駆動への道6ー

 

ここでは正弦波駆動を応用して色々試しましたので報告します。

 

 

ブラシレスモータの回転位置を指定

前回と全く同じシステム構成で可変抵抗でモータの回転位置を指定しての制御を実施しました。

可変抵抗での指定角度と磁気エンコーダによる測定角度の誤差をPI制御して正弦波の波高値にして、位相は測定角度を電気角に変換して使用し制御しています。

ブラシレスモータをサーボ感覚で動かせるようになりました。

[amazonjs asin=”B089SYW5W3″ locale=”JP” title=”ブラシレスモーター ドローンブラシレスモーター 金属材質 2204-260KV 30cm以下/11.8インチ 低ノイズ 低消費電力 Goproカメラジンバル 屋外260KVブラシレスモーター RCドローンアクセサリー”]

 

可変抵抗を6軸 IMUセンサに変えて制御してみました。

前出の可変抵抗によるモータ回転位置指定をIMUの傾き角度に変えただけですが、何かができそうな感じがにわかにあふれ出しました。

[amazonjs asin=”B081RHK82T” locale=”JP” title=”KKHMF 2個 MPU-6050 6DOF GY-521 MPU6050 3軸ジャイロスコープ + 加速度センサーモジュール Arduino用”]

 

おかもちリベンジ

以前製作したおかもちを本システムをベースに作ってみたいと思います。

PI制御

磁気エンコーダを排除して、IMUによる傾き角度を使用して板が平行になるようにブラシレスモータを制御しました。

IMUセンサは板の下側に設置して、板が平行になる(0°)ようにIMUの角度をPI制御した値を正弦波の位相としました。
正弦波の位相は一定 (約1V)としました。

なかなか良い動きを実現できています。

PID制御

さらにIMUによる角速度も使用してPID制御にしてみました。振幅は一定。

制御がよりヌルヌルして外乱に迅速に反応できるようになりました。

PID制御 振幅可変

次に正弦波駆動の位相のPID制御に加えて振幅をIMUによる傾きのP制御で導出するようにしました。

傾きが大きくなるほど振幅が大きくなってモータへの電流が増えます。

動画ではわかりにくいですが、振幅可変によって応答性が少し改善し板の平行保持力が向上しました。

板の保持力は振幅を常に最大(7.4/2 V)にすれば強くなりますが、大きな電流がずっと流れ続けてもったいないので振幅を傾きに応じて可変としました。

 

Arduinoコード

位相をPID制御で振幅をP制御で生成するおかもちのコードを以下に記します。

#include <Kalman.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Wire.h>


#define uHin 25
#define vHin 26
#define wHin 27

int uPWMCH = 0;
int vPWMCH = 1;
int wPWMCH = 2;

float twoPI = 2.0 * PI;
float onePI = PI;

float Vu, Vv, Vw;
float amp = 150.0, amp2, offset = 1.3;

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

uint16_t rotData, rotDataIni;

float thetaM = 0.0;
int i = 0;

float Kp = 60.0, Ki = 0.015, Kd = 0.05, Kpa = 40.0;
float diffI = 0.0;

MPU6050 mpu;

int16_t ax, ay, az;
int16_t gx, gy, gz;

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

float theta = 0.0;
float theta_dot = 0.0;

Kalman kalman;
float kalAngle, kalAngleDot;


//加速度センサから傾きデータ取得 [deg]
void get_theta() {
  mpu.getAcceleration(&ax, &ay, &az);
  accX = ax / 16384.0;
  accY = ay / 16384.0;
  accZ = az / 16384.0;
  
  //傾斜角導出 単位はdeg
  theta  = atan2(-1.0 * accY , -1.0 * accZ) * 180.0/PI;
}

//角速度取得
void get_gyro_data() {
  mpu.getRotation(&gx, &gy, &gz);
  gyroY = gy / 131.072;

  theta_dot = gyroX;
}

//Core0
void rotPosition(void *pvParameters) {
  Wire.begin();
  mpu.initialize();
  
  get_theta();
  kalman.setAngle(theta);
  
  for (;;){  
    nowTime = micros();
    loopTime = nowTime - oldTime;
    oldTime = nowTime;
    
    dt = (float)loopTime / 1000000.0; //sec

    get_theta();
    get_gyro_data();
      
    //カルマンフィルタ 姿勢 傾き
    kalAngle = kalman.getAngle(theta, theta_dot, dt);
    
    //カルマンフィルタ 姿勢 角速度
    kalAngleDot = kalman.getRate();
   

    Serial.print(amp2);
    Serial.print(", ");
    Serial.print(kalAngle);
    Serial.print(", ");
    Serial.println(thetaM);
    
    disableCore0WDT();
  }
}


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

  ledcSetup(uPWMCH, 20000, 10);
  ledcAttachPin(uHin, uPWMCH);
  ledcWrite(uPWMCH, 0);
  ledcSetup(vPWMCH, 20000, 10);
  ledcAttachPin(vHin, vPWMCH);
  ledcWrite(vPWMCH, 0);
  ledcSetup(wPWMCH, 20000, 10);
  ledcAttachPin(wHin, wPWMCH);
  ledcWrite(wPWMCH, 0);


  //core0
  xTaskCreatePinnedToCore(
    rotPosition
    ,  "rotPosition"   // 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() {
  diffI += kalAngle / 180.0;
  thetaM = Kp * kalAngle / 180.0 + Ki * diffI + Kd * kalAngleDot;

  amp2 = amp + Kpa *  fabs(kalAngle);
  amp2 = constrain(amp2, 0.0, 512);
  
  
  Vu = amp2 * sin(thetaM + offset);
  Vv = amp2 * sin(thetaM + offset + twoPI / 3.0);
  Vw = amp2 * sin(thetaM + offset + 2.0 * twoPI / 3.0);
      
  ledcWrite(uPWMCH, int(Vu) + 511);
  ledcWrite(vPWMCH, int(Vv) + 511);
  ledcWrite(wPWMCH, int(Vw) + 511);
}

 

疑似正弦波を生成するためにドライバ駆動ピン(IO25~27)はledcWriteを用いて20kHz PWM出力します。分解能は10ビット(0~1023) (L. 105-113)。

ESP32はデュアルコアで使用しcore0でIMUセンサMPU6050のカルマンフィルタを通した傾きと角速度をセンシングします (L. 67-99)。

core1で正弦波駆動でブラシレスモータを回します。
IMUの傾きと角速度を用いてPID制御で正弦波の位相thetaMを導出します (L. 128, 129)。
IMUの傾きからP制御で正弦波の振幅amp2を導出します (L. 131, 132)。

導出された正弦波をPWM変換してドライバを駆動します (L. 139-141)。

動作

改めて位相をPID制御で振幅をP制御で生成するおかもち動作。

配線が邪魔なので改めてコントローラを選定しドライバも小型化して おかもちを完成させたいと思います。

使用したモータのトルクが大きくないので 、重いものを載せれるようにギアの勉強もしたほうが良いかもしれません。

 

クローズドループ正弦波駆動 リベンジ ーブラシレスモータ駆動への道10ー

HomeMadeGarbage Advent Calendar 2022 |14日目

前回は “50日後にセンサレスベクトル制御してみたい俺” の時に実施したESP32を用いたブラシレスモータのセンサありベクトル制御について報告しました。

クローズドループ正弦波駆動 リベンジ ーブラシレスモータ駆動への道10ー

ここではクローズドループ正弦波駆動に再挑戦しましたので報告します。

 

 

ドライバ修理

50日後にセンサレスベクトル制御してみたい俺” の際にドライバを破壊してしまいましたので まずは修理しました。

 

やはりプリドライバが壊れており、部品交換で無事に治りました。

 

クローズドループ正弦波駆動

磁気エンコーダによってモータの回転位置を検知するクローズドループ正弦波駆動には以前挑戦しておりました。
しかしエンコーダによる回転位置を直接フィードバックしての回転がどうしてもうまくいかず若干変則的な方法で回転を実現させました。

クローズドループ正弦波駆動 その2 ーブラシレスモータ駆動への道6ー

エンコーダ検知遅延

50日後にセンサレスベクトル制御してみたい俺” の際に磁気エンコーダの値検知動作に50msecのディレイがあることに気づきました。

以前うまくいかなかったのはこのディレイのせいである可能性が高いので、今回はこの遅延をなくして実験を進めます。

構成

構成は以前と同様です。
ここでは可変抵抗は使用せず、BlynkレガシーアプリでBLEを介してスマホで変数調整しました。

 

  • ESP32 評価基板
    [bc url=”https://akizukidenshi.com/catalog/g/gM-13628/”]
  • プリドライバIC  IR2302
    [bc url=”https://akizukidenshi.com/catalog/g/gI-15656/”]
  • NMOS 2SK4017
    [bc url=”https://akizukidenshi.com/catalog/g/gI-07597/”]
  • ブートストラップ用ダイオード 1N4148W
    [bc url=”https://akizukidenshi.com/catalog/g/gI-07084/”]
  • ブラシレスモータ
    [amazonjs asin=”B089SYW5W3″ locale=”JP” title=”ブラシレスモーター ドローンブラシレスモーター 金属材質 2204-260KV 30cm以下/11.8インチ 低ノイズ 低消費電力 Goproカメラジンバル 屋外260KVブラシレスモーター RCドローンアクセサリー”]
  • 磁気エンコーダ   AS5048
    [bc url=”https://www.switch-science.com/catalog/3140/”]

システム概要

磁気エンコーダでロータの回転位置と回転速度を検知して、指定の回転速度との差分で正弦波電圧値をPI制御で算出します。
また正弦波の位相は検知したロータ位置を電気角にして使用します。

 

Arduinoコード

#define BLYNK_PRINT Serial
#define BLYNK_USE_DIRECT_CONNECT

#include <BlynkSimpleEsp32_BLE.h>
#include <BLEDevice.h>
#include <BLEServer.h>

#include <AS5048A.h>


char auth[] = "トークン";

AS5048A angleSensor(SS, false);

#define ampPin 39

#define uHin 25
#define vHin 26
#define wHin 27

int uPWMCH = 0;
int vPWMCH = 1;
int wPWMCH = 2;

float twoPI = 2.0 * PI;
float onePI = PI;

float Vu, Vv, Vw, Vmin, Vmax, Vzero;
int amp = 100;
float Vdd = 7.4; //電源電圧[V]

unsigned long oldTime = 0, nowTime, loopTime;
unsigned long measTime = 0;
float rotSpeed;

int State = 0;
uint16_t rotData, rotDataIni;
int rotDataCal, rotDataCalOld, rotDiff;

int BottunState = 0;
float u, v, w;
float uOffset = 0, vOffset = 0;


int IniOK = 0;

float Kpr = 5.0, Kir = 0.5;
float diffPr, diffIr = 0.0;
float rotSpeedTarget = 80; //指定回転速度[rad/s]
 
float Div = 1024.0;

float theta = 0.0;

//Core0
void rotPosition(void *pvParameters) {
  for (;;){
    Blynk.run();
    
    rotData = angleSensor.getRawRotation() >> 4;
 
    if(IniOK == 1){
      delay(200);
      rotDataIni = rotData;
      IniOK = 2;
    }

    if(IniOK == 2){
      nowTime = micros();
    
      rotDataCal = int(rotData - rotDataIni);
      if(rotDataCal < 0) rotDataCal += Div;

      if(BottunState){
        rotDiff = rotDataCalOld - rotDataCal;
      }else{
        rotDiff = rotDataCal - rotDataCalOld;
      }
      
      if(rotDiff < 0){
        rotDiff += Div;
      }

      if(rotDiff > 10){
        rotDataCalOld = rotDataCal;
        loopTime = nowTime - oldTime;
        oldTime = nowTime;
        rotSpeed = rotDiff / Div * twoPI / (loopTime / 1000000.0); //[rad/s]
        rotSpeed = constrain(rotSpeed, 0, 200);
      }

      diffPr = rotSpeedTarget - rotSpeed;
      diffIr += diffPr;
      amp = Kpr / 185.0 * diffPr + Kir / 185.0 * diffIr;
      amp = constrain(amp, 0, 511);


      if(nowTime - measTime > 50000){
        Serial.print(rotSpeedTarget);
        Serial.print(", ");
        Serial.print(rotSpeed);
        
        Serial.println("");
        measTime = nowTime;
      }
    }

    disableCore0WDT();
  }
}



//ヴァーチャルピンデータ受信
BLYNK_WRITE(V0) {
  BottunState = param.asInt();
}

BLYNK_WRITE(V7) {
  amp = param.asInt();
}

BLYNK_WRITE(V9) {
  Kpr = param.asFloat();
}

BLYNK_WRITE(V10) {
  Kir = param.asFloat();
}

BLYNK_WRITE(V3) {
  rotSpeedTarget = param.asFloat();
}

BLYNK_WRITE(V5) {
  diffIr = 0;
  IniOK = 0;
}


void setup() {
  Serial.begin(2000000);
  angleSensor.begin();

  Blynk.setDeviceName("motor");
  Blynk.begin(auth);

  ledcSetup(uPWMCH, 20000, 10);
  ledcAttachPin(uHin, uPWMCH);
  ledcWrite(uPWMCH, 0);
  ledcSetup(vPWMCH, 20000, 10);
  ledcAttachPin(vHin, vPWMCH);
  ledcWrite(vPWMCH, 0);
  ledcSetup(wPWMCH, 20000, 10);
  ledcAttachPin(wHin, wPWMCH);
  ledcWrite(wPWMCH, 0);


  //回転位置検出 タスク
  xTaskCreatePinnedToCore(
    rotPosition
    ,  "rotPosition"   // A name just for humans
    ,  4096  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  3  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  NULL 
    ,  0);
}

void loop() {
  //初期回転
  if(IniOK == 0){
    for(int i = 0; i < 512; i++){
      u = i;
      if(BottunState){
        v = i + 512.0 * 2.0 / 3.0; 
        w  = i + 512.0 / 3.0; 
      }else{
        w = i + 512.0 * 2.0 / 3.0; 
        v  = i + 512.0 / 3.0;
      }
        
    
      if(u >= 512) u -= 512;
      if(v >= 512) v -= 512;
      if(w >= 512) w -= 512;
        
      Vu = 250 *(1.0 + sin(twoPI / (512.0 / 7.0) * u)) / 2.0;
      Vv = 250 *(1.0 + sin(twoPI / (512.0 / 7.0) * v)) / 2.0;
      Vw = 250 *(1.0 + sin(twoPI / (512.0 / 7.0) * w)) / 2.0;
        
      ledcWrite(uPWMCH, int(Vu));
      ledcWrite(vPWMCH, int(Vv));
      ledcWrite(wPWMCH, int(Vw));
      delayMicroseconds(2000);
    }
  
    IniOK = 1;
  }
  
  if(IniOK == 2){
    //ロータ位置 (電気角) [rad]
    theta = rotDataCal / Div * 7.0 * twoPI;
    
    if(BottunState){
      Vu = amp *(1.0 - cos(theta));
      Vv = amp *(1.0 - cos(theta + twoPI / 3.0));
      Vw = amp *(1.0 - cos(theta + 2.0 * twoPI / 3.0));
    }else{
      Vu = amp *(1.0 + cos(theta));
      Vv = amp *(1.0 + cos(theta + twoPI / 3.0));
      Vw = amp *(1.0 + cos(theta + 2.0 * twoPI / 3.0));
    }
    
    //PWM出力
    ledcWrite(uPWMCH, int(Vu));
    ledcWrite(vPWMCH, int(Vv));
    ledcWrite(wPWMCH, int(Vw));
  }
}

 

最新版のBlynkではBLEが使用できないので、以前のバージョンのBlynkレガシーを使用してBLE通信しています。

疑似正弦波を生成するためにドライバ駆動ピン(IO25~27)はledcWriteを用いて20kHz PWM出力します。分解能は10ビット(0~1023) (L. 148-156)。

起動時に1周期分オープンループで回転させてロータ位置のオフセットを導出します (L. 172-199)。

ロータ位置のオフセット導出後はクローズドループで回転し続けます 。

オフセットを加味したモータのロータ位置rotData をデュアルコアのcore0で検出しています。
1周の分解能を14bitから10bit (1024)にしています (L. 60)。
モータ回転速度 rotSpeed を検知して、指定の回転速度 rotSpeedTarget との差分をPI制御して正弦波波高値  amp を算出します (L. 69-95)。

core1でロータ位置からモータの電気角 theta [rad] を算出。使用しているブラシレスモータのロータの極数は14 [7ペア]  (L. 203)。
BottunStateによって正転/逆転を判断してcore0で算出された波高値  ampで正弦波を生成します (L. 205-213)。

導出された正弦波をPWM変換してドライバを駆動します (L. 215-218)。

Blynkアプリで、モータ回転方向BottunStateと回転速度rotSpeedTarget 、PI制御の係数をBLEで受信します (L. 114-138) 。

 

動作

無事に正転反転動作もでき、PI制御の係数を調整して指定の回転速度への制動のとれた追従動作も確認できました。

 

おわりに

ここでは “50日後にセンサレスベクトル制御してみたい俺” によって気づいた磁気エンコーダの遅延を改善してクローズドループ正弦波駆動に再挑戦しました。

無事に回転動作を確認でき、リベンジすることができました。

今回の実験で正弦波駆動でブラシレスモータをかなり自由に動かせることがわかりました。
しばらくはクローズドループ正弦波駆動で色々楽しみたいと考えております。

次の記事

正弦波駆動の応用 ーブラシレスモータ駆動への道11ー

 

ESP32でベクトル制御 ーブラシレスモータ駆動への道9ー

HomeMadeGarbage Advent Calendar 2022 |12日目

50日後にセンサレスベクトル制御してみたい俺” と銘打って毎日少しずつモータの勉強を進めていたことを先日ブログにて報告させていただきました。

50日後にセンサレスベクトル制御してみたい俺 (前編)

ESP32でベクトル制御 ーブラシレスモータ駆動への道9ー

残念ながらセンサレスベクトル制御は達成できませんでしたが、磁気エンコーダで回転をセンシングしたベクトル制御による回転は実現できましたので報告させていただきます。

 

 

システム構成

回路構成は以下の通りです。

クローズドループ正弦波駆動の時の構成に電流センサを追加したのみです。
変数調整用にと可変抵抗を繋げていますがここでは使用せず、BlynkレガシーアプリでBLEで変数調整しました。

部品

  • ESP32 評価ボード
    [bc url=”https://akizukidenshi.com/catalog/g/gM-13628/”]
  • プリドライバ IR2302
    [bc url=”https://akizukidenshi.com/catalog/g/gI-15656/”]
  • ドライバ NMOS 2SK4017
    [bc url=”https://akizukidenshi.com/catalog/g/gI-07597/”]
  • ブラシレスモータ ポール数:14
    [amazonjs asin=”B089SYW5W3″ locale=”JP” title=”ブラシレスモーター ドローンブラシレスモーター 金属材質 2204-260KV 30cm以下/11.8インチ 低ノイズ 低消費電力 Goproカメラジンバル 屋外260KVブラシレスモーター RCドローンアクセサリー”]
  • 磁気エンコーダ AS5048
    [bc url=”https://www.switch-science.com/products/3140#erid10748703″]
  • 電流センサ ACS712
    [amazonjs asin=”B08QMVSK4T” locale=”JP” title=”Hailege 2個セット 入り5A ACS712電流センサーモジュールACS712 5A電流検出範囲Arduino用”]

 

センサありベクトル制御

システムの概要図は以下の通りです。

回転速度を指定してモータを制御します。
磁気エンコーダで回転速度とロータ位置を観測して、3相-2相変換や逆park変換時に使用します。

各PI制御のパラメータをBLEを介してBlynkレガシーアプリで調整して回転速度の応答性を改善しました。

 

コントローラのESP32はデュアルコアで使用し、以下のようにセンシングと回転制御を分担しました。

  • core0:電流センシング、磁気エンコーダからロータ位置と回転速度検出とPI制御、シリアル出力
  • core1:3相-2相変換、逆Park-Clarke変換、3次調波加算、PWM出力

 

 

動作

指定した回転速度に対して実速度がいい感じで追従しています。
負荷をかけて復帰する際にオーバーシュートが観られますのでパラメータはもう少し改善の余地があるかもしれません。

Arduinoコード

#define BLYNK_PRINT Serial
#define BLYNK_USE_DIRECT_CONNECT

#include <BlynkSimpleEsp32_BLE.h>
#include <BLEDevice.h>
#include <BLEServer.h>

#include <AS5048A.h>


char auth[] = "トークン";

AS5048A angleSensor(SS, false);

#define periodPin 36
#define ampPin 39

#define uHin 25
#define vHin 26
#define wHin 27

#define uPin 34
#define vPin 35

int uPWMCH = 0;
int vPWMCH = 1;
int wPWMCH = 2;


float Vu, Vv, Vw, Vmin, Vmax, Vzero;
int period = 2000, amp = 350;
float Vdd = 7.4; //電源電圧[V]

unsigned long oldTime = 0, nowTime, loopTime;
unsigned long measTime = 0;
float rotSpeed;

int State = 0;
uint16_t rotData, rotDataIni;
int rotDataCal, rotDataCalOld, rotDiff;
int angleCal;

int BottunState = 1;
float u, v, w;
float uOffset = 0, vOffset = 0;
float Iu, Iv, Iw, Ia, Ib, Id, Iq, Id2, Iq2;
float Iu_Meas, Iv_Meas, Iw_Meas;

int IniOK = 0;

float Kp = 0.05, Ki = 0.02;
float Kpr = 2.0, Kir = 0.002;
float diffPd, diffId = 0.0, diffPq, diffIq = 0.0, diffPr, diffIr = 0.0;
float IqTarget;
float rotSpeedTarget = 80; //指定回転速度[rad/s]
 
float Div = 512.0;

float Vd, Vq, Va, Vb;
float theta = 0.0;

//Core0
void rotPosition(void *pvParameters) {
  for (;;){
    Blynk.run();
    
    rotData = angleSensor.getRawRotation() >> 5;
 
    if(IniOK == 1){
      delay(200);
      rotDataIni = rotData;
      IniOK = 2;
    }

    if(IniOK == 2){
      nowTime = micros();

      //電流センサ https://amzn.to/3euX3Id VOUT=2.5+0.185*I
      Iu_Meas = (analogReadMilliVolts(uPin) - uOffset) / 185; //[A]
      Iv_Meas = (analogReadMilliVolts(vPin) - vOffset) / 185; //[A]
      Iw_Meas = -Iu_Meas - Iv_Meas;
    
      rotDataCal = int(rotData - rotDataIni);
      if(rotDataCal < 0) rotDataCal += Div;
  
      rotDiff = rotDataCal - rotDataCalOld;
      if(rotDiff < 0){
        rotDiff += Div;
      }

      if(rotDiff > 10){
        rotDataCalOld = rotDataCal;
        loopTime = nowTime - oldTime;
        oldTime = nowTime;
        rotSpeed = rotDiff / Div * 2.0 * PI / (loopTime / 1000000.0); //[rad/s]
      }

      diffPr = rotSpeedTarget - rotSpeed;
      diffIr += diffPr;
      IqTarget  = Kpr / 185.0 * diffPr + Kir / 185.0 * diffIr;
      IqTarget = constrain(IqTarget, 0, 1.2);


      if(nowTime - measTime > 50000){
        Serial.print(IqTarget);
        Serial.print(", ");
        Serial.print(Iq);
        Serial.print(", ");
        

        Serial.print(rotSpeedTarget);
        Serial.print(", ");
        Serial.print(rotSpeed);

        
        
        Serial.println("");
        measTime = nowTime;
      }


      /*
      Serial.print(Vu);
      Serial.print(", ");
      Serial.print(Vv);
      Serial.print(", ");
      Serial.print(Vw);
      Serial.print(", ");
      Serial.println("");
      */
    }

    disableCore0WDT();
  }
}



//ヴァーチャルピンデータ受信
BLYNK_WRITE(V0) {
  BottunState = param.asInt();
}

BLYNK_WRITE(V1) {
  period = param.asInt();
}

BLYNK_WRITE(V2) {
  Kp = param.asFloat();
}

BLYNK_WRITE(V4) {
  Ki = param.asFloat();
}

BLYNK_WRITE(V9) {
  Kpr = param.asFloat();
}

BLYNK_WRITE(V10) {
  Kir = param.asFloat();
}

BLYNK_WRITE(V3) {
  rotSpeedTarget = param.asFloat();
}

BLYNK_WRITE(V5) {
  diffId = 0;
  diffIq = 0;
  diffIr = 0;
}


void setup() {
  Serial.begin(2000000);
  angleSensor.begin();

  Blynk.setDeviceName("motor");
  Blynk.begin(auth);

  pinMode(uPin, ANALOG);
  pinMode(vPin, ANALOG);

  for(int i = 0; i < 100; i++){
    uOffset += analogReadMilliVolts(uPin);
    vOffset += analogReadMilliVolts(vPin);
    delay(10);
  }
  
  uOffset = uOffset / 100.0;
  vOffset = vOffset / 100.0;

  ledcSetup(uPWMCH, 20000, 10);
  ledcAttachPin(uHin, uPWMCH);
  ledcWrite(uPWMCH, 0);
  ledcSetup(vPWMCH, 20000, 10);
  ledcAttachPin(vHin, vPWMCH);
  ledcWrite(vPWMCH, 0);
  ledcSetup(wPWMCH, 20000, 10);
  ledcAttachPin(wHin, wPWMCH);
  ledcWrite(wPWMCH, 0);


  //回転位置検出 タスク
  xTaskCreatePinnedToCore(
    rotPosition
    ,  "rotPosition"   // A name just for humans
    ,  4096  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL
    ,  3  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  NULL 
    ,  0);

  for(int i = 0; i < 512; i++){    
    u = i;
    if(BottunState){
      v = i + 512.0 * 2.0 / 3.0; 
      w  = i + 512.0 / 3.0; 
    }else{
      w = i + 512.0 * 2.0 / 3.0; 
      v  = i + 512.0 / 3.0;
    }
      
  
    if(u >= 512) u -= 512;
    if(v >= 512) v -= 512;
    if(w >= 512) w -= 512;
      
    Vu = amp *(1.0 + sin(2.0 * PI / (512.0 / 7.0) * u)) / 2.0;
    Vv = amp *(1.0 + sin(2.0 * PI / (512.0 / 7.0) * v)) / 2.0;
    Vw = amp *(1.0 + sin(2.0 * PI / (512.0 / 7.0) * w)) / 2.0;
      
    ledcWrite(uPWMCH, int(Vu));
    ledcWrite(vPWMCH, int(Vv));
    ledcWrite(wPWMCH, int(Vw));
    delayMicroseconds(1500);
  }

  IniOK = 1;
}

void loop() {
  if(IniOK == 2){
    //モータ電流 [A]
    Iu = Iu_Meas;
    Iv = Iv_Meas;
    Iw = Iw_Meas;

    //ロータ位置 (電気角) [rad]
    theta = rotDataCal / Div * 7.0 * 2.0 * PI;

    //2相変換 [A]
    //https://jp.mathworks.com/help/sps/ref/parktransform.html [電力不変の a 相を d 軸に揃える場合は、ブロックは次のように実装します]
    Id = sqrt(2.0/3.0) * (Iu * cos(theta) + Iv * cos(theta - (2.0/3.0) * PI) + Iw * cos(theta + (2.0/3.0) * PI));
    Iq = -sqrt(2.0/3.0) * (Iu * sin(theta) + Iv * sin(theta - (2.0/3.0) * PI) + Iw * sin(theta + (2.0/3.0) * PI));

 
    diffPd =  Id;
    diffId += diffPd;
    Vd  = Kp * diffPd + Ki * diffId;

    diffPq =  Iq - IqTarget;
    //diffPq =  Iq - rotSpeedTarget / 1000.0;
    diffIq += diffPq;
    Vq  = Kp * diffPq + Ki * diffIq;

    /*
    Serial.print(Vd);
    Serial.print(", ");
    Serial.print(Vq);
    Serial.print(", ");
    */

    //逆Park変換 [V]
    Va = Vd * cos(theta) - Vq*sin(theta);
    Vb = Vd * sin(theta) + Vq*cos(theta);

    /*
    Serial.print(Va);
    Serial.print(", ");
    Serial.print(Vd);
    Serial.print(", ");
    */

    //逆Clarke変換 [V]
    Vu = sqrt(2.0/3.0) * Va;
    Vv = sqrt(2.0/3.0) * (Va * -0.5 + Vb * sqrt(3)/2.0);
    Vw = sqrt(2.0/3.0) * (Va * -0.5 - Vb * sqrt(3)/2.0);


    //3次調波加算
    if(Vu > Vv){
      if(Vu > Vw){
        Vmax = Vu;
        if(Vv > Vw){
          Vmin = Vw;
        }else{
          Vmin = Vv;
        }
      }else{
        Vmax = Vw;
        Vmin = Vv;
      }
    }else{
      if(Vv > Vw){
        Vmax = Vv;
        if(Vu > Vw){
          Vmin = Vw;
        }else{
          Vmin = Vu;
        }
      }else{
        Vmax = Vw;
        Vmin = Vu;
      }
    }
    Vzero = (Vmax + Vmin)*0.5; /* MaxとMinの平均値 [V]*/
    
    Vu = constrain(((Vu - Vzero) / Vdd) * 1024 + 512, 0, 1023);
    Vv = constrain(((Vv - Vzero) / Vdd) * 1024 + 512, 0, 1023);
    Vw = constrain(((Vw - Vzero) / Vdd) * 1024 + 512, 0, 1023);

    /*
    Serial.print(Vu);
    Serial.print(", ");
    Serial.print(Vv);
    Serial.print(", ");
    Serial.print(Vw);
    Serial.print(", ");
    Serial.println("");
    */
    
    ledcWrite(uPWMCH, int(Vu));
    ledcWrite(vPWMCH, int(Vv));
    ledcWrite(wPWMCH, int(Vw));
  }
}

 

最新版のBlynkではBLEが使用できないので、以前のバージョンのBlynkレガシーを使用してBLE通信しています。

疑似正弦波を生成するためにドライバ駆動ピン(IO25~27)はledcWriteを用いて20kHz PWM出力します。分解能は10ビット(0~1023) (L. 194-202)。

起動の冒頭で電流センサの電圧を測ってオフセット値を取得します (L. 182-192)。
またモータをオープンループ正弦波駆動で一周させてモータの初期位置を設定します (L. 215-238)。

モータの回転位置・回転速度検出と電流センシングをデュアルコアのcore0で実施しています。
1周の分解能を14bitから9bit (512)にしています (L. 67)。
電流センサの電圧値から電流を算出します (L. 78-81)。
PI制御で回転速度の誤差からIqのターゲット値を算出 (L. 98-101)。

電流センサとエンコーダ位相のオフセット導出後はベクトル制御で回転し続けます (L. 244-337)。
3相-2相変換、逆Park-Clarke変換、3次調波加算、PWM出力を実施します。

ロータ位置から電気角を算出(使用しているブラシレスモータのロータの極数は14 [7ペア]) (L. 251)。

Blynkアプリで、モータ回転速度rotSpeedTarget、PI制御の各種係数をBLEで受信します (L. 140-172) 。

参考

[amazonjs asin=”B07TRBSJYY” locale=”JP” title=”モータドライブノートVI:dsPICマイコンによるブラシレスモータの回転数制御”]

[amazonjs asin=”4789846342″ locale=”JP” title=”高トルク&高速応答! センサレス・モータ制御技術 (パワー・エレクトロニクス・シリーズ)”]

[amazonjs asin=”4789848019″ locale=”JP” title=”STマイコンで始めるブラシレス・モータ制御 (トライアルシリーズ)”]

[amazonjs asin=”4789841472″ locale=”JP” title=”ブラシレスDCモータのベクトル制御技術 (メカトロ・シリーズ)”]

おわりに

ここではESP32を用いたブラシレスモータのセンサありベクトル制御を楽しみました。

電流を管理して回転が制御できるようになったのは今後の人生において非常に有用であると考えます。

いつかセンサレスベクトル制御にも挑戦したいです。

次の記事

クローズドループ正弦波駆動 その2 ーブラシレスモータ駆動への道6ー

50日後にセンサレスベクトル制御してみたい俺 (後編)

HomeMadeGarbage Advent Calendar 2022 |8日目

前回からの続きです。

50日後にセンサレスベクトル制御してみたい俺 (前編)

ここからいよいよベクトル制御を目指していきます。

 

 

システム構成

回路構成は以下の通りです。

クローズドループ正弦波駆動の時の構成に電流センサを追加したのみです。
変数調整用にと可変抵抗を繋げていますがここでは使用せず、BlynkレガシーアプリでBLEで変数調整しました。

部品

  • ESP32 評価ボード
    [bc url=”https://akizukidenshi.com/catalog/g/gM-13628/”]
  • プリドライバ IR2302
    [bc url=”https://akizukidenshi.com/catalog/g/gI-15656/”]
  • ドライバ NMOS 2SK4017
    [bc url=”https://akizukidenshi.com/catalog/g/gI-07597/”]
  • ブラシレスモータ ポール数:14
    [amazonjs asin=”B089SYW5W3″ locale=”JP” title=”ブラシレスモーター ドローンブラシレスモーター 金属材質 2204-260KV 30cm以下/11.8インチ 低ノイズ 低消費電力 Goproカメラジンバル 屋外260KVブラシレスモーター RCドローンアクセサリー”]
  • 磁気エンコーダ AS5048
    [bc url=”https://www.switch-science.com/products/3140#erid10748703″]
  • 電流センサ ACS712
    [amazonjs asin=”B08QMVSK4T” locale=”JP” title=”Hailege 2個セット 入り5A ACS712電流センサーモジュールACS712 5A電流検出範囲Arduino用”]

 

磁気エンコーダでベクトル制御を目指す

いよいよベクトル制御 (センサあり)を目指していきます。

各種書籍も読みました。

[amazonjs asin=”4789848019″ locale=”JP” title=”STマイコンで始めるブラシレス・モータ制御 (トライアルシリーズ)”]

[amazonjs asin=”4789841472″ locale=”JP” title=”ブラシレスDCモータのベクトル制御技術 (メカトロ・シリーズ)”]

 

逆Park変換、逆Clarke変換で3相の印可電圧を算出します。
演算に用いた位相は磁気エンコーダによるものを使用しています。
なんとなく回りましたね。

 

コロナになっちまいました。。

 

ミニぷぱで遊んでサボっちまいました。。

 

電圧観測。

カクカクだったので別コア(core0)のシリアル出力で電圧観測

 

逆Park変換、逆Clarke変換で電圧は生成できてるようだが、
どうも高速が回転ができない。。。

[amazonjs asin=”B07TRBSJYY” locale=”JP” title=”モータドライブノートVI:dsPICマイコンによるブラシレスモータの回転数制御”]

低速では回転し、Iqを指定しての追従もできる。

電流センス値を加算平均でフィルタするなど試したが効果出ず。。高速回転ができない。。

 

おすすめしてもらった書籍購入。藁にもすがる思いで。。。

[amazonjs asin=”4789846342″ locale=”JP” title=”高トルク&高速応答! センサレス・モータ制御技術 (パワー・エレクトロニクス・シリーズ)”]

こちらの書籍では駆動電圧が全波の正弦波で説明がされておりました。
前出の2冊では半波の正弦波での駆動が紹介されておりました。
半波より全波の正弦波のほうが回転がなめらかであることは経験によって実感しておりました。

 

早速、書籍に従って逆Clarke変換で得た電圧を全波正弦波の振幅として印可しました。
回転はなめらかになりましたが、相変わらず高速回転ができません。。

とんでもないミスに気付く

高速回転が全くできないので、ここでの設計思想である制御のループ時間を管理せずに2コアでセンシングと回転制御をわけてガンガン回すという方針が間違いなのではと思いはじめループ時間を観てみました。
すると磁気エンコーダのセンシングに非常に時間がかかっていることが判明しました。

磁気エンコーダ AS5048のライブラリとして以下を使用していたのですが、ESP32使用時に50msecのディレイが挿入されておりました。。。(AS5048A.cpp L.367)
https://github.com/eborghi10/AS5048A

以前クローズドループ正弦波駆動をこのエンコーダを使用して実施した際にうまくいかなかったのも、この遅延のせいだったのか…!

センサありベクトル制御

エンコーダの50msecのディレイをなくして、回転させてみました。
高速回転が実現できました!しかし指定の回転速度追従は出来ずパラメータ調整が必要そうです。

 

Iqを指定してドライバに印可する電圧を観測してみました。
Iqを大きくすると振幅が大きくなりVddとGNDでサチっている様子が分かります。

モータにより大きな振幅の電圧を供給できるように3次調波加算 (HIP型変調)を施しました。
これによって相間に印可できる上限電圧は大きくなります。

 

ここでのセンサありベクトル制御のブロック図は以下の通りです。

上図の各PI制御のパラメータを調整して、指定した回転速度に対する応答性向上を目指しました。

なかなかよくなってきました。

更に応答を良くするために磁気エンコーダに加えて電流センシングもcore1からcore0に変えて以下のように完全分業にすることにしました。

  • core0:電流センシング、磁気エンコーダからロータ位置と回転速度検出とPI制御、シリアル出力
  • core1:3相-2相変換、逆Park-Clarke変換、3次調波加算、PWM出力

 

まだ改善の余地はありそうですが、遂にセンサありベクトル制御ができました!!

センサレスベクトル制御

なんとかセンサありのベクトル制御ができました。
遂にセンサレスを目指します。

センサレスベクトル制御の書籍群を紐解くとモータの位置の推定にはモータの定数が必要になるとのことでした。
ちょっとこの時点でテンションが落ち気味。。これってモータ変わるたびに特性把握が必要になるってことだよね。。

まぁしょうがないのでモータの特性を測ってみました。

  • コイル抵抗:5.6 Ω
    テスタで相間を測定

 

  • インダクタンス:0.76 mH
    nanoVNAで測定。相互インダクタンスは無視

 

  • 誘起電圧定数  Ke = 0.021 V/(rad/s)

以下を参考に計測から導出しました。
ブラシレスモータの逆起電力定数

 

モータ位置の推定方法としては誘起電圧オブザーバという手法を試してみました。
ベクトル制御では磁束方向の電流 (Id) をゼロにするように制御します。したがって回転位置を推定してその際にIdによって誘導される電圧があれば推定角度を修正するという方法です。

以下の書籍を参考に誘起電圧から推定するモータの回転位置の誤差$\Delta \theta$を計算します。

[amazonjs asin=”4789846342″ locale=”JP” title=”高トルク&高速応答! センサレス・モータ制御技術 (パワー・エレクトロニクス・シリーズ)”]

$$\Delta \theta = \frac{V_d – R I_d + \omega L I_q}{\omega  K_e}$$

モータの回転位置の誤差$\Delta \theta$を算出するために推定される回転速度$\omega$が用いられているかなり不安な手法です。。
また2相変換されたコイルの抵抗とインダクタンスをどのように扱っていいのか分からなかったため、測定値をそのまま使ってみました。

正弦波駆動で回してエンコーダのオフセットを導出してセンサありベクトル制御でモータを回転させてモータ位置と回転速度を引き継いでセンサレス動作に移行してみました。

計算した回転位置の誤差$\Delta \theta$をPI制御して推定回転速度$\omega$を算出して、$\omega$に測定したループ時間をかけて推定回転位置$\theta$を出してベクトル制御しているのですが。。
センサレスに移行したとたんに電流が設定した上限まで流れてしまいます。

 

METALLICAの新曲が発表されてそれどころじゃなかった。

 

センサレスベクトルでパラメータいじって何とかしようとしていたらプリドライバ壊れた。。

 

プリドライバ交換して検討進めるが上手くいかず。。

 

どうしてもうまくいかないので以下の書籍の手法を試してみました。

[amazonjs asin=”4789848019″ locale=”JP” title=”STマイコンで始めるブラシレス・モータ制御 (トライアルシリーズ)”]

モータの回転位置の誤差$\Delta \theta$を0になるようにPI制御して得られた誤差を、指定した回転速度に加えて推定回転速度を算出する手法でしたがうまくいきません。

 

次に以下の書籍の手法を試してみました。

[amazonjs asin=”4789852989″ locale=”JP” title=”ブラシレスDCモータのベクトル制御技術【オンデマンド版】 (メカトロ・シリーズ)”]

Idによって誘導される電圧Edを算出して、それをPI制御して推定回転速度$\omega$を導出する手法でしたが
試した途端 ぶっこわれました。。。。

またプリドライバが壊れたようですが在庫が尽きたため、私の50日間はあえなく終了いたしました。

おわりに

残念ながらセンサレスベクトル制御は実現できませんでした。
ダメな要因としては全部ということなのですが、以下にわかる範囲でまとめてみました。

  • コントローラの制御ループ時間可変では無理じゃないか?
    モータ推定位置算出に推定回転速度は入るので算出時にループ時間を観測では誤差が拡がる一方ではないか
  • モータの定数の不確かさ
    モータ定数の測定値が正しくない可能性に加えてq軸に変換した際に測定値がそのまま使えるのか疑問
  • モータの駆動方法に問題がありそう
    センサありからセンサレスにいきなり移行するのは問題ありそう
  • 他の推定方法も要検討必要

こんな感じで惨敗でした。

 

しかしセンサありでのベクトル制御はできたので一定の達成感はございます。
電流を管理して回転が制御できるようになったのは今後の人生において非常に有用であると考えます。

ESP32によるセンサありベクトル制御に関しましては別途ソースコードと合わせていつか報告させていただきます。

 

また磁気エンコーダの測定遅延時間が悪さをしており、ソコに気づくのに多大な時間を要してしまいました。
またクローズドループ正弦波制御からやり直したいと思います。

 

この50日間 結構サボったけどほぼ毎日モータのことを考え、ブラシレスモータと私の距離はかなり縮まったと思います。
以前なら理解できなかったであろうベクトル制御に関する資料や書籍も大体読めるようになりました。

とりあえず50日間は終了しましたが引き続き頑張りたいと思います。
エンジニアリングはマラソンです。

次の記事

ESP32でベクトル制御 ーブラシレスモータ駆動への道9ー

50日後にセンサレスベクトル制御してみたい俺 (前編)

HomeMadeGarbage Advent Calendar 2022 |6日目

これまで弊ブログにて ブラシレスモータ駆動への道 と題してブラシレスモータの回転制御に関する勉強をやってきました。

  • センサレス クローズドループ矩形波制御
  • オープンループ正弦波駆動
  • クローズドループ正弦波駆動

ベクトル制御も勉強したいなと思いつつ なんか難しそうだしと敬遠する自分がいたので “50日後にセンサレスベクトル制御してみたい俺” と銘打って毎日少しずつ勉強することにしました。

毎日少しでもモータのことを考えれば少しは理解も深まるだろうと思った次第です。
ここでは前後編に分けて私のこの50日間について記載します。

 

構成と思想

回路構成は以下の通りです。

クローズドループ正弦波駆動の時の構成に電流センサを追加したのみです。
変数調整用にと可変抵抗を繋げていますがここでは使用せず、BlynkレガシーアプリでBLEで変数調整しました。

部品

  • ESP32 評価ボード
    [bc url=”https://akizukidenshi.com/catalog/g/gM-13628/”]
  • プリドライバ IR2302
    [bc url=”https://akizukidenshi.com/catalog/g/gI-15656/”]
  • ドライバ NMOS 2SK4017
    [bc url=”https://akizukidenshi.com/catalog/g/gI-07597/”]
  • ブラシレスモータ ポール数:14
    [amazonjs asin=”B089SYW5W3″ locale=”JP” title=”ブラシレスモーター ドローンブラシレスモーター 金属材質 2204-260KV 30cm以下/11.8インチ 低ノイズ 低消費電力 Goproカメラジンバル 屋外260KVブラシレスモーター RCドローンアクセサリー”]
  • 磁気エンコーダ AS5048
    [bc url=”https://www.switch-science.com/products/3140#erid10748703″]
  • 電流センサ ACS712
    [amazonjs asin=”B08QMVSK4T” locale=”JP” title=”Hailege 2個セット 入り5A ACS712電流センサーモジュールACS712 5A電流検出範囲Arduino用”]

取り組むための思想

“50日後にセンサレスベクトル制御してみたい俺”に取り組む際に意識した点についてまとめます。

  • 専用評価キットや専用コントローラは使用しない
    これは原理を理解するために”ブラシレスモータ駆動への道”から決めている掟です。
    専用のものは高いですし、それを使って回しても私の場合は理解につながらないため。
  • 制御ループを一定で管理しない
    これはESP32を採用した弊害とも言えるかもしれないのですが、仮にモータを自由に制御できるようになったとしてコントローラをモータ専用で使用する機会は私の人生においてないであろうと考えこの思想に至りました。
    モータを回しつつ何かを実行することになるだろうし無線で何かしらしたいであろう。
    ここではESP32を用いて制御ループ時間は一定管理せず2コアで一方でセンシング、他方で駆動で実験を進めました。

3相->2相変換

モータ回転時の3相電流をセンシングして2相変換する手法を学びます。

 

電流は2個のセンサを用いて測定し、Iwは3相平衡が成立していると考えて Iu + Iv +Iw = 0 から導出しました。
またモータは正弦波駆動で回転させています。

 

Iα, Iβ算出 (Clarke変換)

 

Id, Iq算出 (Park変換)

 

電流の2相変換の理解のために色々な記事・書籍を参考にしました。

ここまでは特に以下が参考になりました。

 

正弦波駆動にて正弦波の周期を変えてId, Iqを観測


正弦波駆動にて正弦波の振幅を変えてId, Iqを観測


振幅を変えるとモータに流れる電流が変わりIqが大きく変化する様子が観測できました。

 

3相電流から直接 Id, Iq算出


以下を参考に直接Id, Iqを算出してClarke変換、Park変換して算出した値と比較しましたが違いはありませんでした。

[bc url=”https://jp.mathworks.com/help/sps/ref/parktransform.html”]

以後は3相電流から直接 Id, Iqを算出して使用します。

磁気エンコーダで回転検出

磁気エンコーダ AS5048を1周 (機械角) 0~512の値を出力するようにして観測しました。
モータは正弦波駆動で回しています。

 

モータを正弦波駆動で1周自由回転させて止まった地点のエンコーダ値をゼロとオフセットをかけるようにしました。

 

機械角から電気角を算出。14極 (7ペア)のモータを使用しており電気角は機械角の1/7となり値が荒くなってしまった。。

 

これまではESP32のコアを1つで実験してきました。
以下は2コア用いてcore0でエンコーダ検知とシリアル出力実施してcore1で電流 センシング&モータ正弦波駆動。
コア倍でエンコーダ検出及びシリアル出力処理が早くなった。


メガドライブミニ2をやらなくてはいけないのでサボったよね。(16日目も同様)

 

こちらの書籍も非常に参考になりました。

[amazonjs asin=”B07TRBSJYY” locale=”JP” title=”モータドライブノートVI:dsPICマイコンによるブラシレスモータの回転数制御”]

 

ベクトル制御にむけて準備

電流センスと磁気エンコーダによる回転検出が確認できましたので、更にベクトル制御にむけて調査を進めます。

 

Iqを指定して実測値と差分をとってPI制御によって正弦波駆動の振幅を制御してみた。

エンコーダ値から回転速度算出[rpm]。

ラジオ収録で忙しくてサボり

回転速度を指定して実測値と差分をとってPI制御によって正弦波駆動の周期を制御してみた。

 

正弦波駆動の位相をエンコーダによる電気角にして、Iq指定のPI制御で正弦波駆動の振幅を制御するがうまく回らず。。

しかし低速回転ではうまくいきました。

 

おわりに

ここまでで電流センスによる3相-2相変換と磁気エンコーダによる回転検出が確認できました。

更に低速ではありますが正弦波駆動の位相をエンコーダによる電気角にして、Iqを指定してPI制御で正弦波駆動の振幅を制御する方法でのモータ回転を確認できました。

徐々に機能と原理を確認しつつベクトル制御の準備を進め、実現に向けての機運も高まってまいりました。
しかし、この先には非常に険しい道が待っていたのでした。

後半につづく

バッテリレス超簡易監視カメラ

HomeMadeGarbage Advent Calendar 2022 |5日目

2022年 SPRESENSE 活用コンテストに参加するべくSPRESENSEを用いた超低消費電力の監視カメラを製作したので報告いたします。

 

概要

カメラの構成は以下の通りです。
バッテリレスでソーラ直接駆動による監視カメラを目指します。
撮影した画像はサーバにLTE携帯網でサーバに送信します。
サーバは自宅で立ち上げているラズパイ2サーバを使用しました。

開発の流れは以下の通りです。

  • SPRESENSE-LTEモジュール通信確立
  • カメラ込みでの低消費電力化
  • ソーラ選定
  • 長期安定動作確認

[amazonjs asin=”B07H2CG1HP” locale=”JP” title=”SONY SPRESENSE メインボード CXD5602PWBMAIN1″]

LTE-Mモジュール

通信モジュールはM5Stack UnitCatMを使用しようと考えていたのですが、、、

SPRESENSEとの通信を速くしたいと欲張り、モジュールのボーレートを2900000bpsと高速設定してしまい制御通信できなくしてしまいました。。。(´;ω;`)ウゥゥ

しょうがないのでUnitCatMと同じ通信モジュール SIM7080Gが搭載された評価ボードを使用することにしました。
制御コマンド用ボーレートはマイルドに230400bpsとしました。

通信動作

カメラモジュールを接続してサーバへのhttp送信を目指します。

[amazonjs asin=”B07MTNVG2X” locale=”JP” title=”SONY SPRESENSE カメラモジュール CXD5602PWBCAM1″]

SIM7080Gのhttp通信では大容量データは送れないので、ここではJPEG画像 (QVGA : 320×240)を4KBづつに分割してサーバに送信することにしました。

撮影画像をサーバに4KBづつHTTP PUT送信して最後にデータ送信終了信号をGET送信してサーバ上でデータをマージして画像保存します。

 

消費電力

画像撮影とサーバ送信動作が確認できたので消費電力を測定します。
カメラは撮影後に10分スリープして起床して撮影を繰り返すようにしました。

1時間当たり5回画像を送信して消費電力は116mWhと非常に低い結果となりました。
LTE-MモジュールもSPRESENSEの3.3V出力から給電しているのでスリープ時には停止します。

消費電流のピークはデータ送信時で入力電圧が10Vで約140mA、5.5Vで約220mAでした。

非常に低い消費電力での動作が確認できましたのでソーラ直接駆動が実現できそうです。

ソーラパネル選定

ソーラパネルとSPRESENSE電源入力の間に昇降圧DC-DCコンバータ SC8721を使用しました。ソーラパネルから供給される電圧を4Vに変換してSPRESENSEに給電します。
SC8721は入力電圧範囲が2.7V~22Vと非常に広く、幅広い種類のソーラパネルの採用が可能となります。

[bc url=”https://strawberry-linux.com/catalog/items?code=18721″]

カメラシステム自体の消費電力は非常に小さいのでデータ送信時のピーク電流さえまかなうことができれば小型のソーラパネルでも駆動可能のはずです。

実験に際してカメラは100均のプラケースに収めました (防水機能はゼロですww)。

DCDC出力に大きいコンデンサを接続してピーク電流をまかなおうとしましたが、あまり大きな効果はなく電解コンデンサ1000uFほどつけておけば十分な印象でした。

[amazonjs asin=”B00R72KZKE” locale=”JP” title=”電気二重層コンデンサー 5.5V 0.47F ±20% 1個入 <cap-203>”]

 

1W~5Wの色々なパネルを購入して実験してみました。


1W ソーラは標準電圧:5.5 V、標準電流:170 mAと若干能力が足りずSPRESENSEは動作しましたが
LTE-Mモジュールの通信までは駆動できませんでした。

[bc url=”https://www.switch-science.com/products/793/”]

 

2Wソーラでは問題なく画像送信動作を確認することができました。

[bc url=”https://akizukidenshi.com/catalog/g/gM-08919/”]

 

 

サーバに届いた画像

撮影動作

ベランダに置いて、長時間の撮影動作も確認いたしました。
お母ちゃんにサーバに届いた画像をブラウザで一覧で見れるようにしてもらいました。

 

我が家のベランダは東向きで午前中の短い時間でしか直射日光が当たらないのですが、日照時の撮影を確認することができました。

ソースコード

#include <LowPower.h>
#include <RTC.h>
#include <Camera.h>

int state = 100;
int retry = 5;
String reply;

unsigned long startTime;

CamImage img;
byte pic[100000];
int n;
int cnt = 0, last;

void setup() {
  LowPower.clockMode(CLOCK_MODE_32MHz);
  Serial.begin(115200); //シリアルモニタ用
  Serial2.begin(230400);

  Serial.println("Prepare camera");
  theCamera.begin();

  Serial.println("Set Auto white balance parameter");
  theCamera.setAutoWhiteBalanceMode(CAM_WHITE_BALANCE_DAYLIGHT);

  Serial.println("Set still picture format");
  theCamera.setStillPictureImageFormat(
   CAM_IMGSIZE_QVGA_H,
   CAM_IMGSIZE_QVGA_V,
   CAM_IMAGE_PIX_FMT_JPG);

  RTC.begin();
  LowPower.begin();

  pinMode(21, OUTPUT);
  digitalWrite(21, HIGH);

  Serial.println("start");
}

void loop() {
  int num = 0;
  Serial.println(state);

  //ATコマンド
  switch (state) {
    case 0: //モジュール-SPRESENSE間の通信確認
      Serial2.write("AT\r\n");
      break;
    case 2: //モジュール通信ON
      Serial2.write("AT+CNACT=0,1\r\n");
      break;
    case 30:     
      if(cnt == 0) Serial2.write("AT\r\n");
      else Serial2.write("AT+SHDISC\r\n");
      break;
    case 31: //HTTP設定 
      Serial2.write("AT+SHCONF=\"URL\",\"[サーバURL]\"\r\n");
      break;
    case 32:
      Serial2.write("AT+SHCONF=\"BODYLEN\",4096\r\n");
      break;
    case 33:
      Serial2.write("AT+SHCONF=\"HEADERLEN\",350\r\n");
      break;
    case 34:      
      Serial2.write("AT+SHCONN\r\n");
      break;
    case 4: //HTTP接続確認
      Serial2.write("AT+SHSTATE?\r\n");
      break;
    case 5:     
      Serial2.write("AT+SHAHEAD=\"Content-Type\",\"image/jpeg\"\r\n");
      break;
    case 6: //画像 4KB分セット
      if(n == cnt){
        Serial2.print("AT+SHBOD=");
        Serial2.print(last);
        Serial2.print(",10000\r\n");
      }else{
        Serial2.write("AT+SHBOD=4000,10000\r\n");
      }
      break;
    case 7: //画像データPUTコマンド送信
      Serial2.write("AT+SHREQ=\"/cam\", 3\r\n");

      break;
    case 80: //画像送信終了コマンド   
      if(cnt == 0) Serial2.write("AT\r\n");
      else Serial2.write("AT+SHDISC\r\n");
      break;
    case 81:     
      Serial2.write("AT+SHCONF=\"URL\",\"[サーバURL]\"\r\n");
      break;
    case 82:
      Serial2.write("AT+SHCONF=\"BODYLEN\",4096\r\n");
      break;
    case 83:
      Serial2.write("AT+SHCONF=\"HEADERLEN\",350\r\n");
      break;
    case 84:      
      Serial2.write("AT+SHCONN\r\n");
      break;
    case 9: //画像送信終了 GETコマンド送信
      Serial2.write("AT+SHREQ=\"/camend\", 1\r\n");
      break;
  }

  if(state == 100){ //LTEモジュール起動
    delay(500);
    digitalWrite(21, LOW);
    delay(500);
    digitalWrite(21, HIGH);
    startTime = millis();
    state = 0;
  }else if(state == 0){ //AT導通確認
    while(1){
      reply = "";
      if(Serial2.available()) {
        reply = Serial2.readStringUntil('\n');
      }
      
      num++;
      
      if(reply.indexOf("OK") >= 0) {
        Serial.println(reply);
        state = 1;
        break;
      }else if(num >= retry){
        break;
      }
      delay(300);
    }
  }else if(state == 1){
    Serial2.write("AT+CNACT=0,1\r\n"); //通信開始
    
    //カメラ撮影
    Serial.println("call takePicture()");
    img = theCamera.takePicture();

    Serial.print("ImgSize: "); Serial.println(img.getImgSize());
    n = img.getImgSize() / 4000;
    Serial.print("n: "); Serial.println(n + 1);
    last = img.getImgSize() - 4000 * n;
    Serial.print("last: "); Serial.println(last);
    memcpy(pic, img.getImgBuff(), img.getImgSize());
    
    state = 2;
  }else if(state == 2){   //通信開始確認
    while(1){
      reply = "";
      if(Serial2.available()) {
        reply = Serial2.readStringUntil('\n');
      }
      
      num++;
      
      if(reply.indexOf(",ACTIVE") >= 0) {
        Serial.println(reply);
        state = 30;
        break;
      }else if(num >= retry){
        break;
      }
      delay(300);
    }
  }else if(state == 30 || state == 31 || state == 32 || state == 33 || state == 34){ //HTTP設定
    while(1){
      reply = "";
      if(Serial2.available()) {
        reply = Serial2.readStringUntil('\n');
      }
      
      num++;
      
      if(reply.indexOf("OK") >= 0) {
        Serial.println(reply);
        if(state == 30) state = 31;
        else if(state == 31) state = 32;
        else if(state == 32) state = 33;
        else if(state == 33) state = 34;
        else if(state == 34) state = 4;
        break;
      }else if(num >= retry){
        break;
      }
      delay(300);
    }
  }else if(state == 4){ //HTTPステート確認
    while(1){
      reply = "";
      if(Serial2.available()) {
        reply = Serial2.readStringUntil('\n');
      }
      
      num++;
      
      if(reply.indexOf("SHSTATE: 1") >= 0) {
        Serial.println(reply);
        state = 5;
        break;
      }else if(num >= retry){
        break;
      }
      delay(300);
    }
  }else if(state == 5){ //HTTP body設定
    while(1){
      reply = "";
      if(Serial2.available()) {
        reply = Serial2.readStringUntil('\n');
      }
      
      num++;
      
      if(reply.indexOf("OK") >= 0) {
        Serial.println(reply);
        state = 6;
        break;
      }else if(num >= retry){
        break;
      }
      delay(300);
    }
  }else if(state == 6){ //画像送信
    if(n == cnt){
      for(int i = 4000 * cnt ; i <= img.getImgSize(); i++){
        Serial2.write(pic[i]);
      }
    }else{
      for(int i = 4000 * cnt ; i < (4000 + 4000 * cnt); i++){
        Serial2.write(pic[i]);
      }
    }
    delay(1000);
    state = 7;
  }else if(state == 7){ //HTTP POSTリクエスト
    while(1){
      reply = "";
      if(Serial2.available()) {
        reply = Serial2.readStringUntil('\n');
      }
      
      num++;
      
      if(reply.indexOf("SHREQ: \"POST\",200") >= 0) {
        Serial.println(reply);
        if(cnt < n) state = 30;
        else state = 80;
        cnt++;
        break;
      }else if(num >= retry * 3){
        break;
      }
      delay(300);
    }
  }else if(state == 80 || state == 81 || state == 82 || state == 83 || state == 84){ //終了信号用HTTP設定
    while(1){
      reply = "";
      if(Serial2.available()) {
        reply = Serial2.readStringUntil('\n');
      }
      
      num++;
      
      if(reply.indexOf("OK") >= 0) {
        Serial.println(reply);
        if(state == 80) state = 81;
        else if(state == 81) state = 82;
        else if(state == 82) state = 83;
        else if(state == 83) state = 84;
        else if(state == 84) state = 9;
        break;
      }else if(num >= retry){
        break;
      }
      delay(300);
    }
  }else if(state == 9){ //画像送信終了信号HTTP GETリクエスト
    while(1){
      reply = "";
      if(Serial2.available()) {
        reply = Serial2.readStringUntil('\n');
      }
      
      num++;
      
      if(reply.indexOf("SHREQ: \"GET\",200") >= 0) {
        Serial.println(reply);
        state = 10;
        cnt++;
        break;
      }else if(num >= retry){
        break;
      }
      delay(300);
    }
  }else if(state == 10){ //通信後スリープ
    state = 100;
    cnt = 0;

    //sleep
    Serial.println("sleep");
    LowPower.deepSleep(600); //10分スリープ
  }


  //エラー処理 3分リミット
  if(millis() - startTime > 300000){
    state = 10;
    Serial.println("time limit");
  }

  //delay(1000);
}

SPRESENSEのCPUクロックを32MHzにおとして消費電力を低下させています。

SPRESENSEからLTE-MモジュールにATコマンドを入力して画像データ送信制御を実施しています。
SPRESENSE-LTE-Mモジュール間のボーレートは230400bpsとしました。

SPRESENSEの大まかな動作は以下の通りです。

  • 起床後にLTE-Mモジュールを起動(モジュールのPWRピンにLOWパルス入力)
  • カメラ撮影
  • モジュールとのコマンド通信確立後にHTTP通信設定
  • 撮影画像をサーバに4KBづつHTTP PUT送信
  • 画像データ送信終了後にデータ送信終了信号をGET送信
  • 10分スリープ

LTE-Mモジュール起動後 3分経ってもデータ送信が終了していない場合はエラーとみなして強制的にスリープさせています。

サーバは画像データを4KBづつ受け取り、データ送信終了信号がきたらデータをまとめてJPEGファイルとして保存します。

おわりに

ここではSPRESENSEを用いてバッテリレス超簡易監視カメラを製作しました。
SPRESENSEの省エネ特性により太陽光発電のみで非常にコンパクトな監視カメラの実現ができました。

システム自体が軽量なのでどこにでも持ち込むことができ、設置も容易です。
日照があれば画像を送信し続けます。
山奥のアナログメータの監視や平地の少ない険しい土地の環境監視などに使えるのではないでしょうか。

本監視カメラは日照がある限り画像を送り続けるという発想で製作し、AIなどによる画像認識を行う際にはサーバ側での実施を想定しています。

しかし、SPRESENSE自体の性能も高く、専用の夜間撮影可能な専用高感度カメラも販売されておりますのでバッテリ搭載の本格的監視カメラへの応用も可能です。
エッジAIを用いて画像認識を現場で行う高機能動作もできるでしょう。
例えば野生動物を検知したり川の水位を画像から判断して緊急時には撮影頻度を上げるなど。

SPRESENSEの応用は多岐で大きな利益があると今回の製作を通じて実感できました。

3D Display “VoluMetron”

HomeMadeGarbage Advent Calendar 2022 |1日目

今年も M5Stack Japan Creativity Contest に参加するべく、3D ディスプレイを製作しましたので製作過程など詳細報告させていただきます。

[bc url=”https://protopedia.net/prototype/3191″]

制作に向けて

コンテスト参加に際しまして、以前製作した立体 バーサライタを大きくしてみようと思い立ちました。

この時は5セルのLEDを10段にして立体バーサライタを構成し、
コントローラごと回転させて回転部への給電にワイヤレスチャージモジュールを使用しておりました。

 

今回の製作直前には球体のバーサライタを作製しおり、回転部への給電とLED信号送信にスリップリングを採用しておりました。

今回の3D ディスプレイも球体バーサライタをベースにしてスリップリングによる給電と信号送信を実施することにしました。

3D ディスプレイ製作

今回は10セルのLEDを20セット使用して大きなディスプレイ作成を目指します。
多くのSPI入力LEDを制御することになるので100セルずつ2グループに分けて2つのSPI信号で制御することにしました。
そのためスリップリングは6線のものを使用しました。

構成

  • ATOM Lite
    [amazonjs asin=”B09MVHQ85B” locale=”JP” title=”ATOM Lite”]
  • SPI入力 LED
    [amazonjs asin=”B07VGH4XSQ” locale=”JP” title=”APA102 5050 SMD高輝度チップLEDピクセルフレキシブルストリップライトDC 5V (白 PCB, 1M 144leds IP20)”]
  • ブラシレスモータ
    [amazonjs asin=”B08L7YC9TT” locale=”JP” title=”サーボモーター ミニチュア DCブラシレスサーボモーター 電気モーター 高速 デュアルチャンネル 光学式ロータリーエンコーダー 100ラインドライバー(24V)”]
  • 6線 スリップリング
    [bc url=”https://ja.aliexpress.com/item/4000338803341.html”]

製作

土台はほぼ球体バーサライタのものを流用し、コントローラをATOM Liteにスリップリングを6線のものにしました。

ブレード試作①

LEDの土台を3Dプリントして らせん状に接着しLEDを固定していきました。

 

 

配線完了。


しかし軸がフニャフニャで回せそうもないためやり直し。。

ブレード試作②

軸の強度を高めるために鉄芯を通せるようにしました。

 

配線後に回してみた。


鉄心入れてもかなり軸がぶれます。。らせん状だとバランスが悪いですね。。
以下はキューブを表示していますが上部のブレが激しいです。土台も手で強く押さえています。

ブレード試作③

らせん階段状だとバランスが悪いので2重らせん状にすることにしました。

完ぺきではありませんが、バランスが改善され回転時のブレが低減できました。

キューブ表示にも改善が見られます♪

完成

憧れの3D ディスプレイが完成しました!
“VoluMetron”と命名させていただきました。

10セルのLEDが付いたブレードを20枚回転させて、1周100分解能で絵を表示させています。

 

課題

この制作に際していくつかの課題もみえましたので以下に挙げます。

  • 解像度向上
    今回はM5Stackコンテスト向けに制作したので、ATOM Liteを採用しました。
    ソフトウェアSPI出力2個でLED 100セルずつ制御し、1周 100分解能で表示させました。
    これ以上に分解能をあげたりLEDセルを増やすとなるとハードウェアSPI出力を有するコントローラを採用したほうが良いと考えております。
  • 軸ブレ
    回転ブレード部の強度が十分でないため、高速回転時にどうしてもブレて絵が歪んでしまいます。
    軸を完全に金属がするなどの検討が必要です。
  • 表示画データ
    位相の異なる20枚のブレード用に3次元のLED表示データを用意するのがかなりシンドイ作業です。
    現状はキューブや花火など各表示画ごとに手作業でプログラムしている状況です。
    将来的には容易に3D表示できる環境を整えたいです。3DCGソフトの導入が必要でしょうか。

 

次期モデル Mini Pupper 2 ラズパイCM4で足座標指定

前回はラズパイ CM4で直接サーボ角度を指定してミニぷぱ動作を楽しみました。

次期モデル Mini Pupper 2 ラズパイCM4で足座標指定

 

ここではラズパイ CM4で足の座標を指定しての動作を楽しみました。

 

足座標指定

ここではMini Pupper2に搭載されたラズパイCM4からサーボを制御するESP32に足の座標をシリアルデータ送信して動作させます。

足の座標は各足の付け根を原点にして前後方方向のX [mm]とY方向へのロール角 [°]と高さZ [mm]を指定するようにいたしました。

 

更に座標移動には以前実験したスムージング機能を持たせて、シリアルデータにスムージングの効き具合も一緒に送るようにいたしました。

 

具体的には以下の13のデータを送って動作させます。
[スムージング[%],  前右x, θ. z, 後右x, θ. z, 前左x, θ. z, 後左x, θ. z, ]

スムージングは以下のように実施しています。

$$現在の座標 = 指示座標 * (100 -スムージング) [\%] + 前回の座標 * スムージング [\%] $$

 

動作

まずは各足の高さを指定して屈伸させてみました。
スムージングがない(0%)と動作が急峻になります。

 

スムージング機能によって動作が柔らかくなり振動も減っています。
移動座標と一緒にスムージング具合[%]も送っていることが今回のミソです。

歩行

足の座標を指定して歩行も試してみました。


こちらもスムージングによって動きが非常にスムーズになっています。

 

歩行動作は以下の4ステートをシリアルデータとしてCM4からESP32(シリアルサーボ)に送るだけで実現できました。

各ステートを100msec毎に送信。

 

スムージングによって各ステート間が良い感じで補完されてスムーズな歩行が実現されました。

次の動作座標を動的に動かしても同様にスムーズな動作が可能であると考えます。
足座標指定による動作は非常に有用な方法なのではないでしょうか。

おわりに

ここではラズパイ CM4で足先の座標を指定してのミニぷぱ動作を楽しみました。

スムージング具合も指定することで大まかな座標指定でも滑らかに動けることがわかりました。
CM4でモーション座標をサクサク流したり、もしくはセンシングで動的に座標を指定しても十分な動作が期待できそうで大変喜んでおります。

それではまた。

次期モデル Mini Pupper 2 ラズパイCM4でサーボ角度指定

前回はミニぷぱ2でディスプレイ表示とスピーカによる音声再生を楽しみました。

次期モデル Mini Pupper 2 CM4でメディアを堪能

 

また前々回はRaspberry Pi Compute Module 4 (CM4) からESP32に組み込まれた動作モードを起動しての動作を楽しみました。

次期モデル Mini Pupper 2 に Raspberry Pi Compute Module 4 を搭載

ここではCM4で直接サーボの角度を指定して動かす仕組みを構築して、ミニぷぱを楽しんでみました。

ミニぷぱ2構成

ミニぷぱ2の構成は大まかに以下の通りです (製品版は変更となる可能性がございます)。

前々回はCM4からシリアル通信で1文字モーションコード送って、ESP32に書き込んだ動作を起動させました。

ここではミニぷぱの12個のサーボの角度をCM4から直接指定して動作させてみました。

サーボ角度指定

CM4から12個分のサーボの角度をシリアル送信してESP32でサーボを指定の角度に動かします。

以下の動画のようにCM4のNode-REDでシリアルデータを送信しました。

伏せの状態を初期姿勢としすべてのサーボの角度を0°としています。
Node-REDでは以下のようにシリアル送信しています。

var a = new Int8Array(13);
a[0] = 0;
a[1] = 0;
a[2] = 0;
a[3] = 0;
a[4] = 0;
a[5] = 0;
a[6] = 0;
a[7] = 0;
a[8] = 0;
a[9] = 0;
a[10] = 0;
a[11] = 0;
a[12] = 0;
msg.payload = a + "\n";
return msg;

ミニぷぱのサーボは12個ありIDが1から12まで割り振られています。配列の0が扱いにくいのでa[13]としてa[1]~a[12]をサーボ角度として使用しています。

立ちの姿勢は以下の通り。

var a = new Int8Array(13);
a[0] = 10;
a[1] = 0;
a[2] = 45;
a[3] = 45;
a[4] = 0;
a[5] = -45;
a[6] = -45;
a[7] = 0;
a[8] = 45;
a[9] = 45;
a[10] = 0;
a[11] = -45;
a[12] = -45;
msg.payload = a + "\n";
return msg;

 

ミニぷぱ立つ

CM4から直接サーボの角度を指定できるようになったので、モーションの作りこみが楽になりました。
ESP32は書き込みに時間がかかりますので。。

早速モーションバリエーションを楽しみました。

なんと立ってしまいましたwww

詳細説明は省略しますがNode-REDのフローは以下のような感じ

 

サーボ角度をビシビシ指定しているので動きが急峻ですね。。
動作間のスムージングは必要そうです。

 

本当は立ち上がった後に前進歩行させたかったのだけど。。w
以下のように かかとかつま先のアタッチメントつければいいかもしれませんね。

おわりに

ここではCM4で直接サーボ角度を指定してミニぷぱ動作を楽しみました。

IMUはESP32に接続されているので、IMUと動作を絡める際にはどうすればよいかなどまだまだ検討の余地はあります。

CM4でサクサク モーション指定できるので色々な動作を検討したいです。
指定角度間のスムージングなども検討したいです。

それではまた。

追記

バク宙挑戦 (2022/11/15)

前機種でも試したバク宙。

 

今回もやってみた。CM4によってかなり軽量化されてるのでさてどうなるか。。

ダメでした。。後頭部殴打www

構造上後ろ足が後ろにまっすぐ伸びないのよ。。
前宙も試したけどダメだった。。

サーボのトルクどうこう以前に構造的に無理そうでした。

次の記事

次期モデル Mini Pupper 2 ラズパイCM4で足座標指定

次期モデル Mini Pupper 2 ラズパイCM4でメディアを堪能

前回はRaspberry Pi Compute Module 4 (CM4) をミニぷぱ基板に搭載してESP32との連動やAlexa連携を楽しみました。

次期モデル Mini Pupper 2 CM4でメディアを堪能

ここではミニぷぱ2基板上でCM4に接続されたディスプレイとスピーカの動作を楽しみました。

ここで使用している基板は製品版と異なる場合があります。何卒ご了承ください。

 

ディスプレイ

ディスプレイは前モデルと同様にST7789が使用されておりました。
以前と同じようにディスプレイ表示させてみました。バックライトのGPIOピンが異なるくらいの違いでした。

ロボット犬『Mini Pupperミニぷぱ』Pythonでモーション記述

動作

無事に表示。画像はST7789のpythonライブラリのサンプルを使用。
https://github.com/pimoroni/st7789-python

スピーカ

基板にはスピーカアンプ (NS4890B)も搭載されておりました。

アンプへの電源供給ON/OFFピンがGPIO22、モノラル入力がGPIO12でした。

アンプ出力に手持ちのスピーカを接続しました。
コネクタが持ってない型だったのではんだ付けしました。

動作

GPIO22をHighにしてアンプの電源をONにして、GPIO12からPWMを出力してみました。

ブザーのような音が出力されています。

 

CM4には通常のラズパイのようにヘッドホンジャックがないので、以下をconfig.txt に追加します。

dtoverlay=audremap,pins_12_13 

これによって、音声再生出力がGPIO12, 13からされるようになります。
ミニぷぱ2基板はモノラル出力なのでGPIO12のみ使用します。

CM4でYoutubeを再生しスピーカから音声が再生されることを確認しました。

参考

おわりに

ここではミニぷぱ2でディスプレイ表示とスピーカによる音声再生を楽しみました。

やはりラズパイはメディアに強いなという実感を強く受けました。
ミニぷぱの動作にメディアを絡めることでさらに表現が増すのではないでしょうか。

それではまたお会いしましょう。

追記

移動するメディアプレイヤー (2022/11/17)

CM4でYoutube再生しつつディスプレイにはサムネイルを表示。
ただそれだけww

次の記事

次期モデル Mini Pupper 2 に Raspberry Pi Compute Module 4 を搭載