main.cpp

/* Code for ECE 4180-A Final Design Project  */
/* Kartik Sastry, Krishna Peri, Robert Walsh */
/* mbed Based Fitness Wearable Prototype     */
 
/**
 * Acknowledgements:
 *
 * The core of the GPS and Heart Rate code used in this project was supplied
 * as demonstration code by the manufacturer. The drivers written for the Heart
 * Rate Monitor were altered by Kartik Sastry in order to allow the device
 * to function with the mbed LPC1768 specifically.
 *
 * The uLCD, Temperature, Accelerometer, Control, and data processing are all
 * our original work. The Temperature feature uses a class for the TMP36 written
 * by Prof. James Hamblen, our ECE 4180 professor.
 */
 
/******************************************************************************/
/*                 Devices Used, Wiring, Hardware Info                        */
/******************************************************************************/
/*
 - mbed LPC1768 Microcontroller
    5V, (2A) External Power Supply or 4.5 V, (3 AA Batteries) Power
    Debugging and Additional Feedback over USB Virtual COM Port
 - Heart Rate Sensor / Pulse Oximeter - Maxim Integrated MAXREFDES117# (HR)
    I2C p27, p28
    DigitalOut p20
 - SparkFun Triple Axis Accelerometer Breakout - ADXL335 (IMU)
    AnalogIn p15, p16, p17 (X, Y, Z axes respectively)
 - Adafruit Ultimate GPS Breakout V3 (GPS)
    Serial p13, p14
 - 4D Systems 4DGL-uLCD LCD Display (LCD)
    Serial p9, p10
    DigitalOut p8
 - Pushbutton (Wire one switch pole to p7, the other directly to ground. No need for external pullup resistor.)
    PinDetect p7
 - Adafruit TMP36 Temperature Sensor
    AnalogIn p19
*/
 
/******************************************************************************/
/*                          Libraries and Include Files                       */
/******************************************************************************/
#include "mbed.h"
//#include "rtos.h"
//#include "SDFileSystem.h"
#include "uLCD_4DGL.h"
//#include "LSM9DS1.h"
#include "MBed_Adafruit_GPS.h"
// #include "XNucleo53L0A1.h"
#include "math.h"
#include <stdio.h>
#include "PinDetect.h"
#include "algorithm.h"
#include "MAX30102.h"
#include "TMP36.h"
 
/******************************************************************************/
/*                          I/O Object Declarations                           */
/******************************************************************************/
PinDetect myPushbutton(p7);                                                     // For Mode Selection Feature
Serial pc(USBTX, USBRX);                                                        // Interface to PC over virtual COM
uLCD_4DGL uLCD(p9, p10, p8);                                                    // uLCD
AnalogIn Xval(p15);                                                             // IMU: Output of X-axis at analog p15
AnalogIn Yval(p16);                                                             // IMU: Output of y-axis at analog p16
AnalogIn Zval(p17);                                                             // IMU: Output of z-axis at analog p17
TMP36 myTMP36(p19);                                                             // Analog in                                                          // GPS
// #define VL53L0_I2C_SDA   p28                                                    // LIDAR
// #define VL53L0_I2C_SCL   p27                                                    // I2C sensor pins for LIDAR
// DigitalOut shdn(p26);                                                           // This VL53L0X board test application performs a range measurement in polling mode
//                                                                                 // Use 3.3(Vout) for Vin, p9 for SDA, p10 for SCL, P26 for shdn on mbed LPC1768
 
/******************************************************************************/
/*                    Global Variables (Carefully Managed)                    */
/******************************************************************************/
// // Globals For LIDAR
// static XNucleo53L0A1 *board = NULL;
// int status;
// uint32_t distance;
 
// Globals For GPS
Serial * gps_Serial;
 
// Globals for Heart Rate Sensor
#define MAX_BRIGHTNESS 255
uint32_t aun_ir_buffer[500]; //IR LED sensor data
int32_t n_ir_buffer_length;    //data length
uint32_t aun_red_buffer[500];    //Red LED sensor data
int32_t n_sp02; //SPO2 value
int8_t ch_spo2_valid;   //indicator to show if the SP02 calculation is valid
int32_t n_heart_rate;   //heart rate value
int8_t  ch_hr_valid;    //indicator to show if the heart rate calculation is valid
uint8_t uch_dummy;
// Serial pc(USBTX, USBRX);    //initializes the serial port
// #ifdef TARGET_KL25Z
// PwmOut led(PTB18);  //initializes the pwm output that connects to the on board LED
// DigitalIn myINT(PTD1);  //pin PTD1 connects to the interrupt output pin of the MAX30102
// #endif
// #ifdef TARGET_K64F
// DigitalIn myINT(PTD1);  //pin PTD1 connects to the interrupt output pin of the MAX30102
// #endif
// #ifdef TARGET_MAX32600MBED
PwmOut led(LED1);    // initializes the pwm output that connects to the on board LED
DigitalIn myINT(p20);  // pin p20 connects to the interrupt output pin of the MAX30102
// #endif
 
/******************************************************************************/
/*                    Device Selection / Thread Control                       */
/******************************************************************************/
// Add MODE_LIDAR_SELECT if we can get LIDAR to work
enum DATA_ACQ_MODE {MODE_IMU_SELECT, MODE_GPS_SELECT, MODE_TEMP_SELECT, MODE_HR_SELECT};
volatile int myMode = MODE_IMU_SELECT;                                          // To be changed by pushbutton presses
 
// Short ISR - serviced when interrupt given by myPushbutton hit
void changeMode_ISR(void) {
    myMode = (myMode + 1) % 4;                                                  // mod 4 makes it periodic (0,1,2,3,0)
}
 
/******************************************************************************/
/*                    Function Prototypes of Threads                          */
/******************************************************************************/
void IMU_THREAD();
void GPS_THREAD();
void TEMP_THREAD();
void HR_THREAD();
// void LIDAR_THREAD();
 
/******************************************************************************/
/*                      Main Thread: Initialization                           */
/******************************************************************************/
 
int main() {
 
    // // Set up LIDAR
    // pc.printf("\rSetting Up LIDAR...\n");
    // DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);            // LIDAR Objects:
    // board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);                    // creates the 53L0A1 expansion board singleton obj
    // shdn = 0;                                                                   // must reset sensor for an mbed reset to work
    // wait(0.1);
    // shdn = 1;
    // wait(0.1);
 
    // status = board->init_board();                                               // init the 53L0A1 board with default values
    // while (status) {
    //     pc.printf("\r(LIDAR) Failed to init board! \r\n");
    //     status = board->init_board();
    // }
    // pc.printf("\rSet Up LIDAR.\n");
 
    // Set up uLCD
    pc.printf("\rSetting Up uLCD...\n");
    uLCD.baudrate(31250);
    pc.printf("\rSet Up uLCD.\n");
    uLCD.printf("Welcome!");
 
    // // Set up GPS
    // pc.printf("\rSetting Up GPS...\n");
    // myGPS.begin(9600);                                                          // sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
    //                                                                             // a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
    // myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);                             // these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
    // myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    // myGPS.sendCommand(PGCMD_ANTENNA);
    // pc.printf("\r(GPS) Connection established at 9600 baud...\n");
    // wait(1);
    // refresh_Timer.start();                                                      // starts the clock on the timer
    // pc.printf("\rSet Up GPS.\n");
 
    // Set up Mode Selecting Pushbutton (Debounced, Interrupt Based)
    pc.printf("\rSetting Up Mode Changing PB...\n");
    myPushbutton.mode(PullUp);                                                  // Use internal pullups for pushbutton
    wait(.01);                                                                  // Delay for initial pullup to take effect
    myPushbutton.attach_deasserted(&changeMode_ISR);                            // Setup Interrupt Service Routines. PullUp implies 1->0 change means hit
    myPushbutton.setSampleFrequency();                                          // Start sampling pushbutton inputs using interruptsUsing default 50 Hz (20 ms period)
    pc.printf("\rSet Up Mode Changing PB.\n");
 
/******************************************************************************/
/*                          Main Thread: Devices                              */
/******************************************************************************/
 
    while(true) {
        pc.printf("Entered Main Loop\n");
 
        // Print Current Mode on Top
        uLCD.text_width(1); // normal size text
        uLCD.text_height(1);
        uLCD.background_color(BLACK);
        uLCD.color(GREEN);
        uLCD.locate(0, 0);
        uLCD.printf("  ");
        uLCD.locate(0, 0);
        uLCD.printf("Mode: %d", myMode);
        pc.printf("\rMode: %d", myMode);
 
        // Depending on Mode:
        switch (myMode) {
            case MODE_IMU_SELECT:
                // Print Current Mode on Top
                uLCD.text_width(1); // normal size text
                uLCD.text_height(1);
                uLCD.background_color(BLACK);
                uLCD.color(GREEN);
                uLCD.locate(0, 0);
                uLCD.printf("  ");
                uLCD.locate(0, 0);
                uLCD.printf("Mode: %d", myMode);
                pc.printf("\rMode: %d", myMode);
 
                // Create a RED Outline of the screen
                uLCD.background_color(RED);
                uLCD.cls();
                uLCD.filled_rectangle(4, 4, 124, 124, BLACK);
                while (myMode == MODE_IMU_SELECT) {
                    IMU_THREAD();
                }
                break;
 
            case MODE_GPS_SELECT:
                // Print Current Mode on Top
                uLCD.text_width(1); // normal size text
                uLCD.text_height(1);
                uLCD.background_color(BLACK);
                uLCD.color(GREEN);
                uLCD.locate(0, 0);
                uLCD.printf("  ");
                uLCD.locate(0, 0);
                uLCD.printf("Mode: %d", myMode);
                pc.printf("\rMode: %d", myMode);
 
                // Create a BLUE Outline of the screen
                uLCD.background_color(BLUE);
                uLCD.cls();
                uLCD.filled_rectangle(4, 4, 124, 124, BLACK);
                while (myMode == MODE_GPS_SELECT) {
                    GPS_THREAD();
                }
                break;
 
            case MODE_TEMP_SELECT:
                // Print Current Mode on Top
                uLCD.text_width(1); // normal size text
                uLCD.text_height(1);
                uLCD.background_color(BLACK);
                uLCD.color(GREEN);
                uLCD.locate(0, 0);
                uLCD.printf("  ");
                uLCD.locate(0, 0);
                uLCD.printf("Mode: %d", myMode);
                pc.printf("\rMode: %d", myMode);
 
                // Create a GREEN Outline of the screen
                uLCD.background_color(GREEN);
                uLCD.cls();
                uLCD.filled_rectangle(4, 4, 124, 124, BLACK);
                while (myMode == MODE_TEMP_SELECT) {
                    TEMP_THREAD();
                }
                break;
            case MODE_HR_SELECT:
                // Print Current Mode on Top
                uLCD.text_width(1); // normal size text
                uLCD.text_height(1);
                uLCD.background_color(BLACK);
                uLCD.color(GREEN);
                uLCD.locate(0, 0);
                uLCD.printf("  ");
                uLCD.locate(0, 0);
                uLCD.printf("Mode: %d", myMode);
                pc.printf("\rMode: %d", myMode);
 
                // Create a WHITE Outline of the screen
                uLCD.background_color(WHITE);
                uLCD.cls();
                uLCD.filled_rectangle(4, 4, 124, 124, BLACK);
                // Graphics Boilerplate
                uLCD.text_width(2); // 2X size text
                uLCD.text_height(2);
                uLCD.color(WHITE);
                uLCD.locate(0,0);
                uLCD.printf("HEARTRATE");
                while (myMode == MODE_HR_SELECT) {
                    HR_THREAD();
                }
                break;
            // case MODE_LIDAR_SELECT:
            //     // Print Current Mode on Top
            //     uLCD.text_width(1); // normal size text
            //     uLCD.text_height(1);
            //     uLCD.background_color(BLACK);
            //     uLCD.color(GREEN);
            //     uLCD.locate(0, 0);
            //     uLCD.printf("  ");
            //     uLCD.locate(0, 0);
            //     uLCD.printf("Mode: %d", myMode);
            //     pc.printf("\rMode: %d", myMode);
 
            //     // Create a LGREY Outline of the screen
            //     uLCD.background_color(LGREY);
            //     uLCD.cls();
            //     uLCD.filled_rectangle(4, 4, 124, 124, BLACK);
            //     // Graphics Boilerplate
            //     uLCD.text_width(2); // 2X size text
            //     uLCD.text_height(2);
            //     uLCD.color(LGREY);
            //     uLCD.locate(0,0);
            //     uLCD.printf("PUSH-UPS");
            //     while (myMode == MODE_LIDAR_SELECT) {
            //         LIDAR_THREAD();
            //     }
            //     break;
            default:
                // Print Current Mode on Top
                uLCD.text_width(1); // normal size text
                uLCD.text_height(1);
                uLCD.background_color(BLACK);
                uLCD.color(GREEN);
                uLCD.locate(0, 0);
                uLCD.printf("  ");
                uLCD.locate(0, 0);
                uLCD.printf("Mode: %d", myMode);
                pc.printf("\rMode: %d", myMode);
                uLCD.cls();
                uLCD.printf("INVALID MODE.");
 
        }
    }
} // END OF MAIN!!!!
 
 
/******************************************************************************/
/*                    Thread 1: Heart Rate Monitoring                         */
/******************************************************************************/
void HR_THREAD() {
    uint32_t un_min, un_max, un_prev_data;  //variables to calculate the on-board LED brightness that reflects the heartbeats
    int i;
    int32_t n_brightness;
    float f_temp;
    int32_t myOldHeartRate;
 
    maxim_max30102_reset(); //resets the MAX30102
 
    // // initialize serial communication at 115200 bits per second:
    // pc.baud(9600);
    // pc.format(8,SerialBase::None,1);
    // wait(1);
 
    //read and clear status register
    maxim_max30102_read_reg(0,&uch_dummy);
 
    // //wait until the user presses a key
    // while(pc.readable()==0)
    // {
    //     pc.printf("\x1B[2J");  //clear terminal program screen
    //     pc.printf("Press any key to start conversion\n\r");
    //     wait(1);
    // }
    // uch_dummy=getchar();
 
    maxim_max30102_init();  //initializes the MAX30102
    // pc.printf("\rInitialization Complete - HR\n");
 
    n_brightness=0;
    un_min=0x3FFFF;
    un_max=0;
 
    n_ir_buffer_length=500; //buffer length of 100 stores 5 seconds of samples running at 100sps
 
    //read the first 500 samples, and determine the signal range
    for(i=0;i<n_ir_buffer_length;i++)
    {
        while(myINT.read()==1);   //wait until the interrupt pin asserts
 
        maxim_max30102_read_fifo((aun_red_buffer+i), (aun_ir_buffer+i));  //read from MAX30102 FIFO
 
        if(un_min>aun_red_buffer[i])
            un_min=aun_red_buffer[i];    //update signal min
        if(un_max<aun_red_buffer[i])
            un_max=aun_red_buffer[i];    //update signal max
        // pc.printf("red=");
        // pc.printf("%i", aun_red_buffer[i]);
        // pc.printf(", ir=");
        // pc.printf("%i\n\r", aun_ir_buffer[i]);
    }
    un_prev_data=aun_red_buffer[i];
 
 
    //calculate heart rate and SpO2 after first 500 samples (first 5 seconds of samples)
    maxim_heart_rate_and_oxygen_saturation(aun_ir_buffer, n_ir_buffer_length, aun_red_buffer, &n_sp02, &ch_spo2_valid, &n_heart_rate, &ch_hr_valid);
 
    //Continuously taking samples from MAX30102.  Heart rate and SpO2 are calculated every 1 second
    while (myMode == MODE_HR_SELECT) {
        i=0;
        un_min=0x3FFFF;
        un_max=0;
 
        //dumping the first 100 sets of samples in the memory and shift the last 400 sets of samples to the top
        for(i=100;i<500;i++)
        {
            aun_red_buffer[i-100]=aun_red_buffer[i];
            aun_ir_buffer[i-100]=aun_ir_buffer[i];
 
            //update the signal min and max
            if(un_min>aun_red_buffer[i])
            un_min=aun_red_buffer[i];
            if(un_max<aun_red_buffer[i])
            un_max=aun_red_buffer[i];
        }
 
        //take 100 sets of samples before calculating the heart rate.
        for(i=400;i<500;i++)
        {
            un_prev_data=aun_red_buffer[i-1];
            while(myINT.read()==1);
            maxim_max30102_read_fifo((aun_red_buffer+i), (aun_ir_buffer+i));
 
            if(aun_red_buffer[i]>un_prev_data)
            {
                f_temp=aun_red_buffer[i]-un_prev_data;
                f_temp/=(un_max-un_min);
                f_temp*=MAX_BRIGHTNESS;
                n_brightness-=(int)f_temp;
                if(n_brightness<0)
                    n_brightness=0;
            }
            else
            {
                f_temp=un_prev_data-aun_red_buffer[i];
                f_temp/=(un_max-un_min);
                f_temp*=MAX_BRIGHTNESS;
                n_brightness+=(int)f_temp;
                if(n_brightness>MAX_BRIGHTNESS)
                    n_brightness=MAX_BRIGHTNESS;
            }
            //#if defined(TARGET_KL25Z) || defined(TARGET_MAX32600MBED)
            led.write(1-(float)n_brightness/256);
            //#endif
            //send samples and calculation result to terminal program through UART
            // pc.printf("red=");
            // pc.printf("%i", aun_red_buffer[i]);
            // pc.printf(", ir=");
            // pc.printf("%i", aun_ir_buffer[i]);
            // pc.printf(", HR=%i, ", n_heart_rate);
            // pc.printf("HRvalid=%i, ", ch_hr_valid);
            if (ch_hr_valid == 1) {
                myOldHeartRate = n_heart_rate;
                if ((myOldHeartRate >= 50) && (myOldHeartRate <= 200)) {
                    // Print Out on LCD
                    uLCD.text_width(2); // normal size text
                    uLCD.text_height(2);
                    uLCD.locate(1,3);
                    uLCD.printf("HR:\n\n  %i", myOldHeartRate);
                } else {
                    uLCD.text_width(2); // normal size text
                    uLCD.text_height(2);
                    uLCD.locate(1,3);
                    uLCD.printf("HR:\n\n   --");
                }
 
            }
            // pc.printf("SpO2=%i, ", n_sp02);
            // pc.printf("SPO2Valid=%i\n\r", ch_spo2_valid);
        }
        maxim_heart_rate_and_oxygen_saturation(aun_ir_buffer, n_ir_buffer_length, aun_red_buffer, &n_sp02, &ch_spo2_valid, &n_heart_rate, &ch_hr_valid);
    }
}
 
/******************************************************************************/
/*                       Thread 2: IMU Measurement                            */
/******************************************************************************/
void IMU_THREAD() {
    float x,y,z;                                                                    // Raw data
    float xG, yG, zG;                                                       // IN G
    bool start = 0;
    int count = 0;
 
    while (myMode == MODE_IMU_SELECT) {
        // Graphics Boilerplate
        uLCD.text_width(3); // 3X size text
        uLCD.text_height(3);
        uLCD.color(RED);
        uLCD.locate(0,0);
        uLCD.printf("IMU");
 
        // Get Values
        x = Xval.read();           // Reads X-axis value between 0 and 1
        y = Yval.read();           // Reads Y-axis value
        z = Zval.read();           // Reads Z-axis value
 
        xG = (x * 6.6) - 3.3;       // Scaling into G's
        yG = (y * 6.6) - 3.3;
        zG = (z * 6.6) - 3.3;
 
        pc.printf("\r%f, %f, %f\n", xG, yG, zG);
 
        if (zG > 0.7){
            start = 1;
        }
 
        if (start==1 & zG < 0.5) {
            count+=1;
            start = 0;
        }
 
        // Print Out on LCD
        uLCD.text_width(2); // normal size text
        uLCD.text_height(2);
        uLCD.locate(1,3);
        uLCD.printf("Squats:\n\n  %d", count);
        wait(.25);
    }
}
 
/******************************************************************************/
/*                          Thread 3: GPS Measurement                         */
/******************************************************************************/
void GPS_THREAD() {
    // Graphics Boilerplate
    uLCD.text_width(3); // 3X size text
    uLCD.text_height(3);
    uLCD.color(BLUE);
    uLCD.locate(0,0);
    uLCD.printf("GPS");
    uLCD.text_width(1); // normal size text
    uLCD.text_height(1);
    uLCD.locate(1,3);
    uLCD.printf("GPS Data:\n");
 
    pc.baud(9600); //sets virtual COM serial communication to high rate; this is to allow more time to be spent on GPS retrieval
 
    gps_Serial = new Serial(p13,p14); //serial object for use w/ GPS
    Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
    char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
    Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
    const int refresh_Time = 2000; //refresh time in ms
 
    myGPS.begin(9600);  //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
                        //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
 
    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
    myGPS.sendCommand(PGCMD_ANTENNA);
 
    pc.printf("Connection established at 9600 baud...\n");
 
    wait(1);
 
    refresh_Timer.start();  //starts the clock on the timer
 
    while (myMode == MODE_GPS_SELECT) {
        c = myGPS.read();   //queries the GPS
 
        //if (c) { pc.printf("%c", c); } //this line will echo the GPS data if not paused
 
        //check if we recieved a new message from GPS, if so, attempt to parse it,
        if ( myGPS.newNMEAreceived() ) {
            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
                continue;
            }
        }
 
        //check if enough time has passed to warrant printing GPS info to screen
        //note if refresh_Time is too low or pc.baud is too low, GPS data may be lost during printing
        if (refresh_Timer.read_ms() >= refresh_Time) {
            refresh_Timer.reset();
            uLCD.locate(1, 4);
            pc.printf("\rGPS SAYS:\n\r");
            pc.printf("\rTime: %d:%d:%d.%u\n\r", myGPS.hour, myGPS.minute, myGPS.seconds, myGPS.milliseconds);
            uLCD.printf("\rTime: %d:%d:%d.%u\n\r", myGPS.hour, myGPS.minute, myGPS.seconds, myGPS.milliseconds);
            pc.printf("\rDate: %d/%d/20%d\n\r", myGPS.day, myGPS.month, myGPS.year);
            uLCD.printf("\rDate: %d/%d/20%d\n\r", myGPS.day, myGPS.month, myGPS.year);
            pc.printf("\rFix: %d\n\r", (int) myGPS.fix);
            uLCD.printf("\rFix: %d\n\r", (int) myGPS.fix);
            pc.printf("\rQuality: %d\n\r", (int) myGPS.fixquality);
            if (myGPS.fix) {
                pc.printf("\rLocation: %5.2f%c, %5.2f%c\n\r", myGPS.latitude, myGPS.lat, myGPS.longitude, myGPS.lon);
                pc.printf("\rLocation: %5.2f%c, %5.2f%c\n\r", myGPS.latitude, myGPS.lat, myGPS.longitude, myGPS.lon);
                pc.printf("\rSpeed: %5.2f knots\n\r", myGPS.speed);
                uLCD.printf("\rSpeed: %5.2f mph\n\r", myGPS.speed * 1.15078);           // CONVERT
                pc.printf("\rAngle: %5.2f\n", myGPS.angle);
                pc.printf("\rAltitude: %5.2f\n", myGPS.altitude);
                uLCD.printf("\rAltitude: %5.2f\n", myGPS.altitude);
                pc.printf("\rSatellites: %d\n\r", myGPS.satellites);
            }
        }
    }
}
 
// /******************************************************************************/
// /*                       Thread 4: LIDAR Measurements                         */
// /******************************************************************************/
// void LIDAR_THREAD() {
//     // loop taking and printing distance
//     while (myMode == MODE_LIDAR_SELECT) {
//         status = board->sensor_centre->get_distance(&distance);
//         if (status == VL53L0X_ERROR_NONE) {
//                 pc.printf("\rLIDAR SAYS:\n\r");
//                 pc.printf("\rD=%ld mm\r\n", distance);
//         }
//     }
// }
 
/******************************************************************************/
/*                   Thread 5: Temperature Measurements                       */
/******************************************************************************/
void TEMP_THREAD() {
 
    float myCurrentTemp;
 
    while (myMode == MODE_TEMP_SELECT) {
        // Graphics Boilerplate
        uLCD.text_width(3); // 3X size text
        uLCD.text_height(3);
        uLCD.color(GREEN);
        uLCD.locate(0,0);
        uLCD.printf("TEMP");
 
        // Get Temperature
        myCurrentTemp = myTMP36.read();         // Floating Value
 
        // Print Out on LCD
        uLCD.text_width(2); // normal size text
        uLCD.text_height(2);
        uLCD.locate(1,3);
        uLCD.printf("Temp:\n\n %0.1f C", myCurrentTemp);
        wait(1);
    }
}