The HexiHeart is a demo project product that takes advantage of many of the onboard Hexiwear sensors and capabilities to create a multifunctional fitness and safety watch.

Dependencies:   FXAS21002 FXOS8700 Hexi_KW40Z Hexi_OLED_SSD1351 MAXIM W25Q64FVSSIG HTU21D MPL3115A2 TSL2561

Fork of HexiHeart_Alex by Hexiwear_zeta

main.cpp

Committer:
jmr274
Date:
2018-02-13
Revision:
6:84e3ba00b995
Parent:
5:e1431272be79
Child:
7:3d5a8aea0b63

File content as of revision 6:84e3ba00b995:

/**********************************************************************
Texas State University Senior Project - HexiHeart
Team Zeta: Alex Song, Jasmine Rounsaville, Issam Hichami, Neil Baker
Version: HexiHeart_1st 11/12/17
This version has basic menu layout and screen timeout feature.  The menu
are just placeholders (for the most part) and will be either adjusted or
replaced with graphic images.

***********************************************************************/

#include "mbed.h"
#include "Hexi_KW40Z.h"         // Button and BLE fuctions
#include "FXOS8700.h"           // 3D Accelorometer & Mag
#include "FXAS21002.h"          // 3-Axis Gyroscope
#include "Hexi_OLED_SSD1351.h"  // OLED fuctions
#include "HTU21D.h"             // Non-Freescale HTU21D - combo temperature and Humidity
#include "W25Q64FV.h"           // W25Q64FVSSIG Serial Flash 
                                // Non-Freescale MPL3115A2 - pressure sensor 
#include "Hexi_Battery/hexi_battery.h"  // Battery status
#include "OLED_types.h"         // Text attributs
#include "string.h"
#include "OpenSans_Font.h"
#include "MAX30101.h"           // Non-Freescale MAX30101 - Optical Heart rate sensor


/* We need to confirm whether it's better to include and
configure every module for lowest power, or whether it's
better to save memory by not doing that
*/

// Definitions
#define LED_ON          0
#define LED_OFF         1
#define SCRN_TIME       10.0
#define Debug           1 // If "Debug" is defined, our code will compile for debug.  Comment out for Production code.
#define HIGHEST_ZONE    4
#define LOWEST_ZONE     1
#define ENTER_BELOW     25
#define ENTER_ABOVE     50
#define EXIT_BELOW      75
#define EXIT_ABOVE      100
#define VIB_OPT_2       75
#define FXOS8700_I2C_ADDRESS_ (0x1E<<1) //pins SA0,SA1=0 


void StartHaptic(void);
void StartHaptic(int x);
void StopHaptic(void const *n);
void error_screen(void);
void update_display(void);// Screen lables refreshed
void update_data(void);   // Screen data(only)refreshed 
void Decrement_Age();
void Set_Max_Bpm();
void Set_Zone_Boundaries();
void Increment_Age();
void Increment_Target_Zone();
void Decrement_Target_Zone();
void Increment_HR_Vibr_Pref();
void Decrement_HR_Vibr_Pref();
void Determine_Current_Zone();
void Heart_Rate_Vibrations();
void Increment_Heart_Rate();
void Decrement_Heart_Rate();
void Enable_Heart_Rate();
void Disable_Heart_Rate();
void Led_Zone_Indicator();
void Heat_Index_Calculation();
void fall_config(uint8_t); 
void fall_detect(void);
void fall_det_end(void);
void fall_detect_off(void);
void impact_detect(void);

// *****************  Global variables  ***********************
char text_1[20];            // Text buffer - Do we need more?
char display_buff[30];      //Buffer for conversion to char to display
char text[20];              // Text Buffer for dynamic value displayed
bool Led_Zones = 1;
bool HR_Enable = 0;
bool OLED_ON = 1;           // Turn OLED power on/off
bool Fall_Alert = 1;        // Initialize as no active alert
bool Panic_Alert = 0;       // Initialize as no active alert
bool Fall_Alert_Mode = 1;   // Initialize with fall alert mode on
bool Heart_Rate_Mode = 0;   // Initialize with Heart rate off
float Accel_Mag=0.0;        // Vector magnitude calculated from sensor data
float Accel_Data[3];        // Accel Data from sensor
float Gyro_Mag=0.0;         // Vector magnitude calculated from sensor data
float Gyro_Data[3];         // Gyro data from sensor
float Fall_Thresh=0.5;      // Initialize Fall detect Threshold
float Impact_Thresh=3.0;    // Initialize Impact detect Threshold
float Movement_Thresh=50.0; // Initialize Movement detect Threshold
uint8_t Current_Zone = 1;
uint8_t Prev_Zone = 1;
uint8_t Heart_Rate = 100;
uint8_t HR_buff[250];
uint8_t *HR_return;
uint8_t Age = 50;            // Initialize age
uint8_t Max_Bpm = 220 - Age;      // Initialize Max BPM
uint8_t Screen_Num = 0;      // Initialize to main screen
uint8_t Error_Num = 0;         // Error num for debug
uint8_t HR_Vibration = 2;       //Choose Heart Rate Vibration Options
uint8_t Target_Zone = 2;        //Initialize Target Heart Rate Zone to Zone 3
uint8_t HR_Zone1[2] = {Max_Bpm * .50, Max_Bpm * .60};       //Heart Rate Zone 1
uint8_t HR_Zone2[2] = {HR_Zone1[1] + 1, Max_Bpm * .70};       //Heart Rate Zone 2
uint8_t HR_Zone3[2] = {HR_Zone2[1] + 1, Max_Bpm * .80};       //Heart Rate Zone 3
uint8_t HR_Zone4[2] = {HR_Zone3[1] + 1, Max_Bpm};       //Heart Rate Zone 4
int sample_ftemp;
int sample_humid;
int heat_index;

// ***************** Define pins *****************************
FXAS21002 gyro(PTC11,PTC10); // Gyroscope
SSD1351 oled(PTB22,PTB21,PTC13,PTB20,PTE6, PTD15); // SSD1351 OLED Driver (MOSI,SCLK,POWER,CS,RST,DC)
FXOS8700 accel(PTC11, PTC10); // Accelorometer
FXOS8700 mag(PTC11, PTC10);   // Mag (same chip as Accel)
//MAX30101 heart(PTB1, PTB0); //Heart Rate Chip
HTU21D temphumid(PTB1,PTB0); // HTU21D Sensor

// initialize I2C bus for FXOS8700, FXAS-Gyro, MPL-Pressure
    I2C i2c_bus1(PTC11, PTC10);  // (SDA, SCL)

DigitalOut RED_Led(LED1);
DigitalOut GRN_Led(LED2);
DigitalOut BLU_Led(LED3);
DigitalOut haptic(PTB9);
DigitalOut Led_clk1(PTA12);
DigitalOut Led_clk2(PTA13);
DigitalOut Led_clk3(PTA14);

DigitalOut OLED_PWR(PTC13);     // this pin turns on/off 15V to OLED display
DigitalOut Non_Free_PWR(PTB13); // this pin turns on/off non-freescale sensors (Pres/Temp/Hum/Light)
DigitalOut powerEN (PTB12);     // Power Enable HTU21D Sensor
DigitalOut HR_PWR(PTA29);       // this pin turns on/off Heart rate sensor
//DigitalIn   Sw1(PTA12);       //Switch 'T1' on docking station AND Led_clk1!!
//DigitalIn   Sw2(PTA13);       //Switch 'T2' on docking station AND Led_clk2!!
DigitalIn   Sw3(PTA15);         //Switch 'T3' on docking station 


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

/* Define timer for haptic feedback */
RtosTimer hapticTimer(StopHaptic, osTimerOnce);

//***************** Interrups *****************
InterruptIn Accel_INT1(PTC1);  // Accel sensor's interupt 1
InterruptIn Accel_INT2(PTD13);  // Accel sensor's interupt 2

//***************** Tickers and Timers *****************
Ticker Screen_Timer;// use ticker to turn off OLED

void timout_timer(){ // turn off display mode
//  oled.FillScreen(COLOR_BLACK); // Clear screen.. is there a better command for this?
     OLED_PWR = 0;  // Turn off OLED power supply
     OLED_ON = 0;  // set flag to off
     Screen_Timer.detach();  // detach Ticker
}//end routine

void ButtonUp(void)
{
    Screen_Timer.attach(&timout_timer,(SCRN_TIME));//Is this sufficient to reset/restart ticker timer for OLED?
    if (OLED_ON == 0) {
        OLED_ON = 1; // Scree was off, set to On
        update_display();
    } else {
        switch(Screen_Num) {
            case 0: {// We're in Main Screen
                // do nothing, wrong button
                break;
            }
            case 1: {// Panic Alert option
                StartHaptic();
                Screen_Num = 26;  //Change to screen 5
#ifdef Debug    // in debug show debug/diagnostic screens                         
                Screen_Num = 26;  //Change to screen 20
#endif

                update_display();
                break;
            }
            case 2: {// Fall Alert option
                StartHaptic();
                Screen_Num = 1;  //Change to screen 1
                update_display();
                break;
            }
            case 3: {// Heart Rate Monitoring option
                StartHaptic();
                Screen_Num = 2;  //Change to screen 2
                update_display();
                break;
            }
            case 4: {// Alert History option
                StartHaptic();
                Screen_Num = 3; //Change to screen 3
                update_display();
                break;
            }
            case 5: {// About HexiHeart
                StartHaptic();
                Screen_Num = 4; //Change to screen 4
                update_display();
                break;
            }
            case 6: {// Panic Alert
                StartHaptic();
                Panic_Alert = !Panic_Alert;
                update_display();
                break;
            }
            case 7: {// Heart Rate Zone
                StartHaptic(50);
                Enable_Heart_Rate();
                //heart.enable();
                //HR_Enable = 1;
                //while(HR_Enable == 1)
                //    heart.readRawData(HR_buff, HR_return);
                //update_display();
                break;
            }
            case 8: {// Alert History
                StartHaptic();
                //Increment  alert index
                break;
            }
            case 20: {// Diagnostic/Debug Screens
                StartHaptic();
                Screen_Num = 5;  //Change to screen 5
                update_display();
                break;
            }
            case 21: {// Fall Diagnostic
                StartHaptic();
                Screen_Num = 25;  //Change to screen 25
                update_display();
                break;
            }
            case 22: {// Fall Debug
                StartHaptic();
                Screen_Num = 21;  //Change to screen 21
                update_display();
                break;
            }
            case 23: {// Heart Rate Diagnostic
                StartHaptic();
                Screen_Num = 22;  //Change to screen 22
                update_display();
                break;
            }
            case 24: {// Heart Rate Debug
                StartHaptic();
                Screen_Num = 23;  //Change to screen 23
                update_display();
                break;
            }
            case 25: {// Heat Index Diagnostic
                StartHaptic();
                Screen_Num = 24;  //Change to screen 24
                update_display();
                break;
            }
            case 26: {//Heart Rate Config Option
                StartHaptic();
                Screen_Num = 20;
                update_display();
                break;
            }
            case 27: {//Incrementing Age
                Increment_Age();
                Set_Zone_Boundaries();
                update_display();
                break;
            }
            case 28: {//Changing Heart Rate Vibration Preferences
                Increment_HR_Vibr_Pref();
                update_display();
                break;
            }
            case 30: {//Change Target Heart Rate Zone Preference
                Increment_Target_Zone();
                update_display();
                break;
            }
            case 31: { //Manually Increment Heart Rate by 1
                Increment_Heart_Rate();
                Determine_Current_Zone();
                update_display();
                break;
            }
            default: {
                break;
            }
        }
    }

}

void ButtonDown(void)
{
    Screen_Timer.attach(&timout_timer,(SCRN_TIME));//Is this sufficient to reset/restart ticker timer for OLED?
    if (OLED_ON == 0) {
        OLED_ON = 1; // Screen was off, set to On
        update_display();
    } else {

        switch(Screen_Num) {
            case 0: {// We're in Main Screen
                // do nothing, wrong button
                break;
            }
            case 1: {// Panic Alert option
                StartHaptic();
                Screen_Num = 2;  //Change to screen 2
                update_display();
                break;
            }
            case 2: {// Fall Alert option
                StartHaptic();
                Screen_Num = 3;  //Change to screen 3
                update_display();
                break;
            }
            case 3: {// Heart Rate Monitoring option
                StartHaptic();
                Screen_Num = 4;  //Change to screen 4
                update_display();
                break;
            }
            case 4: {// Alert History option
                StartHaptic();
                Screen_Num = 5;  //Change to screen 5
                update_display();
                break;
            }
            case 5: {// About HexiHeart option
                StartHaptic();
                Screen_Num = 26;  //Change to screen 1
#ifdef Debug    // in debug show debug/diagnostic screens                         
                Screen_Num = 20;  //Change to screen 20
#endif
                update_display();
                break;
            }
            case 6: {// Panic Alert
                // do nothing, wrong button
                break;
            }
            case 7: {// Heart Rate Zone
                StartHaptic(100);
                Disable_Heart_Rate();
                break;
            }
            case 8: {// Alert History
                StartHaptic();
                //decriment alert index
                break;
            }
            case 20: {// Diagnostic/Debug Screens
                StartHaptic();
                Screen_Num = 26;  //Change to screen 1
                update_display();
                break;
            }
            case 21: {// Fall Diagnostic
                StartHaptic();
                Screen_Num = 22;  //Change to screen 22
                update_display();
                break;
            }
            case 22: {// Fall Debug
                StartHaptic();
                Screen_Num = 23;  //Change to screen 23
                update_display();
                break;
            }
            case 23: {// Heart Rate Diagnostic
                StartHaptic();
                Screen_Num = 24;  //Change to screen 24
                update_display();
                break;
            }
            case 24: {// Heart Rate Debug
                StartHaptic();
                Screen_Num = 25;  //Change to screen 25
                update_display();
                break;
            }
            case 25: {// Heat Index Diagnostic
                StartHaptic();
                Screen_Num = 21;  //Change to screen 21
                update_display();
                break;
            }
            case 26: {//Heart Rate Configs
                StartHaptic();
                Screen_Num = 1;
                update_display();
                break;
            }
            case 27: { //Decrement Age
                Decrement_Age();
                Set_Zone_Boundaries();
                update_display();
                break;

            }
            case 28: { //Changing Heart Rate Vibration Preference
                /*
                    StartHaptic();
                    if(HR_Vibration == 1) {
                        HR_Vibration = 3;
                    } else {
                        HR_Vibration -= 1;
                    }
                */
                Decrement_HR_Vibr_Pref();
                update_display();
                break;
            }
            case 30: {//Change Target Heart Rate Zone Preference
                Decrement_Target_Zone();
                update_display();
                break;
            }
            case 31: { //Manually decrement heart rate by 1
                Decrement_Heart_Rate();
                Determine_Current_Zone();
                update_display();
                break;
            }
            default: {
                break;
            }
        }
    }
}

void ButtonRight(void)
{
    Screen_Timer.attach(&timout_timer,(SCRN_TIME));//Is this sufficient to reset/restart ticker timer for OLED?
    if (OLED_ON == 0) {
        OLED_ON = 1; // Screen was off, set to On
        update_display();
    } else {
        switch(Screen_Num) {
            case 0: {// We're in Main Screen
                StartHaptic();
                Screen_Num = 1;  //Change to screen 1
                update_display();
                break;
            }
            case 1: {// Panic Alert option
                StartHaptic();
                Screen_Num = 6;  //Change to screen 6
                update_display();
                break;
            }
            case 2: {// Fall Alert option
                StartHaptic();
                if(Fall_Alert == 1){
                    Accel_INT1.fall(&fall_detect_off); // turn off Accel sensor's int#1 calls interupt routine
                    Fall_Alert = 0;
                    }
                else{     
                    Accel_INT1.fall(&fall_detect); // Accel sensor's int#1 calls interupt routine
                    Fall_Alert = 1;
                    }
                update_display();
                break;
            }
            case 3: {// Heart Rate Monitoring option
                StartHaptic();
                Screen_Num = 7;  //Change to screen 7
                update_display();
                break;
            }
            case 4: {// Alert History option
                StartHaptic();
                Screen_Num = 8;  //Change to screen 8
                update_display();
                break;
            }
            case 5: {// About HexiHeart option
                StartHaptic();
                Screen_Num = 9;  //Change to screen 9
                update_display();
                break;
            }

            case 6: {// Panic Alert
                // do nothing, wrong button
                break;
            }
            case 7: {// Heart Rate Zone
                StartHaptic();
                Screen_Num = 31;
                update_display();
                break;
            }
            case 20: {// Diagnostic/Debug Screens
                StartHaptic();
                Screen_Num = 21;  //Change to screen 21
                update_display();
                break;
            }
            case 26: {//Change to Heart Rate Config Screen
                StartHaptic();
                Screen_Num = 27;
                update_display();
                break;
            }
            case 27: {//Change to Heart Rate Vibration Preferences
                StartHaptic();
                Screen_Num = 28;
                update_display();
                break;
            }
            case 28: {//Change to Heart Rate Zone Boundary Info
                StartHaptic();
                Screen_Num = 29;
                update_display();
                break;
            }
            case 29: {//Change Target Heart Rate Zone Preference
                StartHaptic();
                Screen_Num = 30;
                update_display();
                break;
            }
            case 30: {//Change to Heart Rate Config Screen
                StartHaptic();
                Screen_Num = 27;
                update_display();
                break;
            }
            case 31: {
                StartHaptic();
                Screen_Num = 32;
                update_display();
                break;
            }
            case 32: {
                StartHaptic();
                Screen_Num = 7;
                update_display();
                break;
            }
            default: {
                break;
            }
        }
    }
}

void ButtonLeft(void)
{
    Screen_Timer.attach(&timout_timer,(SCRN_TIME));//Is this sufficient to reset/restart ticker timer for OLED?
    if (OLED_ON == 0) {
        OLED_ON = 1; // Screen was off, set to On
        update_display();
    } else {
        switch(Screen_Num) {
            case 0: {// We're in Main Screen
                // do nothing, wrong button
                break;
            }
            case 1: {// Panic Alert option
                StartHaptic();
                Screen_Num = 0;  //Change to screen 0
                update_display();
                break;
            }
            case 2: {// Fall Alert option
                StartHaptic();
                Screen_Num = 0;  //Change to screen 0
                update_display();
                break;
            }
            case 3: {// Heart Rate Monitoring option
                StartHaptic();
                Screen_Num = 0;  //Change to screen 0
                update_display();
                break;
            }
            case 4: {// Alert History option
                StartHaptic();
                Screen_Num = 0;  //Change to screen 0
                update_display();
                break;
            }
            case 5: {// About HexiHeart option
                StartHaptic();
                Screen_Num = 0;  //Change to screen 0
                update_display();
                break;
            }
            case 6: {// Panic Alert
                StartHaptic();
                Screen_Num = 1;  //Change to screen 1
                update_display();
                break;
            }
            case 7: {// Heart Rate Zone
                StartHaptic();
                Screen_Num = 3;  //Change to screen 3
                update_display();
                break;
            }
            case 8: {// Alert History
                StartHaptic();
                Screen_Num = 4;  //Change to screen 4
                update_display();
                break;
            }
            case 20: {// Diagnostic/Debug Screens
                StartHaptic();
                Screen_Num = 0;  //Change to screen 0
                update_display();
                break;
            }
            case 21: {// Fall Diagnostic
                StartHaptic();
                Screen_Num = 20;  //Change to screen 20
                update_display();
                break;
            }
            case 22: {// Fall Debug
                StartHaptic();
                Screen_Num = 20;  //Change to screen 20
                update_display();
                break;
            }
            case 23: {// Heart Rate Diagnostic
                StartHaptic();
                Screen_Num = 20;  //Change to screen 20
                update_display();
                break;
            }
            case 24: {// Heart Rate Debug
                StartHaptic();
                Screen_Num = 20;  //Change to screen 20
                update_display();
                break;
            }
            case 25: {// Heat Index Diagnostic
                StartHaptic();
                Screen_Num = 20;  //Change to screen 20
                update_display();
                break;
            }
            case 26: {//Heart Rate Config Option
                StartHaptic();
                Screen_Num = 0;
                update_display();
                break;
            }
            case 27: {//Enter Age
                StartHaptic();
                Screen_Num = 26;
                update_display();
                break;
            }
            case 28: {//Heart Rate Vibration Preference Screen
                StartHaptic();
                Screen_Num = 27;
                update_display();
                break;
            }
            case 29: {//Heart Rate Zone Boundary Info
                StartHaptic();
                Screen_Num = 28;
                update_display();
                break;
            }
            case 30: {//Change Target Heart Rate Zone Preference
                StartHaptic();
                Screen_Num = 29;
                update_display();
                break;
            }
            case 31: {
                StartHaptic();
                Screen_Num = 7;
                update_display();
                break;
            }
            case 32: {
                StartHaptic();
                Screen_Num = 31;
                update_display();
                break;
            }
            default: {
                break;
            }
        }
    }
}


void ButtonSlide(void)  // What is this Slide button???
{
    Screen_Timer.attach(&timout_timer,(SCRN_TIME));//Is this sufficient to reset/restart ticker timer for OLED?
    if (OLED_ON == 0) {
        OLED_ON = 1; // Screen was off, set to On
    }
    StartHaptic();
    oled.FillScreen(COLOR_BLACK); // Clear screen
    strcpy((char *) text_1,"Slide Button");
    oled.Label((uint8_t *)text_1,0,40);
}

int main()
{
    OLED_PWR = 1;  // Turn on OLED power supply
    
    
    oled.FillScreen(COLOR_BLACK); // Clear screen
// *****************  Local variables  ***********************
//   float accel_data[3]; float accel_rms=0.0;
    int i;
 
// **************  configure sensor modules  ******************
    //    accel.accel_config();   // configure sensor
    fall_config(1);         // configure sensor for fall detect
    // Fall_config(Fall_Alert_Mode); // configure sensor for fall mode
    mag.mag_config();
    gyro.gyro_config();

    RED_Led = LED_OFF;
    GRN_Led = LED_OFF;
    BLU_Led = LED_OFF;
    Led_clk1 = 0; // LEDs on docking station default to off, need to turn on with a 1
    Led_clk2 = 0; // LEDs on docking station default to off, need to turn on with a 1
    Led_clk3 = 0; // LEDs on docking station default to off, need to turn on with a 1
    Non_Free_PWR = 1;   // Start with non-freescale sensors (Pres/Temp/Hum/Light)on
    HR_PWR = 1;         // Start with Heart rate sensor powered on
    
// ***** Register callbacks/interupts to application functions *********
    kw40z_device.attach_buttonUp(&ButtonUp);
    kw40z_device.attach_buttonDown(&ButtonDown);
    kw40z_device.attach_buttonLeft(&ButtonLeft);
    kw40z_device.attach_buttonRight(&ButtonRight);
 //   kw40z_device.attach_buttonSlide(&ButtonSlide);

// ***** attaching interupts to functions *********
   Accel_INT1.fall(&fall_detect); // Accel sensor's int#1 calls interupt routine
 //  Accel_INT2.fall(&impact_detect); //Accel sensor's int#2 calls interupt routine
   

// **** Get OLED Class Default Text Properties ****************
    oled_text_properties_t textProperties = {0};
    oled.GetTextProperties(&textProperties);

// *********Set text color and screen alignment  **************
    textProperties.fontColor = COLOR_WHITE;
    textProperties.alignParam = OLED_TEXT_ALIGN_LEFT;
    oled.SetTextProperties(&textProperties);

// **************  Display spash screen  **********************

    oled.Label((uint8_t *)"Hexi",20,5); // Display white "Hexi" at x,y
    textProperties.fontColor = COLOR_RED;
    oled.SetTextProperties(&textProperties);
    oled.Label((uint8_t *)"Heart",45,5); // Display red "Heart" at x,y

#ifdef Debug  // if this is non-production version - do this
    strcpy((char *) text_1,"This is Debug Ver");
    oled.Label((uint8_t *)text_1,0,60);  // text_1 at x,y
    StartHaptic();
#endif
    textProperties.fontColor = COLOR_WHITE;
    oled.SetTextProperties(&textProperties);
    wait(3);  // wait 3 seconds
    update_display(); // Displays current screen (screen 0)
    Screen_Timer.attach(&timout_timer,(SCRN_TIME));//start ticker timer for turning off LCD
//  ******************* Main Loop *************************
    while (true) {
            i=0;
    while (i<20)// used for "Heart beat flash and updated any displayed data)
    {
    Thread::wait(500);          // wait 0.5 sec each loop
    if(OLED_PWR==1){
 //       update_data();
        }// end if
    i++;
    }// end while(i<20)
        
//    wait(10);          // wait 10 sec each loop, was orig half sec
    RED_Led = LED_ON;    // Used only for diagnostic of wait command
    Led_clk3 = 1;         // Used only for diagnostic of wait command
    wait(0.01);          // BLIP led 1/10 sec each loop
    RED_Led = LED_OFF;    // Used only for diagnostic of wait command
    Led_clk3 = 0; 
    Thread::wait(490);    // keep up the pace, at 0.5 sec (0.01s+0.49s) update date

//    update_data();        // refresh display date w/o updating entire display 

    } // end of while(true)

}
//  ************** end of main()

void update_display(void)
{
    OLED_PWR = 1;  // make sure OLED power supply is on
    oled_text_properties_t textProperties = {0};  // Need these to change font color
    oled.GetTextProperties(&textProperties);      // Need these to change font color
    switch(Screen_Num) {
        case 0: {// Main Screen
            HexiwearBattery battery;
            battery.sensorOn();  
            oled.FillScreen(COLOR_BLACK); // Clear screen
            
            if (battery.isBatteryCharging()) {
            textProperties.fontColor = COLOR_GREEN;
            oled.SetTextProperties(&textProperties);  
  //          sprintf(text_1, "%s", "chrg");           
            sprintf(text_1, "%i%%+", (uint8_t)battery.readLevelPercent());
            Screen_Timer.attach(&timout_timer,(SCRN_TIME));// Reset/restart ticker timer for OLED while fully charged
            } else {
            sprintf(text_1, "%i%%", (uint8_t)battery.readLevelPercent());
            }
            oled.TextBox((uint8_t *)text_1,60,0,35,15); //show level value of battery originaly at  55,40,35,15
        
        textProperties.fontColor = COLOR_WHITE;
        oled.SetTextProperties(&textProperties);
            
            oled.Label((uint8_t *)"Batt",35,0); // Display "Batt" at x,y
            oled.Label((uint8_t *)"Date",35,20); // Display "Date" at x,y
            oled.Label((uint8_t *)"Time",35,40); // Display "Time" at x,y
//            oled.Label((uint8_t *)"H.I.",10,80); // Display "H.I." at x,y
            oled.Label((uint8_t *)"BT",40,80);  //Display "BT" at x,y
            oled.Label((uint8_t *)"Menu",60,80);  //Display "Menu" at x,y
            
            Heat_Index_Calculation();
            sprintf(text,"%i",heat_index);
            oled.TextBox((uint8_t *)text,3,80,15,15); 
            strcpy((char *) text,"dF");oled.Label((uint8_t *)text,18,80);
            
            if(Heart_Rate_Mode == 1) {
                oled.Label((uint8_t *)"BPM",35,60); // Display "H.I." at x,y
            }
            break;
        }
        case 1: {// Panic Alert option
            oled.FillScreen(COLOR_BLACK); // Clear screen
            oled.Label((uint8_t *)"Panic Alert",20,5); // Display at x,y
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Enter",60,80);  //Display "enter" at x,y
            break;
        }
        case 2: {// Fall Alert option
            oled.FillScreen(COLOR_BLACK); // Clear screen
            oled.Label((uint8_t *)"Fall Alert",20,5); // Display at x,y
            oled.Label((uint8_t *)"Protection",15,25);  
            if (Fall_Alert == 1){  
            oled.Label((uint8_t *)" On ",42,40);   
            }
            else {               
            oled.Label((uint8_t *)" Off ",40,40);   
                } 
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Toggle",60,80);  //Display "Toggle" at x,y
            break;
        }
        case 3: {// Heart Rate Monitoring option
            oled.FillScreen(COLOR_BLACK); // Clear screen
            oled.Label((uint8_t *)"Heart Rate",20,5); // Display at x,y
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Enter",60,80);  //Display at x,y
            break;
        }
        case 4: {// Alert History option
            oled.FillScreen(COLOR_BLACK); // Clear screen
            oled.Label((uint8_t *)"Alert History",5,5); // Display at x,y
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Enter",60,80);  //Display at x,y
            break;
        }
        case 5: {// About HexiHeart Screen

            oled.FillScreen(COLOR_BLACK); // Clear screen
            oled.Label((uint8_t *)"Hexi",20,20); // Display white "Hexi" at x,y
            textProperties.fontColor = COLOR_RED;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"Heart",45,20); // Display red "Heart" at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            strcpy((char *) text_1,"About");
            oled.Label((uint8_t *)text_1,30,5);  // text_1 at x,y
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)" Back ",9,80); // Display "Back" at x,y
            oled.Label((uint8_t *)" Enter ",59,80);  //Display at x,y
            break;
        }

        case 6: {// Panic Alert
            oled.FillScreen(COLOR_BLACK); // Clear screen
            if (Panic_Alert == 0) {
                oled.Label((uint8_t *)"Send ",20,10); // Display at x,y
            } else {
                oled.Label((uint8_t *)"Dismiss ",17,10); // Display at x,y
            }
            oled.Label((uint8_t *)"Panic Alert",15,40); // Display at x,y
            oled.Label((uint8_t *)"-->",80,15); // "*" at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            break;
        }
        case 7: {// Heart Rate Zone

            oled.FillScreen(COLOR_BLACK); // Clear screen
            oled.Label((uint8_t *)"Heart Rate",15,5); // Display at x,y
            oled.Label((uint8_t *)"HR:",15,25); // Display at x,y
            sprintf(display_buff, "%u", Heart_Rate);
            textProperties.fontColor = COLOR_RED; //Change font to red
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)display_buff,43,25); // Display at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"Age: ",15,45); // Display at x,y
            textProperties.fontColor = COLOR_GREEN;
            oled.SetTextProperties(&textProperties); //implements the color change
            sprintf(display_buff, "%u", Age); //Convert int to char array for displaying user age
            oled.Label((uint8_t *)display_buff,43,45); // Display at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"On",80,15); // "+" at x,y
            oled.Label((uint8_t *)"Off",78,60); // "-" at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Next",59,80); // Display "Next" at x,y

            //heart.enable();
            //sprintf(display_buff, "%u", heart.getRevisionID()); //Convert int to char array for displaying user age
            //oled.Label((uint8_t *)display_buff,45,25); // Display at x,y
            //update_display();
            break;
        }
        case 8: {// Alert History
            oled.FillScreen(COLOR_BLACK); // Clear screen
            oled.Label((uint8_t *)"Alert History",5,5); // Display at x,y
            oled.Label((uint8_t *)"Date - Time",20,40); // Display at x,y
            oled.Label((uint8_t *)"Alert Type:",20,60); // Display at x,y
            oled.Label((uint8_t *)"+",85,15); // "*" at x,y
            oled.Label((uint8_t *)"-",85,60); // "*" at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            break;
        }
#ifdef Debug  // if this is non-production/debug version - do this
        case 20: {// Diagnostic/Debug Screens
            oled.FillScreen(COLOR_BLACK); // Clear screen
            textProperties.fontColor = COLOR_RED;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"Diagnostics",10,5); // Display at x,y
            oled.Label((uint8_t *)" Enter ",59,80);  //Display at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)" Back ",9,80); // Display "Back" at x,y

            break;
        }
        case 21: {// Fall Alert Diagnostic Screen
            oled.FillScreen(COLOR_BLACK); // Clear screen
            textProperties.fontColor = COLOR_RED;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"Fall",30,5); // Display at x,y
            oled.Label((uint8_t *)"Diagnostic",25,5); // Display at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            accel.acquire_accel_data_g(Accel_Data);
//                   gyro.acquire_gyro_data_g(Gyro_Data);
            Accel_Mag = sqrt(((Accel_Data[0]*Accel_Data[0])+(Accel_Data[1]*Accel_Data[1])+(Accel_Data[2]*Accel_Data[2])));
            //                  Gyro_Mag = (abs(Gyro_Data[0])+abs(Gyro_Data[1])+abs(Gyro_Data[3]));
            sprintf(text_1," Accel:%2.2f g  ",Accel_Mag);
            oled.Label((uint8_t *)text_1,10,40);// text_1 at x,y
            sprintf(text_1," Gyro:%4.0f D/S  ",Gyro_Mag);
            oled.Label((uint8_t *)text_1,10,60);// text_1 at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            break;
        }
        case 22: {// Fall Alert Debug Screen
            oled.FillScreen(COLOR_BLACK); // Clear screen
            textProperties.fontColor = COLOR_RED;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"Fall Debug",15,5); // Display at x,y
            textProperties.fontColor = COLOR_GREEN;
            oled.SetTextProperties(&textProperties);
            sprintf(text_1," %1.1f g  ",Fall_Thresh);
            oled.Label((uint8_t *)text_1,35,20);// text_1 at x,y
            sprintf(text_1," %2.1f g  ",Impact_Thresh);
            oled.Label((uint8_t *)text_1,35,35);// text_1 at x,y
            sprintf(text_1," %3.0f D/S  ",Movement_Thresh);
            oled.Label((uint8_t *)text_1,35,50);// text_1 at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"F-Th:",5,20); // "*" at x,y
            oled.Label((uint8_t *)"I-Th:",5,35); // "*" at x,y
            oled.Label((uint8_t *)"M-Th:",5,50); // "*" at x,y
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)" Back ",9,80); // Display "Back" at x,y
            //                  oled.Label((uint8_t *)" Enter ",59,80);  //Display at x,y
            break;
        }
        case 23: {// Heart Rate Diagnostic Screen
            oled.FillScreen(COLOR_BLACK); // Clear screen
            textProperties.fontColor = COLOR_RED;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"H.R. Diagnostic",5,5); // Display at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)" Back ",9,80); // Display "Back" at x,y
            //               oled.Label((uint8_t *)" Enter ",59,80);  //Display at x,y
            break;
        }
        case 24: {// Heart Rate Debug Screen
            oled.FillScreen(COLOR_BLACK); // Clear screen
            textProperties.fontColor = COLOR_RED;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"H.R. Debug",10,5); // Display at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)" Back ",9,80); // Display "Back" at x,y
            //               oled.Label((uint8_t *)" Enter ",59,80);  //Display at x,y
            break;
        }
        case 25: {// Heat Index Diagnostic Screen
            oled.FillScreen(COLOR_BLACK); // Clear screen
            textProperties.fontColor = COLOR_RED;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"H.I. Diagnostic",5,5); // Display at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)" Back ",9,80); // Display "Back" at x,y
            //             oled.Label((uint8_t *)" Enter ",59,80);  //Display at x,y

            Heat_Index_Calculation();
            
            oled_text_properties_t textProperties = {0};
            oled.GetTextProperties(&textProperties);
      
            strcpy((char *) text,"Temp.");
            oled.Label((uint8_t *)text,5,40); 
            strcpy((char *) text,"Humidity");
            oled.Label((uint8_t *)text,5,57);   
            strcpy((char *) text,"H.I.");
            oled.Label((uint8_t *)text,5,23);        
      
        /* Set text properties to white and right aligned for the dynamic text */
            textProperties.fontColor = COLOR_GREEN;
            textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
            oled.SetTextProperties(&textProperties);  
            
        /* Format the value */
            sprintf(text,"%i",sample_ftemp);
        
        /* Display time reading in 35px by 15px textbox at(x=55, y=40) */
            oled.TextBox((uint8_t *)text,55,40,15,15); //Increase textbox for more digits
        
        /* Display Units */
            strcpy((char *) text,"dF");
            oled.Label((uint8_t *)text,71,40);       
      
        /* Format the value */
            sprintf(text,"%i",sample_humid);
        
        /* Display time reading in 35px by 15px textbox at(x=55, y=40) */
            oled.TextBox((uint8_t *)text,55,57,15,15); //Increase textbox for more digits
        
        /* Display Units */
            strcpy((char *) text,"%");
            oled.Label((uint8_t *)text,71,57); 
        
         /* Set text properties to white and right aligned for the dynamic text */
            textProperties.fontColor = COLOR_BLUE;
            textProperties.alignParam = OLED_TEXT_ALIGN_RIGHT;
            oled.SetTextProperties(&textProperties);        
      
        /* Format the value */
            sprintf(text,"%i",heat_index);
        
        /* Display time reading in 35px by 15px textbox at(x=55, y=40) */
            oled.TextBox((uint8_t *)text,55,23,15,15); //Increase textbox for more digits
        
        /* Display Units */
            strcpy((char *) text,"dF");oled.Label((uint8_t *)text,71,23);
            
            break;
        }
        case 26: {//Heart Rate Config Option
            oled.FillScreen(COLOR_BLACK); // Clear screen
            oled.Label((uint8_t *)"HR Config",10,5); // Display at x,y
            oled.Label((uint8_t *)"*",85,15); // "*" at x,y
            oled.Label((uint8_t *)"*",85,60); // "*" at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Enter",60,80);  //Display "enter" at x,y
            break;
        }
        case 27: { //Enter Age Screen
            oled.FillScreen(COLOR_BLACK);
            oled.Label((uint8_t *)"Input Age", 10, 5);
            sprintf(display_buff, "%u", Age); //Convert int to char array for displaying user age
            oled.Label((uint8_t *)"Age:", 10, 30);
            oled.Label((uint8_t *)"+",85,15); // "*" at x,y
            oled.Label((uint8_t *)"-",85,60); // "*" at x,y
            oled.Label((uint8_t *)"Menu",10,80); // Display "Menu" at x,y
            oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
            textProperties.fontColor = COLOR_GREEN;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)display_buff,43,30); // Display at x,y
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            oled.Label((uint8_t *)"Max bpm:",10,50);
            textProperties.fontColor = COLOR_RED;
            oled.SetTextProperties(&textProperties); //implements the color change
            sprintf(display_buff, "%u", Max_Bpm); //Convert int to char array for displaying user max bpm
            oled.Label((uint8_t *)display_buff, 65, 50);
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            break;
        }
        case 28: {//Choose Heart Rate Vibration Option
            oled.FillScreen(COLOR_BLACK);
            oled.Label((uint8_t *)"Vibrate Pref", 10, 10);
            oled.Label((uint8_t *)"Option:", 10, 25);
            oled.Label((uint8_t *)"+",85,15); // "+" at x,y
            oled.Label((uint8_t *)"-",85,60); // "-" at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
            sprintf(display_buff, "%u", HR_Vibration); //Convert int to char array for displaying user preference
            textProperties.fontColor = COLOR_GREEN; //Change font to green
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)display_buff,55,25); // Display at x,y
            if(HR_Vibration == 1) {
                textProperties.fontColor = COLOR_RED; //Change font to red
                oled.SetTextProperties(&textProperties); //Implement color change
                oled.Label((uint8_t *)"All On",10,45); // Display at x,y
            } else if(HR_Vibration == 2) {
                textProperties.fontColor = COLOR_RED; //Change font to red
                oled.SetTextProperties(&textProperties);//Implement color change
                oled.Label((uint8_t *)"Only Entering",10,38);// Display at x,y
                oled.Label((uint8_t *)"And Exiting",10,53);// Display at x,y
                oled.Label((uint8_t *)"Target Zone",10,68);// Display at x,y

            } else if(HR_Vibration == 3) {
                textProperties.fontColor = COLOR_RED; //Change font to red
                oled.SetTextProperties(&textProperties); //Implement color change
                oled.Label((uint8_t *)"All Off",10,45);// Display at x,y
            }
            textProperties.fontColor = COLOR_WHITE;
            oled.SetTextProperties(&textProperties);
            break;
        }
        case 29: { //Zone Boundary Info
            oled.FillScreen(COLOR_BLACK);
            oled.Label((uint8_t *)"HR Zone Info", 10, 5);// Display at x,y
            textProperties.fontColor = COLOR_YELLOW; //Change font to yellow
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Z1:", 10, 20);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone1[0]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 30, 20);// Display at x,y
            oled.Label((uint8_t *)"-", 52, 20);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone1[1]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 60, 20);// Display at x,y
            textProperties.fontColor = COLOR_BLUE; //Change font to blue
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Z2:", 10, 35);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone2[0]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 30, 35);// Display at x,y
            oled.Label((uint8_t *)"-", 52, 35);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone2[1]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 60, 35);// Display at x,y
            textProperties.fontColor = COLOR_GREEN; //Change font to green
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Z3:", 10, 50);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone3[0]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 30, 50);// Display at x,y
            oled.Label((uint8_t *)"-", 52, 50);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone3[1]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 60, 50);// Display at x,y
            textProperties.fontColor = COLOR_RED; //Change font to red
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Z4:", 10, 65);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone4[0]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 30, 65);// Display at x,y
            oled.Label((uint8_t *)"-", 52, 65);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone4[1]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 60, 65);// Display at x,y
            textProperties.fontColor = COLOR_WHITE; //Change font to white
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
            break;
        }
        case 30: { //Enter Target Heart Rate Zone Preference
            oled.FillScreen(COLOR_BLACK);
            oled.Label((uint8_t *)"Zone Pref", 10, 5);// Display at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
            oled.Label((uint8_t *)"+",85,15); // "+" at x,y
            oled.Label((uint8_t *)"-",85,60); // "-" at x,y
            oled.Label((uint8_t *)"Target:", 10, 25);// Display at x,y
            sprintf(display_buff, "%u", Target_Zone); // Convert int to char to display
            if(Target_Zone == 1) {
                textProperties.fontColor = COLOR_YELLOW; //Change font to yellow
                oled.SetTextProperties(&textProperties);//Implement color change
            } else if(Target_Zone == 2) {
                textProperties.fontColor = COLOR_BLUE; //Change font to blue
                oled.SetTextProperties(&textProperties);//Implement color change
            } else if(Target_Zone == 3) {
                textProperties.fontColor = COLOR_GREEN; //Change font to green
                oled.SetTextProperties(&textProperties);//Implement color change
            } else if(Target_Zone == 4) {
                textProperties.fontColor = COLOR_RED; //Change font to red
                oled.SetTextProperties(&textProperties);//Implement color change
            }
            oled.Label((uint8_t *)display_buff, 55, 25);// Display at x,y
            textProperties.fontColor = COLOR_WHITE; //Change font to white
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Bounds:", 10, 45);// Display at x,y
            if(Target_Zone == 1) {
                textProperties.fontColor = COLOR_YELLOW; //Change font to yellow
                oled.SetTextProperties(&textProperties);//Implement color change
                sprintf(display_buff, "%u", HR_Zone1[0]); // Convert int to char to display
                oled.Label((uint8_t *)display_buff, 10, 60);// Display at x,y
                oled.Label((uint8_t *)"-", 32, 60);// Display at x,y
                sprintf(display_buff, "%u", HR_Zone1[1]); // Convert int to char to display
                oled.Label((uint8_t *)display_buff, 40, 60);// Display at x,y
            } else if(Target_Zone == 2) {
                textProperties.fontColor = COLOR_BLUE; //Change font to blue
                oled.SetTextProperties(&textProperties);//Implement color change
                sprintf(display_buff, "%u", HR_Zone2[0]); // Convert int to char to display
                oled.Label((uint8_t *)display_buff, 10, 60);// Display at x,y
                oled.Label((uint8_t *)"-", 32, 60);// Display at x,y
                sprintf(display_buff, "%u", HR_Zone2[1]); // Convert int to char to display
                oled.Label((uint8_t *)display_buff, 40, 60);// Display at x,y
            } else if(Target_Zone == 3) {
                textProperties.fontColor = COLOR_GREEN; //Change font to green
                oled.SetTextProperties(&textProperties);//Implement color change
                sprintf(display_buff, "%u", HR_Zone3[0]); // Convert int to char to display
                oled.Label((uint8_t *)display_buff, 10, 60); // Display at x,y
                oled.Label((uint8_t *)"-", 32, 60); // Display at x,y
                sprintf(display_buff, "%u", HR_Zone3[1]); // Convert int to char to display
                oled.Label((uint8_t *)display_buff, 40, 60);// Display at x,y
            } else if(Target_Zone == 4) {
                textProperties.fontColor = COLOR_RED; //Change font to red
                oled.SetTextProperties(&textProperties);//Implement color change
                sprintf(display_buff, "%u", HR_Zone4[0]); // Convert int to char to display
                oled.Label((uint8_t *)display_buff, 10, 60); // Display at x,y
                oled.Label((uint8_t *)"-", 32, 60); // Display at x,y
                sprintf(display_buff, "%u", HR_Zone4[1]); // Convert int to char to display
                oled.Label((uint8_t *)display_buff, 40, 60); // Display at x,y
            }
            textProperties.fontColor = COLOR_WHITE; //Change font to white
            oled.SetTextProperties(&textProperties);//Implement color change
            break;
        }
        case 31: {
            oled.FillScreen(COLOR_BLACK);
            Heart_Rate_Vibrations();
            oled.Label((uint8_t *)"Enter HR", 10, 5);// Display at x,y
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
            oled.Label((uint8_t *)"+",85,15); // "+" at x,y
            oled.Label((uint8_t *)"-",85,60); // "-" at x,y
            oled.Label((uint8_t *)"HR:", 10, 25);
            sprintf(display_buff, "%u", Heart_Rate); // Convert int to char to display
            textProperties.fontColor = COLOR_RED; //Change font to red
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)display_buff, 40, 25);
            textProperties.fontColor = COLOR_WHITE; //Change font to white
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Cur Zone:", 10, 45);
            if(Current_Zone == 1) {
                textProperties.fontColor = COLOR_YELLOW;
                oled.SetTextProperties(&textProperties);
            } else if(Current_Zone == 2) {
                textProperties.fontColor = COLOR_BLUE;
                oled.SetTextProperties(&textProperties);
            } else if(Current_Zone == 3) {
                textProperties.fontColor = COLOR_GREEN;
                oled.SetTextProperties(&textProperties);
            } else if(Current_Zone == 4) {
                textProperties.fontColor = COLOR_RED;
                oled.SetTextProperties(&textProperties);
            }
            sprintf(display_buff, "%u", Current_Zone); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 71, 45);
            textProperties.fontColor = COLOR_WHITE; //Change font to white
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Prev Zone:", 10, 60);
            if(Prev_Zone == 1) {
                textProperties.fontColor = COLOR_YELLOW;
                oled.SetTextProperties(&textProperties);
            } else if(Prev_Zone == 2) {
                textProperties.fontColor = COLOR_BLUE;
                oled.SetTextProperties(&textProperties);
            } else if(Prev_Zone == 3) {
                textProperties.fontColor = COLOR_GREEN;
                oled.SetTextProperties(&textProperties);
            } else if(Prev_Zone == 4) {
                textProperties.fontColor = COLOR_RED;
                oled.SetTextProperties(&textProperties);
            }
            sprintf(display_buff, "%u", Prev_Zone); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 71, 60);
            textProperties.fontColor = COLOR_WHITE; //Change font to white
            oled.SetTextProperties(&textProperties);//Implement color change
            Led_Zone_Indicator();
            break;
        }
        case 32: {
            //Zone Boundary Info
            oled.FillScreen(COLOR_BLACK);
            oled.Label((uint8_t *)"HR Zone Info", 10, 5);// Display at x,y
            textProperties.fontColor = COLOR_YELLOW; //Change font to yellow
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Z1:", 10, 20);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone1[0]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 30, 20);// Display at x,y
            oled.Label((uint8_t *)"-", 52, 20);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone1[1]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 60, 20);// Display at x,y
            textProperties.fontColor = COLOR_BLUE; //Change font to blue
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Z2:", 10, 35);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone2[0]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 30, 35);// Display at x,y
            oled.Label((uint8_t *)"-", 52, 35);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone2[1]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 60, 35);// Display at x,y
            textProperties.fontColor = COLOR_GREEN; //Change font to green
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Z3:", 10, 50);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone3[0]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 30, 50);// Display at x,y
            oled.Label((uint8_t *)"-", 52, 50);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone3[1]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 60, 50);// Display at x,y
            textProperties.fontColor = COLOR_RED; //Change font to red
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Z4:", 10, 65);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone4[0]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 30, 65);// Display at x,y
            oled.Label((uint8_t *)"-", 52, 65);// Display at x,y
            sprintf(display_buff, "%u", HR_Zone4[1]); // Convert int to char to display
            oled.Label((uint8_t *)display_buff, 60, 65);// Display at x,y
            textProperties.fontColor = COLOR_WHITE; //Change font to white
            oled.SetTextProperties(&textProperties);//Implement color change
            oled.Label((uint8_t *)"Back",10,80); // Display "Back" at x,y
            oled.Label((uint8_t *)"Next",60,80);  //Display "Next" at x,y
            break;
        }
#endif  // end of non-production/debug version code
        default: {
            Error_Num=1;
            error_screen(); // Clear screen
            break;
        }
    }
}
void error_screen(void)
{
    oled.FillScreen(COLOR_RED); // Clear screen
    oled.Label((uint8_t *)"Error! ",30,30); // Display error at x,y
    sprintf(text_1," %i  ",Error_Num);
    oled.Label((uint8_t *)text_1,30,60); // Display error at x,y
    wait(3);  // wait 3 seconds
    oled.FillScreen(COLOR_BLACK); // Clear screen
}

/*****************************************************************************
Name: StartHaptic
Purpose: Cause the HexiHeart device to vibrate for a predetermined amount of
         time
Inputs: None
Returns: None
******************************************************************************/
void StartHaptic(void)
{
    hapticTimer.start(30);  // was originaly 50
    haptic = 1;
}

/*****************************************************************************
Name: StartHaptic
Purpose: Cause the HexiHeart device to vibrate for x amount of time
Inputs: An int representing the duration of the vibration
Returns: None
******************************************************************************/
void StartHaptic(int x)
{
    hapticTimer.start(x);
    haptic = 1;
}

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

/*****************************************************************************
Name: Increment_Age
Purpose: Increment the user's age by 1
Inputs: None
Returns: None
******************************************************************************/
void Increment_Age()
{
    StartHaptic();
    if(Age < 100) {
        Age += 1;
        Screen_Num = 27;
    } else {
        Age = 1;
    }
}

/*****************************************************************************
Name: Decrement_Age
Purpose: Decrement the user's age by 1
Inputs: None
Returns: None
******************************************************************************/
void Decrement_Age()
{
    StartHaptic();
    if(Age == 1) {
        Age = 100;
    } else {
        Age -= 1;
        Screen_Num = 27;
    }
}

/*****************************************************************************
Name: Set_Max_Bpm
Purpose: Calculates the user's maximum heart rate based on their age
Inputs: None
Returns: None
******************************************************************************/
void Set_Max_Bpm()
{
    Max_Bpm = 220 - Age;
}

/*****************************************************************************
Name: Set_Zone_Boundaries
Purpose: Calculates the user's heart rate zones' boundaries based on the user's
         maximum heart rate
Inputs: None
Returns: None
******************************************************************************/
void Set_Zone_Boundaries()
{
    Set_Max_Bpm();
    HR_Zone1[0] = Max_Bpm * .50;   //Set Heart Rate Zone 1
    HR_Zone1[1] = Max_Bpm * .60;   //Set Heart Rate Zone 1
    HR_Zone2[0] = HR_Zone1[1] + 1;   //Set Heart Rate Zone 2
    HR_Zone2[1] = Max_Bpm * .70;   //Set Heart Rate Zone 2
    HR_Zone3[0] = HR_Zone2[1] + 1;   //Set Heart Rate Zone 3
    HR_Zone3[1] = Max_Bpm * .80;   //Set Heart Rate Zone 3
    HR_Zone4[0] = HR_Zone3[1] + 1;   //Set Heart Rate Zone 4
    HR_Zone4[1] = Max_Bpm;   //Set Heart Rate Zone 4
}

/*****************************************************************************
Name: Increment_Target_Zone
Purpose: Imcrements the user's target heart rate zone preference by 1
Inputs: None
Returns: None
******************************************************************************/
void Increment_Target_Zone()
{
    StartHaptic();
    if(Target_Zone == 4) {
        Target_Zone = 1;
    } else {
        Target_Zone += 1;
    }
}

/*****************************************************************************
Name: Decrement_Target_Zone
Purpose: Decrements the user's target heart rate zone preference by 1
Inputs: None
Returns: None
******************************************************************************/
void Decrement_Target_Zone()
{
    StartHaptic();
    if(Target_Zone == 1) {
        Target_Zone = 4;
    } else {
        Target_Zone -= 1;
    }
}

/*****************************************************************************
Name: Increment_HR_Vibr_Pref
Purpose: Increment the user's heart rate vibration preference by 1
Inputs: None
Returns: None
******************************************************************************/
void Increment_HR_Vibr_Pref()
{
    StartHaptic();
    if(HR_Vibration == 3) {
        HR_Vibration = 1;
    } else {
        HR_Vibration += 1;
    }
}

/*****************************************************************************
Name: Decrement_HR_Vibr_Pref
Purpose: Decrement the user's heart rate vibration preference by 1
Inputs: None
Returns: None
******************************************************************************/
void Decrement_HR_Vibr_Pref()
{
    StartHaptic();
    if(HR_Vibration == 1) {
        HR_Vibration = 3;
    } else {
        HR_Vibration -= 1;
    }
}

/*****************************************************************************
Name: Enable_Heart_Rate
Purpose: Turn on the HexiHeart heart rate function
Inputs: None
Returns: None
******************************************************************************/
void Enable_Heart_Rate()
{
    Heart_Rate_Mode = true;
    Heart_Rate_Vibrations();
}

/*****************************************************************************
Name: Disable_Heart_Rate
Purpose: Turn off the HexiHeart heart rate function
Inputs: None
Returns: None
******************************************************************************/
void Disable_Heart_Rate()
{
    Heart_Rate_Mode = false;
    Heart_Rate_Vibrations();
}

/*****************************************************************************
Name: Determine_Current_Zone
Purpose: Determines which zone the heart rate is in and assigns the curent
         zone and previous zone
Inputs: None
Returns: None
******************************************************************************/
void Determine_Current_Zone()
{
    Prev_Zone = Current_Zone;

    if(Heart_Rate >= HR_Zone1[0] && Heart_Rate <= HR_Zone1[1]) {
        Current_Zone = 1;
    } else if(Heart_Rate >= HR_Zone2[0] && Heart_Rate <= HR_Zone2[1]) {
        Current_Zone = 2;
    } else if(Heart_Rate >= HR_Zone3[0] && Heart_Rate <= HR_Zone3[1]) {
        Current_Zone = 3;
    } else if(Heart_Rate >= HR_Zone4[0] && Heart_Rate <= HR_Zone4[1]) {
        Current_Zone = 4;
    } else {
        //error reading, don't change anything
    }


}
/*****************************************************************************
Name: Run_Heart_Vibrations
Purpose: Performs the HexiHeart heart rate function
Inputs: None
Returns: None
******************************************************************************/
void Heart_Rate_Vibrations()
{
    if(Heart_Rate_Mode == true) {
        if(HR_Vibration == 1) {
            //All Pre-loaded vibrations enabled
            if(Current_Zone == Prev_Zone) {
                // Do nothing if no zone change
            } else if(Current_Zone == Target_Zone) { //Changed to target zone
                if(Target_Zone == LOWEST_ZONE || Prev_Zone > Target_Zone) { //must have entered from above
                    StartHaptic(ENTER_ABOVE);
                } else if(Target_Zone == HIGHEST_ZONE || Prev_Zone < Target_Zone) { //must have entered from below
                    StartHaptic(ENTER_BELOW);
                }
            } else if(Current_Zone != Target_Zone && Prev_Zone == Target_Zone) {
                if(Target_Zone == HIGHEST_ZONE || Current_Zone < Target_Zone) { //must have exited below
                    StartHaptic(EXIT_BELOW);
                } else if(Target_Zone == LOWEST_ZONE || Current_Zone > Target_Zone) { //must have exited above
                    StartHaptic(EXIT_ABOVE);
                }
            }
        } else if(HR_Vibration == 2) {
            //Only Entering and Exiting target zone
            if(Current_Zone == Prev_Zone) {
                //do nothing
            } else if(Current_Zone == Target_Zone) {
                StartHaptic(VIB_OPT_2);
                wait(0.1);
                StartHaptic(VIB_OPT_2);
            } else if(Current_Zone != Target_Zone && Prev_Zone == Target_Zone) {
                StartHaptic(VIB_OPT_2);
                wait(0.1);
                StartHaptic(VIB_OPT_2);
                wait(0.1);
                StartHaptic(VIB_OPT_2);
            }

        } else  if(HR_Vibration == 3) {
            //No Vibrations

        } else {
            //Error, can only be choices 1-3
            error_screen();
        }
    }
}

/*****************************************************************************
Name: Increment_Heart_Rate
Purpose: Manually increment the the heart rate measurement by 1 for testing
         purposes
Inputs: None
Returns: None
******************************************************************************/
void Increment_Heart_Rate()
{
    //StartHaptic();
    if(Heart_Rate == HR_Zone4[1]) {
        Heart_Rate = HR_Zone1[0];
    } else {
        Heart_Rate += 1;
    }
}

/*****************************************************************************
Name: Decrement_Heart_Rate
Purpose: Manually decrement the the heart rate measurement by 1 for testing
         purposes
Inputs: None
Returns: None
******************************************************************************/
void Decrement_Heart_Rate()
{
    //StartHaptic();
    if(Heart_Rate == HR_Zone1[0]) {
        Heart_Rate = HR_Zone4[1];
    } else {
        Heart_Rate -= 1;
    }
}  // end of Decrement_Heart_Rate

void Led_Zone_Indicator()
{
  if(Led_Zones == true)
  {
    if(Current_Zone == 1)
    {
        BLU_Led = LED_OFF;
        RED_Led = LED_ON;
        GRN_Led = LED_ON;
        
        wait(0.5);
        RED_Led = LED_OFF;
        GRN_Led = LED_OFF;
    }  
    else if(Current_Zone == 2)
    {
        if(Prev_Zone == 1)
        {
            RED_Led = LED_OFF;
            GRN_Led = LED_OFF;
        }
        else if(Prev_Zone == 3)
        {
            GRN_Led = LED_OFF;
        }
        BLU_Led = LED_ON;
        wait(0.5);  
        BLU_Led = LED_OFF; 
    }
    else if(Current_Zone == 3)
    {
        if(Prev_Zone == 2)
        {
            BLU_Led = LED_OFF;
        }
        else if(Prev_Zone == 4)
        {
            RED_Led = LED_OFF;
        }
        GRN_Led = LED_ON;
        wait(0.5);  
        GRN_Led = LED_OFF; 
    }
    else if(Current_Zone == 4)
    {
        GRN_Led = LED_OFF;
        RED_Led = LED_ON;
        wait(0.5);  
        RED_Led = LED_OFF; 
    }
  }  
}//end of Led_Zone_Indicator 

/*****************************************************************************
Name: Heat_Index_Calculation()
Purpose: Calculates the heat index using the temperature and humidity sensors
Inputs: None
Returns: None
******************************************************************************/
void Heat_Index_Calculation(){
    sample_ftemp = temphumid.sample_ftemp();
 
    sample_humid = temphumid.sample_humid();
            
    heat_index = -42.379 +
           2.04901523 * sample_ftemp +
           10.14333127 * sample_humid -
           0.22475541 * sample_ftemp * sample_humid -
           0.00683783 * sample_ftemp * sample_ftemp -
           0.05481717 * sample_humid * sample_humid +
           0.00122874 * sample_ftemp * sample_ftemp * sample_humid +
           0.00085282 * sample_ftemp * sample_humid * sample_humid -
           0.00000199 * sample_ftemp * sample_ftemp * sample_humid * sample_humid;
}

/*****************************************************************************
Name: fall_detect()
Purpose: Interupt rutine called when accelerometer IC has detected a free-fall >= 0.5g

******************************************************************************/

void fall_detect(){// fall detect interupt rutine
if(Fall_Alert == 1){
 
// for now just turn on display and give haptic feedback
   Screen_Num = 22;  //Change to screen 22 (Fall diag screen)
    Screen_Timer.attach(&timout_timer,(SCRN_TIME));// Reset/restart ticker timer for OLED
        if (OLED_ON == 0) {
        OLED_ON = 1; // Scree was off, set to On  
         }  // endif
         
//__disable_irq();    // Disable all Interrupts
//    oled.Label((uint8_t *)" Fall Detected ",05,70); // Display at x,y 
 
    update_display();
    BLU_Led = LED_ON;  // LEDs default to on, need to turn off
    Led_clk2 = 1;  // Turn LED2 on docking station on
    haptic = 1;
    Accel_INT1.rise(&fall_det_end); // Accel sensor's int#1 calls interupt routine
//__enable_irq();     // Enable all Interrupts
}// end if     
}//end fall_detect interupt routine


void fall_detect_off(){// fall detect interupt rutine
// for now just turn on display and give haptic feedback
}//end fall_detect_off interupt routine
  
void fall_det_end(){
    haptic = 0;         // Turn off Haptic
    BLU_Led = LED_OFF;  // Turn off HexiHeart Blue LED
    Led_clk2 = 0;       // Turn off LED2 on docking station on
    oled.Label((uint8_t *)"                         ",05,70); // clear display at x,y
    Accel_INT1.fall(&fall_detect); // Accel sensor's int#1 calls interupt routine
}    //end fall_det_end interupt routine

/*****************************************************************************
Name: impact_detect()
Purpose: Interupt rutine called when accelerometer IC has detected a vector
magnitude acceleration >= 3.0g

******************************************************************************/
      
void impact_detect(){
 //       oled.Label((uint8_t *)" Impact Detected ",05,60); // Display at x,y
 //       GRN_Led = LED_ON;  // LEDs default to on, need to turn off
        Led_clk3 = 1;  // Turn LED2 on docking station on
}//end impact_detect interupt routine

/*****************************************************************************
Name: fall_config()
Purpose: Used to set accelerometer IC's internal registers to set up chip level
interrupts
Inputs: int value from 0 to 256
Returns: None
******************************************************************************/
      

void fall_config(uint8_t Num){ 
// use case switches here to configure for 
switch(Num) {
                case 0: {// configure as normal (or maybe sleep)
                  char d[2]; 
                    d[0] = FXOS8700_CTRL_REG1;                     //Puts device in Standby mode
                    d[1] = 0x00; 
                    i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2);
                    
                    d[0] = FXOS8700_CTRL_REG1;                     //Puts device back into active mode
                    d[1] = 0x01;
                    i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d, 2);
                    oled.Label((uint8_t *)" Mode 0 ",30,60); // Display "mode" at x,y
                    break;
                }
                case 1: {// configure for free-fall int
                    StartHaptic();  // for debug
                    char d[2]; 
                    d[0] = FXOS8700_CTRL_REG1;                      //Config reg1
                    d[1] = 0x00;                                    //Put device in Standby mode
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step1 error ",30,05); // Display "error" at x,y                  
                        wait(3.0); // display for 3 seconds
                    }//end if
                    
                    d[0] = 0x0e;                                    //XYZ_DATA_CFG (set full-scall range)
                    d[1] = 0b00000000;                              //Set data to default range of +/-2g for full range (2x0.488mg/LSB), High-pass filter off
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step1a error ",30,05); // Display "error" at x,y      
                        wait(3.0); // display for 3 seconds
                    }//end if
 
                    d[0] = 0x0a;                                    //TRIG_CFG (address of trigger config reg)
                    d[1] = 0b00000100;                              //Trigger on freefall
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step2 error ",30,05); // Display "error" at x,y      
                        wait(3.0); // display for 3 seconds
                    }//end if
                    
                    d[0] = 0x15;                                    //A_FFMT_CFG (address of Free fall trigger config reg), write in Standby only
                    d[1] = 0b00111000;                              //set to freefall, and look at all axis.
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step3 error ",30,05); // Display "error" at x,y 
                        wait(3.0); // display for 3 seconds
                    }//end if
                       
                    d[0] = 0x17;                                    //A_FFMT_THS (address of Free fall threshold reg), write in Active or Standby
                    d[1] = 0b00001000;                                //set freefall threshold to about 756mg for now                   
  //                 d[1] = uint8_t(1000*Fall_Thresh/63);            //set freefall threshold - Resolution is 63mg/LSB, 0b111_1111 is maximum value
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step4 error ",30,05); // Display "error" at x,y 
                        wait(3.0); // display for 3 seconds
                    }//end if  
                     
                    d[0] = 0x18;                                    //A_FFMT_COUNT (address of Free fall debounce counter), write in Active or Standby
                    d[1] = 0b00000110;                              //with ODR at 100Hz, should equal 60mS debounce time or 120mS in Sleep
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step5 error ",30,05); // Display "error" at x,y 
                        wait(3.0); // display for 3 seconds
                    }//end if 
                           
                    d[0] = 0x2b;                                    //CTRL_REG2 (address of control reg), write in Standby only
                    d[1] = 0b00001101;                              //Turns Auto-Sleep mode on and low-noise, low power
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step6 error ",30,05); // Display "error" at x,y 
                        wait(3.0); // display for 3 seconds
                    }//end if    
                    
                    d[0] = 0x2c;                                    //CTRL_REG3 (address of Int control reg), write in Standby only
                    d[1] = 0b00001000;                              //FFMT will wake chip from sleep, int are active high
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step7 error ",30,05); // Display "error" at x,y 
                        wait(3.0); // display for 3 seconds
                    }//end if    
                        
                    d[0] = 0x2d;                                    //CTRL_REG4 (address of Int enable reg), write in Standby only
                    d[1] = 0b00000100;                              // FFMT int enabled and for debug I'm using a sleep int
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step8 error ",30,05); // Display "error" at x,y 
                        wait(3.0); // display for 3 seconds
                    }//end if            
                 
                    d[0] = 0x2e;                                    //CTRL_REG5 (Int routing reg), write in Standby only
                    d[1] = 0b00000100;                              // Make FFMT int output on pin INT1(PTC1)
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step9 error ",30,05); // Display "error" at x,y 
                        wait(3.0); // display for 3 seconds
                    }//end if                  
                    
                    d[0] = FXOS8700_CTRL_REG1;                     //CTRL_REG1, write in Standby only except for bit[0]
                    d[1] = 0b00011001;                    //Auto-Sleep mode on set to 50Hz, ODR set to 100Hz Puts device back into Active mode 
                    if(i2c_bus1.write(FXOS8700_I2C_ADDRESS_, d,2) ==1){
                        oled.Label((uint8_t *)" Step10 error ",30,05); // Display "error" at x,y 
                        wait(3.0); // display for 3 seconds
                    }//end if  
                    
                    //oled.FillScreen(COLOR_BLACK); // Clear screen
                    oled.Label((uint8_t *)" Mode 1 ",30,60); // Display "mode" at x,y   
                    GRN_Led = LED_ON;  // LEDs default to on, need to turn on     
                    break;
                }
                default: {
                    oled.Label((uint8_t *)" Mode ? ",30,60); // Display "mode" at x,y
                    // unknown config
                    break;
                } 
}// end switch                

}// end Fall_config