星の倒立振子 SHISEIGYO-Star

今年(2020年)のクリスマスに星形の倒立振子を製作しましたので詳細を記載いたします。

まずは早速動作をご覧ください。

ここにPOSTが表示されます

床を傾けても星は倒立姿勢を保ちます。

ここにPOSTが表示されます

倒立にはリアクションホイールを使用しています。
フライホイールをブラシレスモータで回転させて姿勢を保ちます。

制御マイコンにはATOM Matrixを使用し、姿勢角の検出にはATOM Matrix内蔵のIMUセンサ(MPU6886)を使用しています。

倒立は作用反作用の法則に基づいて実現し、倒れる方向にリアクションホイールを回して
反作用で起き上がります。これをマイコンによる傾きセンスとモータ制御を高速に繰り返すことにより
倒立を持続します。

M5Stack ATOM Matrix
フライホイール付きブラシレスモータ ID-549XW
上記Aliexpressリンクのタイプ 3: フライホイールの直径: 80 ミリメートル、
dcブラシレス光学エンコーダ (ロングタイプ)を使用
逆起電力防止用ダイオード IN5819
24V2A 汎用 ACアダプタ
モータ用の電源
降圧DCDCレギュレータ
ACアダプタからの24Vを5Vに降圧してマイコンやモータの制御系の電源に使用。

ここにPOSTが表示されます

筐体は3Dプリンタで製作しました。

ATOM MatrixのLEDマトリクスを姿勢角のインジケータとして使用しています。

以下のライブラリを使用しています。

Kalman Filter Library バージョン1.0.2
　https://github.com/TKJElectronics/KalmanFilter
　姿勢角と角速度の推定値導出にカルマン・フィルタを用いています。
FastLED バージョン3.3.3
　https://github.com/FastLED/FastLED
　ATOM MatrixのLEDマトリクス制御に使用


#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$