LoginSignup
7
7

More than 5 years have passed since last update.

@sasaco(ようすけ 笹澤)

セグウェイを自作してみた ～プログラム編～

Last updated at Posted at 2016-11-08

シリーズ目次

この分野では門外漢の私が、「電子制御」をまったく勉強せずに
セグウェイを作ってみようと思います。
参考にした本はこちら
Arduino Robotics (Technology in Action)
サンプルソース/Segbot

全ソースコード

arduino は c言語で書きます

balancer.ino
// 倒立振子、加速度/ジャイロセンサー(MPU-6050)版
// 2016/3/4

#include <Wire.h>
#include "KalmanFilter.h"
#include <SoftwareSerial.h>
#include <SabertoothSimplified.h>

// モータードライバの制御定数。
SoftwareSerial SWSerial(NOT_A_PIN, 13); // RX on no pin (unused), TX on pin 11 (to S1).
SabertoothSimplified ST(SWSerial); // Use SWSerial as the serial port.

#define SABER_MOTOR1_FULL_FORWARD  -127
#define SABER_MOTOR1_FULL_REVERSE  127
#define SABER_MOTOR1_FULL_STOP  0

#define SABER_MOTOR2_FULL_FORWARD  -127
#define SABER_MOTOR2_FULL_REVERSE  127
#define SABER_MOTOR2_FULL_STOP  0

// 加速度/ジャイロセンサーの制御定数。
#define MPU6050_ADDR         0x68 // MPU-6050 device address
#define MPU6050_SMPLRT_DIV   0x19 // MPU-6050 register address
#define MPU6050_CONFIG       0x1a
#define MPU6050_GYRO_CONFIG  0x1b
#define MPU6050_ACCEL_CONFIG 0x1c
#define MPU6050_ACCEL_XOUT_H 0x3b
#define MPU6050_ACCEL_XOUT_L 0x3c
#define MPU6050_ACCEL_YOUT_H 0x3d
#define MPU6050_ACCEL_YOUT_L 0x3e
#define MPU6050_ACCEL_ZOUT_H 0x3f
#define MPU6050_ACCEL_ZOUT_L 0x40
#define MPU6050_GYRO_XOUT_H  0x43
#define MPU6050_GYRO_XOUT_L  0x44
#define MPU6050_GYRO_YOUT_H  0x45
#define MPU6050_GYRO_YOUT_L  0x46
#define MPU6050_GYRO_ZOUT_H  0x47
#define MPU6050_GYRO_ZOUT_L  0x48
#define MPU6050_PWR_MGMT_1   0x6b
#define MPU6050_WHO_AM_I     0x75

// 加速度/ジャイロセンサーの制御変数。
KalmanFilter gKfx, gKfy; // カルマンフィルタ。
float gCalibrateX; // 初期化時の角度。（＝静止角とみなす）
float gCalibrateY; // 初期化時の角度。（＝静止角とみなす）
long gPrevMicros;  // loop()間隔の計測用。

// 倒立振子の制御変数。
float gPx, gIx, gDx; // 現在出力値とPID成分。
// この数字は試行錯誤で調整。
float gyro_weightX = 0.98;
float integra_weightX = 0;
float accel_weightX = 0.02;

float gPy, gIy, gDy; // 現在出力値とPID成分。
// この数字は試行錯誤で調整。
float gyro_weightY = 0.98;
float integra_weightY = 0;
float accel_weightY = 0.02;

// 加速度/ジャイロセンサーへのコマンド送信。
void writeMPU6050(byte reg, byte data) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

// 加速度/ジャイロセンサーからのデータ読み込み。
byte readMPU6050(byte reg) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(reg);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 1/*length*/, false);
  byte data =  Wire.read();
  Wire.endTransmission(true);
  return data;
}

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

  // モータードライバの初期化。
  SWSerial.begin(9600);

  // 加速度/ジャイロセンサーの初期化。
  Wire.begin();
  if (readMPU6050(MPU6050_WHO_AM_I) != 0x68) {
    Serial.println("\nWHO_AM_I error.");
    while (true) ;
  }
  writeMPU6050(MPU6050_SMPLRT_DIV, 0x07);   // sample rate: 8kHz/(7+1) = 1kHz
  writeMPU6050(MPU6050_CONFIG, 0x00);       // disable DLPF, gyro output rate = 8kHz
  writeMPU6050(MPU6050_GYRO_CONFIG, 0x00);  // gyro range: ±250dps
  writeMPU6050(MPU6050_ACCEL_CONFIG, 0x00); // accel range: ±2g
  writeMPU6050(MPU6050_PWR_MGMT_1, 0x01);   // disable sleep mode, PLL with X gyro
  delay(2000);

  // 重力加速度から求めた角度をカルマンフィルタの初期値とする。
  float ax = (readMPU6050(MPU6050_ACCEL_XOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_XOUT_L);
  float ay = (readMPU6050(MPU6050_ACCEL_YOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_YOUT_L);
  float az = (readMPU6050(MPU6050_ACCEL_ZOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_ZOUT_L);
  float degRoll  = atan2(ay, az) * RAD_TO_DEG;
  float degPitch = atan(-ax / sqrt(ay * ay + az * az)) * RAD_TO_DEG;
  gKfx.setAngle(degRoll);
  gKfy.setAngle(degPitch);
  gCalibrateX = degRoll;
  gCalibrateY = degPitch;
  gPrevMicros = micros();

  //kill motors when first switched on
  setEngineSpeed(0, 0);
}

void loop() {

  // 重力加速度から角度を求める。
  float ax = (readMPU6050(MPU6050_ACCEL_XOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_XOUT_L);
  float ay = (readMPU6050(MPU6050_ACCEL_YOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_YOUT_L);
  float az = (readMPU6050(MPU6050_ACCEL_ZOUT_H) << 8) | readMPU6050(MPU6050_ACCEL_ZOUT_L);
  float degRoll  = atan2(ay, az) * RAD_TO_DEG;
  float degPitch = atan(-ax / sqrt(ay * ay + az * az)) * RAD_TO_DEG;

  // ジャイロで角速度を求める。
  float gx = (readMPU6050(MPU6050_GYRO_XOUT_H) << 8) | readMPU6050(MPU6050_GYRO_XOUT_L);
  float gy = (readMPU6050(MPU6050_GYRO_YOUT_H) << 8) | readMPU6050(MPU6050_GYRO_YOUT_L);
  float gz = (readMPU6050(MPU6050_GYRO_ZOUT_H) << 8) | readMPU6050(MPU6050_GYRO_ZOUT_L);
  float dpsX = gx / 131.0; // LSB sensitivity: 131 LSB/dps @ ±250dps
  float dpsY = gy / 131.0;
  float dpsZ = gz / 131.0;

  // カルマンフィルタで角度(x,y)を計算する。
  long curMicros = micros();
  float dt = (float)(curMicros - gPrevMicros) / 1000000; // μsec -> sec
  gPrevMicros = curMicros;
  float degX = gKfx.calcAngle(degRoll, dpsX, dt);
  float degY = gKfy.calcAngle(degPitch, dpsY, dt);
  degX -= gCalibrateX;
  degY -= gCalibrateY;

  // 倒れたらモーター停止。
  if (30 < abs(degX)) {
    setEngineSpeed(0, 0);
    Serial.print("**** stop ");
    Serial.print(degX);
    return;
  }

  // PID制御でモーター出力を計算。
  gPx = degX / 90;   // P成分：傾き-90～90度 → -1～1
  gIx += gPx;        // I成分：傾きの積算。
  gDx = dpsX / 250;  // D成分：角速度-250～250dps → -1～1

  gPy = degY / 90;   // P成分：傾き-90～90度 → -1～1
  gIy += gPy;        // I成分：傾きの積算。
  gDy = dpsY / 250;  // D成分：角速度-250～250dps → -1～1

  // 前後方向の出力を決定
  float level = (gPx * gyro_weightX + 
                gIx * integra_weightX +
                gDx * accel_weightX) * 400;

  // 左右方向の出力を決定
  float Steer = (gPy * gyro_weightY + 
                gIy * integra_weightY +
                gDy * accel_weightY) * 150;

  float Motor1percent = level - Steer;
  float Motor2percent = level + Steer;

  setEngineSpeed(Motor1percent, Motor2percent);


  // デバッグ用。
  static int ps;
  if (++ps % 1 == 0) {
    Serial.print("dt:"); Serial.print(dt);
    Serial.print("\tdegX:"); Serial.print(degX);
    Serial.print("\tdpsX:"); Serial.print(dpsX);
    Serial.print("\tdegY:"); Serial.print(degY);
    Serial.print("\tdpsY:"); Serial.print(dpsY);
    Serial.print("\tlevel:"); Serial.print(level);
    Serial.print("\tSteer:"); Serial.print(Steer);
    Serial.print("\tMotor1percent:"); Serial.print(Motor1percent);
    Serial.print("\tMotor2percent:"); Serial.print(Motor2percent);
    Serial.println("");
  }
  delay(1);
}

void setEngineSpeed( float Motor1percent, float Motor2percent ){

  Motor1percent = max(-100, min(100, Motor1percent)); // → -100～100
  Motor2percent = max(-100, min(100, Motor2percent)); // → -100～100

  float cSpeedVal_Motor1 = 0;
  float cSpeedVal_Motor2 = 0;

  cSpeedVal_Motor1 = map (Motor1percent,
                         -100,
                         100,
                         SABER_MOTOR1_FULL_REVERSE,
                         SABER_MOTOR1_FULL_FORWARD);

  cSpeedVal_Motor2 = map (Motor2percent,
                         -100,
                          100,
                         SABER_MOTOR2_FULL_REVERSE,
                         SABER_MOTOR2_FULL_FORWARD);

  ST.motor(1, cSpeedVal_Motor1);
  ST.motor(2, cSpeedVal_Motor2);
}
KalmanFilter.h
// カルマンフィルタ。
// 2016/3/3

class KalmanFilter {
private:
    float angle;
    float bias;
    float P[2][2];
public:
    KalmanFilter();
    void setAngle(float newAngle);
    float calcAngle(float newAngle, float newRate, float dt);
};
KalmanFilter.cpp
// カルマンフィルタ。
// 2016/3/3
//
// 計算式の参考資料。
// http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

#include "KalmanFilter.h"

KalmanFilter::KalmanFilter() {
  angle = 0.0;
  bias = 0.0;
  P[0][0] = 0.0;
  P[0][1] = 0.0;
  P[1][0] = 0.0;
  P[1][1] = 0.0;
};

void KalmanFilter::setAngle(float newAngle) {
  angle = newAngle;
};

float KalmanFilter::calcAngle(float newAngle, float newRate, float dt) {

  // variances
  float Q_angle = 0.001;
  float Q_bias = 0.003;
  float R_measure = 0.03;

  // step 1
  float rate = newRate - bias;
  angle += dt * rate;

  // step 2
  P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
  P[0][1] -= dt * P[1][1];
  P[1][0] -= dt * P[1][1];
  P[1][1] += Q_bias * dt;

  // step 3
  float y = newAngle - angle;

  // step 4
  float S = P[0][0] + R_measure;

  // step 5
  float K[2];
  K[0] = P[0][0] / S;
  K[1] = P[1][0] / S;

  // step 6
  angle += K[0] * y;
  bias += K[1] * y;

  // step 7
  float P00_temp = P[0][0];
  float P01_temp = P[0][1];
  P[0][0] -= K[0] * P00_temp;
  P[0][1] -= K[0] * P01_temp;
  P[1][0] -= K[1] * P00_temp;
  P[1][1] -= K[1] * P01_temp;

  return angle;
};

解説

各セクションごとの解説をしていきます。

外部参照モジュール

ジャイロセンサーの制御のために下記のモジュールを使います。
これは、arduino に標準で入っていないので、
下記の URL を参考に 各環境に モジュールを追加してください

MPU-6050 Accelerometer + Gyro

#include "I2Cdev.h"
#include "MPU6050.h"

制御用変数の定義

フルパワーを 500 とします。
どうやら 1023 までいけるようです。

/*  Attitude Control Parameters */
#define FULL_SPEED 500//1023

ジャイロセンサーのアクセス用変数

/*  MPU-6050 Accelerometer + Gyro */
MPU6050 accelgyro;

モーター制御は
下記の pin 番号を通じて制御します。

/*  VNH2SP30 pin definitions */
int inApin[2] = {7, 4};  // INA: Clockwise input
int inBpin[2] = {8, 9}; // INB: Counter-clockwise input
int pwmpin[2] = {5, 6}; // PWM input
int cspin[2] = {2, 3}; // CS: Current sense ANALOG input
int enpin[2] = {0, 1}; // EN: Status of switches output (Analog pin)

モータの回転数を決定するのに
ジャイロセンサーの10回の検出値の平均を用いるようにします。

/*  gyro multiple times to get an average baseline value. */
#define average_times 10
float angular_rate_Y_log[average_times];
float angular_rate_Y_sum;
float angular_rate_Z_log[average_times];
float angular_rate_Z_sum;

ログは毎回出します。

int log_times = 0;

初期化処理

定型分

void setup() {

    Wire.begin();
    Serial.begin(9600);

ジャイロセンサーの初期化

    // initialize device
    accelgyro.initialize();
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

モーター制御の初期化

    // Initialize digital pins as outputs
    Serial.println("Initializing Mortor Driver digital pins...");
    for (int i=0; i<2; i++){
      pinMode(inApin[i], OUTPUT);
      pinMode(inBpin[i], OUTPUT);
      pinMode(pwmpin[i], OUTPUT);
    }
    // Initialize braked
    for (int i=0; i<2; i++){
      digitalWrite(inApin[i], LOW);
      digitalWrite(inBpin[i], LOW);
    }
}

制御処理

ジャイロセンサーの値を角度（°）単位に変換する

void loop() {

  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float acc_x = ax / 16384.0;
  float acc_y = ay / 16384.0; 
  float acc_z = az / 16384.0; 
  float rate_X = atan2(acc_x, acc_z) * 360 / 2.0 / PI; 
  float rate_Y = atan2(acc_y, acc_z) * 360 / 2.0 / PI;
  float rate_Z = atan2(acc_x, acc_y) * 180 / 2.0 / PI;

  Serial.print(rate_X);
  Serial.print("\t"); 
  Serial.print(rate_Y);
  Serial.print("\t"); 
  Serial.print(rate_Z);
  Serial.print("\t");

初回(average_times=10回)は値を保持するだけ
ジャイロセンサーの10回の検出値の平均を計算する

  if(log_times < average_times){
    angular_rate_Y_sum += rate_Y; 
    angular_rate_Y_log[log_times] = rate_Y;
    angular_rate_Z_sum += rate_Z; 
    angular_rate_Z_log[log_times] = rate_Z;
    log_times ++;
  }else{
    //傾きの平均値Xを計算する。
    angular_rate_Y_sum -= angular_rate_Y_log[0];
    angular_rate_Y_sum += rate_Y;
    for (int i=1; i<average_times; i++) 
      angular_rate_Y_log[i-1] = angular_rate_Y_log[i];
    angular_rate_Y_log[average_times-1]= rate_Y;
    float angular_rate_Y_average = angular_rate_Y_sum / average_times;

    //傾きの平均値Zを計算する。
    angular_rate_Z_sum -= angular_rate_Z_log[0];
    angular_rate_Z_sum += rate_Z;
    for (int i=1; i<average_times; i++) 
      angular_rate_Z_log[i-1] = angular_rate_Z_log[i];
    angular_rate_Z_log[average_times-1]= rate_Z;
    float angular_rate_Z_average = angular_rate_Z_sum / average_times;

モーターの回転数を決定する。
Yの角度(10回分の平均値)が 60° 以上 0 以下なら 前進（go_state = 2）

    //モーターの回転数を決定する。
    float pmw1 = 0; //0 to 1023
    float pmw2 = 0; //0 to 1023
    int go_state = 0;
    float delta = 0;

      if (angular_rate_Y_average <= 60 && angular_rate_Y_average >= 0 ){
        angular_rate_Y_average = abs(angular_rate_Y_average);
        float a2 = angular_rate_Y_average / 60;
        pmw1 =  FULL_SPEED * a2;
        pmw2 = pmw1;
        go_state = 2;

Yの角度(10回分の平均値)が 0 以上 -60° 以下なら 後進（go_state = 1）

      }
      if (angular_rate_Y_average >= -60 && angular_rate_Y_average <= 0 ){
        angular_rate_Y_average = abs(angular_rate_Y_average);
        float a1 = angular_rate_Y_average / 60;
        pmw1 =  FULL_SPEED * a1;
        pmw2 = pmw1;
       go_state = 1;
      }

ログと 実際にモーターに命令を出す関数 motorGo を呼ぶ

      Serial.print(go_state);
      Serial.print("\t"); 
      Serial.print(pmw1);
      Serial.print("\t"); 
      Serial.print(pmw2);
      Serial.print("\t"); 

      Serial.print(delta);
      Serial.print("\t"); 

      if ( go_state == 0 ){
        motorOff(0);
        motorOff(1);
      }else{
        motorGo(0, go_state, pmw1); 
        motorGo(1, go_state, pmw2);
      }
    }
    Serial.println("");
 }

動作処理

実際にモーターに命令を出す関数

void motorOff(int motor){
  for (int i=0; i<2; i++){
    digitalWrite(inApin[i], LOW);
    digitalWrite(inBpin[i], LOW);
  }
  analogWrite(pwmpin[motor], 0);
}

void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm)
{
  if (motor <= 1){
    if (direct <=4){
      // Set inA[motor]
      if (direct <=1)
        digitalWrite(inApin[motor], HIGH);
      else
        digitalWrite(inApin[motor], LOW);
      // Set inB[motor]
      if ((direct==0)||(direct==2))
        digitalWrite(inBpin[motor], HIGH);
      else
        digitalWrite(inBpin[motor], LOW);

      analogWrite(pwmpin[motor], pwm);
    }
  }

改良の余地

ソースはここに
https://create.arduino.cc/editor/sasaco/feb5acb3-d17a-4cc3-b572-9ca4e98178e7/preview

まだ、前進・後進しかやっていません
そのうち z軸の傾きを使って 左右のモーターの回転数を変えて
曲がれるようにしたいと思います。

次回は完成編についてお届けします。

7
7
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
7
7