dimel contreras
algoritmo logica difusa sensores navegacion
Dependencies: GPS MODI2C SRF05 mbed HMC5883
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
--- /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();
+}
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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
--- /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, ®, 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, ®, 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
--- /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
--- /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
--- /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