Pitch code

Dependencies:   MMA8451Q mbed

main.cpp

Committer:
maruchik
Date:
2014-09-22
Revision:
0:2418caa8b2a4

File content as of revision 0:2418caa8b2a4:

#include "mbed.h"
#include "MMA8451Q.h"

Serial pc(USBTX, USBRX);
float pitch, roll;
int enter, back;
#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 MMA8451_I2C_ADDRESS (0x1d<<1)

DigitalIn RIGHT(D8);
DigitalIn LEFT(D9);

void pc_listen() {
    if (pc.readable()) {
        char input = pc.getc();
        
        switch(input) {
            case '1': pc.printf("%1.2f,%1.2f, %d, %d\r\n", roll, pitch, enter, back);
            break;
        }    
    }
}

int main(void)
{   
    float x=0, y=0, z=0;
    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) {        
        x = acc.getAccX();
        y = acc.getAccY();
        z = acc.getAccZ();
        
        roll = 180*atan(x / sqrt(z*z + y*y))/3.1415;
        pitch = 180*atan(y / sqrt(x*x + z*z))/3.1415;
        
        rled = 1.0f - x;
        gled = 1.0f - y;
        bled = 1.0f - z;
        enter = !RIGHT.read();
        back = !LEFT.read();
            
//        wait(0.1f);
        pc_listen();

    }
}