#include"mbed.h"
//ポート設定
BufferedSerial uart_usb(USBTX, USBRX,115200); //USB接続
DigitalOut led1(LED1); //ボード上のLED
I2C i2c(PB_9, PB_8); //ジャイロ、加速度センサ
//=========================================================
//ジャイロ加速度センサ設定
int sample_num = 100;
float meas_interval = 0.01;
float theta_mean;
float theta_variance;
float theta_dot_mean;
float theta_dot_variance;
//=========================================================
//=========================================================
// I2C common functions
//=========================================================
//i2c write function
void i2c_mem_write(int device_address, int mem_address, int mem_data)
{
int device_address_temp = device_address<<1;
device_address_temp = device_address_temp & 0xfe;
i2c.start();
i2c.write(device_address_temp);
i2c.write(mem_address);
i2c.write(mem_data);
i2c.stop();
return;
}
//i2c read function
int i2c_mem_read(int device_address, int mem_address)
{
int device_address_temp = device_address<<1;
int device_address_temp_w = device_address_temp & 0xfe;
int device_address_temp_r = device_address_temp | 0x01;
i2c.start();
i2c.write(device_address_temp_w);
i2c.write(mem_address);
i2c.write(device_address_temp_r);
int data = i2c.read(0);
i2c.stop();
return data;
}
//=========================================================
// Accelerometer (BMX055)
//=========================================================
//get data
float get_acc_data()
{
//read ACCD_Y_LSB registor (0x04)
int y_temp_L = i2c_mem_read(0x19, 0x04);
y_temp_L = y_temp_L >> 4;
y_temp_L = y_temp_L & 0x0f;
//read RATE_Y_MSB registor (0x05)
int y_temp_H = i2c_mem_read(0x19, 0x05);
//calculate Y acceleration
int y_data = y_temp_L + 16 * y_temp_H;
if(y_data > 2047)
{
y_data = -1 * (4096 - y_data);
}
//-----------------------------------------------------
//read ACCD_Z_LSB registor (0x06)
int z_temp_L = i2c_mem_read(0x19, 0x06);
z_temp_L = z_temp_L >> 4;
z_temp_L = z_temp_L & 0x0f;
//read RATE_Z_MSB registor (0x07)
int z_temp_H = i2c_mem_read(0x19, 0x07);
//calculate Z acceleration
int z_data = z_temp_L + 16 * z_temp_H;
if(z_data > 2047)
{
z_data = -1 * (4096 - z_data);
}
//-----------------------------------------------------
//calculate theta
float theta_deg = atan( float(y_data) / float(z_data) );
return (float)theta_deg * 57.29578f; //degree
}
//statistical data of accelerometer
void acc_init()
{
//initialize ACC register 0x0F (range)
//Full scale = +/- 2 G
i2c_mem_write(0x19, 0x0f, 0x03);
//initialize ACC register 0x10 (band width)
//Filter bandwidth = 1000 Hz
i2c_mem_write(0x19, 0x10, 0x0f);
//get data
float theta_array[sample_num];
for(int i=0; i<sample_num; i++)
{
theta_array[i] = get_acc_data();
wait_us( meas_interval );
}
//calculate mean
theta_mean = 0;
for(int i=0; i<sample_num; i++)
{
theta_mean += theta_array[i];
}
theta_mean /= sample_num;
//calculate variance
float temp;
theta_variance = 0;
for(int i=0; i<sample_num; i++)
{
temp = theta_array[i] - theta_mean;
theta_variance += temp*temp;
}
theta_variance /= sample_num;
return;
}
//=========================================================
// Gyroscope (BMX055)
//=========================================================
//get data
float get_gyro_data()
{
//read RATE_X_LSB registor (0x02)
int x_temp_L = i2c_mem_read(0x69, 0x02);
//read RATE_X_MSB registor
int x_temp_H = i2c_mem_read(0x69, 0x03);
//calculate X angular ratio
int x_data = x_temp_L + 256 * x_temp_H;
if(x_data > 32767)
{
x_data = -1 * (65536 - x_data);
}
x_data = -1 * x_data;
// +1000 (deg/sec) / 2^15 = 0.0305176
return float(x_data) * -0.0305176f; // deg/sec
}
//statistical data of gyro
void gyro_init()
{
//initialize Gyro register 0x0F (range)
//Full scale = +/- 1000 deg/s
i2c_mem_write(0x69, 0x0f, 0x01);
//initialize Gyro register 0x10 (band width)
//Data rate = 1000 Hz, Filter bandwidth = 116 Hz
i2c_mem_write(0x69, 0x10, 0x02);
//get data
float theta_dot_array[sample_num];
for(int i=0;i<sample_num;i++)
{
theta_dot_array[i] = get_gyro_data();
wait_us(meas_interval);
}
//calculate mean
theta_dot_mean = 0;
for(int i=0;i<sample_num;i++)
{
theta_dot_mean += theta_dot_array[i];
}
theta_dot_mean /= sample_num;
//calculate variance
float temp;
theta_dot_variance = 0;
for(int i=0; i<sample_num; i++)
{
temp = theta_dot_array[i] - theta_dot_mean;
theta_dot_variance += temp*temp;
}
theta_dot_variance /= sample_num;
return;
}
int main()
{
int num = 1 ;
//-------------------------------------------
//I2C initialization
//-------------------------------------------
i2c.frequency(400000); //400 kHz
//UART initialization
//uart_usb.baud(115200); //115.2 kbps
uart_usb.set_format(8, BufferedSerial::None, 1);
printf(“==角度==角速度==\n”);
//-------------------------------------------
//Accelerometer & Gyro initialization
//-------------------------------------------
//センサの初期化
acc_init();
gyro_init();
while(1)
{
printf("%f , %f\n", get_acc_data(), get_gyro_data() );
}
}