nambah buat eksternal
Dependencies: mbed encoderKRAI Motor_new
CMPS12_KRAI/CMPS12_KRAI.cpp@0:49e87dcad299, 2021-06-18 (annotated)
- Committer:
- Yolandataniaa
- Date:
- Fri Jun 18 11:04:08 2021 +0000
- Revision:
- 0:49e87dcad299
nambah buat eksternal
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Yolandataniaa | 0:49e87dcad299 | 1 | /** |
Yolandataniaa | 0:49e87dcad299 | 2 | * adopted from |
Yolandataniaa | 0:49e87dcad299 | 3 | * CMPS03 by: Aarom Berk |
Yolandataniaa | 0:49e87dcad299 | 4 | * |
Yolandataniaa | 0:49e87dcad299 | 5 | * Bismillahirahmanirrahim |
Yolandataniaa | 0:49e87dcad299 | 6 | */ |
Yolandataniaa | 0:49e87dcad299 | 7 | |
Yolandataniaa | 0:49e87dcad299 | 8 | /** |
Yolandataniaa | 0:49e87dcad299 | 9 | * Includes |
Yolandataniaa | 0:49e87dcad299 | 10 | */ |
Yolandataniaa | 0:49e87dcad299 | 11 | #include "CMPS12_KRAI.h" |
Yolandataniaa | 0:49e87dcad299 | 12 | const float radian_to_degree = 57.295779; |
Yolandataniaa | 0:49e87dcad299 | 13 | |
Yolandataniaa | 0:49e87dcad299 | 14 | CMPS12_KRAI::CMPS12_KRAI(PinName sda, PinName scl, int address) { |
Yolandataniaa | 0:49e87dcad299 | 15 | i2c = new I2C(sda, scl); |
Yolandataniaa | 0:49e87dcad299 | 16 | //CMPS11 maksimum 100kHz CMPS12 maksimum 400kHz |
Yolandataniaa | 0:49e87dcad299 | 17 | i2c->frequency(100000); |
Yolandataniaa | 0:49e87dcad299 | 18 | i2cAddress = address; |
Yolandataniaa | 0:49e87dcad299 | 19 | _offset_compass_value = 0; |
Yolandataniaa | 0:49e87dcad299 | 20 | _theta_origin = 0; |
Yolandataniaa | 0:49e87dcad299 | 21 | _theta_offset = 0; |
Yolandataniaa | 0:49e87dcad299 | 22 | |
Yolandataniaa | 0:49e87dcad299 | 23 | } |
Yolandataniaa | 0:49e87dcad299 | 24 | |
Yolandataniaa | 0:49e87dcad299 | 25 | char CMPS12_KRAI::readSoftwareRevision(void){ |
Yolandataniaa | 0:49e87dcad299 | 26 | char registerNumber = SOFTWARE_REVISION_REG; |
Yolandataniaa | 0:49e87dcad299 | 27 | char registerContents = 0; |
Yolandataniaa | 0:49e87dcad299 | 28 | |
Yolandataniaa | 0:49e87dcad299 | 29 | //First, send the number of register we wish to read, |
Yolandataniaa | 0:49e87dcad299 | 30 | //in this case, command register, number 0. |
Yolandataniaa | 0:49e87dcad299 | 31 | i2c->write(i2cAddress, ®isterNumber, 1); |
Yolandataniaa | 0:49e87dcad299 | 32 | |
Yolandataniaa | 0:49e87dcad299 | 33 | //Now, read one byte, which will be the contents of the command register. |
Yolandataniaa | 0:49e87dcad299 | 34 | i2c->read(i2cAddress, ®isterContents, 1); |
Yolandataniaa | 0:49e87dcad299 | 35 | |
Yolandataniaa | 0:49e87dcad299 | 36 | return registerContents; |
Yolandataniaa | 0:49e87dcad299 | 37 | } |
Yolandataniaa | 0:49e87dcad299 | 38 | |
Yolandataniaa | 0:49e87dcad299 | 39 | |
Yolandataniaa | 0:49e87dcad299 | 40 | int CMPS12_KRAI::getAngle(void){ |
Yolandataniaa | 0:49e87dcad299 | 41 | char registerNumber = COMPASS_BEARING_WORD_REG; |
Yolandataniaa | 0:49e87dcad299 | 42 | char registerContents[2] = {0x00, 0x00}; |
Yolandataniaa | 0:49e87dcad299 | 43 | |
Yolandataniaa | 0:49e87dcad299 | 44 | //Mengirim register untuk address pertama dari i2c |
Yolandataniaa | 0:49e87dcad299 | 45 | i2c->write(i2cAddress, ®isterNumber, 1); //1 |
Yolandataniaa | 0:49e87dcad299 | 46 | |
Yolandataniaa | 0:49e87dcad299 | 47 | //Mengambil data dari 2 address I2c |
Yolandataniaa | 0:49e87dcad299 | 48 | i2c->read(i2cAddress, registerContents, 2); |
Yolandataniaa | 0:49e87dcad299 | 49 | |
Yolandataniaa | 0:49e87dcad299 | 50 | //Register 0 adalah 8 bit, harus di shift |
Yolandataniaa | 0:49e87dcad299 | 51 | //Register 1 adalah 16 bit, bisa langsung dibaca |
Yolandataniaa | 0:49e87dcad299 | 52 | int angle = ((int)registerContents[0] << 8) | ((int)registerContents[1]); |
Yolandataniaa | 0:49e87dcad299 | 53 | |
Yolandataniaa | 0:49e87dcad299 | 54 | //kirim data berupa sudut 0-360 |
Yolandataniaa | 0:49e87dcad299 | 55 | return angle; |
Yolandataniaa | 0:49e87dcad299 | 56 | } |
Yolandataniaa | 0:49e87dcad299 | 57 | |
Yolandataniaa | 0:49e87dcad299 | 58 | int CMPS12_KRAI::getPitch(void){ |
Yolandataniaa | 0:49e87dcad299 | 59 | char registerNumber = COMPASS_PITCH_WORD_REG; |
Yolandataniaa | 0:49e87dcad299 | 60 | char registerContents[2] = {0x00, 0x00}; |
Yolandataniaa | 0:49e87dcad299 | 61 | |
Yolandataniaa | 0:49e87dcad299 | 62 | //Mengirim register untuk address pertama dari i2c |
Yolandataniaa | 0:49e87dcad299 | 63 | i2c->write(i2cAddress, ®isterNumber, 1); |
Yolandataniaa | 0:49e87dcad299 | 64 | |
Yolandataniaa | 0:49e87dcad299 | 65 | //Mengambil data dari 2 address I2c |
Yolandataniaa | 0:49e87dcad299 | 66 | i2c->read(i2cAddress, registerContents, 1); |
Yolandataniaa | 0:49e87dcad299 | 67 | |
Yolandataniaa | 0:49e87dcad299 | 68 | //simpan data ke variable pitch |
Yolandataniaa | 0:49e87dcad299 | 69 | int pitch = ((int)registerContents[0] ); |
Yolandataniaa | 0:49e87dcad299 | 70 | |
Yolandataniaa | 0:49e87dcad299 | 71 | return pitch; |
Yolandataniaa | 0:49e87dcad299 | 72 | } |
Yolandataniaa | 0:49e87dcad299 | 73 | |
Yolandataniaa | 0:49e87dcad299 | 74 | int CMPS12_KRAI::getRoll(void){ |
Yolandataniaa | 0:49e87dcad299 | 75 | char registerNumber = COMPASS_ROLL_WORD_REG; |
Yolandataniaa | 0:49e87dcad299 | 76 | char registerContents[2] = {0x00, 0x00}; |
Yolandataniaa | 0:49e87dcad299 | 77 | |
Yolandataniaa | 0:49e87dcad299 | 78 | //Mengirim register untuk address pertama dari i2c |
Yolandataniaa | 0:49e87dcad299 | 79 | i2c->write(i2cAddress, ®isterNumber, 1); |
Yolandataniaa | 0:49e87dcad299 | 80 | |
Yolandataniaa | 0:49e87dcad299 | 81 | //Mengambil data dari 2 address I2c |
Yolandataniaa | 0:49e87dcad299 | 82 | i2c->read(i2cAddress, registerContents, 1); |
Yolandataniaa | 0:49e87dcad299 | 83 | |
Yolandataniaa | 0:49e87dcad299 | 84 | //simpan data ke variable roll |
Yolandataniaa | 0:49e87dcad299 | 85 | int roll = ((int)registerContents[0] ); |
Yolandataniaa | 0:49e87dcad299 | 86 | |
Yolandataniaa | 0:49e87dcad299 | 87 | return roll; |
Yolandataniaa | 0:49e87dcad299 | 88 | } |
Yolandataniaa | 0:49e87dcad299 | 89 | |
Yolandataniaa | 0:49e87dcad299 | 90 | void CMPS12_KRAI::calibrate(void){ |
Yolandataniaa | 0:49e87dcad299 | 91 | char registerNumber = SOFTWARE_REVISION_REG; |
Yolandataniaa | 0:49e87dcad299 | 92 | char calibrate_data1 = 0xF0; |
Yolandataniaa | 0:49e87dcad299 | 93 | char calibrate_data2 = 0xF5; |
Yolandataniaa | 0:49e87dcad299 | 94 | char calibrate_data3 = 0xF7; |
Yolandataniaa | 0:49e87dcad299 | 95 | //kirim data 1 |
Yolandataniaa | 0:49e87dcad299 | 96 | //Thread::wait(25); |
Yolandataniaa | 0:49e87dcad299 | 97 | i2c->write(i2cAddress, ®isterNumber, 1); |
Yolandataniaa | 0:49e87dcad299 | 98 | i2c->write(i2cAddress, &calibrate_data1, 1); |
Yolandataniaa | 0:49e87dcad299 | 99 | //kirim data 2 delay 25ms |
Yolandataniaa | 0:49e87dcad299 | 100 | //Thread::wait(25); |
Yolandataniaa | 0:49e87dcad299 | 101 | i2c->write(i2cAddress, ®isterNumber, 1); |
Yolandataniaa | 0:49e87dcad299 | 102 | i2c->write(i2cAddress, &calibrate_data2, 1); |
Yolandataniaa | 0:49e87dcad299 | 103 | //kirim data 3 delay 25ms |
Yolandataniaa | 0:49e87dcad299 | 104 | //Thread::wait(25); |
Yolandataniaa | 0:49e87dcad299 | 105 | i2c->write(i2cAddress, ®isterNumber, 1); |
Yolandataniaa | 0:49e87dcad299 | 106 | i2c->write(i2cAddress, &calibrate_data3, 1); |
Yolandataniaa | 0:49e87dcad299 | 107 | } |
Yolandataniaa | 0:49e87dcad299 | 108 | |
Yolandataniaa | 0:49e87dcad299 | 109 | void CMPS12_KRAI::stopCalibrate(void){ |
Yolandataniaa | 0:49e87dcad299 | 110 | char registerNumber = SOFTWARE_REVISION_REG; |
Yolandataniaa | 0:49e87dcad299 | 111 | char calibrate_data1 = 0xF8; |
Yolandataniaa | 0:49e87dcad299 | 112 | //kirim data 1 |
Yolandataniaa | 0:49e87dcad299 | 113 | i2c->write(i2cAddress, ®isterNumber, 1); |
Yolandataniaa | 0:49e87dcad299 | 114 | i2c->write(i2cAddress, &calibrate_data1, 1); |
Yolandataniaa | 0:49e87dcad299 | 115 | } |
Yolandataniaa | 0:49e87dcad299 | 116 | |
Yolandataniaa | 0:49e87dcad299 | 117 | |
Yolandataniaa | 0:49e87dcad299 | 118 | int CMPS12_KRAI::getAccelX(void){ |
Yolandataniaa | 0:49e87dcad299 | 119 | char registerNumber = COMPASS_ROLL_WORD_REG; //0x02 |
Yolandataniaa | 0:49e87dcad299 | 120 | char registerContents[16] = {0x00, 0x00}; |
Yolandataniaa | 0:49e87dcad299 | 121 | |
Yolandataniaa | 0:49e87dcad299 | 122 | //Mengirim register untuk address pertama dari i2c |
Yolandataniaa | 0:49e87dcad299 | 123 | i2c->write(i2cAddress, ®isterNumber, 1); |
Yolandataniaa | 0:49e87dcad299 | 124 | |
Yolandataniaa | 0:49e87dcad299 | 125 | //Mengambil data dari 2 address I2c |
Yolandataniaa | 0:49e87dcad299 | 126 | i2c->read(i2cAddress, registerContents, 16); |
Yolandataniaa | 0:49e87dcad299 | 127 | |
Yolandataniaa | 0:49e87dcad299 | 128 | //simpan data ke variable roll |
Yolandataniaa | 0:49e87dcad299 | 129 | int accelX = ((int)registerContents[11]<<8 | (int)registerContents[12] ); |
Yolandataniaa | 0:49e87dcad299 | 130 | |
Yolandataniaa | 0:49e87dcad299 | 131 | return accelX; |
Yolandataniaa | 0:49e87dcad299 | 132 | } |
Yolandataniaa | 0:49e87dcad299 | 133 | |
Yolandataniaa | 0:49e87dcad299 | 134 | void CMPS12_KRAI::compassResetOffsetValue(){ |
Yolandataniaa | 0:49e87dcad299 | 135 | _offset_compass_value = (float)CMPS12_KRAI::getAngle()/10; |
Yolandataniaa | 0:49e87dcad299 | 136 | } |
Yolandataniaa | 0:49e87dcad299 | 137 | |
Yolandataniaa | 0:49e87dcad299 | 138 | void CMPS12_KRAI::compassUpdateValue(){ |
Yolandataniaa | 0:49e87dcad299 | 139 | _theta_origin = (float)CMPS12_KRAI::getAngle()/10; |
Yolandataniaa | 0:49e87dcad299 | 140 | _theta_offset = _theta_origin - _offset_compass_value; |
Yolandataniaa | 0:49e87dcad299 | 141 | } |
Yolandataniaa | 0:49e87dcad299 | 142 | |
Yolandataniaa | 0:49e87dcad299 | 143 | float CMPS12_KRAI::compassValue(){ |
Yolandataniaa | 0:49e87dcad299 | 144 | float theta_transformed; |
Yolandataniaa | 0:49e87dcad299 | 145 | |
Yolandataniaa | 0:49e87dcad299 | 146 | if(_theta_offset > 180.0f && _theta_offset <= 360.0f) |
Yolandataniaa | 0:49e87dcad299 | 147 | theta_transformed = (_theta_origin - 360.0f - _offset_compass_value)/-radian_to_degree; |
Yolandataniaa | 0:49e87dcad299 | 148 | else if(_theta_offset < -180.0f && _theta_offset >= -360.0f) |
Yolandataniaa | 0:49e87dcad299 | 149 | theta_transformed = (_theta_origin + 360.0f - _offset_compass_value)/-radian_to_degree; |
Yolandataniaa | 0:49e87dcad299 | 150 | else |
Yolandataniaa | 0:49e87dcad299 | 151 | theta_transformed = (_theta_origin - _offset_compass_value)/-radian_to_degree; |
Yolandataniaa | 0:49e87dcad299 | 152 | |
Yolandataniaa | 0:49e87dcad299 | 153 | return theta_transformed; |
Yolandataniaa | 0:49e87dcad299 | 154 | } |