File content as of revision 2:342aa0ce937e:

#include "mbed.h"
#include "MMA8451Q.h"
//#include "MMA845x.h"

#if defined (TARGET_KL05Z)
PinName const SDA = PTB4;
PinName const SCL = PTB3;
#else
#error TARGET NOT DEFINED
#endif

#define MMA8451_I2C_ADDRESS (0x1d<<1)

#define LOW_POWER
//#define DEBUG

int main(void)
{
    float x, y, z = 0.00;        /* Lastest X, Y, Z vals */
    float px, py, pz = 0.00;     /* Last cycles X, Y, Z vals */
    float dx, dy, dz = 0.00;     /* Difference */
    float G = 0.0;
    float extra = 0.0;
    
    float ctrl_val = 0.00;          /* Basically controller output U, includes sign */
    float ctrl_mag = 0.00;          /* Magnitude of controller output U */
        
    float Kp = 2.5;
    float Kd = 0.2;
    
    char c;    
    uint8_t data = 0;

    PwmOut en(PTB7); // PWM for enable signal
    DigitalOut m1(PTB6);
    DigitalOut m2(PTA12);

    MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);
    
#ifndef LOW_POWER
    PwmOut rled(LED1);
    rled = 1.0f;
    printf("MMA8451 ID: 0x%02X\n", acc.getWhoAmI());
#endif    
    
#ifdef DEBUG
    for(int i=0; i<0x50; i++)
    {                                //int addr, uint8_t * data, int len)
        acc.readRegs(i, &data, 1);
        printf("Reg 0x%02x: 0x%02x \r\n", i, data);
    }
    c = getchar();
#endif

    

    while (true) {
        
        /* Read accel value */        
        x = acc.getAccX();  
        y = acc.getAccY();
        z = acc.getAccZ();    
        
        G = sqrt( (x*x) + (y*y) + (z*z) );
        extra = 3 * abs(G - 1.0f);
        
        z = z / ( 1 +  extra); // z = z divided by 1 + twice the excess G
        //rled = 1.0f - extra/2;             
        
        /*     
        if(z > 0.3) {
            z = dz + pz;       // Get it from the slope formula d' = ( e(n) - e(n-1) )/ Ts, but Ts = 1 cycle
        }
        */
        
        dx = x - px;    /* Get delta-x value */
        dy = y - py;    /* Get delta-y value */
        dz = z - pz;    /* Get delta-z value */               
        
        /* Y should not respond to vibrations in the Z direction */
        /* Consider 0.25 more than the maximum rate of falling, it must be a vibration */
        /*
        if( (abs(dy) < 0.05) && (abs(dz) > 0.25) ){
            z = dz = 0; // Just do nothing, stay put
        }
        */
                  
        px = x;                /* Update prv x */ 
        py = y;                /* Update prv y */ 
        pz = z;                /* Update prv z */  
          
              
        ctrl_val = ( Kp * z ) + ( Kd * dz );
        
        ctrl_mag = abs(ctrl_val);

        if(ctrl_val > 0) {
            m1 = 0;
            m2 = 1;
#ifndef LOW_POWER
            rled = 1.0f - ctrl_mag; // make the red led proportional to the positive error signal
#endif                 
        } 
        
        /*
        else if (0 == ctrl_val) {
            m1 = 1;
            m2 = 1;             // lock the motors
#ifndef LOW_POWER
            rled = 1.0f; // turn off both leds
#endif                 
        } 
        */
        else {
            m1 = 1;
            m2 = 0;
            //rled = 1.0f - ctrl_mag; // make the blue led proportional to the negative error signal
        }             
        //      x     y     z     dZ    PWM
        printf("%1.2f,%1.2f,%1.2f,%1.2f,%1.2f\r\n", x, y, z, dz, ctrl_val);      

        // Write control value (as percentage)
        en.write(ctrl_mag);

        // Wait Ts
        wait_ms(75);
    }
}