dimel contreras / Mbed 2 deprecated Algoritmo_Fuzzy

algoritmo logica difusa sensores navegacion

Dependencies:   GPS MODI2C SRF05 mbed HMC5883

Sensors/Gyro/L3G4200D.cpp@0:1c15748ff0e1, 2014-07-19 (annotated)

Committer:
arturocontreras
Date:
Sat Jul 19 05:35:58 2014 +0000
Revision:
0:1c15748ff0e1
logica difusa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
arturocontreras 0:1c15748ff0e1 1 #include "L3G4200D.h"
arturocontreras 0:1c15748ff0e1 2
arturocontreras 0:1c15748ff0e1 3 L3G4200D::L3G4200D(PinName sda, PinName scl) : I2C_Sensor(sda, scl, L3G4200D_I2C_ADDRESS)
arturocontreras 0:1c15748ff0e1 4 {
arturocontreras 0:1c15748ff0e1 5 // Turns on the L3G4200D's gyro and places it in normal mode.
arturocontreras 0:1c15748ff0e1 6 // Normal power mode, all axes enabled (for detailed info see datasheet)
arturocontreras 0:1c15748ff0e1 7
arturocontreras 0:1c15748ff0e1 8 //writeRegister(L3G4200D_CTRL_REG2, 0x05); // control filter
arturocontreras 0:1c15748ff0e1 9 writeRegister(L3G4200D_CTRL_REG2, 0x00); // highpass filter disabled
arturocontreras 0:1c15748ff0e1 10 writeRegister(L3G4200D_CTRL_REG3, 0x00);
arturocontreras 0:1c15748ff0e1 11 writeRegister(L3G4200D_CTRL_REG4, 0x20); // sets acuracy to 2000 dps (degree per second)
arturocontreras 0:1c15748ff0e1 12
arturocontreras 0:1c15748ff0e1 13 writeRegister(L3G4200D_REFERENCE, 0x00);
arturocontreras 0:1c15748ff0e1 14 //writeRegister(L3G4200D_STATUS_REG, 0x0F);
arturocontreras 0:1c15748ff0e1 15
arturocontreras 0:1c15748ff0e1 16 writeRegister(L3G4200D_INT1_THS_XH, 0x2C); // TODO: WTF??
arturocontreras 0:1c15748ff0e1 17 writeRegister(L3G4200D_INT1_THS_XL, 0xA4);
arturocontreras 0:1c15748ff0e1 18 writeRegister(L3G4200D_INT1_THS_YH, 0x2C);
arturocontreras 0:1c15748ff0e1 19 writeRegister(L3G4200D_INT1_THS_YL, 0xA4);
arturocontreras 0:1c15748ff0e1 20 writeRegister(L3G4200D_INT1_THS_ZH, 0x2C);
arturocontreras 0:1c15748ff0e1 21 writeRegister(L3G4200D_INT1_THS_ZL, 0xA4);
arturocontreras 0:1c15748ff0e1 22 //writeRegister(L3G4200D_INT1_DURATION, 0x00);
arturocontreras 0:1c15748ff0e1 23
arturocontreras 0:1c15748ff0e1 24 writeRegister(L3G4200D_CTRL_REG5, 0x00); // deactivates the filters (only use one of these options)
arturocontreras 0:1c15748ff0e1 25 //writeRegister(L3G4200D_CTRL_REG5, 0x12); // activates both high and low pass filters
arturocontreras 0:1c15748ff0e1 26 //writeRegister(L3G4200D_CTRL_REG5, 0x01); // activates high pass filter
arturocontreras 0:1c15748ff0e1 27
arturocontreras 0:1c15748ff0e1 28 writeRegister(L3G4200D_CTRL_REG1, 0x0F); // starts Gyro measurement
arturocontreras 0:1c15748ff0e1 29
arturocontreras 0:1c15748ff0e1 30 //calibrate(50, 0.01);
arturocontreras 0:1c15748ff0e1 31 }
arturocontreras 0:1c15748ff0e1 32
arturocontreras 0:1c15748ff0e1 33 void L3G4200D::read()
arturocontreras 0:1c15748ff0e1 34 {
arturocontreras 0:1c15748ff0e1 35 readraw(); // read raw measurement data
arturocontreras 0:1c15748ff0e1 36
arturocontreras 0:1c15748ff0e1 37 for (int i = 0; i < 3; i++)
arturocontreras 0:1c15748ff0e1 38 data[i] = (raw[i] - offset[i])*0.07; // subtract offset from calibration and multiply unit factor (datasheet s.10)
arturocontreras 0:1c15748ff0e1 39 }
arturocontreras 0:1c15748ff0e1 40
arturocontreras 0:1c15748ff0e1 41 int L3G4200D::readTemp()
arturocontreras 0:1c15748ff0e1 42 {
arturocontreras 0:1c15748ff0e1 43 return (short) readRegister(L3G4200D_OUT_TEMP); // read the sensors register for the temperature
arturocontreras 0:1c15748ff0e1 44 }
arturocontreras 0:1c15748ff0e1 45
arturocontreras 0:1c15748ff0e1 46 void L3G4200D::readraw()
arturocontreras 0:1c15748ff0e1 47 {
arturocontreras 0:1c15748ff0e1 48 char buffer[6]; // 8-Bit pieces of axis data
arturocontreras 0:1c15748ff0e1 49
arturocontreras 0:1c15748ff0e1 50 readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C // TODO: why?! | (1 << 7)
arturocontreras 0:1c15748ff0e1 51
arturocontreras 0:1c15748ff0e1 52 raw[0] = (short) (buffer[1] << 8 | buffer[0]); // join 8-Bit pieces to 16-bit short integers
arturocontreras 0:1c15748ff0e1 53 raw[1] = (short) (buffer[3] << 8 | buffer[2]);
arturocontreras 0:1c15748ff0e1 54 raw[2] = (short) (buffer[5] << 8 | buffer[4]);
arturocontreras 0:1c15748ff0e1 55 }
arturocontreras 0:1c15748ff0e1 56
arturocontreras 0:1c15748ff0e1 57 void L3G4200D::calibrate(int times, float separation_time)
arturocontreras 0:1c15748ff0e1 58 {
arturocontreras 0:1c15748ff0e1 59 // calibrate sensor with an average of count samples (result of calibration stored in offset[])
arturocontreras 0:1c15748ff0e1 60 float calib[3] = {0,0,0}; // temporary array for the sum of calibration measurement
arturocontreras 0:1c15748ff0e1 61
arturocontreras 0:1c15748ff0e1 62 for (int i = 0; i < times; i++) { // read 'times' times the data in a very short time
arturocontreras 0:1c15748ff0e1 63 readraw();
arturocontreras 0:1c15748ff0e1 64 for (int j = 0; j < 3; j++)
arturocontreras 0:1c15748ff0e1 65 calib[j] += raw[j];
arturocontreras 0:1c15748ff0e1 66 wait(separation_time);
arturocontreras 0:1c15748ff0e1 67 }
arturocontreras 0:1c15748ff0e1 68
arturocontreras 0:1c15748ff0e1 69 for (int i = 0; i < 3; i++)
arturocontreras 0:1c15748ff0e1 70 offset[i] = calib[i]/times; // take the average of the calibration measurements
arturocontreras 0:1c15748ff0e1 71 }