File content as of revision 4:f412749d800d:

#include "mbed.h"
#include "Hexi_KW40Z.h"
#include "Hexi_OLED_SSD1351.h"
#include "OLED_types.h"
#include "OpenSans_Font.h"
#include "string.h"
#include "FXAS21002.h"
#include "FXOS8700.h"

#define LED_ON      0
#define LED_OFF     1
#define DECLINATION -4.94
#define PI 3.14159


void StartHaptic(void);
void StopHaptic(void const *n);
void txTask(void);

DigitalOut redLed(LED1,1);
DigitalOut greenLed(LED2,1);
DigitalOut blueLed(LED3,1);
DigitalOut haptic(PTB9);


Serial pc(USBTX, USBRX);
/* Define timer for haptic feedback */
RtosTimer hapticTimer(StopHaptic, osTimerOnce);

/* Instantiate the Hexi KW40Z Driver (UART TX, UART RX) */ 
KW40Z kw40z_device(PTE24, PTE25);

/* Instantiate the SSD1351 OLED Driver */ 
SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); /* (MOSI,SCLK,POWER,CS,RST,DC) */

/* Instantiate the gyro */
FXAS21002 gyro(PTC11,PTC10);
FXOS8700 accel(PTC11, PTC10);
FXOS8700 mag(PTC11, PTC10);
// Storage for the data from the sensor
float gyro_data[3];  float gyro_rms=0.0;
float accel_data[3]; float accel_rms=0.0;
float mag_data[3];   float mag_rms=0.0;
float roll,pitch,heading;
float new_roll,new_pitch,new_heading;
int del_roll,del_pitch; 
volatile bool flag=false; 
Timeout timeout;
/*Create a Thread to handle sending BLE Sensor Data */ 
Thread txThread;

 /* Text Buffer */ 
char text[20]; 

uint8_t battery = 100;
uint8_t light = 0;
uint16_t humidity = 4500;
uint16_t temperature = 2000;
uint16_t pressure = 9000;
uint16_t x = 0;
uint16_t y = 5000;
uint16_t z = 10000;

/*Get the new value of roll and pitch*/
void get_new_value(){
    gyro.acquire_gyro_data_dps(gyro_data);
    accel.acquire_accel_data_g(accel_data);
    new_roll = atan2(accel_data[2],accel_data[1]);
    new_pitch = atan2(-accel_data[0],sqrt(accel_data[1]*accel_data[1] + accel_data[2]*accel_data[2]));
    new_pitch *= 180.0 / PI;
    new_roll  *= 180.0 / PI;
    new_roll+=180;
}
/*Click the button to start it*/
void Start_Detect(){
    if(flag==true)
    return;
    flag=true;    
}
/****************************Call Back Functions*******************************/
void ButtonRight(void)
{
    StartHaptic();
    kw40z_device.ToggleAdvertisementMode();
}

void ButtonLeft(void)
{
    StartHaptic();
    kw40z_device.ToggleAdvertisementMode();
}

void PassKey(void)
{
    StartHaptic();
    strcpy((char *) text,"PAIR CODE");
    oled.TextBox((uint8_t *)text,0,25,95,18);
  
    /* Display Bond Pass Key in a 95px by 18px textbox at x=0,y=40 */
    sprintf(text,"%d", kw40z_device.GetPassKey());
    oled.TextBox((uint8_t *)text,0,40,95,18);
}

/***********************End of Call Back Functions*****************************/

/********************************Main******************************************/

int main()
{    
    /* Register callbacks to application functions */
    kw40z_device.attach_buttonLeft(&ButtonLeft);
    kw40z_device.attach_buttonRight(&ButtonRight);
    kw40z_device.attach_passkey(&PassKey);
    kw40z_device.attach_buttonUp(&Start_Detect);//Click the button to start it
    gyro.gyro_config();
    accel.accel_config();
    mag.mag_config();
    /* Turn on the backlight of the OLED Display */
    oled.DimScreenON();
    
    /* Fills the screen with solid black */         
    oled.FillScreen(COLOR_BLACK);

    /* Get OLED Class Default Text Properties */
    oled_text_properties_t textProperties = {0};
    oled.GetTextProperties(&textProperties);    
        
    /* Change font color to Blue */ 
    textProperties.fontColor   = COLOR_BLUE;
    oled.SetTextProperties(&textProperties);
    
    /* Display Bluetooth Label at x=17,y=65 */ 
    strcpy((char *) text,"BLUETOOTH");
    oled.Label((uint8_t *)text,17,65);
    
    /* Change font color to white */ 
    textProperties.fontColor   = COLOR_WHITE;
    textProperties.alignParam = OLED_TEXT_ALIGN_CENTER;
    oled.SetTextProperties(&textProperties);
    
    /* Display Label at x=22,y=80 */ 
    strcpy((char *) text,"Tap Below");
    oled.Label((uint8_t *)text,22,80);
         
    uint8_t prevLinkState = 0; 
    uint8_t currLinkState = 0;
     
    txThread.start(txTask); /*Start transmitting Sensor Tag Data */
    
    while (true) 
    {
        blueLed = !kw40z_device.GetAdvertisementMode(); /*Indicate BLE Advertisment Mode*/   
        Thread::wait(50);
    }
}

/******************************End of Main*************************************/


/* txTask() transmits the sensor data */
void txTask(void){
   
   while (true) 
   {
        
        if(flag){
            kw40z_device.SendSetApplicationMode(GUI_CURRENT_APP_SENSOR_TAG);
            gyro.acquire_gyro_data_dps(gyro_data);
            accel.acquire_accel_data_g(accel_data);
            roll = atan2(accel_data[2],accel_data[1]);
            pitch = atan2(-accel_data[0],sqrt(accel_data[1]*accel_data[1] + accel_data[2]*accel_data[2]));
            pitch *= 180.0 / PI;
            roll  *= 180.0 / PI;
            roll+=180;
            
            Thread::wait(1000);        
            get_new_value();
            float cmd; 
            
            if(abs(new_roll-roll)>abs(new_pitch-pitch)){
                if(new_roll-roll>30)
                    cmd=100;
                else if(new_roll-roll<-30)
                    cmd=200;
                else 
                    cmd=0;   
            }else if(abs(new_roll-roll)<abs(new_pitch-pitch)){
                if(new_pitch-pitch>10)
                    cmd=300;
                else if(new_pitch-pitch<-10)
                    cmd=400;
                else 
                    cmd=0;
            }else   
                    cmd=0;
            pc.printf("cmd=%f",cmd);
            kw40z_device.SendTemperature(cmd);
            Thread::wait(6000);//Make some delay to wait the data upload to the server
            kw40z_device.SendTemperature(0);//send 0 to the server
            flag=false;
        }
        
        Thread::wait(500);                 
    }
}

void StartHaptic(void)  {
    hapticTimer.start(50);
    haptic = 1;
}

void StopHaptic(void const *n) {
    haptic = 0;
    hapticTimer.stop();
}