File content as of revision 0:409d28bad15e:

#include "mbed.h"
#include "MMA8451Q.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)

int main(void)
{
    float y, z;
    float val = 0.00;

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

    MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);

    PwmOut rled(LED1);
    //PwmOut gled(LED2);
    PwmOut bled(LED3);


    printf("MMA8451 ID: %d\n", acc.getWhoAmI());

    while (true) {

        /*
        rled = 1.0f - x;
        gled = 1.0f - y;
        */
        //bled = 1.0f - z;

        // Calculate next value (h computational delay)
        // y = acc.getAccY();
        z = acc.getAccZ();        

        val = sqrt( z * z );
        
        /*
        if(val < 0.02)
            val = 0;
         */   

        if(z > 0) {
            m1 = 0;
            m2 = 1;
            bled = 1.0f;       // turn off blue led
            rled = 1.0f - val; // make the red led proportional to the positive error signal
        } else if (0 == z) {
            m1 = 1;
            m2 = 1;             // lock the motors
            rled = bled = 1.0f; // turn off both leds
        } else {
            m1 = 1;
            m2 = 0;
            rled = 1.0f;       // turn off red led
            bled = 1.0f - val; // make the blue led proportional to the negative error signal
        }
        
        printf("Z: %1.2f, PWM: %1.2f\r\n", z, val);

        // Write control value
        en.write(val);

        // Wait Ts
        wait(0.1f);
    }
}