Revision 0:49465eeab179, committed 2017-11-06

Comitter:

RAMIRORUBIO

Date:

Mon Nov 06 22:37:56 2017 +0000

Commit message:

rami;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMA8451Q.cpp	Mon Nov 06 22:37:56 2017 +0000
@@ -0,0 +1,81 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "MMA8451Q.h"
+
+#define REG_WHO_AM_I      0x0D
+#define REG_CTRL_REG_1    0x2A
+#define REG_OUT_X_MSB     0x01
+#define REG_OUT_Y_MSB     0x03
+#define REG_OUT_Z_MSB     0x05
+
+#define UINT14_MAX        16383
+
+MMA8451Q::MMA8451Q(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
+    // activate the peripheral
+    uint8_t data[2] = {REG_CTRL_REG_1, 0x01};
+    writeRegs(data, 2);
+}
+
+MMA8451Q::~MMA8451Q() { }
+
+uint8_t MMA8451Q::getWhoAmI() {
+    uint8_t who_am_i = 0;
+    readRegs(REG_WHO_AM_I, &who_am_i, 1);
+    return who_am_i;
+}
+
+float MMA8451Q::getAccX() {
+    return (float(getAccAxis(REG_OUT_X_MSB))/4096.0);
+}
+
+float MMA8451Q::getAccY() {
+    return (float(getAccAxis(REG_OUT_Y_MSB))/4096.0);
+}
+
+float MMA8451Q::getAccZ() {
+    return (float(getAccAxis(REG_OUT_Z_MSB))/4096.0);
+}
+
+void MMA8451Q::getAccAllAxis(float * res) {
+    res[0] = getAccX();
+    res[1] = getAccY();
+    res[2] = getAccZ();
+}
+
+int16_t MMA8451Q::getAccAxis(uint8_t addr) {
+    int16_t acc;
+    uint8_t res[2];
+    readRegs(addr, res, 2);
+
+    acc = (res[0] << 6) | (res[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+
+    return acc;
+}
+
+void MMA8451Q::readRegs(int addr, uint8_t * data, int len) {
+    char t[1] = {addr};
+    m_i2c.write(m_addr, t, 1, true);
+    m_i2c.read(m_addr, (char *)data, len);
+}
+
+void MMA8451Q::writeRegs(uint8_t * data, int len) {
+    m_i2c.write(m_addr, (char *)data, len);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMA8451Q.h	Mon Nov 06 22:37:56 2017 +0000
@@ -0,0 +1,110 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef MMA8451Q_H
+#define MMA8451Q_H
+
+#include "mbed.h"
+
+/**
+* MMA8451Q accelerometer example
+*
+* @code
+* #include "mbed.h"
+* #include "MMA8451Q.h"
+* 
+* #define MMA8451_I2C_ADDRESS (0x1d<<1)
+* 
+* int main(void) {
+* 
+* MMA8451Q acc(P_E25, P_E24, MMA8451_I2C_ADDRESS);
+* PwmOut rled(LED_RED);
+* PwmOut gled(LED_GREEN);
+* PwmOut bled(LED_BLUE);
+* 
+*     while (true) {       
+*         rled = 1.0 - abs(acc.getAccX());
+*         gled = 1.0 - abs(acc.getAccY());
+*         bled = 1.0 - abs(acc.getAccZ());
+*         wait(0.1);
+*     }
+* }
+* @endcode
+*/
+class MMA8451Q
+{
+public:
+  /**
+  * MMA8451Q constructor
+  *
+  * @param sda SDA pin
+  * @param sdl SCL pin
+  * @param addr addr of the I2C peripheral
+  */
+  MMA8451Q(PinName sda, PinName scl, int addr);
+
+  /**
+  * MMA8451Q destructor
+  */
+  ~MMA8451Q();
+
+  /**
+   * Get the value of the WHO_AM_I register
+   *
+   * @returns WHO_AM_I value
+   */
+  uint8_t getWhoAmI();
+
+  /**
+   * Get X axis acceleration
+   *
+   * @returns X axis acceleration
+   */
+  float getAccX();
+
+  /**
+   * Get Y axis acceleration
+   *
+   * @returns Y axis acceleration
+   */
+  float getAccY();
+
+  /**
+   * Get Z axis acceleration
+   *
+   * @returns Z axis acceleration
+   */
+  float getAccZ();
+
+  /**
+   * Get XYZ axis acceleration
+   *
+   * @param res array where acceleration data will be stored
+   */
+  void getAccAllAxis(float * res);
+
+private:
+  I2C m_i2c;
+  int m_addr;
+  void readRegs(int addr, uint8_t * data, int len);
+  void writeRegs(uint8_t * data, int len);
+  int16_t getAccAxis(uint8_t addr);
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/fis_header.h	Mon Nov 06 22:37:56 2017 +0000
@@ -0,0 +1,12 @@
+//***********************************************************************
+// Matlab .fis to arduino C converter v2.0.0.29032014                   
+// - Karthik Nadig, USA                                                  
+// Please report bugs to: karthiknadig@gmail.com                         
+//***********************************************************************
+#define FIS_TYPE float
+#define FIS_RESOLUSION 101
+#define FIS_MIN -3.4028235E+38
+#define FIS_MAX 3.4028235E+38
+typedef FIS_TYPE(*_FIS_MF)(FIS_TYPE, FIS_TYPE*);
+typedef FIS_TYPE(*_FIS_ARR_OP)(FIS_TYPE, FIS_TYPE);
+typedef FIS_TYPE(*_FIS_ARR)(FIS_TYPE*, int, _FIS_ARR_OP);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Nov 06 22:37:56 2017 +0000
@@ -0,0 +1,307 @@
+#include "mbed.h"
+#include "MMA8451Q.h"
+#include "fis_header.h"
+#if   defined (TARGET_KL25Z) || defined (TARGET_KL46Z)
+  PinName const SDA = PTE25;
+  PinName const SCL = PTE24;
+#elif defined (TARGET_KL05Z)
+  PinName const SDA = PTB4;
+  PinName const SCL = PTB3;
+#elif defined (TARGET_K20D50M)
+  PinName const SDA = PTB1;
+  PinName const SCL = PTB0;
+#else
+  #error TARGET NOT DEFINED
+#endif
+#define max(a,b)            (((a) > (b)) ? (a) : (b))
+#define min(a,b)            (((a) < (b)) ? (a) : (b))
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+// Number of inputs to the fuzzy inference system
+const int fis_gcI = 1;
+// Number of outputs to the fuzzy inference system
+const int fis_gcO = 1;
+// Number of rules to the fuzzy inference system
+const int fis_gcR = 5;
+
+FIS_TYPE g_fisInput[fis_gcI];
+FIS_TYPE g_fisOutput[fis_gcO];
+
+//***********************************************************************
+// Support functions for Fuzzy Inference System                          
+//***********************************************************************
+// Triangular Member Function
+FIS_TYPE fis_trimf(FIS_TYPE x, FIS_TYPE* p)
+{
+    FIS_TYPE a = p[0], b = p[1], c = p[2];
+    FIS_TYPE t1 = (x - a) / (b - a);
+    FIS_TYPE t2 = (c - x) / (c - b);
+    if ((a == b) && (b == c)) return (FIS_TYPE) (x == a);
+    if (a == b) return (FIS_TYPE) (t2*(b <= x)*(x <= c));
+    if (b == c) return (FIS_TYPE) (t1*(a <= x)*(x <= b));
+    t1 = min(t1, t2);
+    return (FIS_TYPE) max(t1, 0);
+}
+
+FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
+{
+    return min(a, b);
+}
+
+FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)
+{
+    return max(a, b);
+}
+
+FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
+{
+    int i;
+    FIS_TYPE ret = 0;
+
+    if (size == 0) return ret;
+    if (size == 1) return array[0];
+
+    ret = array[0];
+    for (i = 1; i < size; i++)
+    {
+        ret = (*pfnOp)(ret, array[i]);
+    }
+
+    return ret;
+}
+
+
+//***********************************************************************
+// Data for Fuzzy Inference System                                       
+//***********************************************************************
+// Pointers to the implementations of member functions
+_FIS_MF fis_gMF[] =
+{
+    fis_trimf
+};
+
+// Count of member function for each Input
+int fis_gIMFCount[] = { 5 };
+
+// Count of member function for each Output 
+int fis_gOMFCount[] = { 5 };
+
+// Coefficients for the Input Member Functions
+FIS_TYPE fis_gMFI0Coeff1[] = { -60, -40, -20 };
+FIS_TYPE fis_gMFI0Coeff2[] = { -40, -20, 0 };
+FIS_TYPE fis_gMFI0Coeff3[] = { -20, 0, 20 };
+FIS_TYPE fis_gMFI0Coeff4[] = { 0, 20, 40 };
+FIS_TYPE fis_gMFI0Coeff5[] = { 20, 40, 60 };
+FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3, fis_gMFI0Coeff4, fis_gMFI0Coeff5 };
+FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff };
+
+// Coefficients for the Input Member Functions
+FIS_TYPE fis_gMFO0Coeff1[] = { -153, -102, -51 };
+FIS_TYPE fis_gMFO0Coeff2[] = { -102, -51, 0 };
+FIS_TYPE fis_gMFO0Coeff3[] = { -51, -1.332e-15, 51 };
+FIS_TYPE fis_gMFO0Coeff4[] = { 0, 51, 102 };
+FIS_TYPE fis_gMFO0Coeff5[] = { 51, 102, 153 };
+FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3, fis_gMFO0Coeff4, fis_gMFO0Coeff5 };
+FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff };
+
+// Input membership function set
+int fis_gMFI0[] = { 0, 0, 0, 0, 0 };
+int* fis_gMFI[] = { fis_gMFI0};
+
+// Output membership function set
+int fis_gMFO0[] = { 0, 0, 0, 0, 0 };
+int* fis_gMFO[] = { fis_gMFO0};
+
+// Rule Weights
+FIS_TYPE fis_gRWeight[] = { 1, 1, 1, 1, 1 };
+
+// Rule Type
+int fis_gRType[] = { 1, 1, 1, 1, 1 };
+
+// Rule Inputs
+int fis_gRI0[] = { 1 };
+int fis_gRI1[] = { 2 };
+int fis_gRI2[] = { 3 };
+int fis_gRI3[] = { 4 };
+int fis_gRI4[] = { 5 };
+int* fis_gRI[] = { fis_gRI0, fis_gRI1, fis_gRI2, fis_gRI3, fis_gRI4 };
+
+// Rule Outputs
+int fis_gRO0[] = { 1 };
+int fis_gRO1[] = { 2 };
+int fis_gRO2[] = { 3 };
+int fis_gRO3[] = { 4 };
+int fis_gRO4[] = { 5 };
+int* fis_gRO[] = { fis_gRO0, fis_gRO1, fis_gRO2, fis_gRO3, fis_gRO4 };
+
+// Input range Min
+FIS_TYPE fis_gIMin[] = { -60 };
+
+// Input range Max
+FIS_TYPE fis_gIMax[] = { 60 };
+
+// Output range Min
+FIS_TYPE fis_gOMin[] = { -102 };
+
+// Output range Max
+FIS_TYPE fis_gOMax[] = { 102 };
+
+//***********************************************************************
+// Data dependent support functions for Fuzzy Inference System                          
+//***********************************************************************
+FIS_TYPE fis_MF_out(FIS_TYPE** fuzzyRuleSet, FIS_TYPE x, int o)
+{
+    FIS_TYPE mfOut;
+    int r;
+
+    for (r = 0; r < fis_gcR; ++r)
+    {
+        int index = fis_gRO[r][o];
+        if (index > 0)
+        {
+            index = index - 1;
+            mfOut = (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
+        }
+        else if (index < 0)
+        {
+            index = -index - 1;
+            mfOut = 1 - (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
+        }
+        else
+        {
+            mfOut = 0;
+        }
+
+        fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);
+    }
+    return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);
+}
+
+FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)
+{
+    FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);
+    FIS_TYPE area = 0;
+    FIS_TYPE momentum = 0;
+    FIS_TYPE dist, slice;
+    int i;
+
+    // calculate the area under the curve formed by the MF outputs
+    for (i = 0; i < FIS_RESOLUSION; ++i){
+        dist = fis_gOMin[o] + (step * i);
+        slice = step * fis_MF_out(fuzzyRuleSet, dist, o);
+        area += slice;
+        momentum += slice*dist;
+    }
+
+    return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));
+}
+
+//***********************************************************************
+// Fuzzy Inference System                                                
+//***********************************************************************
+void fis_evaluate()
+{
+    FIS_TYPE fuzzyInput0[] = { 0, 0, 0, 0, 0 };
+    FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, };
+    FIS_TYPE fuzzyOutput0[] = { 0, 0, 0, 0, 0 };
+    FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, };
+    FIS_TYPE fuzzyRules[fis_gcR] = { 0 };
+    FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
+    FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
+    FIS_TYPE sW = 0;
+
+    // Transforming input to fuzzy Input
+    int i, j, r, o;
+    for (i = 0; i < fis_gcI; ++i)
+    {
+        for (j = 0; j < fis_gIMFCount[i]; ++j)
+        {
+            fuzzyInput[i][j] =
+                (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
+        }
+    }
+
+    int index = 0;
+    for (r = 0; r < fis_gcR; ++r)
+    {
+        if (fis_gRType[r] == 1)
+        {
+            fuzzyFires[r] = FIS_MAX;
+            for (i = 0; i < fis_gcI; ++i)
+            {
+                index = fis_gRI[r][i];
+                if (index > 0)
+                    fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);
+                else if (index < 0)
+                    fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
+                else
+                    fuzzyFires[r] = fis_min(fuzzyFires[r], 1);
+            }
+        }
+        else
+        {
+            fuzzyFires[r] = FIS_MIN;
+            for (i = 0; i < fis_gcI; ++i)
+            {
+                index = fis_gRI[r][i];
+                if (index > 0)
+                    fuzzyFires[r] = fis_max(fuzzyFires[r], fuzzyInput[i][index - 1]);
+                else if (index < 0)
+                    fuzzyFires[r] = fis_max(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
+                else
+                    fuzzyFires[r] = fis_max(fuzzyFires[r], 0);
+            }
+        }
+
+        fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
+        sW += fuzzyFires[r];
+    }
+
+    if (sW == 0)
+    {
+        for (o = 0; o < fis_gcO; ++o)
+        {
+            g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);
+        }
+    }
+    else
+    {
+        for (o = 0; o < fis_gcO; ++o)
+        {
+            g_fisOutput[o] = fis_defuzz_centroid(fuzzyRuleSet, o);
+        }
+    }
+}
+
+int main(void)
+{
+    MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
+    PwmOut rled(LED1);
+    PwmOut gled(LED2);
+    PwmOut bled(LED3);
+    // initialize the Analog pins for input.
+    // Pin mode for Input: IMU
+    //pinMode(0 , INPUT);
+
+
+    // initialize the Analog pins for output.
+    // Pin mode for Output: Velocidad
+    //pinMode(1 , OUTPUT);
+    while (true) {
+        float x, y, z;
+        int16_t ry;
+        ry = (int16_t)(acc.getAccY()*90);
+        x = abs(acc.getAccX());
+        y = abs(acc.getAccY());
+        z = abs(acc.getAccZ());
+        rled = 1.0f - x;
+        gled = 1.0f - y;
+        bled = 1.0f - z;
+        wait(0.1f);
+            // Read Input: IMU
+    g_fisInput[0] = ry;
+    g_fisOutput[0] = 0;
+    fis_evaluate();
+    // Set output vlaue: Velocidad
+    printf("%f\r\n",g_fisOutput[0]);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Nov 06 22:37:56 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/fb8e0ae1cceb
\ No newline at end of file