日別: 2017年6月9日

サーボの制御も、センサーの制御も全てJAVAでできるようになった。脳に当たるディープラーニングするニューラルネットもJAVAで動かすので、JAVAで閉じられる。
さしあたって、ロボットの制御プログラムをC++から、JAVAに書き換えよう。

加速度センサーKXSD9のデータをJavaで取ろうとしたら、問題にはまって、なかなか解決しなかったがようやく原因がわかって綺麗なデータを取れるようになった。
初めは、なんか、データが飛び飛びになってうまくいかなかったのだ。こんな感じである。さっぱりダメだった。

先の投稿にも書いたように、電圧のせいかと思って、電源からDCコンバーターM78AR033-0.5を入れてコンデンサーつけたりして3.3Vを独立に確保したが、これも結局問題解決にならず。元のC++のプログラムだと綺麗にデータが取れるので、配線の間違いでもセンサーの問題でもRaspberryPIの問題でもない。
結局わかった。問題は、WiringPi経由でデータを取得するとき、C++の場合は、unsigned charのバッファで取得するのだが、Javaの場合はbyte配列を用いる。それが当然だと思って、そうしているだけで何かに暗示されたわけではない。
私はてっきり、byteはunsigned charと同じで整数変数にキャストして入れても負数にはならないだろうと思いこんでいた。つまり、0-255の範囲の値を出すだろうと思っていたのだが、実は、-127から+127の値を出力して、整数変数に自動キャストで入れると突然負の値が出てきてしまうのだ。
センサーデータは、12ビットで出てくるので、C++の場合、一つの軸（例えばX軸）のデータは、以下のように変換する。
xregH = spi_buff[0][1];
xregL = spi_buff[0][2];
xout = xregH << 4 | xregL >> 4;
spi_buffはunsigned charのバッファで、xregH、xregLは上位ビットの整数値、下位ビットの整数値を取り出して、上位を4ビット左にシフトさせ、下位ビットを4ビット右にシフトさせて論理和を取れば12ビットの値が取れる。そこに負が出てくるなど想定しなくていい。
ところが、同じことをunsigned charの代わりにJAVAのbyteでやると簡単に負になってしまって、予期しない値が取れてしまうというわけだ。結局、整数に変換しするときに上位ビットをマスクするという操作が必要だった。
xregH = (spi_buff[0][1] & 0xff);
xregL = (spi_buff[0][2] & 0xff);
xout = xregH << 4 | xregL >> 4;
こうすればうまくいくことがわかったのである。
ちょんちょん！！
JAVAのプログラムは次のようになる。Pi4Jのライブラリが必要になることは、前にこちらに書いたのでそれを参考にしてほしい。

package aicoro;
import com.pi4j.wiringpi.Gpio;
import com.pi4j.wiringpi.Spi;
/**
 *
 * @author Toyoaki WASHIDA
 */
public class Accelerometer {
    int speed = 1000000; //通信速度(Hz)
    int SPI_CHANNEL = Spi.CHANNEL_1;
    int SS_PORT = 7;
    Accelerometer() throws InterruptedException{
        // setup SPI for communication
        System.out.println("SPIチャンネル初期化します");
        int fd = Spi.wiringPiSPISetup(SPI_CHANNEL, speed);
        if (fd <= -1) { System.out.println(" ==>> SPI SETUP FAILED");
            return;
        }
        System.out.println("PIO初期化します");
        if (Gpio.wiringPiSetup() == -1) {
            System.out.println(" ==>> GPIO SETUP FAILED");
            return;
        }
        Gpio.pinMode (SS_PORT, Gpio.OUTPUT);
        //SS信号初期化
        Gpio.digitalWrite(SS_PORT, 1);
        Thread.sleep(0, 130);
        byte com[] = new byte[2];
        com[0] = 0x0c;  // address byte
        com[1] = (byte) 0xe3;  // register byte
        // 設定を（+/-2g 819 counts/g）にして、小さい範囲を細かく見ている
        Gpio.digitalWrite(SS_PORT, 0);
        int err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
        Gpio.digitalWrite(SS_PORT, 1);
        if (err <= -1) {
            System.out.println("加速度センサー設定エラー（１）");
            return;
        }
        Thread.sleep(0, 130);
        com[0] = 0x0d;
        com[1] = 0x40; // こちらはデフォルト設定のまま
        Gpio.digitalWrite(SS_PORT, 0);
        err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
        Gpio.digitalWrite(SS_PORT, 1);
        if (err <= -1) {
            System.out.println("加速度センサー設定エラー（２）");
        }
        /////////////////設定状況を読み取る/////////////////////////////
        Thread.sleep(0, 130);
        com[0] = (byte) 0x8c;
        com[1] = 0x00; // こちらはデフォルト設定のまま
        Gpio.digitalWrite(SS_PORT, 0);
        err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
        Gpio.digitalWrite(SS_PORT, 1);
        if (err <= -1) {
            System.out.println("加速度センサー設定エラー（２）");
        }
        System.out.println("CTL_REGCの値 = "+String.format("0x%02x",com[1] ));
        Thread.sleep(0, 130);
        com[0] = (byte) 0x8d;
        com[1] = 0x00; // こちらはデフォルト設定のまま
        Gpio.digitalWrite(SS_PORT, 0);
        err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, com, 2); //データ送受信
        Gpio.digitalWrite(SS_PORT, 1);
        if (err <= -1) {
            System.out.println("加速度センサー設定エラー（２）");
        }
        System.out.println("CTL_REGBの値 = "+String.format("0x%02x",com[1] ));
    }
    double[] sensorValue() throws InterruptedException {
        // デバイスからデータ取得
        double [] sense = new double[3];
        byte [][] spi_buff = new byte[3][3];    //送受信用バッファ
        int xregH, xregL, xout;
        int yregH, yregL, yout;
        int zregH, zregL, zout;
        double xac, yac, zac;
        for (int i = 0; i < 3; i++) {
            spi_buff[i][0] = (byte)(0x80 + 2 * i);
            spi_buff[i][1] = 0;
            spi_buff[i][2] = 0;
            Gpio.digitalWrite(SS_PORT, 0);
            int err = Spi.wiringPiSPIDataRW(SPI_CHANNEL, spi_buff[i], 3); //データ送受信
            Gpio.digitalWrite(SS_PORT, 1);
            Thread.sleep(1, 130);
            if (err == -1) {
                System.out.println("sensorValue: 書き込みエラー(1)");
                break;
            }
            //System.out.println("データを受け取りました ret = "+err);
        }
        xregH = (spi_buff[0][1] & 0xff);
        xregL = (spi_buff[0][2] & 0xff);
        //System.out.println("xregH = "+xregH+" xregL = "+xregL); //(short) (valueByte & 0xFF);
        xout = xregH << 4 | xregL >> 4;
        yregH = (spi_buff[1][1] & 0xff);
        yregL = (spi_buff[1][2] & 0xff);
        yout = yregH << 4 | yregL >> 4;
        zregH = (spi_buff[2][1] & 0xff);
        zregL = (spi_buff[2][2] & 0xff);
        zout = zregH << 4 | zregL >> 4;
        xac = (double) (xout - 2048) / (double) 819;
        yac = (double) (yout - 2048) / (double) 819;
        zac = (double) (zout - 2048) / (double) 819;
        //
        sense[0] = xac;
        sense[1] = yac;
        sense[2] = zac;
        return sense;
    }
}