Yolanda Tania / Mbed 2 deprecated bacaencoder

nambah buat eksternal

Dependencies:   mbed encoderKRAI Motor_new

CMPS12_KRAI/CMPS12_KRAI.cpp@1:bbe0769f00e9, 2021-06-21 (annotated)

Committer:
Yolandataniaa
Date:
Mon Jun 21 07:48:27 2021 +0000
Revision:
1:bbe0769f00e9
Parent:
0:49e87dcad299 
revisi

Who changed what in which revision?

UserRevisionLine numberNew 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, &registerNumber, 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, &registerContents, 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, &registerNumber, 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, &registerNumber, 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, &registerNumber, 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, &registerNumber, 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, &registerNumber, 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, &registerNumber, 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, &registerNumber, 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, &registerNumber, 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 }