File content as of revision 43:81f314c92d9f:

#include "mbed.h"
#include "rtos.h"
#include "LSM9DS1.h"
#include "SCP1000.h"
#include "PulseSensor.h"
#include "PinDetect.h"
#include "uLCD_4DGL.h"
#include "GPS.h"
#include "MSCFileSystem.h"
 
SCP1000 scp1000(p5,p6,p7,p8);
LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
PulseSensor PPG(p17);
uLCD_4DGL uLCD(p28,p27,p29);
Serial pc(USBTX, USBRX);
DigitalOut one = LED1;
DigitalOut two = LED2;
DigitalOut three = LED3;
DigitalOut four = LED4;
AnalogIn pot(p20);
PinDetect pb(p21);
GPS gps(p13, p14);

#define FSNAME "msc"
MSCFileSystem msc(FSNAME); 
 
int bpm;
int steps = 0;
int flights = 0;
float distance = 0.0;
float calories = 0;
int oldSteps = 0;
int stepGoal = 100;
float stride_length = 0.0;

unsigned long pressure;
float latitude = 0;
float longitude = 0;
#define PI 3.14159
unsigned long p_buff[4];
int count = 0;

int mode = 1;
int oldMode = 1;
bool usePrev = false;
bool readConfig = false;
bool run = true;

int gender;
int weight;
int age;
int screen = 1;
int oldScreen = 1;
bool setup_state = true;
char date[32];

Timer usb_timer;

Thread thread1;
Thread thread2;
Thread thread3;
Thread thread4;
Thread thread5;
Thread thread6;
Mutex serial_mtx;
Mutex usb_mtx;

//Saves current configuration in a text file
//Saved in format gender, age, weight and step goal separated by tabs
void save_config() {
    usb_mtx.lock();
    two = 1;
    FILE *fp = fopen( "/msc/config.txt", "w");
    if(fp == NULL) {
        pc.printf("Could not open file for write\n");
    } else {
        fprintf(fp, "%d\t%d\t%d\t%d\n\r", gender, age, weight, stepGoal);
        //pc.printf("Config saved\r\n");
        fclose(fp);
    }
    two = 0;
    usb_mtx.unlock();
}

//Reads configuration file and sets up user specific values and the last saved 
//activity date
//If files don't exist or are empty, will return false, else returns true
bool read_config() {
    usb_mtx.lock();
    three = 1;
    FILE *fp1 = fopen( "/msc/config.txt", "r");
    FILE *fp2 = fopen("/msc/data.txt", "r");
    if(fp1 == NULL || fp2 == NULL) {
        pc.printf("Could not open file for read\n");
        //Go back to first setup screen if files don't exist
        screen = 1;
        usb_mtx.unlock();
        return false;
    } else {
        char buf[128];
        //Read config file to setup user specific values and data file to load last
        //stored data. If config file empty, then go back to setup screen
        if (fscanf(fp1, "%d\t%d\t%d\t%d", &gender, &age, &weight, &stepGoal) != EOF) {
            fscanf(fp2, "%s\t%d\t%d\t%f\t%f", buf, &steps, &flights, &calories, &distance);
            //pc.printf("Data loaded\r\n");
        } else {
            //files empty, go back to setup screen
            screen = 1;
            fclose(fp1);
            fclose(fp2);
            three = 0;
            usb_mtx.unlock();
            return false;
        }
        fclose(fp1);
        fclose(fp2);
        three = 0;
        usb_mtx.unlock();
        return true;
    }
}

// when the pushbotton is pressed the run flag is set to false and the main 
// function while loop exits so that the data file can be closed 
// so press the button when you're ready to be done collecting data
void button (void) {
    run = false;
}

//Switches to next screen in setup mode with a button press
void next() {
    oldScreen = screen;
    screen++;
    if (usePrev) {
        readConfig = true;
    } else if(screen == 6) {
        setup_state = false;
    }
}
// Reads the value of the potentiometer and averages over 3 readings to get rid 
// of random spikes/zero values. Returns either a number 1-5 that indicates
// which screen should be displayed
void read_pot() {
    float m1; 
    float m2;
    float m3;
    oldMode = mode;
    m1 = pot.read();
    m2 = pot.read();
    m3 = pot.read();
    if(m1 < 0.2 && m2 < 0.2 && m3 < 0.2) {
        mode = 1;
    } else if(m1 >= 0.2 && m1 < 0.4 && m2 >= 0.2 && m2 < 0.4 && m3 >= 0.2 && m3 < 0.4) {
        mode = 2;
    } else if(m1 >= 0.4 && m1 < 0.6 && m2 >= 0.4 && m2 < 0.6 && m3 >= 0.4 && m3 < 0.6) {
        mode = 3;
    } else if(m1 >= 0.6 && m1 < 0.8 && m2 >= 0.6 && m2 < 0.8 && m3 >= 0.6 && m3 < 0.8) {
        mode = 4;
    } else if(m1 >= 0.8 && m2 >= 0.8 && m3 >= 0.8) {
        mode = 5;
    }
}    
 
//Display the time on the top of the screen
void display_time() {
    while(1) {
        serial_mtx.lock();
        uLCD.locate(1, 1);
        uLCD.color(WHITE);
        uLCD.text_width(2);
        uLCD.text_height(3);
        time_t seconds = time(NULL);
        char timeBuffer[32];
        strftime(timeBuffer, 32, "%I:%M %p\r\n", localtime(&seconds));
        uLCD.printf("%s", timeBuffer);
        serial_mtx.unlock();
        Thread::wait(700);
    }
}

//Setup mode where user enters their gender, age, weight, and daily step goal
void setup_screen(void) {
    while(1) {
        serial_mtx.lock();
        //when chnage in screen, clear the LCD screen
        if (oldScreen != screen) {
            uLCD.filled_rectangle(0,0, 128, 128, BLACK);
            oldScreen++;
        }
        switch(screen) {
            //Checks if user wants to enter new configuration or use previous one
            case 1: 
                uLCD.locate(3, 1);
                uLCD.text_width(1);
                uLCD.text_height(1);
                uLCD.puts("Use Previous");
                uLCD.locate(2, 3);
                uLCD.puts("Configuration?");
                uLCD.text_width(3);
                uLCD.text_height(3);
                uLCD.locate(1, 3);
                uLCD.putc('Y');
                uLCD.locate(4, 3);
                uLCD.putc('N');
                if(pot.read() > 0.5) {
                    usePrev = false;
                    uLCD.rectangle(13, 60, 48, 100, BLACK);
                    uLCD.rectangle(75, 60, 110, 100, GREEN);
                } else {
                    usePrev = true;
                    uLCD.rectangle(75, 60, 110, 100, BLACK);
                    uLCD.rectangle(13, 60, 48, 100, GREEN);
                }
                break;
            case 2:
                //Gender
                uLCD.locate(2, 1);
                uLCD.text_width(2);
                uLCD.text_height(2);
                uLCD.puts("Gender");
                uLCD.text_width(3);
                uLCD.text_height(3);
                uLCD.locate(1, 3);
                uLCD.putc('M');
                uLCD.locate(4, 3);
                uLCD.putc('F');
                if(pot.read() > 0.5) {
                    gender = 0;
                    uLCD.rectangle(13, 60, 48, 100, BLACK);
                    uLCD.rectangle(75, 60, 110, 100, GREEN);
                } else {
                    gender = 1;
                    uLCD.rectangle(75, 60, 110, 100, BLACK);
                    uLCD.rectangle(13, 60, 48, 100, GREEN);
                }
                break;
            case 3:
                //Weight
                uLCD.color(WHITE);
                uLCD.locate(9, 14);
                uLCD.text_width(1);
                uLCD.text_height(1);
                uLCD.puts("lbs");
                uLCD.locate(2, 1);
                uLCD.text_width(2);
                uLCD.text_height(2);
                uLCD.puts("Weight");
                weight = (90 + pot.read() * 210);
                char weight_string[3];
                if(weight < 100) {
                    sprintf(weight_string, " %d", weight);
                } else {
                    sprintf(weight_string, "%d", weight);
                }
                weight = 0.45*weight;
                uLCD.text_width(3);
                uLCD.text_height(3);
                uLCD.locate(2, 3);
                uLCD.color(GREEN);
                uLCD.puts(weight_string);
                uLCD.line(35, 100, 110, 100, WHITE);
                break;
            case 4:
                //Age
                uLCD.color(WHITE);
                uLCD.locate(3, 1);
                uLCD.text_width(2);
                uLCD.text_height(2);
                uLCD.puts("Age");
                age = (int) (10 + pot.read() * 89);
                char age_string[2];
                sprintf(age_string, "%d", age);
                uLCD.text_width(3);
                uLCD.text_height(3);
                uLCD.locate(2, 3);
                uLCD.color(GREEN);
                uLCD.puts(age_string);
                uLCD.line(40, 100, 90, 100, WHITE);
                break;
            case 5:
                //Step goal
                uLCD.color(WHITE);
                uLCD.locate(2, 2);
                uLCD.text_width(1);
                uLCD.text_height(1);
                uLCD.puts("Daily Step Goal");
                char step_string[5];
                if(pot.read() < 0.2) {
                    stepGoal = 100;
                    sprintf(step_string, "  %d", stepGoal);
                }else if(pot.read() < 0.4) {
                    stepGoal = 1000;
                    sprintf(step_string, " %d", stepGoal);
                }else if(pot.read() < 0.6) {
                    stepGoal = 5000;
                    sprintf(step_string, " %d", stepGoal);
                }else if(pot.read() < 0.8) {
                    stepGoal = 10000;
                    sprintf(step_string, "%d", stepGoal);
                }else {
                    stepGoal = 20000;
                    sprintf(step_string, "%d", stepGoal);
                }
                uLCD.text_width(2);
                uLCD.text_height(2);
                uLCD.locate(2, 4);
                uLCD.color(GREEN);
                uLCD.puts(step_string);
                uLCD.line(30, 85, 100, 85, WHITE);
                break;
        }
        serial_mtx.unlock();
        Thread::wait(100);
    }
}

//Displays live activity data on LCD Display
void update_screen(void) {
    while(1) {
        read_pot();
        serial_mtx.lock();
        //If change in mode, clear screen
        if (oldMode != mode) {
            uLCD.filled_rectangle(0,0, 128, 128, BLACK);
        }
        switch(mode) {
            case 1:
                //Step count
                uLCD.media_init();
                uLCD.set_sector_address(0x0000, 0x0005);
                uLCD.display_image(50, 45);
                uLCD.filled_rectangle(10, 110, 118, 115, BLACK);
                uLCD.locate(3, 11);
                uLCD.text_height(1);
                uLCD.text_width(1);
                uLCD.color(WHITE);
                uLCD.printf("%4d steps",steps);
                uLCD.filled_rectangle(10, 110, 10 + int(steps * (110/stepGoal)), 115, WHITE);
                break;
            case 2:
                // Heart rate
                uLCD.media_init();
                uLCD.set_sector_address(0x0000, 0x000A);
                uLCD.display_image(50, 45);
                uLCD.locate(5, 11);
                uLCD.text_height(1);
                uLCD.text_width(1);
                uLCD.color(WHITE);
                uLCD.printf("%3d BPM", bpm);
                break;
            case 3:
                //Distance
                uLCD.media_init();
                uLCD.set_sector_address(0x0000, 0x000F);
                uLCD.display_image(50, 45);
                uLCD.locate(6, 11);
                uLCD.text_height(1);
                uLCD.text_width(1);
                uLCD.color(WHITE);
                uLCD.printf("%4.2f ft", distance);
                break;
            case 4:
                //Calories
                uLCD.media_init();
                uLCD.set_sector_address(0x0000, 0x0000);
                uLCD.display_image(50, 45);
                uLCD.locate(4, 11);
                uLCD.text_height(1);
                uLCD.text_width(1);
                uLCD.color(WHITE);
                uLCD.printf("%4d cal", (int)calories);
                break;
            case 5:
                //Floors
                uLCD.media_init();
                uLCD.set_sector_address(0x0000, 0x0014);
                uLCD.display_image(50, 45);
                uLCD.locate(4, 11);
                uLCD.text_height(1);
                uLCD.text_width(1);
                uLCD.color(WHITE);
                uLCD.printf("%2d floors", flights);
                break;
        }
        serial_mtx.unlock();
        Thread::wait(100);
    }
}

//Read heart rate sensor and calculates calories burned
void readHR(){
    while(1) {
        bpm = PPG.get_BPM();
        //Formula that calculates calories based off heart rate and user data
        calories = calories + (.0083)*0.239*(gender*(-55.0969+.6309*bpm+.1988*0.453592*weight
                +.2017*age)+(1-gender)*(-20.4022+.4472*bpm-.1263*0.453592*weight+.074*age));

        //Alternate way to calculate distance (likely more accurate)
        //distance = distance + (steps - oldSteps)* stride_length;
        //oldSteps = steps;
        Thread::wait(500);
    }
}

//Reads barometer and detects floors climbed
void readBarometer()
{
    while(1) {
        pressure = scp1000.readPressure();
        if(count >= 0) count--;
        unsigned long dif;
        if(pressure < p_buff[0]) {
            dif = p_buff[0] - pressure;
        } else {
            dif = 0;
        }
        if(pressure != 0 && p_buff[0] != 0 && dif > 40 && dif < 60 && count < 0) {
            flights++;
            count = 2;
        }
        p_buff[0] = p_buff[1];
        p_buff[1] = p_buff[2];
        p_buff[2] = p_buff[3];
        p_buff[3] = pressure;
        Thread::wait(2000);
    }
}

//Read GPS to get current longitude and latitude and also calculate distance traveled
void readGPS(){
    float old_lat = 0;
    float old_lon = 0;
    while(1) {
        serial_mtx.lock();
        if(gps.connected()) {
            if(gps.sample()) {
                if(gps.ns == 'S') {
                    longitude = gps.longitude*PI/180;
                } else {
                    longitude = -gps.longitude*PI/180;
                }
                if(gps.ew == 'W') {
                    latitude = gps.latitude*PI/180;
                } else {
                    latitude = -gps.latitude*PI/180;
                }
                if(latitude != 0 && longitude != 0 && old_lat != 0 && old_lon != 0) {
                    float a = sinf(old_lat)*sinf(latitude)+cosf(old_lat)*cosf(latitude)*cosf(longitude-old_lon);
                    if(a > 1) a = 1;
                    distance = distance + (.75*acosf(a));
                }
                old_lat = latitude;
                old_lon = longitude;
                //pc.printf("%f, %f, %f\r\n", latitude, longitude, distance);
            }
        }
        serial_mtx.unlock();
        Thread::wait(10000);
    }
}

//Saves current activity data to data.txt
//Data saved in the format date, steps, flights, calories, and distance separated by tabs
//Prev_data.txt stores the daily activity data over the past week
void save_data() {
    // Save the data to the usb flash drive and print to the terminal
    usb_mtx.lock();
    two = 1;
    FILE *fp = fopen( "/msc/data.txt", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    time_t seconds = time(NULL);
    char temp_date[32];
    strftime(temp_date, 32, "%m/%d/%y", localtime(&seconds));
    
    //If the date has changed, indicating new day, then append current data to prev_data.txt
    //Reset all activity data to zero
    if (strcmp(temp_date, date) != 0) {
        FILE *fp2 = fopen("/msc/prev_data.txt", "a");
        if(fp2 == NULL) {
            error("Could not open file for append\n");
        }
        //pc.printf("Writing to prev_data.txt\r\n");
        fprintf(fp2, "%s\t%d\t%d\t%0.2f\t%0.2f\n", date, steps, flights, calories, distance);
        fclose(fp2);
        strcpy(date, temp_date);
        steps = 0;
        flights = 0;
        calories = 0;
        distance = 0;
    } else if (strcmp(temp_date, date) == 0) {
        fprintf(fp, "%s\t%d\t%d\t%0.2f\t%0.2f\n", date, steps, flights, calories, distance);
    }
    fclose(fp);
    usb_mtx.unlock();
}

//Waits for command from GUI and once command received, transfer all the data stored
//in prev_data.txt to the GUI which will be plotted
void serial_USB() {
    float buf_distance[2], buf_calories[2];
    int buf_steps[2],buf_flights[2];
    char buf_date[100];
    char buffer[100];
    while (1) {
        usb_mtx.lock();
        if (pc.readable()) {
            if (pc.getc() =='!') {
                if(pc.getc() == 'p') {
                    FILE *fp = fopen( "/msc/prev_data.txt", "r");
                    if(fp == NULL) {
                        error("Could not open file for write\n");
                    }
                    while (fscanf(fp,"%s\t%d\t%d\t%f\t%f\n\r", &buf_date[0], &buf_steps[0], &buf_flights[0], &buf_calories[0], &buf_distance[0]) != EOF) {
                        //fscanf(fp,"%s\t%d\t%d\t%f\t%f\n\r", &buf_date[0], &buf_steps[0], &buf_flights[0], &buf_calories[0], &buf_distance[0]);
                        sprintf(buffer,"%s\t%d\t%d\t%f\t%f\n", buf_date, buf_steps[0], buf_flights[0], buf_calories[0], buf_distance[0]);
                        pc.printf("%s",buffer);
                    }
                    fclose(fp);
                }
            }
        }
        usb_mtx.unlock();
        Thread::yield();
    }
}
    
int main() {
    //Set RTC time
    set_time(1256729737);
    time_t seconds = time(NULL);
    //Keep track of the date mbed Fitbit was first turned on
    strftime(date, 32, "%m/%d/%y", localtime(&seconds));
    
    // Next screen button
    pb.mode(PullUp);
    pb.attach_deasserted(&next);
    pb.setSampleFrequency();
    
    //set up the display
    uLCD.baudrate(3000000);
    uLCD.background_color(BLACK);
    uLCD.cls();
    //Thread to display setup screens
    thread1.start(setup_screen);
    
    while(setup_state) {
        if (readConfig) {
            if (read_config()) {
                setup_state = false;
            } else {
                setup_state = true;
                usePrev = false;
                readConfig = false;
            }
        }
        Thread::yield();
    }
    thread1.terminate();
    save_config();
    
    // Off button
    pb.attach_deasserted(&button);
    
    // set up the display for displaying real time data and clock
    uLCD.cls();
    thread1.start(update_screen);
    thread2.start(display_time);
    
    // LED indicates whether or not data is being collected
    one = 0;
    two = 0;
    three = 0;
    four = 0;
    // Start sensors
    int sample_num = 1;
    PPG.start();
    IMU.begin();
    IMU.calibrate(1);
    float ax, ay, az;
    float mag = 0;
    float buffer[2] = {0};
    float avg_buffer[2] = {0};
    float avg;
    //float max = 1.0;
    //float min = 1.0;
    //float K = 0.55;
    
    //Start threads for sensors and serial command
    thread3.start(readBarometer);
    thread4.start(readGPS);
    thread5.start(readHR);
    thread6.start(serial_USB);

    usb_timer.start();

    while(run) {
        // Read IMU and get acceleration in all three axis
        IMU.readAccel();
        ax = IMU.calcAccel(IMU.ax);
        ay = IMU.calcAccel(IMU.ay);
        az = IMU.calcAccel(IMU.az);
        // Calculate the 3 point moving average of the magnitude of the 
        // acceleration vector
        mag = sqrt((ax*ax) + (ay*ay) + (az*az));
        avg = (buffer[0] + buffer[1] + mag) / 3;
        buffer[0] = buffer[1];
        buffer[1] = mag;
        // Count a step if the difference between the current and previous avg
        // point crosses a threshold
        if(sample_num > 1) {
            float dif1 = avg_buffer[1] - avg_buffer[0];
            float dif2 = avg_buffer[1] - avg;
            float peak_prominence = 0.01;
            if(dif1 > peak_prominence && dif2 > peak_prominence) {
                steps++;
                //Dynamic stride length calculation using max and min values of
                //z acceleration
                //stride_length = K * pow((double)(max - min), 0.25) * 3.28084;
                //max = 1.0;
                //min = 1.0;
            }
        }
        avg_buffer[0] = avg_buffer[1];
        avg_buffer[1] = avg;
        
        //if (az > max) { max = az;}
        //else if (az < min) {min = az;}
        
        sample_num++;
        one = !one; //led indicating if main is still running
        
        //Every minute, save current activity data to data.txt
        if(usb_timer.read() >= 60) {
            save_data();
            usb_timer.stop();
            usb_timer.reset();
            usb_timer.start();
            two = 0;
        }
        Thread::wait(200);
    }
    one = 0;
    //Turn off all threads reading sensors
    thread3.terminate();
    thread4.terminate();
    thread5.terminate();
    
    usb_timer.stop();
    usb_timer.reset();
    
    save_data();
    //For demo
    usb_mtx.lock();
    FILE *fp1 = fopen("/msc/prev_data.txt", "a");
    fprintf(fp1, "%s\t%d\t%d\t%0.2f\t%0.2f\n", date, steps, flights, calories, distance);
    fclose(fp1);
    usb_mtx.unlock();
    return 0;
}