HomeMadeGarbageのアイコン画像
HomeMadeGarbage 2020年12月30日作成 (2020年12月30日更新)
製作品 製作品 閲覧数 2135
HomeMadeGarbage 2020年12月30日作成 (2020年12月30日更新) 製作品 製作品 閲覧数 2135

星の倒立振子 SHISEIGYO-Star

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

動作

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

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

構成

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

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

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

SHISEIGYO-Star 構成図

部品

筐体


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

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

Arduino ソースコード

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

#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×θ+Kd×θ˙+Kw×θ˙wDuty = Kp \times \theta + Kd\times \dot\theta + Kw \times \dot\theta_w

θ\theta, θ˙\dot\thetaはモジュールの傾きと角速度、 θ˙w\dot\theta_wはホイールの角速度、Kはそれぞれの係数です。

参考文献

メリークリスマス

2
HomeMadeGarbageのアイコン画像
2児と夫婦の4人5脚。手作りを基調に生活してます。ほぼお父ちゃんがつぶやきます。  https://homemadegarbage.com/ ネットショップオープンしました。  https://shop.homemadegarbage.com/product-category/hmg/
HMGfanのアイコン画像
HomeMadeGarbageの活動を報告致します。また作品の感想やつくってみた などいろいろ共有ください♪
ログインしてコメントを投稿する