compass KRI 2018
Embed:
(wiki syntax)
Show/hide line numbers
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, ®isterNumber, 1); 00028 00029 //Now, read one byte, which will be the contents of the command register. 00030 i2c->read(i2cAddress, ®isterContents, 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, ®isterNumber, 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, ®isterNumber, 1); 00059 i2c->write(i2cAddress, &calibrate_data1, 1); 00060 //kirim data 2 delay 25ms 00061 Thread::wait(25); 00062 i2c->write(i2cAddress, ®isterNumber, 1); 00063 i2c->write(i2cAddress, &calibrate_data2, 1); 00064 //kirim data 3 delay 25ms 00065 Thread::wait(25); 00066 i2c->write(i2cAddress, ®isterNumber, 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, ®isterNumber, 1); 00075 i2c->write(i2cAddress, &calibrate_data1, 1); 00076 }
Generated on Tue Aug 9 2022 20:15:42 by 1.7.2