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 const float  radian_to_degree                       = 57.295779;
00013 
00014 CMPS12_KRAI::CMPS12_KRAI(PinName sda, PinName scl, int address) {
00015     i2c = new I2C(sda, scl);
00016     //CMPS11 maksimum 100kHz CMPS12 maksimum 400kHz
00017     i2c->frequency(100000);
00018     i2cAddress = address;
00019     _offset_compass_value = 0;
00020     _theta_origin = 0;
00021     _theta_offset = 0;
00022 
00023 }
00024 
00025 char CMPS12_KRAI::readSoftwareRevision(void){
00026     char registerNumber   = SOFTWARE_REVISION_REG;
00027     char registerContents = 0;
00028 
00029     //First, send the number of register we wish to read,
00030     //in this case, command register, number 0.
00031     i2c->write(i2cAddress, &registerNumber, 1);
00032     
00033     //Now, read one byte, which will be the contents of the command register.
00034     i2c->read(i2cAddress, &registerContents, 1);
00035     
00036     return registerContents; 
00037 }
00038 
00039 
00040 int CMPS12_KRAI::getAngle(void){
00041     char registerNumber = COMPASS_BEARING_WORD_REG;
00042     char registerContents[2] = {0x00, 0x00};
00043     
00044     //Mengirim register untuk address pertama dari i2c
00045     i2c->write(i2cAddress, &registerNumber, 1); //1
00046     
00047     //Mengambil data dari 2 address I2c
00048     i2c->read(i2cAddress, registerContents, 2);
00049     
00050     //Register 0 adalah 8 bit, harus di shift
00051     //Register 1 adalah 16 bit, bisa langsung dibaca
00052     int angle = ((int)registerContents[0] << 8) | ((int)registerContents[1]);
00053     
00054     //kirim data berupa sudut 0-360
00055     return angle;   
00056 }
00057 
00058 int CMPS12_KRAI::getPitch(void){
00059     char registerNumber = COMPASS_PITCH_WORD_REG;
00060     char registerContents[2] = {0x00, 0x00};
00061     
00062     //Mengirim register untuk address pertama dari i2c
00063     i2c->write(i2cAddress, &registerNumber, 1);
00064     
00065     //Mengambil data dari 2 address I2c
00066     i2c->read(i2cAddress, registerContents, 1);
00067     
00068     //simpan data ke variable pitch
00069     int pitch = ((int)registerContents[0] );
00070     
00071     return pitch;   
00072 }
00073 
00074 int CMPS12_KRAI::getRoll(void){
00075     char registerNumber = COMPASS_ROLL_WORD_REG;
00076     char registerContents[2] = {0x00, 0x00};
00077     
00078     //Mengirim register untuk address pertama dari i2c
00079     i2c->write(i2cAddress, &registerNumber, 1);
00080     
00081     //Mengambil data dari 2 address I2c
00082     i2c->read(i2cAddress, registerContents, 1);
00083     
00084     //simpan data ke variable roll
00085     int roll = ((int)registerContents[0] );
00086     
00087     return roll;   
00088 }
00089 
00090 void CMPS12_KRAI::calibrate(void){
00091     char registerNumber   = SOFTWARE_REVISION_REG;
00092     char calibrate_data1 = 0xF0;
00093     char calibrate_data2 = 0xF5;
00094     char calibrate_data3 = 0xF7;
00095     //kirim data 1
00096     //Thread::wait(25);
00097     i2c->write(i2cAddress, &registerNumber, 1);
00098     i2c->write(i2cAddress, &calibrate_data1, 1);
00099     //kirim data 2 delay 25ms
00100     //Thread::wait(25);
00101     i2c->write(i2cAddress, &registerNumber, 1);
00102     i2c->write(i2cAddress, &calibrate_data2, 1);
00103     //kirim data 3 delay 25ms
00104     //Thread::wait(25);
00105     i2c->write(i2cAddress, &registerNumber, 1);
00106     i2c->write(i2cAddress, &calibrate_data3, 1);
00107 }
00108 
00109 void CMPS12_KRAI::stopCalibrate(void){
00110     char registerNumber   = SOFTWARE_REVISION_REG;
00111     char calibrate_data1 = 0xF8;
00112     //kirim data 1
00113     i2c->write(i2cAddress, &registerNumber, 1);
00114     i2c->write(i2cAddress, &calibrate_data1, 1);
00115 }
00116 
00117 
00118 int CMPS12_KRAI::getAccelX(void){
00119     char registerNumber = COMPASS_ROLL_WORD_REG; //0x02
00120     char registerContents[16] = {0x00, 0x00};
00121     
00122     //Mengirim register untuk address pertama dari i2c
00123     i2c->write(i2cAddress, &registerNumber, 1);
00124     
00125     //Mengambil data dari 2 address I2c
00126     i2c->read(i2cAddress, registerContents, 16);
00127     
00128     //simpan data ke variable roll
00129     int accelX = ((int)registerContents[11]<<8 | (int)registerContents[12] );
00130     
00131     return accelX;   
00132 }
00133 
00134 void CMPS12_KRAI::compassResetOffsetValue(){
00135     _offset_compass_value = (float)CMPS12_KRAI::getAngle()/10;
00136 }
00137 
00138 void CMPS12_KRAI::compassUpdateValue(){
00139     _theta_origin = (float)CMPS12_KRAI::getAngle()/10;
00140     _theta_offset = _theta_origin - _offset_compass_value;
00141 }
00142 
00143 float CMPS12_KRAI::compassValue(){
00144     float theta_transformed;
00145 
00146     if(_theta_offset > 180.0f && _theta_offset <= 360.0f)
00147         theta_transformed = (_theta_origin - 360.0f - _offset_compass_value)/-radian_to_degree;
00148     else if(_theta_offset < -180.0f && _theta_offset >= -360.0f)
00149         theta_transformed = (_theta_origin  + 360.0f - _offset_compass_value)/-radian_to_degree;
00150     else
00151         theta_transformed = (_theta_origin - _offset_compass_value)/-radian_to_degree;
00152 
00153     return theta_transformed; 
00154 }