Diff: CMPS12_KRAI/CMPS12_KRAI.cpp

Revision:

0:49e87dcad299

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CMPS12_KRAI/CMPS12_KRAI.cpp	Fri Jun 18 11:04:08 2021 +0000
@@ -0,0 +1,154 @@
+/**
+ * adopted from
+ * CMPS03 by: Aarom Berk
+ * 
+ * Bismillahirahmanirrahim
+ */
+
+/**
+ * Includes
+ */
+#include "CMPS12_KRAI.h"
+const float  radian_to_degree                       = 57.295779;
+
+CMPS12_KRAI::CMPS12_KRAI(PinName sda, PinName scl, int address) {
+    i2c = new I2C(sda, scl);
+    //CMPS11 maksimum 100kHz CMPS12 maksimum 400kHz
+    i2c->frequency(100000);
+    i2cAddress = address;
+    _offset_compass_value = 0;
+    _theta_origin = 0;
+    _theta_offset = 0;
+
+}
+
+char CMPS12_KRAI::readSoftwareRevision(void){
+    char registerNumber   = SOFTWARE_REVISION_REG;
+    char registerContents = 0;
+
+    //First, send the number of register we wish to read,
+    //in this case, command register, number 0.
+    i2c->write(i2cAddress, &registerNumber, 1);
+    
+    //Now, read one byte, which will be the contents of the command register.
+    i2c->read(i2cAddress, &registerContents, 1);
+    
+    return registerContents; 
+}
+
+
+int CMPS12_KRAI::getAngle(void){
+    char registerNumber = COMPASS_BEARING_WORD_REG;
+    char registerContents[2] = {0x00, 0x00};
+    
+    //Mengirim register untuk address pertama dari i2c
+    i2c->write(i2cAddress, &registerNumber, 1); //1
+    
+    //Mengambil data dari 2 address I2c
+    i2c->read(i2cAddress, registerContents, 2);
+    
+    //Register 0 adalah 8 bit, harus di shift
+    //Register 1 adalah 16 bit, bisa langsung dibaca
+    int angle = ((int)registerContents[0] << 8) | ((int)registerContents[1]);
+    
+    //kirim data berupa sudut 0-360
+    return angle;   
+}
+
+int CMPS12_KRAI::getPitch(void){
+    char registerNumber = COMPASS_PITCH_WORD_REG;
+    char registerContents[2] = {0x00, 0x00};
+    
+    //Mengirim register untuk address pertama dari i2c
+    i2c->write(i2cAddress, &registerNumber, 1);
+    
+    //Mengambil data dari 2 address I2c
+    i2c->read(i2cAddress, registerContents, 1);
+    
+    //simpan data ke variable pitch
+    int pitch = ((int)registerContents[0] );
+    
+    return pitch;   
+}
+
+int CMPS12_KRAI::getRoll(void){
+    char registerNumber = COMPASS_ROLL_WORD_REG;
+    char registerContents[2] = {0x00, 0x00};
+    
+    //Mengirim register untuk address pertama dari i2c
+    i2c->write(i2cAddress, &registerNumber, 1);
+    
+    //Mengambil data dari 2 address I2c
+    i2c->read(i2cAddress, registerContents, 1);
+    
+    //simpan data ke variable roll
+    int roll = ((int)registerContents[0] );
+    
+    return roll;   
+}
+
+void CMPS12_KRAI::calibrate(void){
+    char registerNumber   = SOFTWARE_REVISION_REG;
+    char calibrate_data1 = 0xF0;
+    char calibrate_data2 = 0xF5;
+    char calibrate_data3 = 0xF7;
+    //kirim data 1
+    //Thread::wait(25);
+    i2c->write(i2cAddress, &registerNumber, 1);
+    i2c->write(i2cAddress, &calibrate_data1, 1);
+    //kirim data 2 delay 25ms
+    //Thread::wait(25);
+    i2c->write(i2cAddress, &registerNumber, 1);
+    i2c->write(i2cAddress, &calibrate_data2, 1);
+    //kirim data 3 delay 25ms
+    //Thread::wait(25);
+    i2c->write(i2cAddress, &registerNumber, 1);
+    i2c->write(i2cAddress, &calibrate_data3, 1);
+}
+
+void CMPS12_KRAI::stopCalibrate(void){
+    char registerNumber   = SOFTWARE_REVISION_REG;
+    char calibrate_data1 = 0xF8;
+    //kirim data 1
+    i2c->write(i2cAddress, &registerNumber, 1);
+    i2c->write(i2cAddress, &calibrate_data1, 1);
+}
+
+
+int CMPS12_KRAI::getAccelX(void){
+    char registerNumber = COMPASS_ROLL_WORD_REG; //0x02
+    char registerContents[16] = {0x00, 0x00};
+    
+    //Mengirim register untuk address pertama dari i2c
+    i2c->write(i2cAddress, &registerNumber, 1);
+    
+    //Mengambil data dari 2 address I2c
+    i2c->read(i2cAddress, registerContents, 16);
+    
+    //simpan data ke variable roll
+    int accelX = ((int)registerContents[11]<<8 | (int)registerContents[12] );
+    
+    return accelX;   
+}
+
+void CMPS12_KRAI::compassResetOffsetValue(){
+    _offset_compass_value = (float)CMPS12_KRAI::getAngle()/10;
+}
+
+void CMPS12_KRAI::compassUpdateValue(){
+    _theta_origin = (float)CMPS12_KRAI::getAngle()/10;
+    _theta_offset = _theta_origin - _offset_compass_value;
+}
+
+float CMPS12_KRAI::compassValue(){
+    float theta_transformed;
+
+    if(_theta_offset > 180.0f && _theta_offset <= 360.0f)
+        theta_transformed = (_theta_origin - 360.0f - _offset_compass_value)/-radian_to_degree;
+    else if(_theta_offset < -180.0f && _theta_offset >= -360.0f)
+        theta_transformed = (_theta_origin  + 360.0f - _offset_compass_value)/-radian_to_degree;
+    else
+        theta_transformed = (_theta_origin - _offset_compass_value)/-radian_to_degree;
+
+    return theta_transformed; 
+}
\ No newline at end of file