プログラム
KXP84(ライブラリ)
I2Cを利用して、センサーの値を得ます。
//#include <I2C.h>
#include <Wire.h>
#define KXP 0x18 //I2C address for ADRS=H
#define REGA 0x0c
#define REGB 0x0b
#define REGC 0x0a
int kxp84_init(){
//I2c.begin();
Wire.begin();
Serial.println("setup kxp");
//I2c.write(KXP, REGB, 0);//clear
Wire.beginTransmission(KXP);
Wire.write(REGB);
Wire.write(byte(0));
Wire.endTransmission();
//I2c.write(KXP, REGC, 0);//clear
Wire.beginTransmission(KXP);
Wire.write(REGC);
Wire.write(byte(0));
Wire.endTransmission();
}//kxp84_init
//バイトアレイで値を返す
void kxp84_read(byte val[]){
int vH,vL;
int wV;
//x-high/low
//I2c.read(KXP,0x0,1);
//vH = I2c.receive();
Wire.beginTransmission(KXP);
Wire.write(byte(0x0));//x-high
Wire.endTransmission();
Wire.requestFrom(KXP,1);
if(1 <= Wire.available())
vH = Wire.read();
//I2c.read(KXP,0x1,1);
//vL = I2c.receive();
Wire.beginTransmission(KXP);
Wire.write(0x1);//x-low
Wire.endTransmission();
Wire.requestFrom(KXP,1);
if(1 <= Wire.available())
vL = Wire.read();
wV = (int)((vH << 4) + (vL >> 4));
val[0] = highByte(wV);
val[1] = lowByte(wV);
//y-high/low
//I2c.read(KXP,0x2,1);
//vH = I2c.receive();
Wire.beginTransmission(KXP);
Wire.write(0x02);//y-high
Wire.endTransmission();
Wire.requestFrom(KXP,1);
if(1 <= Wire.available())
vH = Wire.read();
//I2c.read(KXP,0x3,1);
//vL = I2c.receive();
Wire.beginTransmission(KXP);
Wire.write(0x3);//y-low
Wire.endTransmission();
Wire.requestFrom(KXP,1);
if(1 <= Wire.available())
vL = Wire.read();
wV = (int)((vH << 4) + (vL >> 4));
val[2] = highByte(wV);
val[3] = lowByte(wV);
//z-high/low
//I2c.read(KXP,0x4,1);
//vH = I2c.receive();
Wire.beginTransmission(KXP);
Wire.write(0x4);//z-hign
Wire.endTransmission();
Wire.requestFrom(KXP,1);
if(1 <= Wire.available())
vH = Wire.read();
//I2c.read(KXP,05,1);
//vL = I2c.receive();
Wire.beginTransmission(KXP);
Wire.write(0x5);
Wire.endTransmission();
Wire.requestFrom(KXP,1);
if(1 <= Wire.available())
vL = Wire.read();
wV = (int)((vH << 4) + (vL >> 4));
val[4] = highByte(wV);
val[5] = lowByte(wV);
}//kxp84_read
実行
前回のステッピングモーターのプログラムに、kxp84からのセンサー情報を得るプログラムが加わっています。
//STEPPING MOTOR Control
//DRIVER TD62064
//Motor SPG 20
//ユニポーラ、2相励磁
//モータ端子 AC,BC はセンタータップ
// A(白),B,AC(黒),BC(青),^A,^B(緑)
//ドライバ TD62064
//50V 1.5A
//i1:3 o2:2 i2:6 o2:7 i3:11 o3:9 i4:14 o4:16
//gnd:4,5,13,12 com:1,8(ダイオード)
//接続
//VBB:モータ用電源1.5V*3
//Ard.D2>Td.i1 Ard.D3>Td.i2 Ard.D4>Td.i3 Ard.D55>Td.i4
//Td.o1>SPG.A Td.o2>SPG.B Td.o3>SPG.^A Td.o4>SPG.^B
//SPG.AC>SPG.BC>SPG.COM>VCC
int SA = 2;
int SB = 3;
int NA = 4;
int NB = 5;
//励磁パターン
byte cw[4] = {0xC, 0x6, 0x3, 0x9};
int pt;
int dir;
int count;
byte rb[6];
int rd[3];
int sd[3];
void setup() {
// initialize the digital pin
pinMode(SA, OUTPUT);
pinMode(SB, OUTPUT);
pinMode(NA, OUTPUT);
pinMode(NB, OUTPUT);
digitalWrite(SA, LOW);
digitalWrite(SB, LOW);
digitalWrite(NA, LOW);
digitalWrite(NB, LOW);
pt = 0;
dir = 0;
count = 0;
Serial.begin(9600);
kxp84_init();
}//setup
void loop() {
digitalWriteA(cw[pt]); //
delay(500); //4以下だと回転しない
if(dir) pt++;
else pt--;//回転方向の制御
if(pt == 4) pt = 0;//
if(pt < 0) pt = 3;
count++;
if(count == 360) {//反転
if(dir) dir = 0;else dir = 1;
count = 0;
}//if count
/*
kxp84_read(rb);
rd[0] = rb[0] * 256 + rb[1];
rd[1] = rb[2] * 256 + rb[3];
rd[2] = rb[4] * 256 + rb[5];
*/
for (int i = 0;i < 3;i++) sd[i] = 0;
for(int k = 0;k < 5;k++){
kxp84_read(rb);
rd[0] = rb[0] * 256 + rb[1];
rd[1] = rb[2] * 256 + rb[3];
rd[2] = rb[4] * 256 + rb[5];
for (int i = 0;i < 3;i++)
sd[i] = sd[i] + rd[i];
delay(50);
}//for k
for(int k = 0;k < 3;k++) rd[k] = sd[k] / 5;
Serial.print(rd[0]);
Serial.print(',');
Serial.print(rd[1]);
Serial.print(',');
Serial.println(rd[2]);
}//loop
void digitalWriteA(byte pat){
//Serial.print(pat,HEX);//チェック用
//Serial.println();
//パターンとともに変更が必要
digitalWrite(NB,(pat & 0x1));//^B
digitalWrite(NA,(pat & 0x2) >> 1);//^A
digitalWrite(SB,(pat & 0x4) >> 2);//B
digitalWrite(SA,(pat & 0x8) >> 3);//A
}//digitalWriteA
※kxp84のコードと、このコードを別々のタブに記述しなければ実行できません。