プログラムの方も形が整ったので、書いておこう。ロボットの頭頂部につけたセンサーをSPIで動かして捉えた結果の図は次のようになった。
Y軸方向（横方向）にロボットを揺らした。結果、X軸方向に細かい揺れも起こっているが、さすがに上下（Z軸方向）には動いていない。縦方向には、重力１（1g）がかかったままである。また、最初から、X軸方向に少し傾きがあることもわかる。
仕様書によると、データは、上記のSDOの様に入ってくる。基本３バイトの最初のバイトは、軸の指定。その後に続く２バイトは軸のデータの上位１バイトと下位１バイトの順に入ってくる。レジスタ二つ使っているので、３バイトのデータを送りながら２倍とずつずらすという作業が必要だ。以下のプログラムでその様にしているのがわかるでしょう。
したがって、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;
}
