CMPS12_KRAI.cpp

00001 /**
00002  * adopted from
00003  * CMPS03 by: Aarom Berk
00004  * 
00005  * Bismillahirahmanirrahim
00006  */
00007 
00008 /**
00009  * Includes
00010  */
00011 #include "CMPS12_KRAI.h"
00012 
00013 CMPS12_KRAI::CMPS12_KRAI(PinName sda, PinName scl, int address) {
00014     i2c = new I2C(sda, scl);
00015     //CMPS11 maksimum 100kHz CMPS12 maksimum 400kHz
00016     i2c->frequency(100000);
00017     i2cAddress = address;
00018 
00019 }
00020 
00021 char CMPS12_KRAI::readSoftwareRevision(void){
00022     char registerNumber   = SOFTWARE_REVISION_REG;
00023     char registerContents = 0;
00024 
00025     //First, send the number of register we wish to read,
00026     //in this case, command register, number 0.
00027     i2c->write(i2cAddress, &registerNumber, 1);
00028     
00029     //Now, read one byte, which will be the contents of the command register.
00030     i2c->read(i2cAddress, &registerContents, 1);
00031     
00032     return registerContents; 
00033 }
00034 
00035 
00036 int CMPS12_KRAI::getAngle(void){
00037     char registerNumber = COMPASS_BEARING_WORD_REG;
00038     char registerContents[2] = {0x00, 0x00};
00039     
00040     //Mengirim register untuk address pertama dari i2c
00041     i2c->write(i2cAddress, &registerNumber, 1); //1
00042     
00043     //Mengambil data dari 2 address I2c
00044     i2c->read(i2cAddress, registerContents, 2);
00045     
00046     //Register 0 adalah 8 bit, harus di shift
00047     //Register 1 adalah 16 bit, bisa langsung dibaca
00048     int angle = ((int)registerContents[0] << 8) | ((int)registerContents[1]);
00049     
00050     //kirim data berupa sudut 0-360
00051     return angle;   
00052 }
00053 
00054 int CMPS12_KRAI::getPitch(void){
00055     char registerNumber = COMPASS_PITCH_WORD_REG;
00056     char registerContents[2] = {0x00, 0x00};
00057     
00058     //Mengirim register untuk address pertama dari i2c
00059     i2c->write(i2cAddress, &registerNumber, 1);
00060     
00061     //Mengambil data dari 2 address I2c
00062     i2c->read(i2cAddress, registerContents, 1);
00063     
00064     //simpan data ke variable pitch
00065     int pitch = ((int)registerContents[0] );
00066     
00067     return pitch;   
00068 }
00069 
00070 int CMPS12_KRAI::getRoll(void){
00071     char registerNumber = COMPASS_ROLL_WORD_REG;
00072     char registerContents[2] = {0x00, 0x00};
00073     
00074     //Mengirim register untuk address pertama dari i2c
00075     i2c->write(i2cAddress, &registerNumber, 1);
00076     
00077     //Mengambil data dari 2 address I2c
00078     i2c->read(i2cAddress, registerContents, 1);
00079     
00080     //simpan data ke variable roll
00081     int roll = ((int)registerContents[0] );
00082     
00083     return roll;   
00084 }
00085 
00086 void CMPS12_KRAI::calibrate(void){
00087     char registerNumber   = SOFTWARE_REVISION_REG;
00088     char calibrate_data1 = 0xF0;
00089     char calibrate_data2 = 0xF5;
00090     char calibrate_data3 = 0xF7;
00091     //kirim data 1
00092     //Thread::wait(25);
00093     i2c->write(i2cAddress, &registerNumber, 1);
00094     i2c->write(i2cAddress, &calibrate_data1, 1);
00095     //kirim data 2 delay 25ms
00096     //Thread::wait(25);
00097     i2c->write(i2cAddress, &registerNumber, 1);
00098     i2c->write(i2cAddress, &calibrate_data2, 1);
00099     //kirim data 3 delay 25ms
00100     //Thread::wait(25);
00101     i2c->write(i2cAddress, &registerNumber, 1);
00102     i2c->write(i2cAddress, &calibrate_data3, 1);
00103 }
00104 
00105 void CMPS12_KRAI::stopCalibrate(void){
00106     char registerNumber   = SOFTWARE_REVISION_REG;
00107     char calibrate_data1 = 0xF8;
00108     //kirim data 1
00109     i2c->write(i2cAddress, &registerNumber, 1);
00110     i2c->write(i2cAddress, &calibrate_data1, 1);
00111 }
00112 
00113 
00114 int CMPS12_KRAI::getAccelX(void){
00115     char registerNumber = COMPASS_ROLL_WORD_REG; //0x02
00116     char registerContents[16] = {0x00, 0x00};
00117     
00118     //Mengirim register untuk address pertama dari i2c
00119     i2c->write(i2cAddress, &registerNumber, 1);
00120     
00121     //Mengambil data dari 2 address I2c
00122     i2c->read(i2cAddress, registerContents, 16);
00123     
00124     //simpan data ke variable roll
00125     int accelX = ((int)registerContents[11]<<8 | (int)registerContents[12] );
00126     
00127     return accelX;   
00128 }