#include <stdio.h>
#include <stdlib.h>
#include <wiringPi.h>
#include <wiringPiSPI.h>
#define L6470_SPI_CHANNEL 0
// 関数プロトタイプ。
void L6470_write(unsigned char data);
void L6470_init(void);
void L6470_run(long speed);
int main(int argc, char **argv)
{
int i;
long speed = 0;
printf("***** start spi test program *****\n");
// SPI channel 0 を 1MHz で開始。
if (wiringPiSPISetup(L6470_SPI_CHANNEL, 1000000) < 0)
{
printf("SPI Setup failed:\n");
}
// L6470の初期化。
L6470_init();
while(1)
{
for (i = 0; i < 5; i++)
{
speed += 10000;
L6470_run(speed);
delay (5000);
}
for (i= 0; i < 5; i++)
{
speed -= 10000;
L6470_run(speed);
delay (5000);
}
}
return 0;
}
void L6470_write(unsigned char data)
{
wiringPiSPIDataRW(L6470_SPI_CHANNEL, &data, 1);
}
void L6470_init()
{
// MAX_SPEED設定。
/// レジスタアドレス。
L6470_write(0x07);
// 最大回転スピード値(10bit)
L6470_write(0x00);
L6470_write(0x20);
// KVAL_HOLD設定。
/// レジスタアドレス。
L6470_write(0x09);
// モータ停止中の電圧設定(8bit)
L6470_write(0xFF);
// KVAL_RUN設定。
/// レジスタアドレス。
L6470_write(0x0A);
// モータ定速回転中の電圧設定(8bit)
L6470_write(0xFF);
// KVAL_ACC設定。
/// レジスタアドレス。
L6470_write(0x0B);
// モータ加速中の電圧設定(8bit)
L6470_write(0xFF);
// KVAL_DEC設定。
/// レジスタアドレス。
L6470_write(0x0C);
// モータ減速中の電圧設定(8bit)
L6470_write(0xFF);
// OCD_TH設定。
/// レジスタアドレス。
L6470_write(0x13);
// オーバーカレントスレッショルド設定(4bit)
L6470_write(0x0F);
// STALL_TH設定。
/// レジスタアドレス。
L6470_write(0x14);
// ストール電流スレッショルド設定(4bit)
L6470_write(0x7F);
}
void L6470_run(long speed)
{
unsigned short dir;
unsigned long spd;
unsigned char spd_h;
unsigned char spd_m;
unsigned char spd_l;
// 方向検出。
if (speed < 0)
{
dir = 0x50;
spd = -1 * speed;
}
else
{
dir = 0x51;
spd = speed;
}
// 送信バイトデータ生成。
spd_h = (unsigned char)((0x0F0000 & spd) >> 16);
spd_m = (unsigned char)((0x00FF00 & spd) >> 8);
spd_l = (unsigned char)(0x00FF & spd);
// コマンド(レジスタアドレス)送信。
L6470_write(dir);
// データ送信。
L6470_write(spd_h);
L6470_write(spd_m);
L6470_write(spd_l);
}
2012年9月22日土曜日
Raspberry Pi でSPI ストリナステッピングモータドライバ
ArduinoのSPIでストリナのSPI接続ステッピングモータドライバを動かせた ので、同じ事を Raspberry Piでもできないか調べてみました。Raspberry PiでのSPIはドライバをインストールすればI2Cのようにファイルディスクリプタでアクセスできるようですが、いつも使わせてもらっている GordonさんとことのwiringPiにラッパー関数があったので、それを使わせてもらうことにしました。SPIのドライバをインストールする方法も同サイトに記述があります。
2012年9月17日月曜日
Arduino でラジコンサーボ(インチキレンジファインダ)
Arduino と ストリナのドライバで ステッピングモータを回せた ので、次はラジコンサーボを動かしてみようと思います。といっても、Arduinoにはラジコンサーボ用のライブラリがあるので、単純に動かすだけなら線をつないで簡単にできてしまいます。以前、H8やSHを使って、サーボモータを動かしたときは、駆動用のパルスから自分で生成しないといけないので面倒でした(まあ、パルスの周期などでトルクなどを細かく変えたり自由度は高いですが)。
ただ、サーボを回すだけだと面白く無いので、サーボホーンに(とりあえず両面テープでだけど)PSD距離センサを貼り付けて動かしてみました。
当初、動かすのが速すぎて、距離がうまく取れませんでした。GP2D12のデータシートを見たら、ちゃんと、最大50msほど測定にかかると書いてありました。そこで、一点で停止→100ms待ち→測定→次の点へを繰り返すようにしました。
正面20cmくらいのところに壁がある状態で、こんな感じの測定結果。
壁は正面に垂直に対していますが、距離センサが軸を中心に回転して測定するので、両端は当然距離が長くなります。なんとなく良い感じでとれてるかな。
次に、こんな感じで正面に障害物を置いてみると
真ん中の凹んで(距離が短くなって)いる部分が障害物。
さらに、正面だけ開いている状態だと
真ん中だけ、距離が大きくなってます。思ったよりもいい感じ。
これを簡易レンジファインダとして移動ロボットに載せたりできないかな?あいかわらず、具体的な使い道を考えずに、突発的にやっているだけなんだけど。
ただ、サーボを回すだけだと面白く無いので、サーボホーンに(とりあえず両面テープでだけど)PSD距離センサを貼り付けて動かしてみました。
当初、動かすのが速すぎて、距離がうまく取れませんでした。GP2D12のデータシートを見たら、ちゃんと、最大50msほど測定にかかると書いてありました。そこで、一点で停止→100ms待ち→測定→次の点へを繰り返すようにしました。
正面20cmくらいのところに壁がある状態で、こんな感じの測定結果。
壁は正面に垂直に対していますが、距離センサが軸を中心に回転して測定するので、両端は当然距離が長くなります。なんとなく良い感じでとれてるかな。
次に、こんな感じで正面に障害物を置いてみると
真ん中の凹んで(距離が短くなって)いる部分が障害物。
さらに、正面だけ開いている状態だと
真ん中だけ、距離が大きくなってます。思ったよりもいい感じ。
これを簡易レンジファインダとして移動ロボットに載せたりできないかな?あいかわらず、具体的な使い道を考えずに、突発的にやっているだけなんだけど。
#include <Servo.h>
// PSDセンサのアナログ信号入力ピン。
const int PIN_PSD_IN1 = 5;
// センサの近接側測定不可領域閾値。
const int PSD_DEADZONE = 80;
// 待ち時間。
const int WAIT_TIME = 100;
Servo servo1;
// 電圧をcmに変換。
int analogToCentimeter(int analogValue) {
return (int)((6787 / (analogValue -3)) -4 + 5);
}
void setup()
{
Serial.begin(9600);
Serial.println("Servo set center");
servo1.attach(9);
servo1.write(90);
delay(2000);
}
void loop()
{
int i;
for (i = 30; i <= 150; i += 10)
{
servo1.write(i);
delay(WAIT_TIME);
// センサのアナログ値を読み取り。
int value1 = analogRead(PIN_PSD_IN1);
// 読み取り値が閾値を超えていれば、距離に変換。
if (value1 > PSD_DEADZONE) {
int range1 = analogToCentimeter(value1);
// シリアルへ出力。
Serial.print(i, DEC);
Serial.print(", ");
Serial.print(range1, DEC);
Serial.println();
}
else {
Serial.println("RANGE OUT, ");
}
}
for (i = 150; i >= 30; i -= 10)
{
servo1.write(i);
delay(WAIT_TIME);
// センサのアナログ値を読み取り。
int value1 = analogRead(PIN_PSD_IN1);
// 読み取り値が閾値を超えていれば、距離に変換。
if (value1 > PSD_DEADZONE) {
int range1 = analogToCentimeter(value1);
// シリアルへ出力。
Serial.print(i, DEC);
Serial.print(", ");
Serial.print(range1, DEC);
Serial.println();
}
else {
Serial.println("RANGE OUT, ");
}
}
}
ストリナのSPIステッピングモータドライバ
今回の小物は ストロベリーリナックスさんのSPI接続ステッピングモータドライバ です。以外にも?自分はステッピングモータを使ったことがありませんでした。モータというとDCモータばかりです。いつもガラクタを作るのに使うのはタミヤの工作用ギアボックスなんかに入っている小さいやつですし、パワーが欲しい場合はタミヤの遊星ギヤ付きモーターとか。以前仕事で使ったMAXONのエンコーダ付きモータは大きさ(細さ)に比してトルクが大きくて素晴らしいですが、値段とか入手性を考えると日曜電子工作にはちょっと・・・ね。あとは、ぐるぐる回らなくていいなら、ラジコンのサーボですよね。オープン(センサでフィードバックしない)前提で位置制御できるのは楽ちんです。
で、ステッピングモータも基本オープンで(脱調しなければ)何ステップ動かしたから、ここを向いているはずだということができて、しかもグルグル回せる(ラジコンサーボみたいに180度くらいを往復ではなくて)ので、車輪型ロボットの足回りに使ってみたいなと思っていたんですが、DCモータと違い、電気通せば回る(DCモータもそんなんじゃだめだろ?)っていうもんでもないので、使ったことがありませんでした。今回はそんなわけで、とりあえずステッピングモータを単純に回すところまで挑戦です。ここのところI2Cもののセンサが続きましたが、じつはSPIは使ったことがありませんでした。しかし、Arduinoなら、ちょいちょいとつないで、ライブラリを使えばOKです。(いや、素晴らしい。Arduinoあるいはライブラリを用意してくれた人たちに感謝しなくちゃね。)
パラメータとかスピードの設定によっては脱調して回らなくなって、電流ばっかり流れてモータが熱くなったりするので気をつけましょう。(→ 自分)
で、ステッピングモータも基本オープンで(脱調しなければ)何ステップ動かしたから、ここを向いているはずだということができて、しかもグルグル回せる(ラジコンサーボみたいに180度くらいを往復ではなくて)ので、車輪型ロボットの足回りに使ってみたいなと思っていたんですが、DCモータと違い、電気通せば回る(DCモータもそんなんじゃだめだろ?)っていうもんでもないので、使ったことがありませんでした。今回はそんなわけで、とりあえずステッピングモータを単純に回すところまで挑戦です。ここのところI2Cもののセンサが続きましたが、じつはSPIは使ったことがありませんでした。しかし、Arduinoなら、ちょいちょいとつないで、ライブラリを使えばOKです。(いや、素晴らしい。Arduinoあるいはライブラリを用意してくれた人たちに感謝しなくちゃね。)
パラメータとかスピードの設定によっては脱調して回らなくなって、電流ばっかり流れてモータが熱くなったりするので気をつけましょう。(→ 自分)
#include <Arduino.h>
#include <SPI.h>
// ピン定義。
#define PIN_SPI_MOSI 11
#define PIN_SPI_MISO 12
#define PIN_SPI_SCK 13
#define PIN_SPI_SS 10
#define PIN_L6470_SS PIN_SPI_SS
// 関数プロトタイプ。
void L6470_write(unsigned char data);
void L6470_init(void);
void L6470_run(long speed);
// グローバル変数。
long speed = 10000;
void setup()
{
// ピン設定。
pinMode(PIN_SPI_MOSI, OUTPUT);
pinMode(PIN_SPI_MISO, INPUT);
pinMode(PIN_SPI_SCK, OUTPUT);
pinMode(PIN_L6470_SS, OUTPUT);
// SSはHigh(~SSをdisable)状態に。
digitalWrite(PIN_L6470_SS, HIGH);
// SPI開始。
SPI.begin();
// シリアル通信。
Serial.begin(9600);
// L6470の初期化。
L6470_init();
Serial.println("Start...");
// テスト回転。
L6470_run(speed);
}
void loop()
{
for (int i = 0; i < 5; i++)
{
speed += 10000;
L6470_run(speed);
delay (5000);
}
for (int i= 0; i < 5; i++)
{
speed -= 10000;
L6470_run(speed);
delay (5000);
}
}
void L6470_write(unsigned char data)
{
// ~SSイネーブル。
digitalWrite(PIN_L6470_SS, LOW);
// SPI転送。
SPI.transfer(data);
// ~SSディスエーブル。
digitalWrite(PIN_L6470_SS, HIGH);
}
void L6470_init()
{
// MAX_SPEED設定。
/// レジスタアドレス。
L6470_write(0x07);
// 最大回転スピード値(10bit)
L6470_write(0x00);
L6470_write(0x20);
// KVAL_HOLD設定。
/// レジスタアドレス。
L6470_write(0x09);
// モータ停止中の電圧設定(8bit)
L6470_write(0xFF);
// KVAL_RUN設定。
/// レジスタアドレス。
L6470_write(0x0A);
// モータ定速回転中の電圧設定(8bit)
L6470_write(0xFF);
// KVAL_ACC設定。
/// レジスタアドレス。
L6470_write(0x0B);
// モータ加速中の電圧設定(8bit)
L6470_write(0xFF);
// KVAL_DEC設定。
/// レジスタアドレス。
L6470_write(0x0C);
// モータ減速中の電圧設定(8bit)
L6470_write(0xFF);
// OCD_TH設定。
/// レジスタアドレス。
L6470_write(0x13);
// オーバーカレントスレッショルド設定(4bit)
L6470_write(0x0F);
// STALL_TH設定。
/// レジスタアドレス。
L6470_write(0x14);
// ストール電流スレッショルド設定(4bit)
L6470_write(0x7F);
}
void L6470_run(long speed)
{
unsigned short dir;
unsigned long spd;
unsigned char spd_h;
unsigned char spd_m;
unsigned char spd_l;
// 方向検出。
if (speed < 0)
{
dir = 0x50;
spd = -1 * speed;
}
else
{
dir = 0x51;
spd = speed;
}
// 送信バイトデータ生成。
spd_h = (unsigned char)((0x0F0000 & spd) >> 16);
spd_m = (unsigned char)((0x00FF00 & spd) >> 8);
spd_l = (unsigned char)(0x00FF & spd);
// コマンド(レジスタアドレス)送信。
L6470_write(dir);
// データ送信。
L6470_write(spd_h);
L6470_write(spd_m);
L6470_write(spd_l);
}
2012年9月9日日曜日
Arduino、GR-SAKURA、Raspberry Pi でI2C Compassと6DOFを試す
前回からしばらく更新の間があきましたが、GR-SAKURAのI2Cがうまく行かず悩んでいました。結論としては、GR-SAKURAは諦める方向で検討中です。
さて、今回はまず、ストリナのI2CディジタルコンパスをArduino と GR-SAKURA と Raspberry Pi に接続して、方位データの取得を試してみました。Arduino と Raspberry Pi では方位データが取得できましたが、GR-SAKURAではダメ(おかしな値が取得される)でした。ロジアナでみると最後にNACKが返っているんですがコンパス側なのかGR-SAKURA側なのか問題の切り分けはできていません。
同様にもう一つ別のデバイスも試しました。sparkufunの6DOF(三軸ジャイロと三軸加速度センサの組み合わせボード)ですが、これも Arduino と Raspberry Pi は動作して、GR-SAKURAはだめでした。
これまでの各種I2Cデバイスとボードとの組み合わせの勝敗表です。
I2C デバイス
|
Arduino
|
GR-SAKURA
|
Raspberry Pi
|
| 秋月RTC | OK | OK | OK |
| 秋月LCD | OK | NG | NG |
| ストリナLCD | OK | OK(ただし、NACK) | OK |
| ストリナCompass | OK | NG | OK |
| Sparkfun 6DOF | OK | NG | OK |
現状、GR-SAKURAが劣勢というかダメダメです。ただし、上記の結果は、自分の作成したプログラムや使用方法に問題がある可能性が大です。というか、きっとそうなんでしょう。たしか、GR-SAKURA(のライブラリ)はRXマイコンのRIICモジュールを使わずにソフトウェア処理でI2C通信を行なっていると思うのですが、この辺りが関係するのでしょうか。Arduinoとは違い、自分のような素人には GR-SAKURAは向かないようです。一度、ライブラリを使わずにHEWで RIICモジュールを使って試したいところですが、そこまでして GR-SAKURAを使わなくても、Arduinoでいいと思えてきました。日本製マイコンだし、処理能力も高いのでArduinoから乗り換えを考えていましたが、やっぱり簡単に使える方が便利だし。重い処理が必要なら、Arduino + Raspberry Pi ってのもありだし。
で、以下お試しのコード。単純に値を取り出しているだけで使える状態の値には加工してません。実際に使うにはオフセットやらリニアリティ補正やら積分やら結構面倒。機会があればロボットに載せてみたいけど。
Arduinoのコード(スケッチ)
#include <Adruino.h>
#include <Wire.h>
// コンパスに関する定義。
#define COMPASS_ADDRESS (0b0100001)
// ジャイロに関する定義。
#define GYRO_ADDRESS (0xd0 >> 1)
#define GYRO_WHO_AM_I 0x00
#define GYRO_SMPLRT_DEV 0x15
#define GYRO_DLPF_FS 0x16
#define GYRO_INT_CFG 0x17
#define GYRO_INT_STATUS 0x1A
#define GYRO_TEMP_OUT_H 0x1B
#define GYRO_TEMP_OUT_L 0x1C
#define GYRO_XOUT_H 0x1D
#define GYRO_XOUT_L 0x1E
#define GYRO_YOUT_H 0x1F
#define GYRO_YOUT_L 0x20
#define GYRO_ZOUT_H 0x21
#define GYRO_ZOUT_L 0x22
#define GYRO_PWR_MGM 0x3E
// 加速度センサに関する定義。
#define ACC_ADDRESS (0xa6 >> 1)
#define ACC_POWER_CTL 0x2D
#define ACC_DATA_X0 0x32
#define ACC_DATA_X1 0x33
#define ACC_DATA_Y0 0x34
#define ACC_DATA_Y1 0x35
#define ACC_DATA_Z0 0x36
#define ACC_DATA_Z1 0x37
// 関数プロトタイプ。
int getCompass(void);
void getGyro(short *x, short *y, short *z);
void initAcc(void);
void getAcc(short *x, short *y, short *z);
//
unsigned char buf[8];
void setup()
{
// シリアルポートを9600bpsで開始。
Serial.begin(9600);
// 受信バッファにデータが入るまで待機。
while (Serial.available() == 0);
Serial.println("Initializing...");
// 一旦、受信バッファにデータがあれば、
if (Serial.available() > 0) {
// 読み込んで、廃棄。
char c = Serial.read();
}
Serial.println("Initializing...Please wait...");
delay(500);
// Bus-MasterとしてI2Cを開始。
Wire.begin();
// 加速度センサの初期化。
initAcc();
}
void loop()
{
int cmps;
short gyroX, gyroY, gyroZ;
short accX, accY, accZ;
cmps = getCompass();
getGyro(&gyroX, &gyroY, &gyroZ);
getAcc(&accX, &accY, &accZ);
Serial.print("COMPASS:");
Serial.println(cmps, DEC);
Serial.print("GYRO X:");
Serial.print(gyroX, DEC);
Serial.print(" Y:");
Serial.print(gyroY, DEC);
Serial.print(" Z:");
Serial.print(gyroZ, DEC);
Serial.println();
Serial.print("ACC X:");
Serial.print(accX, DEC);
Serial.print(" Y:");
Serial.print(accY, DEC);
Serial.print(" Z:");
Serial.print(accZ, DEC);
Serial.println();
delay(200);
}
int getCompass(void)
{
int data;
// I2C通信開始。
Wire.beginTransmission(COMPASS_ADDRESS);
// Get dataコマンド送信。
Wire.write('A');
//Wire.write(0x41);
// キューの送信。
Wire.endTransmission();
// データ要求。
Wire.requestFrom(COMPASS_ADDRESS, 2);
// データ受信。
for (int i = 0; i < 2; i++) {
while (Wire.available() == 0);
buf[i] = Wire.read();
}
// データ変換。
data = (unsigned short)buf[0];
data = (data << 8) + (unsigned short)buf[1];
return data;
}
void getGyro(short *x, short *y, short *z)
{
// I2C通信開始。
Wire.beginTransmission(GYRO_ADDRESS);
// レジスタアドレスGYRO_TEMP_OUT_Hを指定。
Wire.write(GYRO_TEMP_OUT_H);
// キューの送信。
Wire.endTransmission();
delay(1);
// I2C通信開始。
Wire.beginTransmission(GYRO_ADDRESS);
// データ要求。
Wire.requestFrom(GYRO_ADDRESS, 8);
// データ受信。
for (int i = 0; i < 8; i++) {
while (Wire.available() == 0);
buf[i] = Wire.read();
}
Wire.endTransmission();
*x = (((short)buf[2]) << 8) | buf[3];
*y = (((short)buf[4]) << 8) | buf[5];
*z = (((short)buf[6]) << 8) | buf[7];
}
void initAcc(void)
{
Wire.beginTransmission(ACC_ADDRESS);
Wire.write(ACC_POWER_CTL);
Wire.write(0b00001000);
Wire.endTransmission();
Serial.println("Accelerometer Initialized.");
}
void getAcc(short *x, short *y, short *z)
{
// I2C通信開始。
Wire.beginTransmission(ACC_ADDRESS);
// レジスタアドレスACC_DATA_X0 を指定。
Wire.write(ACC_DATA_X0);
// キューの送信。
Wire.endTransmission();
delay(1);
// I2C通信開始。
Wire.beginTransmission(ACC_ADDRESS);
// データ要求。
Wire.requestFrom(ACC_ADDRESS, 6);
// データ受信。
for (int i = 0; i < 6; i++) {
while (Wire.available() == 0);
buf[i] = Wire.read();
}
Wire.endTransmission();
*x = (((short)buf[1]) << 8) | buf[0];
*y = (((short)buf[3]) << 8) | buf[2];
*z = (((short)buf[5]) << 8) | buf[4];
}
Raspberry Pi のコード(例によってWiringPiのdelay使用)
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <unistd.h>
#include <linux/i2c-dev.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <wiringPi.h>
#define COMPASS_ADDRESS (0b0100001)
#define GYRO_ADDRESS (0xd0 >> 1)
#define ACCELEROMETER_ADDRESS (0xa6 >> 1)
// 関数プロトタイプ。
int getCompass(int addr);
void getGyro(int addr, short *x, short *y, short *z);
void initAcc(int addr);
void getAcc(int adr, short *x, short *y, short *z);
//
char *i2cFileName = "/dev/i2c-0"; // I2Cドライバファイル名。
int main(int argc, char **argv)
{
int cmpsAddr = COMPASS_ADDRESS; // I2C Compass のアドレス。
int gyroAddr = GYRO_ADDRESS; // I2C Gyro のアドレス。
int accAddr = ACCELEROMETER_ADDRESS; // 加速度センサのアドレス。
unsigned short cmps;
short gyroX, gyroY, gyroZ;
short accX, accY, accZ;
printf("***** start test program *****\n");
initAcc(accAddr);
while(1)
{
// コンパスの値取得。
cmps = getCompass(cmpsAddr);
// ジャイロの値取得。
getGyro(gyroAddr, &gyroX, &gyroY, &gyroZ);
// 加速度センサの値取得。
getAcc(accAddr, &accX, &accY, &accZ);
printf("COMPASS:%d\n", cmps);
printf("GYRO X:%d, Y:%d, Z:%d\n", gyroX, gyroY, gyroZ);
printf("ACC X:%d, Y:%d, Z:%d\n", accX, accY, accZ);
delay(200);
}
return 0;
}
int getCompass(int addr)
{
int cmps; // ファイルディスクリプタ。
unsigned char buf[2];
int deg;
// I2CポートをRead/Write属性でオープン。
if ((cmps = open(i2cFileName, O_RDWR)) < 0)
{
printf("Faild to open i2c port\n");
exit(1);
}
// 通信先アドレスの設定。
if (ioctl(cmps, I2C_SLAVE, addr) < 0)
{
printf("Unable to get bus access to talk to slave\n");
exit(1);
}
buf[0] = 'A';
if ((write(cmps, buf, 1)) != 1) {
printf("Error writing to Compass\n");
exit(1);
}
delay(1);
// データ読み込み。
if (read(cmps, buf, 2) != 2)
{
printf("Uneble to read from Compass\n");
exit(1);
}
// クローズ。
close(cmps);
deg = (unsigned short)buf[0];
deg = (deg << 8) + (unsigned short)buf[1];
return deg;
}
void getGyro(int addr, short *x, short *y, short *z)
{
int gyro; // ファイルディスクリプタ。
unsigned char buf[8];
// I2CポートをRead/Write属性でオープン。
if ((gyro = open(i2cFileName, O_RDWR)) < 0)
{
printf("Faild to open i2c port\n");
exit(1);
}
// 通信先アドレスの設定。
if (ioctl(gyro, I2C_SLAVE, addr) < 0)
{
printf("Unable to get bus access to talk to slave\n");
exit(1);
}
buf[0] = 0x1b;
if ((write(gyro, buf, 1)) != 1) {
printf("Error writing to Gyro\n");
exit(1);
}
delay(1);
// データ読み込み。
if (read(gyro, buf, 8) != 8)
{
printf("Uneble to read from Gyro\n");
exit(1);
}
// クローズ。
close(gyro);
*x = (((short)buf[2]) << 8) | buf[3];
*y = (((short)buf[4]) << 8) | buf[5];
*z = (((short)buf[6]) << 8) | buf[7];
}
void initAcc(int addr)
{
int acc; // ファイルディスクリプタ。
unsigned char buf[2];
// I2CポートをRead/Write属性でオープン。
if ((acc = open(i2cFileName, O_RDWR)) < 0)
{
printf("Faild to open i2c port\n");
exit(1);
}
// 通信先アドレスの設定。
if (ioctl(acc, I2C_SLAVE, addr) < 0)
{
printf("Unable to get bus access to talk to slave\n");
exit(1);
}
buf[0] = 0x2d;
buf[1] = 0b00001000;
if ((write(acc, buf, 2)) != 2) {
printf("Error writing to Accelerometer\n");
exit(1);
}
delay(1);
// クローズ。
close(acc);
}
void getAcc(int addr, short *x, short *y, short *z)
{
int acc; // ファイルディスクリプタ。
unsigned char buf[6];
// I2CポートをRead/Write属性でオープン。
if ((acc = open(i2cFileName, O_RDWR)) < 0)
{
printf("Faild to open i2c port\n");
exit(1);
}
// 通信先アドレスの設定。
if (ioctl(acc, I2C_SLAVE, addr) < 0)
{
printf("Unable to get bus access to talk to slave\n");
exit(1);
}
buf[0] = 0x32;
if ((write(acc, buf, 1)) != 1) {
printf("Error writing to Accelerometer\n");
exit(1);
}
delay(1);
// データ読み込み。
if (read(acc, buf, 6) != 6)
{
printf("Uneble to read from Accelereometer\n");
exit(1);
}
// クローズ。
close(acc);
*x = (((short)buf[1]) << 8) | buf[0];
*y = (((short)buf[3]) << 8) | buf[2];
*z = (((short)buf[5]) << 8) | buf[4];
}
登録:
コメント (Atom)





