Revision 0:1c15748ff0e1, committed 2014-07-19

Comitter:

arturocontreras

Date:

Sat Jul 19 05:35:58 2014 +0000

Commit message:

logica difusa

Changed in this revision

diff -r 000000000000 -r 1c15748ff0e1 BMP085/BMP085_old.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085/BMP085_old.cpp	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,150 @@
+
+#include "mbed.h"
+#include "BMP085_old.h"
+
+//I2C Adresse
+#define BMP085_ADRESS 0xEE
+
+#define xpow(x, y) ((long)1 << y)
+
+
+// Constructor
+// -----------------------------------------------
+BMP085_old::BMP085_old(PinName sda, PinName scl) : i2c_(sda, scl)
+    {
+    Init();
+    // MYINIT -------
+    oss = 0; //Oversampling des Barometers setzen
+    // MYINIT -------
+    }
+    
+
+// Temperatur und Druck auslesen und berechnen
+// -----------------------------------------------
+void BMP085_old::Update () 
+    {
+    long P, UTemp, UPressure, X1, X2, X3, B3, B5, B6;
+    unsigned long B4, B7;
+
+    twi_writechar(BMP085_ADRESS, 0xf4, 0x2e);
+    // Wait at least 4.5ms
+    wait(0.005);
+    UTemp = twi_readshort(BMP085_ADRESS, 0xf6);
+    
+    X1 = ((UTemp - AC6) * AC5) >> 15;
+    X2 = (MC << 11) / (X1 + MD);
+    B5 = X1 + X2;
+    Temperature = (float)((B5 + 8) >> 4)/10.0;
+
+    twi_writechar(BMP085_ADRESS, 0xf4, 0x34 + (oss << 6));
+    // Wait at least 4.5ms
+    wait(0.005);
+    UPressure = twi_readlong(BMP085_ADRESS, 0xf6) >> (8 - oss);
+
+    B6 = B5 - 4000;
+    X1 = (B2 * (B6 * B6) >> 12) >> 11;
+    X2 = (AC2 * B6) >> 11;
+    X3 = X1 + X2;
+    B3 = ((AC1 * 4 + X3) << oss) >> 2;    
+
+    X1 = (AC3 * B6) >> 13;
+    X2 = (B1 * (B6 * B6) >> 12) >> 16;
+    X3 = ((X1 + X2) + 2) >> 2;
+    B4 = AC4 * (X3 + 32768) >> 15;
+    
+    B7 = (unsigned long)(UPressure - B3) * (50000 >> oss);
+    
+    if (B7 < 0x80000000) 
+        {
+        P = (2 * B7) / B4;
+        }
+    else 
+        {
+        P = 2* (B7 / B4);
+        }
+    X1 = (P >> 8) * (P >> 8);
+    X1 = (X1 * 3038) >> 16;
+    X2 = (-7357 * P) >> 16;    
+    P = P + ((X1 + X2 + 3791) >> 4);
+    Pressure = (float)P / 100.0;
+    }
+
+
+// Hoehe u.M. berechnen  (Druck in hPa)
+// -----------------------------------------------
+float BMP085_old::CalcAltitude(float Press)
+    {
+    float A = Press/1013.25;
+    float B = 1/5.25588;
+    float C = pow(A,B);
+    
+    C = 1 - C;
+    C = C / 22.5577e-6;
+    return C;
+    }
+    
+        
+// Drucksensor initialisieren  
+// -----------------------------------------------
+void BMP085_old::Init () 
+    {
+    AC1 = twi_readshort(BMP085_ADRESS, 0xaa);
+    AC2 = twi_readshort(BMP085_ADRESS, 0xac);
+    AC3 = twi_readshort(BMP085_ADRESS, 0xae);
+    AC4 = twi_readshort(BMP085_ADRESS, 0xb0);
+    AC5 = twi_readshort(BMP085_ADRESS, 0xb2);
+    AC6 = twi_readshort(BMP085_ADRESS, 0xb4);
+    B1 = twi_readshort(BMP085_ADRESS, 0xb6);
+    B2 = twi_readshort(BMP085_ADRESS, 0xb8);
+    MB = twi_readshort(BMP085_ADRESS, 0xba);
+    MC = twi_readshort(BMP085_ADRESS, 0xbc);
+    MD = twi_readshort(BMP085_ADRESS, 0xbe);
+    }
+    
+    
+// -----------------------------------------------
+unsigned short BMP085_old::twi_readshort (int id, int addr) {
+    unsigned short i;
+
+    i2c_.start();
+    i2c_.write(id);
+    i2c_.write(addr);
+
+    i2c_.start();
+    i2c_.write(id | 1);
+    i = i2c_.read(1) << 8;
+    i |= i2c_.read(0);
+    i2c_.stop();
+
+    return i;
+}
+
+
+// -----------------------------------------------
+unsigned long BMP085_old::twi_readlong (int id, int addr) {
+    unsigned long i;
+
+    i2c_.start();
+    i2c_.write(id);
+    i2c_.write(addr);
+
+    i2c_.start();
+    i2c_.write(id | 1);
+    i = i2c_.read(1) << 16;
+    i |= i2c_.read(1) << 8;
+    i |= i2c_.read(0);
+    i2c_.stop();
+
+    return i;
+}
+
+
+// -----------------------------------------------
+void BMP085_old::twi_writechar (int id, int addr, int dat) {
+
+    i2c_.start();
+    i2c_.write(id);
+    i2c_.write(addr);
+    i2c_.write(dat);
+    i2c_.stop();
+}

diff -r 000000000000 -r 1c15748ff0e1 BMP085/BMP085_old.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085/BMP085_old.h	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,34 @@
+ 
+#ifndef BMP085_I2C_H
+#define BMP085_I2C_H
+
+#include "mbed.h"
+
+class BMP085_old
+    {
+    private:
+        I2C i2c_;
+        
+    public:
+        BMP085_old(PinName sda, PinName scl);
+    
+        void    Update();
+        float   Temperature;
+        float   Pressure;
+        float   CalcAltitude(float Press);
+        short   oss;
+    
+    protected:
+        void Init();
+        unsigned short twi_readshort (int, int);
+        unsigned long twi_readlong (int, int);
+        void twi_writechar (int, int, int);
+    
+    
+    private:
+    
+        short AC1, AC2, AC3, B1, B2, MB, MC, MD;
+        unsigned short AC4, AC5, AC6;
+    };
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 1c15748ff0e1 GPS.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GPS.lib	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/GPS/#15611c7938a3

diff -r 000000000000 -r 1c15748ff0e1 MODI2C.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODI2C.lib	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/Sissors/code/MODI2C/#ff579e7e8efa

diff -r 000000000000 -r 1c15748ff0e1 SRF05.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SRF05.lib	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/SRF05/#e758665e072c

diff -r 000000000000 -r 1c15748ff0e1 Sensors/Acc/ADXL345.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Acc/ADXL345.cpp	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,44 @@
+#include "ADXL345.h"
+
+ADXL345::ADXL345(PinName sda, PinName scl) : I2C_Sensor(sda, scl, ADXL345_I2C_ADDRESS)
+{
+    // Set Offset  - programmed into the OFSX, OFSY, and OFXZ registers, respectively, as 0xFD, 0x03 and 0xFE.
+    writeRegister(ADXL345_OFSX_REG, 0xFA); // to get these offsets just lie your sensor down on the table always the axis pointing down to earth has 200+ and the others should have more or less 0
+    writeRegister(ADXL345_OFSY_REG, 0xFE);
+    writeRegister(ADXL345_OFSZ_REG, 0x0A);
+ 
+    writeRegister(ADXL345_BW_RATE_REG, 0x0F); // 3200Hz BW-Rate
+    writeRegister(ADXL345_DATA_FORMAT_REG, 0x0B); // set data format to full resolution and +-16g
+    writeRegister(ADXL345_POWER_CTL_REG, 0x08); // set mode
+}
+
+void ADXL345::read(){
+    readraw();
+    for (int i = 0; i < 3; i++)
+        data[i] = raw[i] - offset[i]; // TODO: didnt care about units 
+}
+
+void ADXL345::readraw(){
+    char buffer[6];    
+    readMultiRegister(ADXL345_DATAX0_REG, buffer, 6);
+    
+    raw[0] = (short) ((int)buffer[1] << 8 | (int)buffer[0]);
+    raw[1] = (short) ((int)buffer[3] << 8 | (int)buffer[2]);
+    raw[2] = (short) ((int)buffer[5] << 8 | (int)buffer[4]);
+}
+
+void ADXL345::calibrate(int times, float separation_time)
+{
+    // calibrate sensor with an average of count samples (result of calibration stored in offset[])
+    float calib[3] = {0,0,0};                           // temporary array for the sum of calibration measurement
+    
+    for (int i = 0; i < times; i++) {                   // read 'times' times the data in a very short time
+        readraw();
+        for (int j = 0; j < 3; j++)
+            calib[j] += raw[j];
+        wait(separation_time);
+    }
+    
+    for (int i = 0; i < 2; i++)
+        offset[i] = calib[i]/times;                     // take the average of the calibration measurements
+}
\ No newline at end of file

diff -r 000000000000 -r 1c15748ff0e1 Sensors/Acc/ADXL345.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Acc/ADXL345.h	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,65 @@
+// based on http://mbed.org/users/Digixx/code/ADXL345/
+
+#ifndef ADXL345_H
+#define ADXL345_H
+
+#include "mbed.h"
+#include "I2C_Sensor.h"
+
+#define ADXL345_I2C_ADDRESS    0xA6
+//the ADXL345 7-bit address is 0x53 when ALT ADDRESS is low as it is on the sparkfun chip: when ALT ADDRESS is high the address is 0x1D
+//when ALT ADDRESS pin is high:  
+//#define ADXL345_I2C_ADDRESS    0x3A 
+
+// register addresses
+#define ADXL345_DEVID_REG          0x00
+#define ADXL345_THRESH_TAP_REG     0x1D
+#define ADXL345_OFSX_REG           0x1E
+#define ADXL345_OFSY_REG           0x1F
+#define ADXL345_OFSZ_REG           0x20
+#define ADXL345_DUR_REG            0x21
+#define ADXL345_LATENT_REG         0x22
+#define ADXL345_WINDOW_REG         0x23
+#define ADXL345_THRESH_ACT_REG     0x24
+#define ADXL345_THRESH_INACT_REG   0x25
+#define ADXL345_TIME_INACT_REG     0x26
+#define ADXL345_ACT_INACT_CTL_REG  0x27
+#define ADXL345_THRESH_FF_REG      0x28
+#define ADXL345_TIME_FF_REG        0x29
+#define ADXL345_TAP_AXES_REG       0x2A
+#define ADXL345_ACT_TAP_STATUS_REG 0x2B
+#define ADXL345_BW_RATE_REG        0x2C
+#define ADXL345_POWER_CTL_REG      0x2D
+#define ADXL345_INT_ENABLE_REG     0x2E
+#define ADXL345_INT_MAP_REG        0x2F
+#define ADXL345_INT_SOURCE_REG     0x30
+#define ADXL345_DATA_FORMAT_REG    0x31
+#define ADXL345_DATAX0_REG         0x32
+#define ADXL345_DATAX1_REG         0x33
+#define ADXL345_DATAY0_REG         0x34
+#define ADXL345_DATAY1_REG         0x35
+#define ADXL345_DATAZ0_REG         0x36
+#define ADXL345_DATAZ1_REG         0x37
+#define ADXL345_FIFO_CTL           0x38
+#define ADXL345_FIFO_STATUS        0x39
+
+#define ADXL345_X           0x00
+#define ADXL345_Y           0x01
+#define ADXL345_Z           0x02
+
+typedef char byte;
+
+class ADXL345 : public I2C_Sensor
+{
+    public:
+        ADXL345(PinName sda, PinName scl);                  // constructor, uses I2C_Sensor class
+        virtual void read();                                // read all axis to array
+        
+        float offset[3];                                    // offset that's subtracted from every measurement
+        void calibrate(int times, float separation_time);   // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving)
+       
+    private:
+        virtual void readraw();
+};
+
+#endif

diff -r 000000000000 -r 1c15748ff0e1 Sensors/Alt/BMP085.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Alt/BMP085.cpp	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,57 @@
+#include "BMP085.h"
+
+BMP085::BMP085(PinName sda, PinName scl) : I2C_Sensor(sda, scl, BMP085_I2C_ADDRESS)
+{
+    // initialize BMP085 with settings
+    //writeRegister(0xf4, 0x2e); // TODO: was macht das + register in header!
+    
+}
+
+/*void BMP085::read() 
+    {
+    long P, UTemp, UPressure, X1, X2, X3, B3, B5, B6;
+    unsigned long B4, B7;
+
+    // TODO: writeRegister(0xf4, 0x2e); ?!!!
+    twi_writechar(BMP085_ADRESS, 0xf4, 0x2e);
+    // Wait at least 4.5ms
+    wait(0.005);
+    UTemp = twi_readshort(BMP085_ADRESS, 0xf6);
+    
+    X1 = ((UTemp - AC6) * AC5) >> 15;
+    X2 = (MC << 11) / (X1 + MD);
+    B5 = X1 + X2;
+    Temperature = (float)((B5 + 8) >> 4)/10.0;
+
+    twi_writechar(BMP085_ADRESS, 0xf4, 0x34 + (oss << 6));
+    // Wait at least 4.5ms
+    wait(0.005);
+    UPressure = twi_readlong(BMP085_ADRESS, 0xf6) >> (8 - oss);
+
+    B6 = B5 - 4000;
+    X1 = (B2 * (B6 * B6) >> 12) >> 11;
+    X2 = (AC2 * B6) >> 11;
+    X3 = X1 + X2;
+    B3 = ((AC1 * 4 + X3) << oss) >> 2;    
+
+    X1 = (AC3 * B6) >> 13;
+    X2 = (B1 * (B6 * B6) >> 12) >> 16;
+    X3 = ((X1 + X2) + 2) >> 2;
+    B4 = AC4 * (X3 + 32768) >> 15;
+    
+    B7 = (unsigned long)(UPressure - B3) * (50000 >> oss);
+    
+    if (B7 < 0x80000000) 
+        {
+        P = (2 * B7) / B4;
+        }
+    else 
+        {
+        P = 2* (B7 / B4);
+        }
+    X1 = (P >> 8) * (P >> 8);
+    X1 = (X1 * 3038) >> 16;
+    X2 = (-7357 * P) >> 16;    
+    P = P + ((X1 + X2 + 3791) >> 4);
+    Pressure = (float)P / 100.0;
+    }*/
\ No newline at end of file

diff -r 000000000000 -r 1c15748ff0e1 Sensors/Alt/BMP085.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Alt/BMP085.h	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,34 @@
+// based on http://mbed.org/users/okini3939/code/BMP085/
+
+#ifndef BMP085_H
+#define BMP085_H
+
+#include "mbed.h"
+#include "I2C_Sensor.h"
+
+#define BMP085_I2C_ADDRESS 0xEE
+
+class BMP085 : public I2C_Sensor
+{           
+    public:
+        BMP085(PinName sda, PinName scl);
+        
+        //virtual void read();
+        
+        void calibrate(int s);
+        
+        float get_height();
+         
+    private:
+        // raw data and function to measure it
+        int raw[3];
+        //void readraw();
+        
+        // calibration parameters and their saving
+        int Min[3];
+        int Max[3];
+        float scale[3];
+        float offset[3];
+};
+
+#endif

diff -r 000000000000 -r 1c15748ff0e1 Sensors/Gyro/L3G4200D.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Gyro/L3G4200D.cpp	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,71 @@
+#include "L3G4200D.h"
+
+L3G4200D::L3G4200D(PinName sda, PinName scl) : I2C_Sensor(sda, scl, L3G4200D_I2C_ADDRESS)
+{
+    // Turns on the L3G4200D's gyro and places it in normal mode.
+    // Normal power mode, all axes enabled (for detailed info see datasheet)
+    
+    //writeRegister(L3G4200D_CTRL_REG2, 0x05);               // control filter
+    writeRegister(L3G4200D_CTRL_REG2, 0x00);            // highpass filter disabled
+    writeRegister(L3G4200D_CTRL_REG3, 0x00);
+    writeRegister(L3G4200D_CTRL_REG4, 0x20);            // sets acuracy to 2000 dps (degree per second)
+    
+    writeRegister(L3G4200D_REFERENCE, 0x00);
+    //writeRegister(L3G4200D_STATUS_REG, 0x0F);
+    
+    writeRegister(L3G4200D_INT1_THS_XH, 0x2C); // TODO: WTF??
+    writeRegister(L3G4200D_INT1_THS_XL, 0xA4);
+    writeRegister(L3G4200D_INT1_THS_YH, 0x2C);
+    writeRegister(L3G4200D_INT1_THS_YL, 0xA4);
+    writeRegister(L3G4200D_INT1_THS_ZH, 0x2C);
+    writeRegister(L3G4200D_INT1_THS_ZL, 0xA4);
+    //writeRegister(L3G4200D_INT1_DURATION, 0x00);
+    
+    writeRegister(L3G4200D_CTRL_REG5, 0x00);            // deactivates the filters (only use one of these options)
+    //writeRegister(L3G4200D_CTRL_REG5, 0x12);          // activates both high and low pass filters
+    //writeRegister(L3G4200D_CTRL_REG5, 0x01);          // activates high pass filter
+    
+    writeRegister(L3G4200D_CTRL_REG1, 0x0F);            // starts Gyro measurement
+    
+    //calibrate(50, 0.01);
+}
+
+void L3G4200D::read()
+{
+    readraw();                                          // read raw measurement data
+    
+    for (int i = 0; i < 3; i++)
+            data[i] = (raw[i] - offset[i])*0.07;               // subtract offset from calibration and multiply unit factor (datasheet s.10)
+}
+
+int L3G4200D::readTemp()
+{
+    return (short) readRegister(L3G4200D_OUT_TEMP);     // read the sensors register for the temperature
+}
+
+void L3G4200D::readraw()
+{
+    char buffer[6];                                     // 8-Bit pieces of axis data
+    
+    readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C   // TODO: why?!   | (1 << 7)
+    
+    raw[0] = (short) (buffer[1] << 8 | buffer[0]);     // join 8-Bit pieces to 16-bit short integers
+    raw[1] = (short) (buffer[3] << 8 | buffer[2]);
+    raw[2] = (short) (buffer[5] << 8 | buffer[4]);
+}
+
+void L3G4200D::calibrate(int times, float separation_time)
+{
+    // calibrate sensor with an average of count samples (result of calibration stored in offset[])
+    float calib[3] = {0,0,0};                           // temporary array for the sum of calibration measurement
+    
+    for (int i = 0; i < times; i++) {                   // read 'times' times the data in a very short time
+        readraw();
+        for (int j = 0; j < 3; j++)
+            calib[j] += raw[j];
+        wait(separation_time);
+    }
+    
+    for (int i = 0; i < 3; i++)
+        offset[i] = calib[i]/times;                     // take the average of the calibration measurements
+}
\ No newline at end of file

diff -r 000000000000 -r 1c15748ff0e1 Sensors/Gyro/L3G4200D.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Gyro/L3G4200D.h	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,56 @@
+// based on http://mbed.org/users/shimniok/code/L3G4200D/
+
+#ifndef L3G4200D_H
+#define L3G4200D_H
+
+#include "mbed.h"
+#include "I2C_Sensor.h"
+
+#define L3G4200D_I2C_ADDRESS    0xD0
+
+// register addresses
+#define L3G4200D_WHO_AM_I       0x0F
+
+#define L3G4200D_CTRL_REG1      0x20
+#define L3G4200D_CTRL_REG2      0x21
+#define L3G4200D_CTRL_REG3      0x22
+#define L3G4200D_CTRL_REG4      0x23
+#define L3G4200D_CTRL_REG5      0x24
+#define L3G4200D_REFERENCE      0x25
+#define L3G4200D_OUT_TEMP       0x26
+#define L3G4200D_STATUS_REG     0x27
+
+#define L3G4200D_OUT_X_L        0x28
+#define L3G4200D_OUT_X_H        0x29
+#define L3G4200D_OUT_Y_L        0x2A
+#define L3G4200D_OUT_Y_H        0x2B
+#define L3G4200D_OUT_Z_L        0x2C
+#define L3G4200D_OUT_Z_H        0x2D
+
+#define L3G4200D_FIFO_CTRL_REG  0x2E
+#define L3G4200D_FIFO_SRC_REG   0x2F
+
+#define L3G4200D_INT1_CFG       0x30
+#define L3G4200D_INT1_SRC       0x31
+#define L3G4200D_INT1_THS_XH    0x32
+#define L3G4200D_INT1_THS_XL    0x33
+#define L3G4200D_INT1_THS_YH    0x34
+#define L3G4200D_INT1_THS_YL    0x35
+#define L3G4200D_INT1_THS_ZH    0x36
+#define L3G4200D_INT1_THS_ZL    0x37
+#define L3G4200D_INT1_DURATION  0x38
+
+class L3G4200D : public I2C_Sensor
+{
+    public:            
+        L3G4200D(PinName sda, PinName scl);                 // constructor, uses I2C_Sensor class
+        virtual void read();                                // read all axis from register to array data
+        float offset[3];                                    // offset that's subtracted from every measurement
+        void calibrate(int times, float separation_time);   // calibration from 'times' measurements with 'separation_time' time between (get an offset while not moving)
+        int readTemp();                                     // read temperature from sensor
+        
+    private:
+        virtual void readraw();
+};
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 1c15748ff0e1 Sensors/HMC5883.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/HMC5883.lib	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/caroe/code/HMC5883/#a5e06bb74915

diff -r 000000000000 -r 1c15748ff0e1 Sensors/I2C_Sensor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/I2C_Sensor.cpp	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,56 @@
+#include "I2C_Sensor.h"
+
+// calculate the 8-Bit write/read I2C-Address from the 7-Bit adress of the device
+#define GET_I2C_WRITE_ADDRESS(ADR)  (ADR << 1&0xFE) // ADR & 1111 1110
+#define GET_I2C_READ_ADDRESS(ADR)   (ADR << 1|0x01) // ADR | 0000 0001
+
+I2C_Sensor::I2C_Sensor(PinName sda, PinName scl, char i2c_address) : i2c_init(sda, scl), i2c(sda, scl), local("local")
+{
+    I2C_Sensor::i2c_address = i2c_address;
+    i2c_init.frequency(400000); // standard speed
+    i2c.frequency(400000); // standard speed
+    //i2c.frequency(1500000); // ultrafast!
+}
+
+void I2C_Sensor::saveCalibrationValues(float values[], int size, char * filename)
+{
+    FILE *fp = fopen(strcat("/local/", filename), "w");
+    for(int i = 0; i < size; i++)
+        fprintf(fp, "%f\r\n", values[i]);
+    fclose(fp);
+}
+
+void I2C_Sensor::loadCalibrationValues(float values[], int size, char * filename)
+{
+    FILE *fp = fopen(strcat("/local/", filename), "r");
+    for(int i = 0; i < size; i++)
+        fscanf(fp, "%f", &values[i]);
+    fclose(fp);
+}
+
+//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+// ATTENTION!!! there was a problem with other interrupts disturbing the i2c communication of the chip... that's why I use I2C to initialise the sonsors and MODI2C to get the data (only made with readMultiRegister)
+// IT DIDN'T WORK STABLE IN OTHER COMBINATIONS (if someone has an idea why please let me know)
+//--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
+
+char I2C_Sensor::readRegister(char reg)
+{
+    char value = 0;
+    
+    i2c_init.write(i2c_address, &reg, 1);
+    i2c_init.read(i2c_address, &value, 1);
+
+    return value;
+}
+
+void I2C_Sensor::writeRegister(char reg, char data)
+{
+    char buffer[2] = {reg, data};
+    i2c_init.write(i2c_address, buffer, 2);
+}
+
+void I2C_Sensor::readMultiRegister(char reg, char* output, int size)
+{
+    i2c.write (i2c_address, &reg, 1); // tell register address of the MSB get the sensor to do slave-transmit subaddress updating.
+    i2c.read  (i2c_address, output, size); // tell it where to store the data read
+}
\ No newline at end of file

diff -r 000000000000 -r 1c15748ff0e1 Sensors/I2C_Sensor.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/I2C_Sensor.h	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,40 @@
+// by MaEtUgR
+
+#ifndef I2C_Sensor_H
+#define I2C_Sensor_H
+
+#include "mbed.h"
+#include "MODI2C.h"
+
+class I2C_Sensor
+{           
+    public:
+        I2C_Sensor(PinName sda, PinName scl, char address);
+        
+        float data[3];                  // where the measured data is saved
+        virtual void read() = 0;        // read all axis from register to array data
+        //TODO: virtual void calibrate() = 0;   // calibrate the sensor and if desired write calibration values to a file
+        
+    protected:
+        // Calibration
+        void saveCalibrationValues(float values[], int size, char * filename);
+        void loadCalibrationValues(float values[], int size, char * filename);
+    
+        // I2C functions
+        char readRegister(char reg);
+        void writeRegister(char reg, char data);
+        void readMultiRegister(char reg, char* output, int size);
+        
+        // raw data and function to measure it
+        int raw[3];
+        virtual void readraw() = 0;
+        
+    private:
+        I2C i2c_init;       // original mbed I2C-library just to initialise the control registers
+        MODI2C i2c;         // I2C-Bus
+        char i2c_address;   // address
+        
+        LocalFileSystem local; // file access to save calibration values
+};
+
+#endif

diff -r 000000000000 -r 1c15748ff0e1 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,364 @@
+#include "mbed.h"       // Standard Library
+#include "HMC5883.h"    // Comp (Compass)
+#include "GPS.h"
+#include "SRF05.h"
+
+#define PI             3.1415926535897932384626433832795
+DigitalOut myled(LED1);
+I2C I2CBus(p9, p10);
+Timer GlobalTime;
+
+SRF05 Ult1(p5, p6);
+SRF05 Ult2(p7, p8);
+SRF05 Ult3(p11, p12);
+SRF05 Ult4(p16, p15);
+
+Serial pc(USBTX, USBRX);
+HMC5883     Mag(I2CBus, GlobalTime);
+GPS gps(p13, p14);
+
+float longitud,latitud,ul1,ul2,ul3,ul4;
+int orientacion;
+
+
+void GPfuzzy(float P[],int n,float v,float GP[],int mf[]);         
+float min(float a,float b);
+float Sugeno(int v1,int v2,float P[],int n);
+float Centroide(int v1,int v2,float P[],int n);
+void GPIfuzzy(float P[],int n,int gx,float GP,float x[]);
+void EntradaU(float F[],float O[],int ultrasonico,float tetha,float entrada[],float gp[],int mf[]);
+int  reglas(int mf[],int mfrueda[],float gpin[],float gprueda[]);
+void desfuzzy(float Pm[],int n,int mfrueda[],float gprueda[],int size,float dutty[]);
+
+
+int HMC5883_getAngle(float x, float y)
+{
+float heading = atan2((float)y,(float)x); 
+// Your mrad result / 1000.00 (to turn it into radians).
+  float declinationAngle = 21.18  / 1000.0;
+  // If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle
+  heading += declinationAngle;  
+if(heading < 0)  heading += 2*PI;// Correct for when signs are reversed.
+if(heading > 2*PI)   heading -= 2*PI; // Check for wrap due to addition of declination.
+return(heading * 180/PI); // Convert radians to degrees for readability.
+}
+
+int main() { // main programm for initialisation and debug output
+      // pc.baud(9600);
+  
+    I2CBus.frequency(400000);
+    GlobalTime.start();
+
+    Mag.Init();
+    #if 1
+        Mag.AutoCalibration= 1;                     //In Echtzeit kalibrieren
+    #else
+        short MagRawMin[3]= {-400, -400, -400};     //Gespeicherte Werte
+        short MagRawMax[3]= {400, 400, 400};
+        Mag.Calibrate(MagRawMin, MagRawMax);
+    #endif
+    
+   int size,n=5,ultrasonico=3,mfs[4],mfrueda[4];
+   float dutty[2],entrada[2],tetha,gradosp[4],gprueda[4];
+   float Pm[5]={0,25,50,75,100};//RANGO DE DUTTY PARA MOTORES (p.e. de 0 a 100)
+    
+    while(1) { 
+    
+    Mag.Update();
+       
+    printf("grader = %i \n",HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1])); 
+    wait_ms(20);
+    
+ 
+    if(gps.sample()) {
+    myled = 1;//indica si el GPS esta enviando valores buenos
+    latitud=gps.latitude;
+    longitud=gps.longitude;
+    } 
+    
+
+    orientacion=HMC5883_getAngle(Mag.Mag[0],Mag.Mag[1]);
+    
+  //  beta=atan((F[0]-O[0])/(F[1]-O[1]))*57.29582790879777;
+    
+
+   
+//CONFIGURACION DE ENTRADAS A LA LOGICA FUZZY********************************************
+   
+   float O[]={0,0};//POSICION INICIAL SEGUN GPS, EN EL PLANO
+   float F[]={3,4};//OBJETIVO O POSICION FINAL
+   tetha=orientacion;//en sexagesimales, ang de la brujula medido desde el eje NORTE sentido horario(positivo)
+   ultrasonico=1;
+//fin de CONFIG DE ENTRADAS******************************************************
+
+ 
+    EntradaU(F,O,ultrasonico,tetha,entrada,gradosp,mfs);//hace la fuzzyfication
+
+   size=reglas(mfs,mfrueda,gradosp,gprueda);//esta funcion bota los vectores mfrueda y sus gprueda
+
+    desfuzzy(Pm,n,mfrueda,gprueda,size,dutty);
+    printf("\fdutty1 %f \ndutty2 %f",dutty[0],dutty[1]);
+    //wait_ms(100);
+    
+    
+    }
+}
+//*****************DESARROLLO DE FUNCIONES**************************
+void GPfuzzy(float P[],int n,float v,float GP[],int mf[]){//v==> valor eje x
+    int i;
+    for( i=0;i<n;i++){//
+        if(P[i]<=v && v<=P[i+1]){
+            GP[0]=1.0-(v-P[i])*1.3333/(P[i+1]-P[i]);
+            GP[1]=1.0+1.33333*(v-P[i+1])/(P[i+1]-P[i]);
+            if(GP[1]<0){GP[1]=0;}
+            if(GP[0]<0){GP[0]=0;}
+            mf[0]=i+1;
+            mf[1]=i+2;
+            break;
+        }
+    }
+}
+float min(float a,float b){
+    if(a<=b)return a;
+    else return b;
+}
+float max(float a,float b){
+    if(a>=b)return a;
+    else return b;
+}
+float Sugeno(int v1,int v2,float P[],int n){//bota el centroide
+    int i;
+    float a,GP1[3],GP2[3],num=0,area=0,mf[3];
+    for(i=1;i<=(v2-v1);i++){
+      /*  GPfuzzy(P,n,v1+i-1,GP1,mf);
+        GPfuzzy(P,n,v1+i,GP2,mf);*/
+        a=(max(GP1[0],GP1[1])+max(GP2[0],GP2[1]))/2;
+        num=num+a*max(GP1[0],GP1[1])/2;
+        area=area+a;
+    }
+
+    return num/area;
+}
+
+float Centroide(int v1,int v2,float P[],int n){//bota el centroide
+    int i,L=50;
+    float a,GP1[3],GP2[3],num=0,area=0;
+    for(i=1;i<=L;i++){
+        //GPfuzzy(P,n,v1+i-1,GP1,mf);
+        //GPfuzzy(P,n,v1+i,GP2,mf);
+        a=(max(GP1[0],GP1[1])+max(GP2[0],GP2[1]))/2;
+        num=num+a*max(GP1[0],GP1[1])/2;
+        area=area+a;
+    }
+    return num/area;
+}
+
+
+void GPIfuzzy(float P[],int n,int gx,float GP,float x[]){//1<=gx<=n ,, 0<=GP<=1
+    
+    if(gx==1){
+        x[0]=P[0];
+        x[1]=(1-GP)*(P[1]-P[0])/1.33333+P[0];
+    }
+    else{
+        if(gx<n){
+        x[0]=(GP-1)*(P[gx-1]-P[gx-2])/1.333333+P[gx-1];
+        x[1]=(1-GP)*(P[gx]-P[gx-1])/1.333333+P[gx-1];
+        }
+        if(gx==n){
+        x[0]=(GP-1)*(P[gx-1]-P[gx-2])/1.333333+P[gx-1];
+        x[1]=P[n-1];
+        }
+
+    }
+}
+
+
+void EntradaU(float F[],float O[],int ultrasonico,float tetha,float entrada[],float gp[],int mf[]){
+    //        ex_izq  muy_izqu  izqui  frente  dere  muy_der  ex_dere
+    //salidau   1        2        3      4      5      6         7
+    // entrada[0]==> entrada  fuzzy phi brujula
+    // entrada[1] ==> entrada fuzzy ultrasonico
+    //para el phi brujula
+    float gradop[2] = {0.0,0.0};
+    int mfi[2]={0,0};
+    float P[5]={-180,-90,0,90,180};//es decir entre -180 y 180
+   float beta;
+   beta=atan((F[0]-O[0])/(F[1]-O[1]))*57.29582790879777;
+    entrada[0]=beta-tetha;//degrees  phi
+    switch(ultrasonico){
+        case 0:{
+        if(entrada[0]>0)entrada[1]=7;//entrada[1] de ultrasonico
+        if(entrada[0]<0)entrada[1]=1;
+        break;}
+        case 2:{
+        if(entrada[0]>0)entrada[1]=7;
+        if(entrada[0]<0)entrada[1]=1;
+        break;}
+        case 5:{
+        if(entrada[0]>0)entrada[1]=6;
+        if(entrada[0]<0)entrada[1]=2;
+        break;}
+        case 1:{entrada[1]=6;
+        break;}
+        case 3:{entrada[1]=5;
+        break;}
+        case 7:{if(entrada[0]>0)entrada[1]=6;//entrada[1] de ultrasonico
+            if(entrada[0]<0)entrada[1]=2;//*cambie orden 7 con  1
+            break;}
+        case 6:{entrada[1]=3;
+        break;}
+        case 4:{entrada[1]=2;
+        break;}
+    }
+    /*//para el phi brujula
+    float gradop[2] = {0.0,0.0};
+    int mfi[2]={0,0};
+    float P[5]={-144,-72,0,72,144};//es decir entre -180 y 180*/
+    //printf("\nentrada[0]%f\n",entrada[0]);
+    GPfuzzy(P,5,entrada[0],gradop,mfi);
+    gp[0]=gradop[0];  mf[0]=mfi[0];
+    gp[1]=gradop[1];  mf[1]=mfi[1];
+
+    //para el ultrasonico
+    //GPfuzzy(P,5,entrada[1],gradop,mf);
+    gp[2]=1;  mf[2]=entrada[1];
+    gp[3]=0.5;  mf[3]=entrada[1]+1;
+}
+
+
+//reglas fuzzy
+int  reglas(int mf[],int mfrueda[],float gpin[],float gprueda[]){
+    //mf[0]==>mf      phi brujula
+    //mf[1]==>mf sgte brujula
+    //mf[2] mf ultrasonico
+    //mf[3]  mf sgte ultra
+    //gpin[0]  brujula primer grado de pertenencia
+    //gpin[1]  brujula segundo grado de pertencia
+    //gpin[2]  ultrasonico pro=imer gp
+    //gpin[3]  ultrasonico segundo gp
+    //rueda[0]==>rueda1
+    //rueda[1]==>rueda2
+
+    //mfgps[0]==>latitud
+    //mfgps[1]==>longtud
+    //k==> ultimo elem: gprueda[k+1]
+    int i,j;
+    int k=0;
+    //printf("\ngpin[]phi %f %f\n",gpin[0],gpin[1]);
+    //printf("\ngpin[]ultrasonico %f %f\n",gpin[2],gpin[3]);
+    //printf("\nmf[] %d %d %d %d\n",mf[0],mf[1],mf[2],mf[3]);
+    //mfrueda[6]=7;
+    //mfrueda[5]=7;
+
+//falta ve lo de la interrupcion del gps XD
+    //if((mfgps[0]==1)&&(mfgps[1]==1)){gprueda[0]=gpgps(0);gprueda[1]=gpgps[1];}//a parte del phi y el ultrasonico
+    if(mf[0]==3||mf[1]==3){i=1;if(mf[0]==3)i=0;
+        if(mf[2]==1||mf[3]==1){j=2;if(mf[3]==1)j=3;mfrueda[k]=1;mfrueda[k+1]=4;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==2||mf[3]==2){j=2;if(mf[3]==2)j=3;mfrueda[k]=1;mfrueda[k+1]=4;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==3||mf[3]==3){j=2;if(mf[3]==3)j=3;mfrueda[k]=2;mfrueda[k+1]=3;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==7||mf[3]==7){j=2;if(mf[3]==7)j=3;mfrueda[k]=3;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==6||mf[3]==6){j=2;if(mf[3]==6)j=3;mfrueda[k]=2;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==5||mf[3]==5){j=2;if(mf[3]==5)j=3;mfrueda[k]=3;mfrueda[k+1]=2;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=3;mfrueda[k+1]=3;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+    }
+//printf("\n1gprueda[]phi %f %f\n",gpin[0],gpin[1]);
+
+    if(mf[0]==4||mf[1]==4){i=1;if(mf[0]==4)i=0;//si brujula es POS
+        if(mf[2]==1||mf[3]==1){j=2;if(mf[3]==1)j=3;mfrueda[k]=1;mfrueda[k+1]=4;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==3||mf[3]==3){j=2;if(mf[3]==3)j=3;mfrueda[k]=1;mfrueda[k+1]=3;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==3||mf[3]==3){j=2;if(mf[3]==3)j=3;mfrueda[k]=4;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==5||mf[3]==5){j=2;if(mf[3]==5)j=3;mfrueda[k]=3;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=1;mfrueda[k+1]=3;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+    }
+
+    if(mf[0]==5||mf[1]==5){i=1;if(mf[0]==5)i=0;//si brujula es APOS
+        if(mf[2]==3||mf[3]==3){j=2;if(mf[3]==3)j=3;mfrueda[k]=1;mfrueda[k+1]=5;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==5||mf[3]==5){j=2;if(mf[3]==5)j=3;mfrueda[k]=5;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+        if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=1;mfrueda[k+1]=5;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+    }
+    if(mf[0]==2||mf[1]==2){i=1;if(mf[0]==2)i=0;//si brujula es NEG
+        if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=1;mfrueda[k+1]=5;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+    }
+    if(mf[0]==1||mf[1]==1){i=1;if(mf[0]==1)i=0;//si brujula es ANEG
+        if(mf[2]==4||mf[3]==4){j=2;if(mf[3]==4)j=3;mfrueda[k]=5;mfrueda[k+1]=1;gprueda[k]=min(gpin[i],gpin[j]);gprueda[k+1]=min(gpin[i],gpin[j]);k=k+2;}
+    }//llega hasta gprueda[k+1]
+return k;//size de mf y gprueda[]
+}
+//desfuzzyfication
+void desfuzzy(float Pm[],int n,int mfrueda[],float gprueda[],int size,float dutty[]){
+      //rueda1*********************************
+    float x[3],ar=0,num=0,x1,area=0,area1,x2,area2;
+    int a,b,i,mfmin,mfmax=1;
+   mfmin=n;
+//el sgte for fue modificado para casos de mfmax y mfmin q se repiten despues de las reglas
+    for(i=0;i<size;i=i+2){//ojo desde i=0
+        if(mfrueda[i]<=mfmin){if(mfrueda[i]<mfmin){mfmin=mfrueda[i];a=i;}
+                                if(mfrueda[i]=mfmin)b=i;}
+        if(mfrueda[i]>mfmax){mfmax=mfrueda[i];b=i;}
+    }
+
+    
+    //hallar %dutty de rueda1
+        //centroide de area1
+    GPIfuzzy(Pm,n,mfmin,gprueda[a],x);
+
+    for(i=0;i<gprueda[a]*100;i++){//*ojo con gprueda[a]*100
+        ar=0.01*(Pm[mfmin-1]-(i*0.01-1)/1.3333*(Pm[mfmin]-Pm[mfmin-1])-x[0]);
+        area=area+ar;
+        num=num+(ar*100/2+x[0])*ar;//ahhhhhhhhhhhhhhhhhhh
+    }
+    if(area!=0)x1=num/area;
+   else x1=0;
+    area1=area;
+
+        //centroide de area2
+    ar=0,num=0,area=0;
+    GPIfuzzy(Pm,n,mfmax,gprueda[b],x);
+
+    for(i=0;i<gprueda[b]*100;i++){
+        ar=0.01*(x[1]-(Pm[mfmax-1]+(i*0.01-1)/1.333333*(Pm[mfmax-1]-Pm[mfmax-2])));
+        area=area+ar;
+       num=num+(100*ar/2+ (x[1]-ar*100)   )*ar;
+    }
+    if(area!=0)x2=num/area;else x2=0;
+    area2=area;
+
+   dutty[0]=(x1*area1+x2*area2)/(area1+area2);
+
+    //rueda2************************************************
+
+   
+    mfmin=n,mfmax=1;ar=0,area=0,num=0;
+//el sgte for fue modificado para casos de mfmax y mfmin q se repiten despues de las reglas
+     for(i=1;i<size;i=i+2){//ojo desde i=1
+        if(mfrueda[i]<=mfmin){if(mfrueda[i]<mfmin){mfmin=mfrueda[i];a=i;}
+                                if(mfrueda[i]=mfmin)b=i;}
+        if(mfrueda[i]>mfmax){mfmax=mfrueda[i];b=i;}
+    }
+  
+    //hallar %dutty de rueda2
+        //centroide de area1
+    GPIfuzzy(Pm,n,mfmin,gprueda[a],x);
+    for(i=0;i<gprueda[a]*100;i++){//*ojo con gprueda[a]*100
+        ar=0.01*(Pm[mfmin-1]-(i*0.01-1)/1.3333*(Pm[mfmin]-Pm[mfmin-1])-x[0]);
+        area=area+ar;
+        num=num+(ar*100/2+x[0])*ar;//ahhhhhhhhhhhhhhhhhhh
+    }
+    if(area!=0) x1=num/area;
+   else x1=0;
+    area1=area;
+        //centroide de area2
+    ar=0,num=0,area=0;
+    GPIfuzzy(Pm,n,mfmax,gprueda[b],x);
+    for(i=0;i<gprueda[b]*100;i++){
+       ar=0.01*(x[1]-(Pm[mfmax-1]+(i*0.01-1)/1.333333*(Pm[mfmax-1]-Pm[mfmax-2])));
+        area=area+ar;
+       num=num+(100*ar/2+ (x[1]-ar*100)   )*ar;//ahhhhhhhhhh
+    }
+    if(area!=0)x2=num/area;
+   else x2=0;
+    area2=area;
+
+    dutty[1]=(x1*area1+x2*area2)/(area1+area2);
+}
\ No newline at end of file

diff -r 000000000000 -r 1c15748ff0e1 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Jul 19 05:35:58 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/e3affc9e7238
\ No newline at end of file