3軸加速度センサーKXSD9-2050をRaspberryPIのSPIで動かす(2)

プログラムの方も形が整ったので、書いておこう。ロボットの頭頂部につけたセンサーをSPIで動かして捉えた結果の図は次のようになった。

Y軸方向(横方向)にロボットを揺らした。結果、X軸方向に細かい揺れも起こっているが、さすがに上下(Z軸方向)には動いていない。縦方向には、重力1(1g)がかかったままである。また、最初から、X軸方向に少し傾きがあることもわかる。

仕様書によると、データは、上記のSDOの様に入ってくる。基本3バイトの最初のバイトは、軸の指定。その後に続く2バイトは軸のデータの上位1バイトと下位1バイトの順に入ってくる。レジスタ二つ使っているので、3バイトのデータを送りながら2倍とずつずらすという作業が必要だ。以下のプログラムでその様にしているのがわかるでしょう。

したがって、C++のint値に変換するには、まず、intでとった上位ビットのデータを4ビットだけ左にシフトさせる。下位ビットは左側に4ビットしかないので、とったintデータを4ビットだけ右にシフトさせて、両者の論理和を取ると一つの整数データになることがわかる。以下のプログラムでそのような作業をしている。

この辺りの複雑さは、I2Cでやってもほぼ同じ。

以下にプログラムを貼り付ける。CとC++がごちゃまぜで、nanosleepなんかも使わなくても良いような気がするが、全部、動かしたまま貼り付けている。
※ 作成にあたって「Raspberry PiでSPI通信」を参照させていただいた。

#include <cstdlib>
#include <iostream>
#include <wiringPiSPI.h>
#include <wiringPi.h>
#include <stdio.h>
#include <time.h>
#include <errno.h>
using namespace std;

//定数定義
#define SS_PORT 8           //GPIO 8
#define SPI_CHANNEL 0       //SPIチャンネル

int main(void) {
    int speed; //通信速度(Hz)
    unsigned char spi_buff[3][3];    //送受信用バッファ
    struct timespec req;

    //sleep設定
    req.tv_sec = 0;
    req.tv_nsec = 130;    // 130ns

    speed = 1000000; //通信速度100kHz
    //バッファ初期化
    for(int i = 0; i < 3; i++){
        for(int j = 0; j < 3; j++){
            spi_buff[i][j] = 0x0;
        }
    }

    cout << "SPIチャンネル初期化します" << endl;
    int spi_fd = 0;
    if ((spi_fd=wiringPiSPISetup(SPI_CHANNEL, speed)) < 0) {//SPIチャンネル初期化
        printf("wiringPiSPISetup error \n");
        return -1;
    }
    cout << "(使わない)spi_fd = " << spi_fd << endl;
    cout << "PIO初期化します" << endl;
    if (wiringPiSetupGpio() == -1) { //GPIO初期化
        printf("wiringPiSetupGpio error\n");
        return -1;
    }
    pinMode(SS_PORT, OUTPUT); //GPIO8を出力に設定
    digitalWrite(SS_PORT, 1); //SS信号初期化
    unsigned char com[2];
    com[0] = 0x0c;
    com[1] = 0xe3; // デフォルト設定を変更している
    digitalWrite(SS_PORT, 0);
    wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
    digitalWrite(SS_PORT, 1);
    com[0] = 0x0d;
    com[1] = 0x40; // デフォルト設定のまま
    nanosleep(&req, NULL);
    digitalWrite(SS_PORT, 0);
    wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
    digitalWrite(SS_PORT, 1);
    int iter = 0;
    int err;

    int xregH, xregL, xout;
    int yregH, yregL, yout;
    int zregH, zregL, zout;
    double xac, yac, zac;

    while (true) {
        //送信用データをバッファにセット
        //printf("******** iter %d: ********** \n", iter);
        for(int i=0;i<3;i++) {
            spi_buff[i][0] = 0x80+2*i;
            spi_buff[i][1] = 0; 
            spi_buff[i][2] = 0; 
            //printf("write <%d = 0x%x>\n", i, spi_buff[i][0]); //受信データを出力
            digitalWrite(SS_PORT, 0); //SS信号をLOW出力にして通信開始
            err = wiringPiSPIDataRW(SPI_CHANNEL, spi_buff[i], 3); //データ送受信
            if(err == -1){
                cout << "書き込みエラー(1) errno = " << errno << endl;
                break;
            }
            digitalWrite(SS_PORT, 1); //SS信号をHIGH出力にして通信終了

        }
            // デバイスからデータ取得
        xregH = spi_buff[0][1];
        xregL = spi_buff[0][2];
        xout = xregH << 4 | xregL >> 4;

        yregH = spi_buff[1][1];
        yregL = spi_buff[1][2];
        yout = yregH << 4 | yregL >> 4;

        zregH = spi_buff[2][1];
        zregL = spi_buff[2][2];
        zout = zregH << 4 | zregL >> 4;

        xac = (double) (xout - 2048) / (double) 819;
        yac = (double) (yout - 2048) / (double) 819;
        zac = (double) (zout - 2048) / (double) 819;
        cout << "No. : " << iter << "  X軸 : " << xac << "   Y軸 : " << yac << "   Z軸 : " << zac << endl;
            
        iter++;
        delay(10);
        if(iter == 500) break;
    }
    return 0;
}