投稿者: ehp

ローバーには大きく分けてスピードコントローラ、ステアリングサーボなど複数の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);  
}