サーボとの組み合わせ

ローバーには大きく分けてスピードコントローラ、ステアリングサーボなど複数のPWM出力を使用する。

まだ最終採択ではないが、これらを制御するために PCA9685 搭載16チャネル PWM/サーボ ドライバーをテストしている。 < https://www.switch-science.com/catalog/961/ >

これを磁気センサと組み合わせて使ってみた。 結果から言うと特に問題はなさそう。

配線は以下の通り

本プログラムでは、先の磁気センサのアプリに追加したものである。

そこで気になるのがサーボのビビりである。 原因は磁気センサの値が揺れているためにおこるものであり、その値をフィルタリングすることで逃げている。

フィルタリング手法はいくつかあるのだが、なんとなくコーディングの楽なRCフィルタリングを用いた

参考にこの部分である。

  new_angle =k_rc*new_angle+(1.0-k_rc)*rad;  // RC filter

k_rcはフィルタリングの強さで0~1の値を入れるが、1に近くなるほどフィルタリングが強く反応も遅くなる。0.8程度を入れたが、チューニング要素である。

// This software is released under the MIT License.
// See license.txt at http://moon.robots.jp/ehp/ 
// The software is provided "AS IS", without warranty of any kind, 
// express or implied, including but not limited to the warranties
// of merchantability. (c) 2021 Team Katy
// 
// 2021/5/23 ver 1.0 first released by S.K
//

#include <Wire.h>
#include <Servo.h>
#include <PCA9685.h>     


////// Other Variables
static float k_rc = 0.8;           // RC filter factor

////// Servo
#define SERVOMIN_VAL 110     // minimum pulse width for servo
#define SERVOMAX_VAL 460     // maximum pulse width for servo
#define SERVO_CH 1           // channel for servo
PCA9685 pwm = PCA9685(0x40); // No address jumper connection in PCA9685

////// AK8963 register 
const int   I2C_AK8963_addr=0x0c;   // i2c: ak8963 slave address
const int AK_ID= 0x00;  //  00H READ  Device ID; ID is 0x48
const int AK_ST1= 0x02; //  02H READ  Status 1  DataStatus; D0:True; data ready D1:True data overrun
const int AK_HXL= 0x03; //  03H READ  X-axis data 8190:4912uT -8190:-4912uT
const int AK_CNTL1= 0x0A; //  0AH READ/WRITE  Control1  Function Control
                          // see data sheet D0:3 mode; D4:0 is 14 bit, 1 is 16bit
const int AK_ASAX= 0x10;  //  10H READ  X-axis sensitivity adjustment value Fuse ROM
const int   AK_WAIT=100;     // Wait for mode change [us]
const int   AK_MOD_PWRDWN=0x0;   // Power down mode 
const int   AK_MOD_READ_METHOD=0x1;   
                // Read method:0x1:Single
                //             0x2:Continus 8Hz
                //             0x3:Continus 100Hz
const int   AK_MOD_FUSE=0x0f;    // mode change to read FUSE
const int   AK_MOD_BIT=0x10;   // # of read bits: 0x10:16 bit, 0x00:14 bit
const int   AK_I2C_ADD=0x48;   // I2C address for AK8963
static float x_bias = -122; // calibration value
static float x_gain = 1; // calibration value
static float y_bias = -45.5; // calibration value
static float y_gain = 1; // calibration value
static float z_bias = -122; // calibration value
static float z_gain = 1; // calibration value

static int  i2c_buf[8];    // read buffer for I2C
const int   max_char=8;    // max read characters from serial monitor

// convert to pulse and write to servo 
void write_servo(int ch, int ang){ 
  int pulse = map(ang, 0, 180, SERVOMIN_VAL, SERVOMAX_VAL); // convert angle to servo pulse value
  Serial.print("Pulse width = ");
  Serial.println(pulse);  
  pwm.setPWM(ch, 0, pulse);
}


//********** i2c_read **********//
int i2c_read(int addr, int cnt= 1, int *buf=NULL)
{
  if (cnt<1 or 128<=cnt) return(-1);
  int   rtn_code, i, dat;

  Wire.beginTransmission(I2C_AK8963_addr);
  Wire.write(addr);
  rtn_code= Wire.endTransmission(false);
  if (rtn_code!=0) return(-1);

  Wire.requestFrom(I2C_AK8963_addr, cnt);
  for (i=0; i<cnt; i++)
  {
    dat=Wire.read();
    if (buf==NULL)
      break;
    else
      buf[i]=dat;
  }

  while (Wire.available()) Wire.read();

  if (buf == NULL)
    return(dat);
  else 
    return(buf[0]);
}


//********** i2c_write **********//
int i2c_write(int addr, int dat)
{
  int rtn_code;

  Wire.beginTransmission(I2C_AK8963_addr);
  Wire.write(addr);
  Wire.write(dat);
  rtn_code=Wire.endTransmission();
  if (rtn_code!=0) 
    return (-1);
  else 
    return(0);
}

//********** read_serial **********//
int read_serial()
{
  if (Serial.available()){
    char ch = Serial.read();
    return((int) ch );
  }
  else return(0);
}



//********** mag_read **********//
int mag_read(float &x, float &y, float &z)
{
  int   rtn_code;
  delayMicroseconds(AK_WAIT);
  rtn_code=i2c_write(AK_CNTL1, AK_MOD_READ_METHOD | AK_MOD_BIT);
  if (rtn_code!=0) {
    Serial.println("Error in configuration setting for AK8963");
    return(-1);
  }

  while ((i2c_read(AK_ST1) and 0x01)==0); // wait until ready.

  i2c_read(AK_HXL, 7,i2c_buf); // read measure data and status2 
  if (i2c_buf[6] & 0x08)        // check sensor overflow in status2
  {
    Serial.println(" Sensor overflow ");
    return (-1);
  }

  x=int ((i2c_buf[1]<<8) + i2c_buf[0]);
  y=int ((i2c_buf[3]<<8) + i2c_buf[2]);
  z=int ((i2c_buf[5]<<8) + i2c_buf[4]);

  if (x>32767) x= x-65536;
  if (y>32767) y= y-65536;
  if (z>32767) z= z-65536;
  
  x+=x_bias;
  y+=y_bias;
  z+=z_bias;

  return (0);
}

//********** calib_mag **********//
void calib_mag()
{
  int   i, pre_cnt = 10;
  float xmin=9999,xmax=0,ymin=9999,ymax=0,zmin=9999,zmax=0;
  float x,y,z;
  int   calib_count = 150;
  float cx[calib_count];
  float cy[calib_count];
  float cz[calib_count];
  x_bias =0;
  y_bias =0;
  z_bias =0;
 
  Serial.println("Entering Mag");

  for (i=0; i<=pre_cnt; i++){
    delay(1000);
    Serial.print(pre_cnt-i);
    Serial.println(" sec reamin for calibration.");
  }
  
  Serial.println(" Start calibration, please rotate the sensor.");
  for (i=0; i<calib_count-1; i++){
    mag_read(x,y,z);
    Serial.print(calib_count-i);
    Serial.print("/");
    Serial.print(calib_count);
    Serial.print(": x="); Serial.print(x);
    Serial.print(": y="); Serial.print(y);
    Serial.print(": z="); Serial.println(z);
    cx[i]=x;
    cy[i]=y;
    cz[i]=z;
    if(xmin>x) xmin = x;
    if(ymin>y) ymin = y;
    if(zmin>z) zmin = z;
    if(xmax<x) xmax = x;
    if(ymax<y) ymax = y;
    if(zmax<z) zmax = z;
    delay(100);
  }
  x_bias = 0 - (xmax + xmin) / 2;
  y_bias = 0 - (ymax + ymin) / 2;
  z_bias = 0 - (zmax + zmin) / 2;
  x_gain = 1 / ((xmax - xmin) / 2);
  y_gain = 1 / ((ymax - ymin) / 2);
  z_gain = 1 / ((zmax - zmin) / 2);
  Serial.print("Calibrated Values are X Bias ="); Serial.print(x_bias);
  Serial.print(", Y Bias "); Serial.print(y_bias,3);
  Serial.print(", Z Bias "); Serial.print(x_bias,3);
  Serial.print(", X Gain "); Serial.print(x_gain,5);
  Serial.print(", Y Gain "); Serial.print(y_gain,5);
  Serial.print(", Z Gain "); Serial.println(z_gain,5);
  
  Serial.println("Wait a 15 seconds");
  delay(15000);
}

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

  pwm.begin();                // no connection for address on PCA9685 0x40
  pwm.setPWMFreq(50);         // pwm period
 
  Wire.begin();
  if (i2c_read(AK_ID) != AK_I2C_ADD){
     Serial.println("NOT FOUND AK8963");
  }
  delayMicroseconds(AK_WAIT);
  
}

//********** loop **********//
void  loop(void)
{
  int ch_i = read_serial();
  if ( ch_i >= 49){ // ch_i = '1' 
    Serial.println("Calib Start");
    calib_mag();
  }
  float x,y,z;
  int rad;
  mag_read(x,y,z);
  rad=atan(y/x)*(180/3.1416) + ((x>=0)? 0: (y<0)? -180: 180);
  Serial.print(rad); 
  Serial.print(": "); 
  Serial.print(x,5); 
  Serial.print(","); 
  Serial.print(y,5); 
  Serial.print(","); 
  Serial.println(z,5); 

// control 
static float new_angle;

  if (rad < -90 or rad > 175){
    rad = 175;
  }
  else{
    if (rad < 5){
      rad = 5;
    }
  }

  new_angle =k_rc*new_angle+(1.0-k_rc)*rad;  // RC filter

// Servo
  write_servo(SERVO_CH, new_angle);
  Serial.print("Angle = ");
  Serial.println(new_angle);  
}

磁気センサの校正について (キャリブレーション)

今回使用したAK8963でそのまま読み取ると以下の感じである。

(マンションの中で、15cm程度の近くにPCや汎用電源、スマホがある状態で使っている。)

まず、目的はローバーへの搭載のためなので、基本水平として想定しZ軸は無視。

とは言いつつも、地軸は斜めになっているので以下のイメージとなる。

でも、欲しいのは方位なので、実際にはX軸、Y軸さえあればよい。ローバーがその場で360度回転するとすると地軸の動作はぐるっと回る。 まぁ。 結局X-Yでいいのだが。

では、実際にサンプルデータとして水平に置いて水平旋回させて読み取ったデータを散布図に表してみると。

思った以上に円になってる。中心がずれているのはしょうがない。

ここでの構成としては、150個のデータを取得して各軸の最大値/最小値を取得して中点を求めるもの。

至極簡単だが、校正した結果は下記となる。

校正自体より磁気センサとの通信に時間がかかったというのが正直なところ。 このプログラムはシリアルモニターで見ていれば、校正結果がモニターに一定時間表示するので、その値をプログラムの中にハードコーディングしておくことを進める。

また、キャリブレーション指令としてはシリアルモニタに’1’を入力してください。

約10秒間の待ち時間があり、ここでセンサを水平旋回する準備。
測定は約15秒間で150センシングします。 この間に2-3周回するとよいでしょう。

すると 測定されたBIASが表示されるので参考としてください。

Gainは正規可用途で作っていましたが、今のところどこでも使っていません。

この測定されたBIASは自動的に磁気センサからの読み込みデータの補正に使用されますが、

電源を切ってしまうと無くなってしまうので、この値を初期値として入れておくことを薦めます。

磁気センサについて MPU-9250 (AK8963)

これまで、方位についてはGPSから入力される信号を用いていたのだが、

利点として

・移動時の測定においては移動方向の方位が取得できる。

・GPSの精度が良ければ方位も十分な精度が期待できる。

 ここでいう精度とは再現性である。

欠点として

・移動していないと方位の取得が取れない

これまでは、基本的に移動している状態で方位を算出していたので、GPSでの方位取得を行ってきた。

しかし、今回は必要性に迫られた部分もあり磁気センサから方位算出を行うこととした。

使用したのは、 MPU9250 と呼ばれる9軸センサ。
ジャイロ、加速度、磁気センサが入って9軸というわけだが、今後の遊びも含めてということでこれを選定。

値段もまぁ許される程度です。

しかし、中身はちょっと素直ではなさそう。

いくつかのサンプルを試してみたが、高速で読もうとすると時々0が戻ってきたりする。 サンプルが悪い気もしなくはないのだが、データシートを見ると、磁気センサはSDA/SCLというよりも AUX_CL,AUX_DAに接続されているので、そちらをI2Cと想定して試してみた。 というわけであくまで保証はできなく参考程度にしてください。 また、この磁気センサは旭化成の AK8963 であり、使い方などはAK8963 のデータシートが参考になる。 下記はMPU-925のブロック図です。

ESP32との配線は、以下の感じ。

MPU-9250のSCL/SDAには接続せずEDA/ECLに配線することによって磁気センサに直接アクセスが可能となる。

参考のためにソースコードを記載する。 転用などは特に連絡不要ですが無保証です。

また、勝手な想像によるキャリブレーション 機能も搭載しています。

// This software is released under the MIT License.
// See license.txt at http://moon.robots.jp/ehp/ 
// The software is provided "AS IS", without warranty of any kind, 
// express or implied, including but not limited to the warranties
// of merchantability. (c) 2021 Team Katy
// 
// 2021/5/23 ver 1.0 first released by S.K
//
//

#include <Wire.h>

const int   I2C_AK8963_addr=0x0c;   // i2c: ak8963 slave address

////// AK8963 register 
const int AK_ID= 0x00;  //  00H READ  Device ID; ID is 0x48
const int AK_ST1= 0x02; //  02H READ  Status 1  DataStatus; D0:True; data ready D1:True data overrun
const int AK_HXL= 0x03; //  03H READ  X-axis data 8190:4912uT -8190:-4912uT
const int AK_CNTL1= 0x0A; //  0AH READ/WRITE  Control1  Function Control
                          // see data sheet D0:3 mode; D4:0 is 14 bit, 1 is 16bit
const int AK_ASAX= 0x10;  //  10H READ  X-axis sensitivity adjustment value Fuse ROM
const int   AK_WAIT=100;     // Wait for mode change [us]
const int   AK_MOD_PWRDWN=0x0;   // Power down mode 
const int   AK_MOD_READ_METHOD=0x1;   
                // Read method:0x1:Single
                //             0x2:Continus 8Hz
                //             0x3:Continus 100Hz
const int   AK_MOD_FUSE=0x0f;    // mode change to read FUSE
const int   AK_MOD_BIT=0x10;   // # of read bits: 0x10:16 bit, 0x00:14 bit
const int   AK_I2C_ADD=0x48;   // I2C address for AK8963
static float x_bias = 0; // calibration value
static float x_gain = 0; // calibration value
static float y_bias = 0; // calibration value
static float y_gain = 1; // calibration value
static float z_bias = 1; // calibration value
static float z_gain = 1; // calibration value

static int  i2c_buf[8];    // read buffer for I2C
const int   max_char=8;    // max read characters from serial monitor

//********** i2c_read **********//
int i2c_read(int addr, int cnt= 1, int *buf=NULL)
{
  if (cnt<1 or 128<=cnt) return(-1);
  int   rtn_code, i, dat;

  Wire.beginTransmission(I2C_AK8963_addr);
  Wire.write(addr);
  rtn_code= Wire.endTransmission(false);
  if (rtn_code!=0) return(-1);

  Wire.requestFrom(I2C_AK8963_addr, cnt);
  for (i=0; i<cnt; i++)
  {
    dat=Wire.read();
    if (buf==NULL)
      break;
    else
      buf[i]=dat;
  }

  while (Wire.available()) Wire.read();

  if (buf == NULL)
    return(dat);
  else 
    return(buf[0]);
}

//********** i2c_write **********//
int i2c_write(int addr, int dat)
{
  int rtn_code;

  Wire.beginTransmission(I2C_AK8963_addr);
  Wire.write(addr);
  Wire.write(dat);
  rtn_code=Wire.endTransmission();
  if (rtn_code!=0) 
    return (-1);
  else 
    return(0);
}

//********** mag_read **********//
int mag_read(float &x, float &y, float &z)
{
  int   rtn_code;
  delayMicroseconds(AK_WAIT);
  rtn_code=i2c_write(AK_CNTL1, AK_MOD_READ_METHOD | AK_MOD_BIT);
  if (rtn_code!=0) {
    Serial.println("Error in configuration setting for AK8963");
    return(-1);
  }

  while ((i2c_read(AK_ST1) and 0x01)==0); // wait until ready.

  i2c_read(AK_HXL, 7,i2c_buf); // read measure data and status2 
  if (i2c_buf[6] & 0x08)        // check sensor overflow in status2
  {
    Serial.println(" Sensor overflow ");
    return (-1);
  }

  x=int ((i2c_buf[1]<<8) + i2c_buf[0]);
  y=int ((i2c_buf[3]<<8) + i2c_buf[2]);
  z=int ((i2c_buf[5]<<8) + i2c_buf[4]);

  if (x>32767) x= x-65536;
  if (y>32767) y= y-65536;
  if (z>32767) z= z-65536;
  
  x+=x_bias;
  y+=y_bias;
  z+=z_bias;

  return (0);
}

//********** calib_mag **********//
void calib_mag()
{
  int   i, pre_cnt = 10;
  float xmin=9999,xmax=0,ymin=9999,ymax=0,zmin=9999,zmax=0;
  float x,y,z;
  int   calib_count = 150;
  float cx[calib_count];
  float cy[calib_count];
  float cz[calib_count];
  x_bias =0;
  y_bias =0;
  z_bias =0;
 
  Serial.println("Entering Mag");

  for (i=0; i<=pre_cnt; i++){
    delay(1000);
    Serial.print(pre_cnt-i);
    Serial.println(" sec reamin for calibration.");
  }
  
  Serial.println(" Start calibration, please rotate the sensor.");
  for (i=0; i<calib_count-1; i++){
    mag_read(x,y,z);
    Serial.print(calib_count-i);
    Serial.print("/");
    Serial.print(calib_count);
    Serial.print(": x="); Serial.print(x);
    Serial.print(": y="); Serial.print(y);
    Serial.print(": z="); Serial.println(z);
    cx[i]=x;
    cy[i]=y;
    cz[i]=z;
    if(xmin>x) xmin = x;
    if(ymin>y) ymin = y;
    if(zmin>z) zmin = z;
    if(xmax<x) xmax = x;
    if(ymax<y) ymax = y;
    if(zmax<z) zmax = z;
    delay(100);
  }
  x_bias = 0 - (xmax + xmin) / 2;
  y_bias = 0 - (ymax + ymin) / 2;
  z_bias = 0 - (zmax + zmin) / 2;
  x_gain = 1 / ((xmax - xmin) / 2);
  y_gain = 1 / ((ymax - ymin) / 2);
  z_gain = 1 / ((zmax - zmin) / 2);
  Serial.print("Calibrated Values are X Bias ="); Serial.print(x_bias);
  Serial.print(", Y Bias "); Serial.print(y_bias,3);
  Serial.print(", Z Bias "); Serial.print(x_bias,3);
  Serial.print(", X Gain "); Serial.print(x_gain,5);
  Serial.print(", Y Gain "); Serial.print(y_gain,5);
  Serial.print(", Z Gain "); Serial.println(z_gain,5);
  
  Serial.println("Wait a 15 seconds");
  delay(15000);
}

//********** read_serial **********//
int read_serial()
{
  if (Serial.available()){
    char ch = Serial.read();
    return((int) ch );
  }
  else return(0);
}

//********** setup **********//
void  setup (void)
{
  Serial.begin(115200);
  Wire.begin();
  if (i2c_read(AK_ID) != AK_I2C_ADD) Serial.println("NOT FOUND AK8963");
  delayMicroseconds(AK_WAIT);
  
}

//********** loop **********//
void  loop(void)
{
  int ch_i = read_serial();
  Serial.print("ch = ");   Serial.print(ch_i); 
  
  if ( ch_i >= 49){ // ch_i = '1' 
    Serial.println("Calib Start");
    calib_mag();
  }
  float x,y,z;
  int rad;
  mag_read(x,y,z);
  rad=atan(y/x)*(180/3.1416) + ((x>=0)? 0: (y<0)? -180: 180);
  Serial.print(rad); 
  Serial.print(": "); 
  Serial.print(x,5); 
  Serial.print(","); 
  Serial.print(y,5); 
  Serial.print(","); 
  Serial.println(z,5); 
}