HexiBubble - HexiWear Bubble Level

#include "mbed.h"
#include <SSD1351_SPI.h>
#include <HexiBubble.h>
#include "FXOS8700.h"

//SSD1351_SPI ( mosi, miso, sclk, cs, dc ) ;  
SSD1351_SPI OLED96x96(PTB22,PTB23,PTB21,PTB20,PTD15); 
// Pin connections for Hexiwear
FXOS8700 accel(PTC11, PTC10);
// Storage for the data from the sensor
float accel_data[3]; float accel_rms=0.0;
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut BOOSTEN(PTC13);  //oled power enable
int count = 0;
extern const uint8_t  Main_screen_bmp[ 36864 ];
int argb = 0xff000000; 
int rgbd = 0x00000000;
int xposg = 0;
int yposg = 0;
int main() {
    led1 = 1;
    led2 = 1;
    led3 = 1;
    // Configure Accelerometer FXOS8700
    accel.accel_config();
    BOOSTEN = 1;
    OLED96x96.open();
    OLED96x96.state(Display::DISPLAY_ON);
    OLED96x96.fillRect(16,0,111,96,0xff000000);//alpha, BGR
    for(int xpos=0;xpos<97;xpos++)
    {
        for(int ypos=0;ypos<96;ypos++)
        {
            rgbd = ((Main_screen_bmp[count+3]<<24)|(Main_screen_bmp[count]<<16)|(Main_screen_bmp[count+1]<<8)|(Main_screen_bmp[count+2]));
            OLED96x96.drawPixel(ypos+16,xpos,rgbd);
            count=count+4;
        }
    } 
    OLED96x96.drawCircle(61,47,4,0xff00ff00);
    OLED96x96.drawCircle(61,47,3,0xff00ff00);
    OLED96x96.drawCircle(61,47,2,0xff00ff00); 
    while(1)
    {    
        accel.acquire_accel_data_g(accel_data);
        xposg=(accel_data[1]*(-40.0));
        if (xposg > 27)
            xposg = 27;
        if (xposg < -27)
            xposg = -27;
        yposg=accel_data[0]*40.0;
        if (yposg > 27)
            yposg = 27;
        if (yposg < -27)
            yposg = -27;
        if((xposg == 0) and (yposg == 0))
        {
            led1 = 1;
            led2 = 0;
        }
        OLED96x96.drawCircle(61,47,8,0xff00ff00);
        OLED96x96.drawLine(61,7,61,87,0xff00ff00);
        OLED96x96.drawLine(21,47,101,47,0xff00ff00);
        OLED96x96.drawCircle(61-xposg,47+yposg,4,0xff00ff00);
        OLED96x96.drawCircle(61-xposg,47+yposg,3,0xff00ff00);
        OLED96x96.drawCircle(61-xposg,47+yposg,2,0xff00ff00); 
        Thread::wait(20);
        OLED96x96.drawCircle(61-xposg,47+yposg,4,0xff000000);
        OLED96x96.drawCircle(61-xposg,47+yposg,3,0xff000000);
        OLED96x96.drawCircle(61-xposg,47+yposg,2,0xff000000); 
        led1 = 0;
        led2 = 1;
    }

}