LancerStratos’s blog

第23回ロボットグランプリに向けたロボットランサー”ランサーストラトス”開発記録

新しいIMUの実験（その１）

色々なことがあり、しばらく書き込みをしていませんでした。

前回は秋月PicoのＬチカをやりましたが、今回は秋月の９軸センサフュージョンモジュールBNO055との接続を試みました。やはりプログラミング素人にとっては開発環境の変化は影響が大きく、参考になりそうなウェブ上の書き込みを探しては読み比べてその意味を理解するのに時間がかかるので、かなりしんどいです。

色々と調べているうちに、ようやく有力な参考に辿り着きました。Youtube動画（Raspberry Pi Pico での I2C の初心者ガイド (BNO055 IMU の例) - YouTube）のサンプルコードを使って、３軸の加速度をTeraTermでモニタできました。

ハードウェアの接続は、ほぼサンプル通りですが、Adafruits製BNO055ブレークアウトボードでは信号ラインがプルアップされているところ、秋月BNO055モジュールにはそれが無いため、SDA、SCLラインをそれぞれ10kの抵抗を介してプルアップしています。なお、Adafruitsのよりも秋月製モジュールの方が、BNO055マニュアルに記載された回路例に近いものになっているようですが、ほぼ同じと考えていいと思います。

秋PicoとBNO055モジュールの接続

CMakeListsは以下のとおりです。後で分かりやすいようにコメントを充実させました。

#Set the mimum required version of CMake
cmake_minimum_required(VERSION 3.12)

# Pull in SDK (must be before project)
include(pico_sdk_import.cmake)

#Set the project name, the programming language
project(akiPico-i2c-akiBNO055)
set(CMAKE_C_STANDARD 11)
set(CMAKE_CXX_STANDARD 17)

# Initialize the SDK
pico_sdk_init()

#Add an executable target to be build from the source files
add_executable(picoi2c055
    picoi2c055.c
)

#Pull in our pico_stdlib which aggregates commonly used features
target_link_libraries(picoi2c055
    pico_stdlib
    hardware_i2c
)

#Enable USB output, disable UART output
pico_enable_stdio_usb(picoi2c055 1)
pico_enable_stdio_uart(picoi2c055 0)

#Create map/bin/hex/uf2 file etc.
pico_add_extra_outputs(picoi2c055)

 

次にCソースファイルpicoi2c055.cです。

#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/i2c.h"

#define I2C_PORT i2c0

static int addr = 0x28;

// Initialise Accelerometer Function
void accel_init(void){
    // Check to see if connection is correct
    sleep_ms(1000); // Add a short delay to help BNO005 boot up
    uint8_t reg = 0x00;
    uint8_t chipID[1];
    i2c_write_blocking(I2C_PORT, addr, &reg, 1, true);
    i2c_read_blocking(I2C_PORT, addr, chipID, 1, false);

    if(chipID[0] != 0xA0){
        while(1){
            printf("Chip ID Not Correct - Check Connection!");
            sleep_ms(5000);
        }
    }

    // Use external oscillator; bit7 to 1
    uint8_t data[2];
    data[0] = 0x3F;
    data[1] = 0xC0; //11000000  set bit6 to "1" to reset all interrupt status bits
    i2c_write_blocking(I2C_PORT, addr, data, 2, true);

    // Rewrite to default value of bit6 and set bit0 to "1" to start Built in Test function
    data[0] = 0x3F;
    data[1] = 0x81; //10000001
    i2c_write_blocking(I2C_PORT, addr, data, 2, true);
    //　多分、Built In Testが終わるとbit0は自動的に0に戻るものと思われる。

    // Configure Power Mode
    data[0] = 0x3E;
    data[1] = 0x00;
    i2c_write_blocking(I2C_PORT, addr, data, 2, true);
    sleep_ms(50);

    // Defaul Axis Configuration
    data[0] = 0x41;
    data[1] = 0x24;
    i2c_write_blocking(I2C_PORT, addr, data, 2, true);

    // Default Axis Signs
    data[0] = 0x42;
    data[1] = 0x00;
    i2c_write_blocking(I2C_PORT, addr, data, 2, true);

    // Set units to m/s^2
    data[0] = 0x3B;
    data[1] = 0b0001000;
    i2c_write_blocking(I2C_PORT, addr, data, 2, true);
    sleep_ms(30);

    // Set operation to acceleration only
    data[0] = 0x3D;
    data[1] = 0x0C; //00001100  is Fusion mode NDoF
    i2c_write_blocking(I2C_PORT, addr, data, 2, true);
    sleep_ms(100);
}

int main(void){
    stdio_init_all(); // Initialise STD I/O for printing over serial

    // Configure the I2C Communication
    i2c_init(I2C_PORT, 400 * 1000);
    gpio_set_function(4, GPIO_FUNC_I2C);
    gpio_set_function(5, GPIO_FUNC_I2C);
    gpio_pull_up(4);
    gpio_pull_up(5);

    // Call accelerometer initialisation function
    accel_init();

    uint8_t accel[6]; // Store data from the 6 acceleration registers
    int16_t accelX, accelY, accelZ; // Combined 3 axis data
    float f_accelX, f_accelY, f_accelZ; // Float type of acceleration data
    uint8_t val = 0x08; // Start register address

    // Infinite Loop
    while(1){
        i2c_write_blocking(I2C_PORT, addr, &val, 1, true);
        i2c_read_blocking(I2C_PORT, addr, accel, 6, false);

        accelX = ((accel[1]<<8) | accel[0]);
        accelY = ((accel[3]<<8) | accel[2]);
        accelZ = ((accel[5]<<8) | accel[4]);

        f_accelX = accelX / 100.00;
        f_accelY = accelY / 100.00;
        f_accelZ = accelZ / 100.00;

        // Print to serial monitor
        printf("X: %6.2f    Y: %6.2f    Z: %6.2f\n", f_accelX, f_accelY, f_accelZ);
        sleep_ms(300);
    }
}

ソースファイル中の太字部分がサンプルからの修正箇所ですが、これはセンサモジュールに搭載されている外部クロックを使用するためのものです。

BNO055は内蔵ファームウェアで処理済みの安定したオイラー角他を出力できるのが最大の魅力ですが、現時点では３軸の加速度をそのまま取り出しているだけです。とはいえ最初の接続ができるまでが一番大変なので、大きな山は越えたと思っています。今後、他の信号を取り出せるようにプログラムを改造していきます。