【倒立振子part6】モータを回す
前回までは振子を倒立させるのに必要なモデルと状態空間モデルまで作成した。
今回からはハードの設計と製作を行う。
前提
前回のパラメータ同定でDCモータのパラメータ同定を行ったが、よく調べるとエンコーダの分解能が13パルス/周であり0.48rad(27度)毎でしか測定できない。
A相B相を使って4逓倍しても0.12rad(6.75度)毎でしか測定できないため、倒立振子の制御には向かないのではないかと思ったので選定し直した。
AliExpreseで購入したときに100パルス/周のブラシレスDCモータ版も売っていたので再購入した。
(というか元々はブラシレスDCモータのほうを買おうと思ってたけど間違って購入してしまってた。。。)
いろいろ調べていると慣性の倒立振子について参考としてHomeMadeGarbage様のサイトにたどり着いて大いに参考にさせていただいた。圧倒的感謝！！！
homemadegarbage.com
HomeMadeGarbage様のブラシレスDCモータを回転させるためにされているブログを参考にさせていただいて自分でも実験していく。
ブラシレスモータについて
ブラシ付きDCモータは端子間に電圧を印可するだけで回転するため回転させるのがめっちゃ簡単。
それに対してブラシレスDCモータはブラシ付きモータとは違い回転させるためにはインバータ回路が必要となるため簡単に回せないのがデメリット。
(構造の違いについては不明点があれば追記していく)
違いについてはTOSHIBAのサイトに詳しく記載されていたので引用させていただく。
今回使用するブラシレスDCモータには基板が内蔵してあるため自分でホールセンサもインバータ回路が必要なくPWMで回転の制御ができる。
ブラシレスDCモータを回転させる
ブラシレスDCモータを回転させるためにはPWM入力が必要のため試験のためにPWM波が必要となる。
今回は回転と速度変化させることを目標とする。
制御器としてジャイロセンサが内蔵してあるため「Atom Matrix」を使用する。
ATOM Matrixwww.switch-science.com
ピン番号、機能とかは公式サイトを参考とする。
docs.m5stack.com
Atom MatrixはArduino IDEで開発する。
開発の環境設定は以前終わらせていたので割愛するが公式サイトにやり方が載っている。
docs.m5stack.com
LEDを光らせる
ボタンでPWMの表面ボタンでDuty比が変わるようなプログラムを作成する。
まず表面ボタンを押すとLEDで数字が出るようなプログラムを作った。
#include "M5Atom.h" // LEDマトリクス表示指定配列（0:消灯、1以上：色指定配列で指定した色に指定（color配列1次要素番号）） int matrix_0[5][5]= {{0,1,1,1,0}, {0,1,0,1,0}, {0,1,0,1,0}, {0,1,0,1,0}, {0,1,1,1,0}}; int matrix_1[5][5]= {{0,0,1,0,0}, {0,1,1,0,0}, {0,0,1,0,0}, {0,0,1,0,0}, {0,1,1,1,0}}; int matrix_2[5][5]= {{0,1,1,1,0}, {0,0,0,1,0}, {0,1,1,1,0}, {0,1,0,0,0}, {0,1,1,1,0}}; int matrix_3[5][5]= {{0,1,1,1,0}, {0,0,0,1,0}, {0,1,1,1,0}, {0,0,0,1,0}, {0,1,1,1,0}}; int matrix_4[5][5]= {{0,1,0,1,0}, {0,1,0,1,0}, {0,1,1,1,0}, {0,0,0,1,0}, {0,0,0,1,0}}; // 変数宣言 int num = 0; //matrix配列の色番号格納用 // 色指定配列（1次要素を色番号として表示する色を2次要素の3原色{赤、緑、青}で設定） // LED色： {{0:消灯} , {1:ピンク} , {2:オレンジ} , {3:グリーン} , {4:パープル} , {5:ホワイト}} int color[][3] = {{0, 0, 0}, {255, 0, 0}, {255, 70, 0}, {70, 255, 0}, {70, 0, 255}, {255, 255, 255}}; int pwmDuty; int count=0; // FastLEDライブラリの設定（CRGB構造体） CRGB dispColor(uint8_t r, uint8_t g, uint8_t b) { return (CRGB)((r << 16) | (g << 8) | b); } void setup() { // 本体初期化（UART無効, I2C無効, LED有効） M5.begin(false, false, true); // LED全消灯（赤, 緑, 青） M5.dis.drawpix(0, dispColor(0, 0, 0)); } void loop() { M5.update(); // 本体のボタン状態更新 if(M5.Btn.isPressed()){ if(count==1){ for (int i = 0; i < 5; i++) { //matrix行ループ for (int j = 0; j < 5; j++) { //matrix列ループ num = matrix_1[i][j]; //matrix配列の色番号取得 //matrix行ごと（0,5,10,15,20）に左から右へ色番号の色で表示 M5.dis.drawpix(i*5+j, dispColor(color[num][0], color[num][1], color[num][2])); } } delay(100); } if(count==2){ for (int i = 0; i < 5; i++) { //matrix行ループ for (int j = 0; j < 5; j++) { //matrix列ループ num = matrix_2[i][j]; //matrix配列の色番号取得 //matrix行ごと（0,5,10,15,20）に左から右へ色番号の色で表示 M5.dis.drawpix(i*5+j, dispColor(color[num][0], color[num][1], color[num][2])); } } delay(100); } if(count==3){ for (int i = 0; i < 5; i++) { //matrix行ループ for (int j = 0; j < 5; j++) { //matrix列ループ num = matrix_3[i][j]; //matrix配列の色番号取得 //matrix行ごと（0,5,10,15,20）に左から右へ色番号の色で表示 M5.dis.drawpix(i*5+j, dispColor(color[num][0], color[num][1], color[num][2])); } } delay(100); } if(count==4){ for (int i = 0; i < 5; i++) { //matrix行ループ for (int j = 0; j < 5; j++) { //matrix列ループ num = matrix_4[i][j]; //matrix配列の色番号取得 //matrix行ごと（0,5,10,15,20）に左から右へ色番号の色で表示 M5.dis.drawpix(i*5+j, dispColor(color[num][0], color[num][1], color[num][2])); } } count=0; delay(100); } count++; delay(100); //100ms待機 } }
PWM出力をする
次にPWM波形を出力するプログラムを作っていく。
ピン機能的に26ピンを使用するとPWMを出力できそう(DAC)。
#include "M5Atom.h" #define PWM_pin 25 int pwmDuty; void setup() { M5.begin(true, false, true); ledcSetup(0, 20000, 8); ledcAttachPin(PWM_pin, 0); } void loop() { pwmDuty = 100; //0-255の間で選定する ledcWrite(0, pwmDuty); delay(50); }
プログラムのメモとしては
ledcSetup(chan,freq,bit_num); 引数はチャンネル,周波数,ビット数
ledcAttachPin(pin,chan); 引数はピン番号,チャンネル
ledcWrite(chan,duty); 引数はチャンネル,デューティ(アナログで0-255)
map(value, fromLow, fromHigh, toLow, toHigh);
引数は下記でありvalueの値のスケールを変更することができる。
fromLow：値の元の範囲の下限
fromHigh：値の元の範囲の上限
toLow：値の再マッピング後の範囲の下限
toHigh：値の再マッピング後の範囲の上限
ボタンを押してPWMをパターンごと出力する
上の2つのプログラムを組み合わせると
#include "M5Atom.h" #define PWM_pin 25 // LEDマトリクス表示指定配列（0:消灯、1以上：色指定配列で指定した色に指定（color配列1次要素番号）） int matrix_0[5][5]= {{0,1,1,1,0}, {0,1,0,1,0}, {0,1,0,1,0}, {0,1,0,1,0}, {0,1,1,1,0}}; int matrix_1[5][5]= {{0,0,1,0,0}, {0,1,1,0,0}, {0,0,1,0,0}, {0,0,1,0,0}, {0,1,1,1,0}}; int matrix_2[5][5]= {{0,1,1,1,0}, {0,0,0,1,0}, {0,1,1,1,0}, {0,1,0,0,0}, {0,1,1,1,0}}; int matrix_3[5][5]= {{0,1,1,1,0}, {0,0,0,1,0}, {0,1,1,1,0}, {0,0,0,1,0}, {0,1,1,1,0}}; int matrix_4[5][5]= {{0,1,0,1,0}, {0,1,0,1,0}, {0,1,1,1,0}, {0,0,0,1,0}, {0,0,0,1,0}}; // 変数宣言 int num = 0; //matrix配列の色番号格納用 // 色指定配列（1次要素を色番号として表示する色を2次要素の3原色{赤、緑、青}で設定） // LED色： {{0:消灯} , {1:ピンク} , {2:オレンジ} , {3:グリーン} , {4:パープル} , {5:ホワイト}} int color[][3] = {{0, 0, 0}, {255, 0, 0}, {255, 70, 0}, {70, 255, 0}, {70, 0, 255}, {255, 255, 255}}; int pwmDuty; int count=0; // FastLEDライブラリの設定（CRGB構造体） CRGB dispColor(uint8_t r, uint8_t g, uint8_t b) { return (CRGB)((r << 16) | (g << 8) | b); } void setup() { // 本体初期化（UART無効, I2C無効, LED有効） M5.begin(false, false, true); // LED全消灯（赤, 緑, 青） M5.dis.drawpix(0, dispColor(0, 0, 0)); ledcSetup(0, 20000, 8); ledcAttachPin(PWM_pin, 0); } void loop() { M5.update(); // 本体のボタン状態更新 if(M5.Btn.wasPressed()){ if(count==1){ pwmDuty = 0; for (int i = 0; i < 5; i++) { //matrix行ループ for (int j = 0; j < 5; j++) { //matrix列ループ num = matrix_1[i][j]; //matrix配列の色番号取得 //matrix行ごと（0,5,10,15,20）に左から右へ色番号の色で表示 M5.dis.drawpix(i*5+j, dispColor(color[num][0], color[num][1], color[num][2])); } } ledcWrite(0, pwmDuty); delay(100); } if(count==2){ pwmDuty = 255*0.5; for (int i = 0; i < 5; i++) { //matrix行ループ for (int j = 0; j < 5; j++) { //matrix列ループ num = matrix_2[i][j]; //matrix配列の色番号取得 //matrix行ごと（0,5,10,15,20）に左から右へ色番号の色で表示 M5.dis.drawpix(i*5+j, dispColor(color[num][0], color[num][1], color[num][2])); } } ledcWrite(0, pwmDuty); delay(100); } if(count==3){ pwmDuty = 255*0.75; for (int i = 0; i < 5; i++) { //matrix行ループ for (int j = 0; j < 5; j++) { //matrix列ループ num = matrix_3[i][j]; //matrix配列の色番号取得 //matrix行ごと（0,5,10,15,20）に左から右へ色番号の色で表示 M5.dis.drawpix(i*5+j, dispColor(color[num][0], color[num][1], color[num][2])); } } ledcWrite(0, pwmDuty); delay(100); } if(count==4){ pwmDuty = 255*1; for (int i = 0; i < 5; i++) { //matrix行ループ for (int j = 0; j < 5; j++) { //matrix列ループ num = matrix_4[i][j]; //matrix配列の色番号取得 //matrix行ごと（0,5,10,15,20）に左から右へ色番号の色で表示 M5.dis.drawpix(i*5+j, dispColor(color[num][0], color[num][1], color[num][2])); } } ledcWrite(0, pwmDuty); count=0; delay(100); } count++; delay(100); //100ms待機 } }
今回はDuty比=pwmDuty/255となるため
・count=1の時はduty比が0%
・count=2の時はduty比が50%
・count=3の時はduty比が75%
・count=4の時はduty比が100%
となるように設定した。
