File content as of revision 34:1614f4f2b841:

/********************* CODE INFORMATIE ******************************
Date of creation:           30-09-2017
Authors:                    Danny Eldering & Ricardo Molenaar
co-authors:                 Menno Gravemaker
(c) Copyright by Momo Medical BV.

Current version name:                   2.1.2
Date of modification:                   11-10-2017
Purpose of this file:                   Oneliner of purpose
Update ‘what’s new in this version?’:   short meaningful description (no more than 3 lines)
Todo:
Source file:                            http://mbed.com

*/

/************************ CONFIG ***********************************/

#include "mbed.h"
#include "Adafruit_ADS1015.h"
#include "MPU6050.h"
#include "MPU6050_belt.h"
#include "neopixel.h"
#define NLED (3)                                                            // to do: nled uitschrijven
#define ONE_COLOR

InterruptIn lock(p15);                                                      // Interrupts for buttons. todo: button toevoegen
InterruptIn reposition(p17);
InterruptIn mute(p16);
InterruptIn new_patient(p18);

DigitalOut  LED_intern1(LED1);                                              // todo: intern veranderen in on_dev_board oid
DigitalOut  LED_intern2(LED2);
DigitalOut  LED_intern3(LED3);
DigitalOut  LED_intern4(LED4);
neopixel::PixelArray array(p11);                                            // todo: array -> wat ermee gedaan wordt

Timer lock_hold_timer;
Timer calibration_hold_timer;
Timer delay;                                                                // todo: delay -> delay_between_2_buttons_pressed
Timer speaker_timer;
Timer test_timer;                                                           // todo: naar testsectie

DigitalOut speaker1(p21);
DigitalOut speaker2(p22);
PwmOut lock_LED(p23);                                                       // todo: button toevoegen bijv. feedback_LED_lock
PwmOut reposition_LED(p25);
PwmOut mute_LED(p26);
PwmOut new_patient_LED(p24);
// todo: comments for library hardcoding etc.
I2C i2c(p28, p27);                  // I2C   todo: i2c_sensorplaat
I2C i2cAccu(p9, p10);               // I2C for accupack todo: i2c_voltage_measurement
MPU6050 agu(p28,p27);               // Accelerometer/Gyroscope Unit
MPU6050_belt agu_belt(p28,p27);     // Accelerometer/Gyroscope Unit Belt
Adafruit_ADS1115 pr1(&i2c, 0x48);   // first PiëzoResistive ADC
Adafruit_ADS1115 pr2(&i2c, 0x49);   // second PiëzoResistive ADC
Adafruit_ADS1115 pel(&i2c, 0x4B);   // PiëzoElectric ADC
Adafruit_ADS1115 adsAccu(&i2cAccu, 0x48);
Serial pc(USBTX, USBRX); // tx, rx  // Serial USB connection todo -> test en uart toevoegen ook bij pi
Serial pi(p13, p14);     // tx, rx  // Setup serial communication for pi.
Timer t;                            // Timer for equally time-spaced samples
Ticker sample_cycle;                // Polling cycle

// todo: uitschrijven namen

int boot_delay_ms = 500;
int cycle_time = 100000;            // Cycle time in us
int i2c_freq = 400000;              // I2C Frequency
int baud = 115200;                  // Baud rate
short res[8] = {0,0,0,0,0,0,0,0};   // 8 PR sensors 1 time per cycle
short elec[5] = {0,0,0,0,0};        // 1 PE sensor 5 times per cycle
int angle = 0;                      // Accelerometer Z-axis
int k = 0;
float acce[3];                      // Raw accelerometer data
float gyro[3];                      // Raw gyroscope data
float acce_belt[3];                 // Raw accelerometer data from belt
float gyro_belt[3];                 // Raw gyroscope data from belt
char LED_colour = 'g';                                                            // Variable to set LED colour.
bool lock_state = 0, lock_flag = 0, mute_state = 0, alarm = 0, calibration_flag = 0, intensity_select = 1;            // Boolean variables for states logging.
bool mute_flag = 0, new_patient_flag = 0, reposition_flag = 0;
bool speaker_state = 0, LED_red_state = 0, LED_yellow_state = 0, LED_green_state = 0, power_plug_state = 0;
bool speaker_logged = 0, LED_red_logged = 0, LED_yellow_logged = 0, LED_green_logged = 0, power_plug_logged = 0;
int locktime_ms = 2000;                                                     // Waittime for lock user interface in ms.
int calibrationtime_ms = 5000;                                              // Time to press new_patient button for calibration system.
int calibration_flash;                                                      // Variable for flash LED's to indicate calibration.
int buttondelay_ms = 750;                                                   // Button delay in ms.
int delay_lock_interface = 3000*60;                                         // Delay for non using interface locktime.
int speaker_active_ms = 750;                                                // Time to iterate speaker on and off when alarm occurs.
int alarm_voltage = 5867;                                                   // Needed voltage for alarm expressed as a digital 15 bit value (=20% of max battery voltage)
int red_var, green_var, blue_var;                                           // Variables to set LED intensity.
short batteryvoltage_current = 0, batteryvoltage_last = 0, powervoltage_current, powervoltage_last;                  // Variables to manage batteryvoltage.
int intensity_day = 40, intensity_night = 10;                               // Intensity settings for LED's to wall.
double intensity, control_LED_intensity = 0;                                // Variable between 0 and 1 to set the intensity of the LED's above the buttons.
int a; // Test

/*************************** TEST ********************************/
// Verify algoritm function: for belt activation, set test_belt 1 (connect pin p20 to 3.3V)
DigitalIn test_pin(p30, PullDown);
bool test_belt = 1; //test_pin;

// Verify if interrupts are working properly


// Verify if parameters are
bool test_mode = 0;


/*************************** CODE ********************************/

void set_intensity()                                                        // Function to set the intensity for the LED's.
{
    if (intensity_select == 1) {
        intensity = intensity_day;
    } else {
        intensity = intensity_night;
    }
    control_LED_intensity = (intensity/100);

    if (test_mode == 1) {
        pc.printf("Intensity LED's shines to wall = %f\n", intensity);
        pc.printf("Intensity LED's above buttons = %f\n", control_LED_intensity);
    }
}

void serial_read()                                                          // Serial read for select LED intensity and colour.
{
    if (pi.readable()) {
        char message[10];
        pi.scanf("%s", message);
        pc.printf("Message = %s, Intensity_select = %d en LED_colour = %c\n", message, intensity_select, LED_colour);
        
        if (intensity_select != (message[0]-'0')) {
            intensity_select = (message[0]-'0');
        }

        if (LED_colour != message[1]) {
            LED_colour = message[1];
        }

        pc.printf("Intensity_select = %d en LED_colour = %c\n", intensity_select, LED_colour);

        if (test_mode == 1) {
            pc.printf("message: %s\n", message);
        }
    }
}

void serial_log()
{
    if (mute_flag == 1) {
        pi.printf(">01\n");

        if (test_mode == 1) {
            pc.printf(">01\n");
        }

        mute_flag = 0;
    }

    if (new_patient_flag == 1) {
        pi.printf(">03\n");

        if (test_mode == 1) {
            pc.printf(">03\n");
        }

        new_patient_flag = 0;
    }

    if (reposition_flag == 1) {
        pi.printf(">02\n");

        if (test_mode == 1) {
            pc.printf(">02\n");
        }

        reposition_flag = 0;
    }

    if (batteryvoltage_current != batteryvoltage_last) {
        pi.printf("%%" "%d\n", batteryvoltage_current);

        if (test_mode == 1) {
            pc.printf("%%" "%d\n", batteryvoltage_current);
        }

        batteryvoltage_last = batteryvoltage_current;
    }

    if (LED_red_logged != LED_red_state) {
        if (LED_red_state == 1) {
            pi.printf("&04\n");
            LED_red_logged = LED_red_state;
            if (test_mode == 1) {
                pc.printf("&04\n");
            }
        }
        if (LED_red_state == 0) {
            pi.printf("&40\n");
            LED_red_logged = LED_red_state;
            if (test_mode == 1) {
                pc.printf("&40\n");
            }
        }
    }

    if (LED_yellow_logged != LED_yellow_state) {
        if (LED_yellow_state == 1) {
            pi.printf("&06\n");
            LED_yellow_logged = LED_yellow_state;
            if (test_mode == 1) {
                pc.printf("&06\n");
            }
        }
        if (LED_yellow_state == 0) {
            pi.printf("&60\n");
            LED_yellow_logged = LED_yellow_state;
            if (test_mode == 1) {
                pc.printf("&60\n");
            }
        }
    }

    if (LED_green_logged != LED_green_state) {
        if (LED_green_state == 1) {
            pi.printf("&05\n");
            LED_green_logged = LED_green_state;
            if (test_mode == 1) {
                pc.printf("&05\n");
            }
        }
        if (LED_green_state == 0) {
            pi.printf("&50\n");
            LED_green_logged = LED_green_state;
            if (test_mode == 1) {
                pc.printf("&50\n");
            }
        }
    }

    if (speaker_logged != speaker_state) {
        if (speaker_state == 1) {
            pi.printf("&07\n");
            speaker_logged = speaker_state;
            if (test_mode == 1) {
                pc.printf("&07\n");
            }
        }
        if (speaker_state == 0) {
            pi.printf("&70\n");
            speaker_logged = speaker_state;
            if (test_mode == 1) {
                pc.printf("&70\n");
            }
        }
    }

    if (power_plug_logged != power_plug_state) {
        if (power_plug_state == 1) {
            pi.printf("#08\n");
            if (test_mode == 1) {
                pc.printf("#08\n");
            }
            power_plug_logged = power_plug_state;
        }
        if (power_plug_state == 0) {
            pi.printf("#80\n");
            if (test_mode == 1) {
                pc.printf("#80\n");
            }
            power_plug_logged = power_plug_state;
        }
    }
    if (a == 1) {
        //receiving order: 8 resistive sensors, 5 electric readings
        pi.printf("!,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n", res[4], res[7], res[6], res[5], res[1], res[0], res[2], res[3], elec[0], elec[1], elec[2], elec[3], elec[4]); // print all to serial port
        if (test_mode == 1) {
            pc.printf("!,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n", res[4], res[7], res[6], res[5], res[1], res[0], res[2], res[3], elec[0], elec[1], elec[2], elec[3], elec[4]); // print all to serial port
        }
    }

}

void colour_select(char LED_colour)                                         // Function to select the colour.
{
    set_intensity();                                                        // Call function set_intensity
    
    if ((LED_colour == 'r') || (LED_colour == 'g') || (LED_colour == 'b') || (LED_colour == 'y')) { 
        red_var = 0;
        green_var = 0;
        blue_var = 0;
        
        if (LED_colour == 'r') {
            red_var = (2.55*intensity);
            green_var = 0;
            blue_var = 0;
            LED_red_state = 1;
        } else {
            LED_red_state = 0;
        }
    
        if (LED_colour == 'y') {
            red_var = (2.55*intensity);
            green_var = (2.55*intensity);
            blue_var = 0;
            LED_yellow_state = 1;
        } else {
            LED_green_state = 0;
        }
    
        if (LED_colour == 'g') {
            red_var = 0;
            green_var = (2.55*intensity);
            blue_var = 0;
            LED_green_state = 1;
        } else {
            LED_green_state = 0;
        }
    
        if (LED_colour == 'b') {
            red_var = 0;
            green_var = 0;
            blue_var = (2.55*intensity);
        }
    } else {
        LED_colour = LED_colour;
    }

    if (calibration_flash >= 1) {
        if ((calibration_flash % 2) == 0) {
            red_var = 255;
            green_var = 255;
            blue_var = 255;
            LED_intern4 = 1;
        } else {
            red_var = 0;
            green_var = 0;
            blue_var = 0;
            LED_intern4 = 0;
        }
        calibration_flash--;
    }
}

void trigger_lock()                                                         // If rising edge lock button is detected start locktimer.
{
    if (test_mode == 1) {
        pc.printf("Lock triggered.\n");
    }
    lock_hold_timer.reset();
    lock_hold_timer.start();
    delay.reset();
    delay.start();
}

void timer_lock()                                                           // End timer lock.
{
    if (test_mode == 1) {
        pc.printf("Lock released.\n");
    }
    lock_flag = 0;                                                          // Set lock_flag off.
    lock_hold_timer.stop();                                                 // Stop and reset holdtimer
    lock_hold_timer.reset();
}

void trigger_reposition()
{
    if (lock_state == 1 | (delay.read_ms() < buttondelay_ms)) {             // Control statement for lock interface and delay for non using buttons at the same time.
    } else {
        delay.reset();
        delay.start();
        if (test_mode == 1) {
            pc.printf("Reposition triggered.\n");
            LED_intern1 = !LED_intern1;
        }
        reposition_flag = 1;

        reposition_LED = control_LED_intensity;
    }
}

void rise_reposition()
{
    if (test_mode == 1) {
        pc.printf("Reposition released.\n");
    }
    reposition_LED = 0;

}

void trigger_mute()
{

    if (lock_state == 1 | (delay.read_ms() < buttondelay_ms)) {             // Control statement for lock interface and delay for non using buttons at the same time.
    } else {
        delay.reset();
        delay.start();
        mute_state = !mute_state;
        if (mute_state == 1) {
            mute_LED = control_LED_intensity;
        } else {
            mute_LED = 0;
        }
        if (test_mode == 1) {
            pc.printf("Mute triggered %d.\n",mute_state);
            LED_intern1 = !LED_intern1;
        }

        mute_flag = 1;
    }
}

void trigger_new_patient()                                                  // Function to trigger hold timer for new patient calibration function.
{

    if (lock_state == 1 | (delay.read_ms() < buttondelay_ms)) {
    } else {
        calibration_hold_timer.reset();
        calibration_hold_timer.start();
        new_patient_LED = control_LED_intensity;;
        if (test_mode == 1) {
            pc.printf("New patient triggered.\n");
        }
    }
}

void timer_calibration()                                                    // Timer calibration function.
{
    if (test_mode == 1) {
        pc.printf("New patient released.\n");
    }
    new_patient_LED = 0;

    if (0 < calibration_hold_timer.read_ms() < calibrationtime_ms) {
        new_patient_flag = 1;
    }

    calibration_hold_timer.stop();
    calibration_hold_timer.reset();

    if (lock_state == 1 | (delay.read_ms() < buttondelay_ms)) {             // Control statement for lock interface and delay for non using buttons at the same time.
    } else {
        if (calibration_flag == 0) {

            if (LED_intern1 == 0) {
                LED_intern1 = 1;
            } else {
                LED_intern1 = 0;
            }

        } else {
            calibration_flag = 0;
        }
    }
}

void timer_functions()
{
    if ((lock_hold_timer.read_ms() > locktime_ms) && lock_flag == 0 && lock == 0) { // If statement for lock function.
        lock_flag = 1;
        LED_intern2 = !LED_intern2;
        lock_state = !lock_state;
        if (lock_state == 0) {
            lock_LED = control_LED_intensity;
        } else {
            lock_LED = 0;
        }
    }

    if ((calibration_hold_timer.read_ms() > calibrationtime_ms) && calibration_flag == 0 && new_patient == 0 && lock_state == 0) { // If statement for calibration system.
        calibration_flag = 1;
        calibration_flash = 11;
        if (test_mode == 1) {
            pc.printf("Calibrate triggered.\n");
        }
        pi.printf(">30\n");                                                 // Print statement for serial communication to inform algorithm to calibrate.
    }

    if (delay.read_ms() > delay_lock_interface) {                           // If buttons are not pressed for 3 minutes, set lock active.
        lock_state = 1;
        LED_intern2 = 1;
        lock_LED = 0;
    }
}

void generate(neopixel::Pixel * out, uint32_t index, uintptr_t val)         // Generate LED colour.
{
    out->red   = red_var;
    out->green = green_var;
    out->blue  = blue_var;
}

void set_ui_LED()                                                           // Control functions for LED above buttons (added because of failures).
{
    if (lock_state == 1) {
    } else {
        if (reposition == 0) {
            reposition_LED = control_LED_intensity;
        } else {
            reposition_LED = 0;
        }

        if (new_patient == 0) {
            new_patient_LED = control_LED_intensity;
        } else {
            new_patient_LED = 0;
        }
    }
}

void read_voltage()
{
    if (power_plug_state == 1) {                                            // If supplyvoltage (readed from input) is greater then the setted alarmvoltage.
        alarm = 0;                                                          // Alarm is off.
        speaker_state = 0;
    } else {
        alarm = 1;                                                          // Else alarm is on.
        speaker_state = 1;
    }


    if (alarm == 1 && mute_state == 1 && (batteryvoltage_current > alarm_voltage)) {                                  // Set speaker on for 750 ms.
        speaker1 = 0;                                                       // Set speaker.
        speaker2 = 0;
    }

    if ((alarm == 1 && mute_state == 0 && (speaker_timer.read_ms() < speaker_active_ms)) || ((batteryvoltage_current < alarm_voltage) && (speaker_timer.read_ms() < speaker_active_ms) && power_plug_state == 0)) { // Set speaker on for 750 ms.
        speaker1 = 1;                                                       // Set speaker.
        speaker2 = 1;
        speaker_timer.start();                                              // Set timer for speaker to iterate on and off.
    }

    if ((speaker_timer.read_ms() > speaker_active_ms) && (speaker_timer.read_ms() < (speaker_active_ms*2))) {
        speaker1 = 0;                                                       // Turn off speaker (use two outputs because of currentlimiting of one).
        speaker2 = 0;
    }

    if (speaker_timer.read_ms() > (speaker_active_ms*2)) {
        speaker_timer.stop();                                               // Stop speaker timer.
        speaker_timer.reset();
    }

    batteryvoltage_current = adsAccu.readADC_SingleEnded(0);                // Read channel 0 from external ADC.
    powervoltage_current = adsAccu.readADC_SingleEnded(1);                  // Read channel 1 from external ADC.

    if (powervoltage_current < 20000) {
        power_plug_state = 0;
        LED_colour = 'b';
    } else {
        power_plug_state = 1;
    }
}

void read_adc()
{
    t.reset();
    t.start();
    a = agu.testConnection();/*
        pc.printf("a= %d\n",a);
    if( a==0)
    {
        lock_state = 1;
        LED_intern2 = 1;
        lock_LED = 0;
        }*/
    if (a == 1) {
        elec[0] = pel.readADC_SingleEnded(0);                               // First PE readout

        for (k = 0; k < 4; k = k + 1) {
            res[k] =    pr1.readADC_SingleEnded(k);                         // First 4 PR readout
        }
        while(t.read_us()<(1*(cycle_time/5))) {}                            // Wait untill 20% of cycle

        elec[1] = pel.readADC_SingleEnded(0);                               // Second PE readout

        for (k = 0; k < 4; k = k + 1) {
            res[k+4] =  pr2.readADC_SingleEnded(k);                         // Last 4 PR readout
        }
        while(t.read_us()<(2*(cycle_time/5))) {}                            // Wait untill 40% of cycle

        elec[2] = pel.readADC_SingleEnded(0);                               // Third PE readout

        agu.getAccelero(acce);                                              // Get accelerometer data
        angle = acce[2]*100;
        if(angle == 0) {
            MPU6050 agu(p28,p27);
            agu.getAccelero(acce);
            angle = acce[2]*100;
        }
        agu.getGyro(gyro);                                                  // Get gyroscope data

        if (test_belt == 1) {
            agu_belt.getGyro(gyro_belt);                                        // Get gyroscope data from Belt
            agu_belt.getAccelero(acce_belt);                                    // Get accelerometer data from belt
        }
        pi.printf("?,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,\n", acce[0], acce[1], acce[2], gyro[0], gyro[1], gyro[2],acce_belt[0], acce_belt[1], acce_belt[2], gyro_belt[0], gyro_belt[1], gyro_belt[2]);

        while(t.read_us()<(3*(cycle_time/5))) {}                            // Wait untill 60% of cycle

        elec[3] = pel.readADC_SingleEnded(0);                               // Fourth PE readout
    }

    timer_functions();

    batteryvoltage_current = batteryvoltage_last;
    powervoltage_current = powervoltage_last;
    read_voltage();                                                         // Supplyvoltage control for alarm.
    if (test_mode == 1) {
        pc.printf("Voltage = %d   ,   %d\n", batteryvoltage_current, powervoltage_current);
    }
    uint32_t val = 0;
    colour_select(LED_colour);
    array.update(generate, NLED, val);
    set_ui_LED();

    while(t.read_us()<(4*(cycle_time/5))) {}                                // Wait untill 80% of cycle

//    pc.printf("2e = %d\n",agu.testConnection());
    if (a == 1) {
        elec[4] = pel.readADC_SingleEnded(0);                               // Fifth PE readout
    }

    while(t.read_us()<(4.25*(cycle_time/5))) {}                             // Wait untill 85% of cycle

    serial_read();
    serial_log();
    if (test_mode ==1) {
        pc.printf("Loop time: %d ms\n",t.read_ms());
    }
    if (test_pin == 1) {
        test_mode = 1;
        pc.printf("%d\n",test_mode);
    }
    if (test_pin == 0) {
        test_mode = 0;
        pc.printf("%d\n",test_mode);
    }
    if (test_mode == 1) {
        //cycle_time = 500000;
    } else {
        cycle_time = 100000;
    }
    pc.printf("Loop time: %d ms\n",t.read_ms());
}

int main()
{
    wait_ms(boot_delay_ms);                                                 // Wait to boot sensorplate first
    i2c.frequency(i2c_freq);
    i2cAccu.frequency(i2c_freq);
    pc.baud(baud);
    pi.baud(baud);
    pr1.setGain(GAIN_TWOTHIRDS); // set range to +/-6.144V
    pr2.setGain(GAIN_TWOTHIRDS); // set range to +/-6.144V
    pel.setGain(GAIN_TWOTHIRDS); // set range to +/-6.144V
    adsAccu.setGain(GAIN_TWOTHIRDS); // set range to +/-6.144V
    pi.format(8, SerialBase::None, 1);

    lock.fall(&trigger_lock);                                               // Interrupt for rising edge lock button.
    lock.rise(&timer_lock);
    reposition.fall(&trigger_reposition);
    reposition.rise(&rise_reposition);
    mute.fall(&trigger_mute);
    new_patient.fall(&trigger_new_patient);                                 // New patient/calibration button rising event.
    new_patient.rise(&timer_calibration);                                   // Falling edge for calibration algorithm option.
    delay.reset();                                                          // Delaytimer reset en start.
    delay.start();

    set_intensity();
    lock_LED = control_LED_intensity;                                       // Lock LED initialization.


    sample_cycle.attach_us(&read_adc, cycle_time);

    while (1) {
        wait_us(cycle_time+1); // wait indefinitely because the ticker restarts every 50 ms
    }
}