編集履歴一覧に戻る
HomeMadeGarbageのアイコン画像

HomeMadeGarbage が 2020年12月30日03時53分05秒 に編集

初版

タイトルの変更

+

星の倒立振子 SHISEIGYO-Star

タグの変更

+

秋葉原2021

+

M5Stack

+

倒立振子

+

クリスマス

+

Arduino

本文の変更

+

今年(2020年)のクリスマスに星形の倒立振子を製作しましたので詳細を記載いたします。 動作 ==== まずは早速動作をご覧ください。 @[twitter](https://twitter.com/H0meMadeGarbage/status/1340971097219428354)     床を傾けても星は倒立姿勢を保ちます。 @[twitter](https://twitter.com/H0meMadeGarbage/status/1341002947388305408) 構成 ==== 倒立にはリアクションホイールを使用しています。 フライホイールをブラシレスモータで回転させて姿勢を保ちます。 制御マイコンにはATOM Matrixを使用し、姿勢角の検出にはATOM Matrix内蔵のIMUセンサ(MPU6886)を使用しています。 倒立は作用反作用の法則に基づいて実現し、倒れる方向にリアクションホイールを回して 反作用で起き上がります。これをマイコンによる傾きセンスとモータ制御を高速に繰り返すことにより 倒立を持続します。 ![SHISEIGYO-Star 構成図](https://camo.elchika.com/2705a7584801997ce09a8a7296fafd1c6a8723f8/687474703a2f2f73746f726167652e676f6f676c65617069732e636f6d2f656c6368696b612f76312f757365722f39396533383932642d313165622d346636362d393638622d3230623961353138303030652f35666636386130662d373831352d346232342d616463302d316639396532323438326137/) ### 部品 - [M5Stack ATOM Matrix](https://amzn.to/3jmYV2b) - [フライホイール付き ブラシレスモータ ID-549XW](https://ja.aliexpress.com/item/4000314159240.html?spm=a2g11.12057483.0.0.4cde2fd0t3ecdq) 上記Aliexpressリンクの タイプ 3: フライホイールの直径: 80 ミリメートル、 dcブラシレス光学エンコーダ (ロングタイプ)を使用 - [逆起電力防止用ダイオード IN5819](https://amzn.to/2YEznG6 ) - [24V2A 汎用 ACアダプタ](https://amzn.to/3llbLQz) モータ用の電源 - [降圧DCDCレギュレータ](https://amzn.to/2G2xlcn) ACアダプタからの24Vを5Vに降圧してマイコンやモータの制御系の電源に使用。 ### 筐体 @[twitter](https://twitter.com/H0meMadeGarbage/status/1343979280359849984) 筐体は3Dプリンタで製作しました。 ATOM MatrixのLEDマトリクスを姿勢角のインジケータとして使用しています。 Arduino ソースコード ==== 以下のライブラリを使用しています。 - Kalman Filter Library バージョン1.0.2  https://github.com/TKJElectronics/KalmanFilter  姿勢角と角速度の推定値導出にカルマン・フィルタを用いています。 - FastLED バージョン3.3.3  https://github.com/FastLED/FastLED  ATOM MatrixのLEDマトリクス制御に使用 ```SHISEIGYO-Star_2.ino #include "MPU6886.h" #include <Kalman.h> #include "FastLED.h" #define ENC_A 22 #define ENC_B 19 #define brake 23 #define rote_pin 32 #define PWM_pin 26 #define button 39 #define led_pin 27 #define NUM_LEDS 25 CRGB leds[NUM_LEDS]; MPU6886 IMU; unsigned long oldTime = 0, loopTime, nowTime; float dt; volatile byte pos; volatile int enc_count = 0; float Kp = 4; float Kd = 6; float Kw = 0.6; int DutyIni = 1007, pwmDuty; int GetUP = 0; int GetUpCnt = 0; float M; float Deg = 2.3; 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; //オフセット float accXoffset = 0, accYoffset = 0, accZoffset = 0; float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0; Kalman kalmanY; float kalAngleY, kalAngleDotY; //加速度センサから傾きデータ取得 [deg] float get_theta_acc() { IMU.getAccelData(&accX,&accY,&accZ); //傾斜角導出 単位はdeg theta_acc = atan(-1.0 * (accX - accXoffset) / (accZ - accZoffset)) * 57.29578f; return theta_acc; } //Y軸 角速度取得 float get_gyro_data() { IMU.getGyroData(&gyroX,&gyroY,&gyroZ); theta_dot = gyroY - gyroYoffset; return theta_dot; } void setup() { Serial.begin(115200); FastLED.addLeds<WS2812B, led_pin, GRB>(leds, NUM_LEDS); FastLED.setBrightness(20); 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); } void loop() { 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 (fabs(kalAngleY + Deg) < 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(", DutyIni: "); Serial.print(DutyIni); Serial.print(", kalAngleY: "); Serial.print(kalAngleY); if(GetUP == 99 || GetUP == 80){ //ブレーキ if(fabs(kalAngleY) > 20.0){ digitalWrite(brake, LOW); 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{ M = Kp * (kalAngleY + Deg) / 90.0 + Kd * kalAngleDotY / 500.0 + Kw * theta_dotWheel / 10000.0; } M = max(-1.0f, min(1.0f, M)); pwmDuty = DutyIni * (1.0 - fabs(M)); //回転方向 if(pwmDuty > DutyIni){ digitalWrite(brake, LOW); ledcWrite(0, 1023); }else 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); //LED表示 for(int i = 0; i < NUM_LEDS; i++){ leds[i] = CRGB::Black; } if((kalAngleY + Deg) > 20.0){ for(int i = 0; i < 5; i++){ leds[i * 5] = CRGB::Red; } }else if((kalAngleY + Deg) <= 20.0 && (kalAngleY + Deg) > 12.0){ for(int i = 0; i < 5; i++){ leds[i * 5] = CRGB::Green; } }else if((kalAngleY + Deg) <= 12.0 && (kalAngleY + Deg) > 4.0){ for(int i = 0; i < 5; i++){ leds[i * 5 + 1] = CRGB::Green; } }else if((kalAngleY + Deg) <= 4.0 && (kalAngleY + Deg) > 1.0){ for(int i = 0; i < 5; i++){ leds[i * 5 + 2] = CRGB::Green; } }else if(fabs(kalAngleY + Deg) <= 1.0){ for(int i = 0; i < 5; i++){ leds[i * 5 + 2] = CRGB::Blue; } }else if((kalAngleY + Deg) >= -4.0 && (kalAngleY + Deg) < -1.0){ for(int i = 0; i < 5; i++){ leds[i * 5 + 2] = CRGB::Green; } }else if((kalAngleY + Deg) >= -12.0 && (kalAngleY + Deg) < -4.0){ for(int i = 0; i < 5; i++){ leds[i * 5 + 3] = CRGB::Green; } }else if((kalAngleY + Deg) >= -20.0 && (kalAngleY + Deg) < -12.0){ for(int i = 0; i < 5; i++){ leds[i * 5 + 4] = CRGB::Green; } }else if((kalAngleY + Deg) < -20.0){ for(int i = 0; i < 5; i++){ leds[i * 5 + 4] = CRGB::Red; } } FastLED.show(); delay(2); Serial.println(""); } 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; } } ``` ブラシレスモータのエンコーダ出力を割り込み処理でエンコーダ出力のA相、B相信号から方向と位置(パルスカウント)を検出しモータの角速度を算出しています。 モータのエンコーダのカウント方法は以下を参考にさせていただいております。  https://jumbleat.com/2016/12/17/encoder_1/ モータ制御 ==== モータの回転数はPWM信号のデューティ比で決定され以下の式に基づいて制御しています。 $$Duty = Kp \times \theta + Kd\times \dot\theta + Kw \times \dot\theta_w$$ $\theta$, $\dot\theta$はモジュールの傾きと角速度、 $\dot\theta_w$はホイールの角速度、Kはそれぞれの係数です。 参考文献 ==== - [トランジスタ技術 2020年 06 月号](https://amzn.to/34At8qq) メリークリスマス ==== @[twitter](https://twitter.com/H0meMadeGarbage/status/1342063470322143241)