CMPS_KRAI.cpp

00001 /**
00002  * adopted from
00003  * CMPS03 by: Aarom Berk
00004  * 
00005  * Bismillahirahmanirrahim
00006  */
00007 
00008 /**
00009  * Includes
00010  */
00011 #include "CMPS_KRAI.h"
00012 
00013 CMPS_KRAI::CMPS_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 CMPS_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 int CMPS_KRAI::readBearing(void){
00036     char registerNumber = COMPASS_BEARING_WORD_REG;
00037     char registerContents[2] = {0x00, 0x00};
00038     
00039     //Mengirim register untuk address pertama dari i2c
00040     i2c->write(i2cAddress, &registerNumber, 1);
00041     
00042     //Mengambil data dari 2 address I2c
00043     i2c->read(i2cAddress, registerContents, 2);
00044     
00045     //Register 0 adalah 8 bit, harus di shift
00046     //Register 1 adalah 16 bit, bisa langsung dibaca
00047     int bearing = ((int)registerContents[0] << 8) | ((int)registerContents[1]);
00048     
00049     return bearing;   
00050 }
00051 void CMPS_KRAI::calibrate(void){
00052     char registerNumber   = SOFTWARE_REVISION_REG;
00053     char calibrate_data1 = 0xF0;
00054     char calibrate_data2 = 0xF5;
00055     char calibrate_data3 = 0xF7;
00056     //kirim data 1
00057     Thread::wait(25);
00058     i2c->write(i2cAddress, &registerNumber, 1);
00059     i2c->write(i2cAddress, &calibrate_data1, 1);
00060     //kirim data 2 delay 25ms
00061     Thread::wait(25);
00062     i2c->write(i2cAddress, &registerNumber, 1);
00063     i2c->write(i2cAddress, &calibrate_data2, 1);
00064     //kirim data 3 delay 25ms
00065     Thread::wait(25);
00066     i2c->write(i2cAddress, &registerNumber, 1);
00067     i2c->write(i2cAddress, &calibrate_data3, 1);
00068 }
00069 
00070 void CMPS_KRAI::stopCalibrate(void){
00071     char registerNumber   = SOFTWARE_REVISION_REG;
00072     char calibrate_data1 = 0xF8;
00073     //kirim data 1
00074     i2c->write(i2cAddress, &registerNumber, 1);
00075     i2c->write(i2cAddress, &calibrate_data1, 1);
00076 }