File content as of revision 0:6b088c9d1ac8:

#include "mbed.h"
#include "LPC17xx.h"

//Serial pc(USBTX,USBRX);
I2C accelero(p28, p27);
DigitalOut myled(LED1);
char *afficheur = (char *) 0x40010000;



const char reg[] = {0x0d};
const int device = 0x3A;
char data[1];
int radtodeg = 57;

int16_t normal[3] = {0,0,1};

int empty = 0x1 << 5;
void write(int c){
    while(!LPC_UART1->LSR & empty){}
    *afficheur = c;
}

void initUART1(uint32_t baud)
{
    uint32_t dll = (SystemCoreClock / (16 * baud)); // Baud rate formula

    LPC_SC->PCONP |= (1 << 4); //Power to UART1
    
    LPC_SC->PCLKSEL0 &= ~(3 << 8); 
    LPC_SC->PCLKSEL0 |=  (1 << 8); 

    LPC_UART1->LCR = 0x03; //8 bits length
    LPC_UART1->LCR |= (1 << 7); // DLAB = 1;

    LPC_UART1->DLM = (dll >> 8) & 0xFF;
    LPC_UART1->DLL = (dll) & 0xFF;
    
    LPC_UART1->LCR &= ~(1 << 7); // DLAB = 0 

    LPC_UART1->FCR |= 0x7;  //Set FIFO = 1 (Enable), RX and TX reset    

    LPC_PINCON->PINSEL0 &= ~0xC0000000; // Clear bit 31:30
    LPC_PINCON->PINSEL0 |=  0x40000000; // set  bit 31:30 to 01
    LPC_PINCON->PINSEL1 &= ~0x00000003; // Clear bit 1:0
    LPC_PINCON->PINSEL1 |=  0x00000001;  // set  bit 1:0 to 0   
}

int printDecimalNumber(int i){
    char chiffres[4];
    if (i > 9999) {
        write(0x76);
        write('e');
        write('e');
        write('e');
        write('e');
    }
    else {
        // Convert number to char list
        chiffres[0] = i / 1000;
        chiffres[1] = (i - int(chiffres[0]) * 1000) / 100;
        chiffres[2] = (i - int(chiffres[0]) * 1000 - int(chiffres[1]) * 100) / 10;
        chiffres[3] = i - int(chiffres[0]) * 1000 - int(chiffres[1]) * 100 - int(chiffres[2]) * 10;
        
        write(0x76);
        write(0x77);
        write(0b00000010);
        for (int x = 0; x < 4; x++) {
            write(chiffres[x]);
        }
    }
    return 0;
}

int main()
{
    myled = 0;
    //pc.printf("Begin");
    
    initUART1(9600);

    accelero.write(device,reg,1,true);
    accelero.read(device,data,1);

    // pc.printf("Result : %d \r\n", data[0]);
    printDecimalNumber((int) data[0]);

    // Set 2g max
    char XYZ_DATA_CFG[2] = {0x0e, 0};
    char value[1];
    accelero.read(device, XYZ_DATA_CFG, 1, true);
    accelero.write(device, value, 1);
    value[0] = value[0] & 0xFC;

    XYZ_DATA_CFG[1] = value[0];
    accelero.write(device, XYZ_DATA_CFG, 2);

    // Activate accelerometer
    char ctrlReg1[2] = {0x2a, 0};
    accelero.write(device, ctrlReg1, 1, true);
    accelero.read(device, value, 1);
    value[0] = value[0] | 0x1;

    ctrlReg1[1] = value[0];
    accelero.write(device, ctrlReg1, 2);

    char XYZ_Data = 0x01;
    char data[6];
    int16_t x, y, z;
    int16_t buffer;

    int buffersize = 128;
    int shiftsize = 7;
    int mean[buffersize];
    int angle;

    int iteration = 0;
    while(1) {
        accelero.write(device, &XYZ_Data, 1, true);
        accelero.read(device, data, 6);

        ((char*) &buffer)[0]= data[1];    //LSB
        ((char*) &buffer)[1]= data[0];    //MSB
        x = buffer >> 4;
        ((char*) &buffer)[0]= data[3];    //LSB
        ((char*) &buffer)[1]= data[2];    //MSB
        y = buffer >> 4;
        ((char*) &buffer)[0]= data[5];    //LSB
        ((char*) &buffer)[1]= data[4];    //MSB
        z = buffer >> 4;
        
        z = std::abs(z);
        if (z > 1024)
            z = 1024;

        int theta = std::acos((double) z / (double) 1024) * radtodeg * 100;
        mean[iteration % buffersize] = theta >> shiftsize;

        if (iteration % 64 == 0) {
            angle = 0;
            for (int i = 0; i < buffersize; i++) {
                angle += mean[i];
            }
            printDecimalNumber(angle);
        }
        wait(0.0006);
        iteration++;
    }
}