#シリーズ目次

この分野では門外漢の私が、「電子制御」をまったく勉強せずに
セグウェイを作ってみようと思います。
参考にした本はこちら
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軸の傾きを使って左右のモーターの回転数を変えて
曲がれるようにしたいと思います。

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