KXP84.cpp

00001 #include "mbed.h"
00002 #include "KXP84.h"
00003 
00004 void KXP84::init() {
00005     char cmd[1];
00006     //reset=0;
00007     i2c.frequency(400000);
00008 //CTRL_REGB
00009     i2c.start();
00010     wait_us(1);
00011     i2c.write(0x30);
00012     wait_us(1);
00013     *cmd=0x06;
00014     i2c.write(0x0B,cmd,1);
00015     wait_us(1);
00016     i2c.stop();
00017 //CTRL_REGC
00018     i2c.start();
00019     wait_us(1);
00020     i2c.write(0x30);
00021     wait_us(1);
00022     *cmd=0x00;
00023     i2c.write(0x0A,cmd,1);
00024     wait_us(1);
00025     i2c.stop();
00026 //FF_INT
00027     i2c.start();
00028     wait_us(1);
00029     i2c.write(0x30);
00030     wait_us(1);
00031     *cmd=0x14;
00032     i2c.write(0x06,cmd,1);
00033     wait_us(1);
00034     i2c.stop();
00035 //FF_DELAY
00036     i2c.start();
00037     wait_us(1);
00038     i2c.write(0x30);
00039     wait_us(1);
00040     *cmd=0x14;
00041     i2c.write(0x07,cmd,1);
00042     wait_us(1);
00043     i2c.stop();
00044 //MOT_INT
00045     i2c.start();
00046     wait_us(1);
00047     i2c.write(0x30);
00048     wait_us(1);
00049     *cmd=0x4D;
00050     i2c.write(0x08,cmd,1);
00051     wait_us(1);
00052     i2c.stop();
00053 //MOT_DELAYC
00054     i2c.start();
00055     wait_us(1);
00056     i2c.write(0x30);
00057     wait_us(1);
00058     *cmd=0x14;
00059     i2c.write(0x09,cmd,1);
00060     wait_us(1);
00061     i2c.stop();
00062 }
00063 
00064 void KXP84::get(int *x, int *y, int *z) {
00065     char data[6];
00066     int dx,dy,dz;
00067     i2c.start();
00068     wait_us(1);
00069     i2c.write(0x30);
00070     wait_us(1);
00071     i2c.write(0x00);
00072     wait_us(1);
00073     i2c.read(0x30,data,6);
00074     wait_us(1);
00075     i2c.stop();
00076 
00077     dx=data[0]<<4 | data[1]>>4;
00078     dy=data[2]<<4 | data[3]>>4;
00079     dz=data[4]<<4 | data[5]>>4;
00080 
00081     //if (dx>2048)
00082     //    *x=dx-2048;
00083     //else
00084     *x=(int)(( (dx - 2048) * 1.24)+0.5);
00085 
00086     //if (dy>2048)
00087     //    *y=dy-2048;
00088     //else
00089     *y=(int)(( (dy - 2048) * 1.24)+0.5);
00090     //if (dz>2048)
00091     //    *z=dz-2048;
00092     //else
00093     *z=(int)(( (dz - 2048) * 1.24)+0.5);
00094 }