File content as of revision 55:b74e7076d7a0:

/********************* CODE INFORMATON ******************************
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.4
Date of modification:                   8-1-2018
Purpose of this file:                   Code for LPC1768 microcontroller for controlling buttons, LED's and communicate to PI
Update ‘what’s new in this version?’:   Sensorplate connection test changed from software to hardware check.
                                        ADC readout for accu deleted.
Todo:                                   -> Fix LED issue (yellow and red flashes at random moments);
                                        -> Optimize functions / improve readability;
                                        -> Split functions in seperate files?;
                                        -> Fix when sensorplate is not connected;
                                        -> Rule 570: if statement change to turn off LED's when power is plugged out (also related to rule 106).
                                        -> For the speaker two outputs of the uC are used. Add MOSFET with external supply and control these by uC?
Source file:                            http://mbed.com/

Information files:
(1) Flowchart:
(2) Table serial communication:         https://docs.google.com/spreadsheets/d/1kHlithHxtoMDGvbcdH8vwSw5W5ArxlwDPsyfra1dtQM/edit?usp=drive_web
(3) Technical manual CU-/software:
*/

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

#include "mbed.h"                                                                   // Include files and define parameters.
#include "Adafruit_ADS1015.h"
#include "MPU6050.h"
#include "MPU6050_belt.h"
#include "PixelArray.h"
#include "WS2812.h"

#define WS2812_BUF 3
#define NUM_COLORS 5
#define NUM_LEDS_PER_COLOR 3
#define NUMBER_LED_FRONT (3)
#define ONE_COLOR

InterruptIn button_lock(PC_1);                                                       // Input on intterupt base decleration.
InterruptIn button_reposition(PC_3);
InterruptIn button_mute(PC_2);
InterruptIn button_new_patient(PC_0);

DigitalIn intensity_code(PA_12);
DigitalIn colour_code_1(PA_11);
DigitalIn colour_code_0(PB_12);
DigitalIn testpin_sensorplate(PC_6);

DigitalOut  LED_on_dev_board1(LED1);                                                // Decleration of digital outputs.
DigitalOut  LED_on_dev_board2(LED2);
DigitalOut  LED_on_dev_board3(LED3);
DigitalOut  LED_on_dev_board4(LED4);
DigitalOut speaker1(PC_12);               // relatie aangeven!
//neopixel::PixelArray indicator_LEDs(PA_7);
PixelArray px(WS2812_BUF);
//WS2812 ws(PA_7, WS2812_BUF, 3, 9, 9, 6);
WS2812 ws(PA_7, WS2812_BUF, 3, 9, 9, 6);

PwmOut lock_feedback_LED(PB_13);        //(PB_1);                                                      // Declaration of pulse with modulation outputs.
PwmOut mute_feedback_LED(PB_14);        //(PB_15);
PwmOut new_patient_feedback_LED(PB_15); //(PB_14);
PwmOut reposition_feedback_LED(PB_1);   //(PB_13);

Timer button_lock_hold_timer;                                                       // Timer for time lock button should be pressed.
Timer button_calibration_hold_timer;                                                // Timer for calibration function (new patient holding 5 seconds).
Timer delay_between_button_pressed;                                                 // Timer for time between two buttons (to prevent pressing buttons simultaneously).
Timer speaker_timer;                                                                // Timer for speaker activation.
Timer piezo_electric_sample_timer;                                                  // Timer for equally time-spaced samples.
Timer Knight_Rider_Timer;
/*
The code underneath this commentbox has some fixed parameters for serial/ADC reading:
-> The address for the angle_device_reference_belt is set to 0x68 in the file MPU6050_belt (rule number: 19);
-> The adress for the angle_device_sensorplate is set to 0x69 in the file MPU6050.h (rule number: 19);
-> This is because of using the same I2C line;
-> For detailed information/questions about this item, please read the technical manual or contact: Ricardo Molenaar | ricardo.molenaar@gmail.com
*/
I2C i2c_sensorplate_adc(PB_9, PB_8);                                                  // I2C for sensorplate.
MPU6050_belt angle_device_sensorplate(PB_9, PB_8);                                          // i2c pins // i2c address hardcoded 0x68.
MPU6050_belt angle_device_reference_belt(PB_9, PB_8);                                  // i2c pins // i2c address hardcoded 0x69.
Adafruit_ADS1115 piezo_resistive_adc1(&i2c_sensorplate_adc, 0x48);                  // i2c pins, i2c address.
Adafruit_ADS1115 piezo_resistive_adc2(&i2c_sensorplate_adc, 0x49);                  // i2c pins, i2c address.
Adafruit_ADS1115 piezo_electric_adc(&i2c_sensorplate_adc, 0x4B);                    // i2c pins, i2c address.
Serial usb_serial(SERIAL_TX, SERIAL_RX);                                            // tx, rx
Serial pi_serial(PC_10, PC_11);                                                     // tx, rx
Ticker total_readout_cycle;                                                         // Polling cycle.
// End of commentbox related to the serial configuration/ADC reading components.

int boot_delay_ms = 500;
int total_readout_cycle_time_us = 100000;                                           // Cycle time in us.
int total_knight_rider_cycle_time_ms = 1000;
int i2c__frequency = 400000;                                                              // I2C Frequency.
int baud_rate = 115200;                                                             // Baud rate.
short piezo_resistive_array[8] = {0,0,0,0,0,0,0,0};                                 // 8 PR sensors 1 time per cycle.
short piezo_electric_array[5] = {0,0,0,0,0};                                        // 1 PE sensor 5 times per cycle.
int angle = 0;                                                                      // Accelerometer Z-axis.
float accelerometer_sensorplate[3] = {0.0, 0.0, 0.0};                               // Raw accelerometer data.
float gyroscope_sensorplate[3];                                                     // Raw gyroscope data.
float accelerometer_reference_belt[3];                                              // Raw accelerometer data from belt.
float gyroscope_reference_belt[3];                                                  // Raw gyroscope data from belt.
int colourbuf[NUM_COLORS] = {0xff0000,0x00ff00,0x0000ff,0xffa500,0xffffff};         // hex codes for the different colours
char LED_colour = 'w';                                                              // Variable to set LED colour (standard set to green, untill PI sends other character). Other possible colours: red ('r') & yellow ('y').
bool lock_state = false, lock_flag = 0, mute_state = 0, alarm = 0, calibration_flag = 0, intensity_select = 1;            // Boolean variables for logging states.
bool mute_flag = 0, new_patient_flag = 0, reposition_flag = 0, new_patient_lock_flag = 0, reposition_lock_flag = 0, mute_lock_flag;                      // Flag variables.
bool speaker_state = 0, LED_red_state = 0, LED_yellow_state = 0, LED_green_state = 0, power_plug_state = 0;
bool auto_lock_led_logged = 0, lock_is_logged = 0, speaker_logged = 0, LED_red_logged = 0, LED_yellow_logged = 0, LED_green_logged = 0, power_plug_logged = 0; // is toevoegen
int locktime_ms = 2000;                                                             // Waittime for lock user interface in ms.
int calibrationtime_ms = 2000;                                                      // Time to press new_patient button for calibration system.
int calibration_flash = 0;                                                          // Variable for flash LED's to indicate calibration.
int lock_flash = 0;
int buttondelay_ms = 250;                                                           // Button delay in ms. Default: 750
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 = 2400;                                                           // Needed voltage for alarm expressed as a digital 15 bit value (= 20% of max battery voltage).
int LED_red_intensity = 0, LED_blue_intensity = 0, LED_green_intensity = 0;         // Variables to set LED intensity.
//short batteryvoltage_current = 0, batteryvoltage_last = 0, powervoltage_current, powervoltage_last;                  // Variables to manage batteryvoltage. Maybe change current to other?
//const int digital_value_ADC_powervoltage_unplugged = 15000;                         // Digital value to set the indicating LEDs to wall blue (should be set off later). const in hoofdletters
int intensity_day = 20, intensity_night = 5;                                       // Intensity settings for LED's to wall.
double intensity = 0.0, control_LED_intensity = 0.0;                                // Variable between 0 and 1 to set the intensity of the LED's above the buttons. Intensity change to smart name!
int colour_code = 0b00;
bool pi_active = false;
/*************************** TEST ********************************/
// Verify algoritm function: for belt activation, set test_belt 1 (connect pin p20 to 3.3V).
Timer test_timer;
DigitalIn test_pin(PA_11, PullDown);

// Variable to set if belt is used to test algorithm:
bool test_belt = 0;

// Set test mode on (log functions to pc serial: interrupts, LED intensity and serial messages):
bool test_mode = 1;

// Variable for connection test (should be changed):
bool connection_test_sensorplate;

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

void set_intensity_LEDs()                                                           // 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) {                                                           // If statement for test purposal LED_intensity values. if def gebruiken voor testmode
//        usb_serial.printf("Intensity LED's shines to wall = %f\n", intensity);
//        usb_serial.printf("Intensity LED's above buttons = %f\n", control_LED_intensity);
    }
}

void serial_read()                                                                  // Function for serial read for select LED intensity and colour.
{
    colour_code = (colour_code_1 << 1 | colour_code_0);
    if(colour_code != 0b00 && pi_active == false) {
        pi_active = true;
    }
    if(pi_active) {
        intensity_select = intensity_code;
        switch(colour_code) {
            case 0b11 :
                LED_colour = 'y';
                break;
            case 0b10 :
                LED_colour = 'b';
                break;
            case 0b01 :
                LED_colour = 'g';
                break;
            case 0b00 :
                LED_colour = 'r';
                break;
            default   :
                LED_colour = 'w';
                break;
        }
    }

    if (test_mode == 1) {                                                       // If statement for test purposal.
//        usb_serial.printf("Intensity_select = %d en LED_colour = %d\n", intensity_select, LED_colour);
    }

    if (test_mode == 0) {
        //usb_serial.printf("Message: %s\n", message);
        usb_serial.printf("Intensity_select = %d en LED_colour = %d\n", intensity_select, LED_colour);

    }
}

void serial_log() {                                                                   // Function for serial logging. See link to table with code declarations above in code.

    if (reposition_flag == 1) {                                                     // If statement to control logging for reposition button.
        pi_serial.printf(">01\n");

        if (test_mode == 1) {                                                       // If statement for test purposal.
            usb_serial.printf(">01\n");
        }

        reposition_flag = 0;
    }
    
    if (reposition_lock_flag == 1) {                                                     // If statement to control logging for reposition button.
        pi_serial.printf(">10\n");

        if (test_mode == 1) {                                                       // If statement for test purposal.
            usb_serial.printf(">10\n");
        }

        reposition_lock_flag = 0;
    }

    if (new_patient_flag == 1) {                                                    // If statement to control logging for new patient button.
        pi_serial.printf(">02\n");

        if (test_mode == 1) {                                                       // If statement for test purposal.
            usb_serial.printf(">02\n");
        }

        new_patient_flag = 0;
    }
    
    if (new_patient_lock_flag == 1) {                                                    // If statement to control logging for new patient button.
        pi_serial.printf(">20\n");

        if (test_mode == 1) {                                                       // If statement for test purposal.
            usb_serial.printf(">20\n");
        }

        new_patient_lock_flag = 0;
    }
    // The new calibration button trigger
    if (mute_flag == 1) {                                                    // If statement to control logging for new patient button.
        pi_serial.printf(">03\n");

        if (test_mode == 1) {                                                       // If statement for test purposal.
            usb_serial.printf(">03\n");
        }

        mute_flag = 0;
    }
    // The new calibration button trigger
    if (mute_lock_flag == 1) {                                                    // If statement to control logging for new patient button.
        pi_serial.printf(">30\n");

        if (test_mode == 1) {                                                       // If statement for test purposal.
            usb_serial.printf(">30\n");
        }

        mute_lock_flag = 0;
    }
    
    if (lock_flag == 1 && !lock_is_logged) {
        if (lock_state == 0)                                                     // If statement to control logging for lock button.
            pi_serial.printf(">04\n");
        else if(lock_state == 1)
            pi_serial.printf(">40\n");

        if (test_mode == 1) {
            if (lock_state == 0)                                                     // If statement to control logging for lock button.
                usb_serial.printf(">04\n");
            else if(lock_state == 1)
                usb_serial.printf(">40\n");                                         // If statement for test purposal.
        }
        lock_is_logged = 1;
    }

    if (LED_red_logged != LED_red_state) {                                          // If statement to control logging for LED_red.
        if (LED_red_state == 1) {
            pi_serial.printf("&01\n");
            LED_red_logged = LED_red_state;
            if (test_mode == 1) {
                usb_serial.printf("&01\n");
            }
        }

        if (LED_red_state == 0) {
            pi_serial.printf("&10\n");
            LED_red_logged = LED_red_state;
            if (test_mode == 1) {
                usb_serial.printf("&10\n");
            }
        }
    }

    if (LED_yellow_logged != LED_yellow_state) {                                    // If statement to control logging for LED_yellow.
        if (LED_yellow_state == 1) {
            pi_serial.printf("&02\n");
            LED_yellow_logged = LED_yellow_state;
            if (test_mode == 1) {
                usb_serial.printf("&02\n");
            }
        }
        if (LED_yellow_state == 0) {
            pi_serial.printf("&20\n");
            LED_yellow_logged = LED_yellow_state;
            if (test_mode == 1) {
                usb_serial.printf("&20\n");
            }
        }
    }

    if (LED_green_logged != LED_green_state) {                                      // If statement to control logging for LED_green.
        if (LED_green_state == 1) {
            pi_serial.printf("&03\n");
            LED_green_logged = LED_green_state;

            if (test_mode == 1) {
                usb_serial.printf("&03\n");
            }
        }

        if (LED_green_state == 0) {
            pi_serial.printf("&30\n");
            LED_green_logged = LED_green_state;

            if (test_mode == 1) {
                usb_serial.printf("&30\n");
            }
        }
    }

    if (speaker_logged != speaker_state) {                                          // If statement to control logging for speaker.
        if (speaker_state == 1) {
            pi_serial.printf("&04\n");
            speaker_logged = speaker_state;

            if (test_mode == 1) {                                                   // If statement for test purposal.
                usb_serial.printf("&04\n");
            }
        }

        if (speaker_state == 0) {
            pi_serial.printf("&40\n");
            speaker_logged = speaker_state;

            if (test_mode == 1) {                                                   // If statement for test purposal.
                usb_serial.printf("&40\n");
            }
        }
    }

//    if (power_plug_logged != power_plug_state) {                                    // If statement to control the logging for the state of the power plug.
//        if (power_plug_state == 1) {
//            pi_serial.printf("#08\n");
//
//            if (test_mode == 1) {                                                   // If statement for test purposal.
//                usb_serial.printf("#08\n");
//            }
//            power_plug_logged = power_plug_state;
//        }
//
//        if (power_plug_state == 0) {
//            pi_serial.printf("#80\n");
//
//            if (test_mode == 1) {                                                   // If statement for test purposal.
//                usb_serial.printf("#80\n");
//            }
//            power_plug_logged = power_plug_state;
//        }
//    }

    if (connection_test_sensorplate == 1) {                                                     // If statement for sending serial information sensorplate data when connection test is active.
        // Receiving order sensor information: 8 resistive sensors, 5 electric readings. Is splitted in two parts - part 1/2.
        pi_serial.printf("!,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n", piezo_resistive_array[0], piezo_resistive_array[1], piezo_resistive_array[2], piezo_resistive_array[3], piezo_resistive_array[4], piezo_resistive_array[5], piezo_resistive_array[6], piezo_resistive_array[7], piezo_electric_array[0], piezo_electric_array[1], piezo_electric_array[2], piezo_electric_array[3], piezo_electric_array[4]); // print all to serial port

        if (test_mode == 1) {
//            usb_serial.printf("!,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n", piezo_resistive_array[0], piezo_resistive_array[1], piezo_resistive_array[2], piezo_resistive_array[3], piezo_resistive_array[4], piezo_resistive_array[5], piezo_resistive_array[6], piezo_resistive_array[7], piezo_electric_array[0], piezo_electric_array[1], piezo_electric_array[2], piezo_electric_array[3], piezo_electric_array[4]); // print all to serial port
        }
    } else {
        pi_serial.printf("!,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n",0,0,0,0,0,0,0,0,0,0,0,0,0);
    }

}

void colour_select_indicating_LED_wall(char nLED_colour)                             // Function to select the colour for LED's to wall (values comes from algorithm).
{
    set_intensity_LEDs();                                                           // Call function set_intensity_LEDs to set the intensity for LED's to wall and above buttons.
    ws.setII(2.55*intensity);
    switch(nLED_colour) {
        case 'r' :
            px.SetAll(colourbuf[0]);
            LED_red_state = 1;
            LED_yellow_state = 0;
            break;
        case 'g' :
            px.SetAll(colourbuf[1]);
            LED_green_state = 1;
            LED_red_state = 0;
            break;
        case 'b' :
            px.SetAll(colourbuf[2]);
            break;
        case 'y' :
            px.SetAll(colourbuf[3]);
            LED_yellow_state = 1;
            LED_green_state = 0;
            break;
        default  :
            px.SetAll(colourbuf[4]);
    }

    if (calibration_flash >= 1) {
        if ((calibration_flash % 2) == 0) {
            px.SetAll(colourbuf[2]);
        } else {
            ws.setII(0);
        }
        calibration_flash--;
    }
    //for (int z=WS2812_BUF; z >= 0 ; z--) {
    ws.write_offsets(px.getBuf(),0,0,0);
    //}
}

void trigger_lock()                                                                 // If rising edge lock button is detected start locktimer.
{
    if (test_mode == 1) {
        usb_serial.printf("Lock triggered.\n");
    }
    if (lock_state == 0) pi_serial.printf(">44\n");
    else if (lock_state == 1) pi_serial.printf(">00\n");
    button_lock_hold_timer.reset();
    button_lock_hold_timer.start();
    delay_between_button_pressed.reset();
    delay_between_button_pressed.start();
}

void end_timer_lock_button()                                                        // End timer lock.
{
    if (test_mode == 1) {                                                           // If statement for test purposal.
        usb_serial.printf("Lock released.\n");
    }
    lock_flag = 0;                                                                  // Set lock_flag off.
    lock_is_logged = 0;
    button_lock_hold_timer.stop();                                                  // Stop and reset holdtimer
    button_lock_hold_timer.reset();
}

void reposition_button_triggered()
{
    if (lock_state == 1 | (delay_between_button_pressed.read_ms() < buttondelay_ms)) {  // Control statement for lock interface and delay for non using buttons at the same time.
        lock_flash = 10;
    } 
    
    delay_between_button_pressed.reset();
    delay_between_button_pressed.start();
    if (test_mode == 1) {                                                       // If statement for test purposal.
        usb_serial.printf("Reposition triggered.\n");
        LED_on_dev_board1 = !LED_on_dev_board1;
    }
    
    if (lock_state == 1) reposition_lock_flag = 1; 
    else { 
        reposition_flag = 1;
        reposition_feedback_LED = control_LED_intensity;
        pi_serial.printf("&05\n");
        if(test_mode == 1) usb_serial.printf("&05\n");    
    }
}

void rise_reposition()                                                              // Interrupt for rising edge reposition function (deactivation; active low).
{
    if (test_mode == 1) {                                                           // If statement for test purposal.
        usb_serial.printf("Reposition released.\n");
    }
    
    if (reposition_feedback_LED != 0) {
        pi_serial.printf("&50\n");
        
        if(test_mode == 1) usb_serial.printf("&50\n");
    }
    
    reposition_feedback_LED = 0;
}
//TODO rename to calibration
void mute_button_triggered()
{

    if (lock_state == 1 | (delay_between_button_pressed.read_ms() < buttondelay_ms)) {  // Control statement for lock interface and delay for non using buttons at the same time.
        lock_flash = 10;
    }
    
    delay_between_button_pressed.reset();
    delay_between_button_pressed.start();
    button_calibration_hold_timer.reset(); // inline ?
    button_calibration_hold_timer.start();
    
    if (lock_state == 1) {
        mute_lock_flag = 1;
    }
    
    else {
         mute_feedback_LED = control_LED_intensity;
         pi_serial.printf("&07\n");
         if (test_mode == 1) usb_serial.printf("&07\n");
         mute_flag = 1;   
    }
    
    if (test_mode == 1) {                                                       // If statement for test purposal.
        usb_serial.printf("Calibration triggered\n");
        LED_on_dev_board1 = !LED_on_dev_board1;
    }

    
}

void rise_mute()                                                              // Interrupt for rising edge reposition function (deactivation; active low).
{
    if (test_mode == 1) {                                                           // If statement for test purposal.
        usb_serial.printf("Calibration released.\n");
    }
    if(lock_state == 0 && mute_feedback_LED != 0) {
        pi_serial.printf("&70\n");
        if (test_mode == 1) usb_serial.printf("&70\n");
    }
        
    mute_feedback_LED = 0;

    button_calibration_hold_timer.stop();                                           // Timer reset for calibration function of new patient button.
    button_calibration_hold_timer.reset();

    if (lock_state == 1 | (delay_between_button_pressed.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_on_dev_board1 == 0) {                                           // If statement for test purposal.
                LED_on_dev_board1 = 1;
            } else {
                LED_on_dev_board1 = 0;
            }

        } else {
            calibration_flag = 0;
        }
    }

}

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

    if (lock_state == 1 | (delay_between_button_pressed.read_ms() < buttondelay_ms)) {
        lock_flash = 10;
    }
    
    delay_between_button_pressed.reset();
    delay_between_button_pressed.start();
    
    if (lock_state == 1) new_patient_lock_flag = 1;
    else {
        new_patient_feedback_LED = control_LED_intensity;
        pi_serial.printf("&06\n");
        new_patient_flag = 1;
        
        if(test_mode == 1){
            usb_serial.printf("&06\n");
        }
    }
    
    if (test_mode == 1) {                                                       // If statement for test purposal.
        usb_serial.printf("New patient triggered.\n");
    }
    
}

void activate_new_patient_function()                                                            // Timer calibration function.
{
    if (test_mode == 1) {                                                           // If statement for test purposal.
        usb_serial.printf("New patient released.\n");
        
    }
    
    if (new_patient_feedback_LED != 0) {
        pi_serial.printf("&60\n");
        if(test_mode) usb_serial.printf("&60\n");
    }
    new_patient_feedback_LED = 0;
}

void timer_functions()                                                              // Function which contains statements using timers.
{
    if (button_lock == 1) {
        button_lock_hold_timer.stop();
        button_lock_hold_timer.reset();
    }
    if ((button_lock_hold_timer.read_ms() > locktime_ms) && lock_flag == 0 && button_lock == 0) { // If statement for lock function.
        lock_flag = 1;
        LED_on_dev_board2 = !LED_on_dev_board2;
        lock_state = !lock_state;

        if (lock_state == 0) {                                                      // If statement to control lock feedback LED above button.
            lock_feedback_LED = control_LED_intensity;
            pi_serial.printf("&08\n");
            if(test_mode == 1) usb_serial.printf("&08\n"); 
        } else {
            lock_feedback_LED = 0;
            pi_serial.printf("&80\n");
            if(test_mode == 1) usb_serial.printf("&80\n");
        }
    }

    if (button_mute == 1) {
        button_calibration_hold_timer.stop();
        button_calibration_hold_timer.reset();
    }

    if ((button_calibration_hold_timer.read_ms() > calibrationtime_ms) && calibration_flag == 0 && button_mute == 0 && lock_state == 0) { // If statement for calibration algorithm.
        calibration_flag = 1;
        calibration_flash = 11;
        
        if (test_mode == 1) {                                                    // If statement for test purposal.
            usb_serial.printf("Calibrate triggered.\n");
        }
        pi_serial.printf(">33\n");
        
    }

    if (delay_between_button_pressed.read_ms() > delay_lock_interface) {            // If buttons are not pressed for 3 minutes, set lock active.
        lock_state = 1;
        LED_on_dev_board2 = 1;
        lock_feedback_LED = 0;
        if (!auto_lock_led_logged) { 
            pi_serial.printf("&80\n");
            if(test_mode == 1) usb_serial.printf("&80\n");
            auto_lock_led_logged = 1;
        }
    } else {
        auto_lock_led_logged = 0;   
    }
}

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

        if (button_new_patient == 0) {
            new_patient_feedback_LED = control_LED_intensity;
        } else {
            new_patient_feedback_LED = 0;
        }
    }
    if (lock_flash >= 1 && lock_state == 1) {
        if ((lock_flash % 2) == 0) {
            lock_feedback_LED = control_LED_intensity;
            pi_serial.printf("&08\n");
            if(test_mode == 1) usb_serial.printf("&08\n");
        } else {
            lock_feedback_LED = 0;
            pi_serial.printf("&80\n");
            if(test_mode == 1) usb_serial.printf("&80\n");
        }
        lock_flash--;
    } else {
        lock_flash = 0;
    }
}

int main()                                                                          // Main function. inline function "Momo Init" bijvoorbeeld
{
    ws.useII(WS2812::GLOBAL);                                                       // use global intensity scaling
    set_intensity_LEDs();                                                           // Initialize intensity for user interface LED's and LED's shines to wall.
    colour_select_indicating_LED_wall(LED_colour);

    speaker1 = 1;
    wait_ms(boot_delay_ms);                                                         // Wait to boot sensorplate first.
    speaker1 = 0;
    i2c_sensorplate_adc.frequency(i2c__frequency);                                        // Set frequency for i2c connection to sensorplate (variable is declared in config part).
    usb_serial.baud(baud_rate);                                                     // Set serial USB connection baud rate (variable is declared in config part).
    pi_serial.baud(baud_rate);                                                      // Same as line 697, but now for serial PI connection.
    piezo_resistive_adc1.setGain(GAIN_TWOTHIRDS);                                   // Set ranges of ADC to +/-6.144V (end is marked with #):
    piezo_resistive_adc2.setGain(GAIN_TWOTHIRDS);
    piezo_electric_adc.setGain(GAIN_TWO);
    pi_serial.format(8, SerialBase::None, 1);                                       // Set serial communication line with PI.

    button_lock.mode(PullUp);
    button_reposition.mode(PullUp);
    button_mute.mode(PullUp);
    button_new_patient.mode(PullUp);

    button_lock.fall(&trigger_lock);                                                // Interrupt for rising edge lock button.
    button_lock.rise(&end_timer_lock_button);
    button_reposition.fall(&reposition_button_triggered);
    button_reposition.rise(&rise_reposition);
    button_mute.fall(&mute_button_triggered);
    button_mute.rise(&rise_mute);
    button_new_patient.fall(&trigger_new_patient);                                  // New patient/calibration button rising event.
    button_new_patient.rise(&activate_new_patient_function);                        // Falling edge for calibration algorithm option.

    __disable_irq();
    while(!pi_active) {
        Knight_Rider_Timer.reset();
        Knight_Rider_Timer.start();
        reposition_feedback_LED = control_LED_intensity;
        new_patient_feedback_LED = 0;
        while(Knight_Rider_Timer.read_ms()<(1*(total_knight_rider_cycle_time_ms/8))) {}
        new_patient_feedback_LED = control_LED_intensity;
        while(Knight_Rider_Timer.read_ms()<(2*(total_knight_rider_cycle_time_ms/8))) {}
        mute_feedback_LED = control_LED_intensity;
        reposition_feedback_LED = 0;
        while(Knight_Rider_Timer.read_ms()<(3*(total_knight_rider_cycle_time_ms/8))) {}
        lock_feedback_LED = control_LED_intensity;
        new_patient_feedback_LED = 0;
        while(Knight_Rider_Timer.read_ms()<(4*(total_knight_rider_cycle_time_ms/8))) {}
        mute_feedback_LED = 0;
        while(Knight_Rider_Timer.read_ms()<(5*(total_knight_rider_cycle_time_ms/8))) {}
        mute_feedback_LED = control_LED_intensity;
        while(Knight_Rider_Timer.read_ms()<(6*(total_knight_rider_cycle_time_ms/8))) {}
        new_patient_feedback_LED = control_LED_intensity;
        lock_feedback_LED = 0;
        while(Knight_Rider_Timer.read_ms()<(7*(total_knight_rider_cycle_time_ms/8))) {}
        reposition_feedback_LED = control_LED_intensity;
        mute_feedback_LED = 0;

        serial_read();
        colour_select_indicating_LED_wall(LED_colour);
        while(Knight_Rider_Timer.read_ms()<(8*(total_knight_rider_cycle_time_ms/8))) {}

    }
    __enable_irq();
    delay_between_button_pressed.reset();                                           // Delaytimer reset en start.
    delay_between_button_pressed.start();

    reposition_feedback_LED = 0;
    new_patient_feedback_LED = 0;
    mute_feedback_LED = 0;
    usb_serial.printf("Lock State: %d\n",lock_state);
    lock_feedback_LED = control_LED_intensity;                                      // Lock LED initialization.

    while (1) {
        piezo_electric_sample_timer.reset();                                            // Clock gebruiken o.i.d.?
        piezo_electric_sample_timer.start();
        connection_test_sensorplate = !testpin_sensorplate && pi_active;

        if (test_mode == 1) {
//            usb_serial.printf("Connection test sensorplate = %d\n", connection_test_sensorplate);
        }
        if (test_mode == 1) {
//            usb_serial.printf("Loop time: %d ms\n",piezo_electric_sample_timer.read_ms());
        }

        if (connection_test_sensorplate == 1) {
            piezo_electric_array[0] = piezo_electric_adc.readADC_Differential_0_1();            // First PE readout.

            for (uint8_t k = 0; k < 4; ++k) {
                piezo_resistive_array[k] =    piezo_resistive_adc1.readADC_SingleEnded(k);  // First 4 PR readout.
            }
            if (test_mode == 1) {
//                usb_serial.printf("Loop time: %d ms\n",piezo_electric_sample_timer.read_ms());
            }
            while(piezo_electric_sample_timer.read_us()<(1*(total_readout_cycle_time_us/5))) {} // Wait untill 20% of cycle. Energy efficiency is not fine in this situation, correct if low energy is needed.

            piezo_electric_array[1] = piezo_electric_adc.readADC_Differential_0_1();            // Second PE readout.

            for (uint8_t k = 0; k < 4; ++k) {
                piezo_resistive_array[k+4] =  piezo_resistive_adc2.readADC_SingleEnded(k);  // Last 4 PR readout.
            }
            if (test_mode == 1) {
//                usb_serial.printf("Loop time: %d ms\n",piezo_electric_sample_timer.read_ms());
            }

            while(piezo_electric_sample_timer.read_us()<(2*(total_readout_cycle_time_us/5))) {} // Wait untill 40% of cycle. Energy efficiency is not fine in this situation, correct if low energy is needed.

            piezo_electric_array[2] = piezo_electric_adc.readADC_Differential_0_1();            // Third PE readout.
            angle_device_sensorplate.getAccelero(accelerometer_sensorplate);                // Get accelerometer data.
            angle = accelerometer_sensorplate[2]*100;
            if(angle == 0) {
                MPU6050_belt angle_device_sensorplate(PB_9, PB_8);
                angle_device_sensorplate.getAccelero(accelerometer_sensorplate);
                angle = accelerometer_sensorplate[2]*100;
            }
            if((abs(accelerometer_sensorplate[0])+abs(accelerometer_sensorplate[1])+abs(accelerometer_sensorplate[2]))<= 6) {
                for(uint8_t k = 0; k < 3; ++k) {
                    accelerometer_sensorplate[k]=accelerometer_sensorplate[k]*2;
                }
            }
            angle_device_sensorplate.getGyro(gyroscope_sensorplate);                        // Get gyroscope data.
            if (test_mode == 1) {
//                usb_serial.printf("Loop time: %d ms\n",piezo_electric_sample_timer.read_ms());
            }
            if (test_belt == 1) {
                angle_device_reference_belt.getGyro(gyroscope_reference_belt);              // Get gyroscope data from Belt.
                angle_device_reference_belt.getAccelero(accelerometer_reference_belt);      // Get accelerometer data from belt.
            }

            if (connection_test_sensorplate == 1) {
                if (test_belt == 0) {                                                     // If statement for sending serial information sensorplate data when connection test is active.
                    pi_serial.printf("?,%f,%f,%f,%f,%f,%f,0,0,0,0,0,0,\n", accelerometer_sensorplate[0], accelerometer_sensorplate[1], accelerometer_sensorplate[2], gyroscope_sensorplate[0], gyroscope_sensorplate[1], gyroscope_sensorplate[2]);
                } else {
                    // Receiving order sensor information: 3 accelero sensors & 3 gyroscope sensors from sensorplate; 3 accelero sensors & 3 gyroscope sensors from belt. Is splitted in two parts - part 2/2.
                    pi_serial.printf("?,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,\n", accelerometer_sensorplate[0], accelerometer_sensorplate[1], accelerometer_sensorplate[2], gyroscope_sensorplate[0], gyroscope_sensorplate[1], gyroscope_sensorplate[2], accelerometer_reference_belt[0], accelerometer_reference_belt[1], accelerometer_reference_belt[2], gyroscope_reference_belt[0], gyroscope_reference_belt[1], gyroscope_reference_belt[2]);
                }
            } // binair print and convert in pi
            else {
                pi_serial.printf("?,%f,%f,%f,%f,%f,%f,0,0,0,0,0,0,\n",0,0,0,0,0,0);
            }

            while(piezo_electric_sample_timer.read_us()<(3*(total_readout_cycle_time_us/5))) {}  // Wait untill 60% of cycle. Energy efficiency is not fine in this situation, correct if low energy is needed.

            piezo_electric_array[3] = piezo_electric_adc.readADC_Differential_0_1();        // Fourth PE readout.
        }
        if (test_mode == 1) {
//            usb_serial.printf("Loop time: %d ms\n",piezo_electric_sample_timer.read_ms());
        }
        timer_functions();
        if (test_mode == 1) {
//            usb_serial.printf("Loop time after timer_functions: %d ms\n",piezo_electric_sample_timer.read_ms());
        }

        colour_select_indicating_LED_wall(LED_colour);                                  // Function to select colour.
        set_userinterface_LED();                                                        // Set LED's of user interface (LED's above buttons).

        while(piezo_electric_sample_timer.read_us()<(4*(total_readout_cycle_time_us/5))) {}  // Wait untill 80% of cycle. Energy efficiency is not fine in this situation, correct if low energy is needed.

        if (test_mode == 1) {                                                           // If statement for test purposal.
//            usb_serial.printf("Angle device sensorplate = %d\n",angle_device_sensorplate.testConnection());
        }

        if (connection_test_sensorplate == 1) {
            piezo_electric_array[4] = piezo_electric_adc.readADC_Differential_0_1();         // Fifth PE readout.
        }

        while(piezo_electric_sample_timer.read_us()<(4.25*(total_readout_cycle_time_us/5))) {}  // Wait untill 85% of cycle. Energy efficiency is not fine in this situation, correct if low energy is needed.

        if (test_mode == 1) {
//            usb_serial.printf("Loop time: %d ms\n",piezo_electric_sample_timer.read_ms());
        }

        if (test_mode == 0) {                                                            // If statements for test purposal (untill * mark).
//            usb_serial.printf("Loop time pre serial: %d ms\n",piezo_electric_sample_timer.read_ms());
        }
        serial_read();                                                                   // Call function for reading information from PI by serial connection.
        serial_log();                                                                    // Call function for logging information to PI by serial connection.

        if (test_mode == 0) {                                                            // If statements for test purposal (untill * mark).
            usb_serial.printf("Loop time: %d ms\n",piezo_electric_sample_timer.read_ms());
        }

        while(piezo_electric_sample_timer.read_us()<(total_readout_cycle_time_us)) {}  // Wait untill 100% of cycle. Energy efficiency is not fine in this situation, correct if low energy is needed.
        //if (test_pin == 1) {
//            test_mode = 1;
//            usb_serial.printf("%d\n",test_mode);
//        }
//        if (test_pin == 0) {
//            test_mode = 0;
//            usb_serial.printf("%d\n",test_mode);
//        }
    }
}