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