File content as of revision 38:764847892afc:

/********************* 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.3
Date of modification:                   18-10-2017
Purpose of this file:                   Code for LPC1768 microcontroller for controlling buttons, LED's and communicate to PI
Update ‘what’s new in this version?’:   New structure added.
                                        Readability improved.                                        
                                        Code optimized (variables and functions).
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 "neopixel.h"

#define NUMBER_LED_FRONT (3)  // declaren waarvoor dient                                                     
#define ONE_COLOR

InterruptIn button_lock(p15);                                                       // Input on intterupt base decleration. 
InterruptIn button_reposition(p17);
InterruptIn button_mute(p16);
InterruptIn button_new_patient(p18);

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(p21); // relatie aangeven!
DigitalOut speaker2(p22);
neopixel::PixelArray indicator_LEDs(p11);                                           

PwmOut lock_feedback_LED(p23);                                                      // Declaration of pulse with modulation outputs.                                
PwmOut reposition_feedback_LED(p25);
PwmOut mute_feedback_LED(p26);
PwmOut new_patient_feedback_LED(p24);

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.

/* 
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(p28, p27);                                                  // I2C for sensorplate.
I2C i2c_power_adc(p9, p10);                                                         // I2C for accupack. 
MPU6050 angle_device_sensorplate(p28,p27);                                          // i2c pins // i2c address hardcoded 0x68.
MPU6050_belt angle_device_reference_belt(p28,p27);                                  // 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.
Adafruit_ADS1115 adsAccu(&i2c_power_adc, 0x48);                                     // i2c pins, i2c address.
Serial usb_serial(USBTX, USBRX);                                                    // tx, rx  
Serial pi_serial(p13, p14);                                                         // 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 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.
char LED_colour = 'g';                                                              // 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;                      // Flag variables.
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; // is toevoegen
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 = 0;                                                          // 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 = 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 = 40, intensity_night = 10;                                       // 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!

/*************************** TEST ********************************/
// Verify algoritm function: for belt activation, set test_belt 1 (connect pin p20 to 3.3V).
Timer test_timer;                                                           
DigitalIn test_pin(p30, 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 = 0;

// Variable for connection test (should be changed):
int 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.
{
    if (pi_serial.readable()) {                                                     // Function to check if pi is readable.
        char message[10];
        pi_serial.scanf("%s", message);
        
        if (test_mode == 1) {                                                       // If statement for test purposal. 
            usb_serial.printf("Message = %s, Intensity_select = %d en LED_colour = %c\n", message, intensity_select, LED_colour);
        }
        
        if (intensity_select != (message[0]-'0')) {                                 // Read intensity for LED's variable from PI. 
            intensity_select = (message[0]-'0');
        }

        if (LED_colour != message[1]) {                                             // Read character from PI to set LED_colour.
            LED_colour = message[1];
        }

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

void serial_log()                                                                   // Function for serial logging. See link to table with code declarations above in code.
{
    if (mute_flag == 1) {                                                           // If statement to control logging for mute button. 
        pi_serial.printf(">01\n");

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

        mute_flag = 0;
    }

    if (new_patient_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");
        }

        new_patient_flag = 0;
    }

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

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

        reposition_flag = 0;
    }

    if (batteryvoltage_current != batteryvoltage_last) {                            // If statement to control logging for batteryvoltage.           
        pi_serial.printf("%%" "%d\n", batteryvoltage_current);

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

        batteryvoltage_last = batteryvoltage_current;
    }

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

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

    if (LED_green_logged != LED_green_state) {                                      // If statement to control logging for LED_green.
        if (LED_green_state == 1) {
            pi_serial.printf("&05\n");
            LED_green_logged = LED_green_state;
        
            if (test_mode == 1) {
                usb_serial.printf("&05\n");
            }
        }
        
        if (LED_green_state == 0) {
            pi_serial.printf("&50\n");
            LED_green_logged = LED_green_state;
        
            if (test_mode == 1) {
                usb_serial.printf("&50\n");
            }
        }
    }

    if (speaker_logged != speaker_state) {                                          // If statement to control logging for speaker. 
        if (speaker_state == 1) {
            pi_serial.printf("&07\n");
            speaker_logged = speaker_state;
            
            if (test_mode == 1) {                                                   // If statement for test purposal.
                usb_serial.printf("&07\n");
            }
        }
        
        if (speaker_state == 0) {
            pi_serial.printf("&70\n");
            speaker_logged = speaker_state;
            
            if (test_mode == 1) {                                                   // If statement for test purposal.
                usb_serial.printf("&70\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[4], piezo_resistive_array[7], piezo_resistive_array[6], piezo_resistive_array[5], piezo_resistive_array[1], piezo_resistive_array[0], piezo_resistive_array[2], piezo_resistive_array[3], 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[4], piezo_resistive_array[7], piezo_resistive_array[6], piezo_resistive_array[5], piezo_resistive_array[1], piezo_resistive_array[0], piezo_resistive_array[2], piezo_resistive_array[3], 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
        }
    }

}

void colour_select_indicating_LED_wall(char LED_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.
    
    if ((LED_colour == 'r') || (LED_colour == 'g') || (LED_colour == 'b') || (LED_colour == 'y')) { // If statement to prevent potential errors in communication.
        LED_red_intensity = 0;                                                                // Reset 
        LED_green_intensity = 0;
        LED_blue_intensity = 0;
        
        if (LED_colour == 'r') {                                                    // Set LED_colour to red.
            LED_red_intensity = (2.55*intensity);                                             // 255 / 100 = 2.55 (8 - bit digital value; 0-255 = 256 steps); intensity is a value between 0 and 100. 
            LED_green_intensity = 0;
            LED_blue_intensity = 0;
            LED_red_state = 1;
        } else {
            LED_red_state = 0;
        }
    
        if (LED_colour == 'y') {                                                    // Set LED_colour to yellow.
            LED_red_intensity = (2.55*intensity);
            LED_green_intensity = (2.55*intensity);
            LED_blue_intensity = 0;
            LED_yellow_state = 1;
        } else {
            LED_green_state = 0;
        }
    
        if (LED_colour == 'g') {                                                    // Set LED_colour to green.
            LED_red_intensity = 0;
            LED_green_intensity = (2.55*intensity);
            LED_blue_intensity = 0;
            LED_green_state = 1;
        } else {
            LED_green_state = 0;
        }
    
        if (LED_colour == 'b') {                                                    // Set LED_colour to blue.
            LED_red_intensity = 0;
            LED_green_intensity = 0;
            LED_blue_intensity = (2.55*intensity);
        }
    }

    if (calibration_flash >= 1) {                                                   // If statement for flashing LED's (colour = white) when calibration is active. 
        if ((calibration_flash % 2) == 0) {                                         // If value can not be devided by two, set LED's on.
            LED_red_intensity = 255;
            LED_green_intensity = 255;
            LED_blue_intensity = 255;
            LED_on_dev_board4 = 1;
        } else {                                                                    // Else set LED's off.
            LED_red_intensity = 0;
            LED_green_intensity = 0;
            LED_blue_intensity = 0;
            LED_on_dev_board4 = 0;
        }
        calibration_flash--;
    }
}

void trigger_lock()                                                                 // If rising edge lock button is detected start locktimer.
{
    if (test_mode == 1) {
        usb_serial.printf("Lock triggered.\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.
    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.
    } else {
        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;
        }
        reposition_flag = 1;

        reposition_feedback_LED = control_LED_intensity;
    }
}

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");
    }
    reposition_feedback_LED = 0;

}

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.
    } else {
        delay_between_button_pressed.reset();
        delay_between_button_pressed.start();
        mute_state = !mute_state;
        
        if (mute_state == 1) {                                                      // If statement for if mute_state is active, set mute feedback LED active.
            mute_feedback_LED = control_LED_intensity;
        } else {
            mute_feedback_LED = 0;
        }
        
        if (test_mode == 1) {                                                       // If statement for test purposal. 
            usb_serial.printf("Mute triggered %d.\n",mute_state);
            LED_on_dev_board1 = !LED_on_dev_board1;
        }

        mute_flag = 1;
    }
}

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)) {
    } else {
        button_calibration_hold_timer.reset(); // inline ?
        button_calibration_hold_timer.start();
        new_patient_feedback_LED = control_LED_intensity;;
        
        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");
    }
    new_patient_feedback_LED = 0;

    if (0 < button_calibration_hold_timer.read_ms() < calibrationtime_ms) {         // If statement for new_patient function: holdtime for calibration is les then set time to calibrate algorithm. && toevoegen? -. als mogelijk mailtje naar Bart: bart@straightupalgorithms.com
        new_patient_flag = 1;
    }

    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 timer_functions()                                                              // Function which contains statements using timers. 
{
    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;
        } else {
            lock_feedback_LED = 0;
        }
    }

    if ((button_calibration_hold_timer.read_ms() > calibrationtime_ms) && calibration_flag == 0 && button_new_patient == 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(">30\n");                                                  // Print statement for serial communication to inform algorithm to calibrate.
    }

    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;
    }
}

void generate(neopixel::Pixel * out, uint32_t index, uintptr_t val)                 // Generate LED colour function (library function PixelArray is used for this item).
{
    out->red   = LED_red_intensity;
    out->green = LED_green_intensity;
    out->blue  = LED_blue_intensity;
}

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;
        }
    }
}

void read_voltage()                                                                 // Function for reading voltages from power and battery.
{
    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. Use PWM? => Split in more functions. 
        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();
    }
                                                                                    // Read channel 0 from external ADC (batteryvoltage); voltagedeviders are used, 
    batteryvoltage_current = adsAccu.readADC_SingleEnded(0);                        // because of higher voltage then Vcc of ADC (5.3 V (= Vcc + 0.3 V) max possible at each analog input).
    powervoltage_current = adsAccu.readADC_SingleEnded(1);                          // Read channel 1 from external ADC (powervoltage).

    if (powervoltage_current < digital_value_ADC_powervoltage_unplugged) {                                             // If statement to set LED's to blue. 
        power_plug_state = 0;
        LED_colour = 'b';
    } else {
        power_plug_state = 1;
    }
}

void read_adc()
{
    piezo_electric_sample_timer.reset();                                        // Clock gebruiken o.i.d.?
    piezo_electric_sample_timer.start();
    connection_test_sensorplate = angle_device_sensorplate.testConnection();
    
    if (test_mode == 1) {
        usb_serial.printf("Connection test sensorplate = %d\n", connection_test_sensorplate);
    }
    
    /*
    if (connection_test_sensorplate == 0) {
        lock_state = 1;
        LED_on_dev_board2 = 1;
        lock_feedback_LED = 0;
    }*/
    
    if (connection_test_sensorplate == 1) {
        piezo_electric_array[0] = piezo_electric_adc.readADC_SingleEnded(0);            // 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.
        }

        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_SingleEnded(0);            // 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.
        }

        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_SingleEnded(0);            // Third PE readout.

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

        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 statement for sending serial information sensorplate data when connection test is active. 
            // 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
        
        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_SingleEnded(0);        // Fourth PE readout.
    }

    timer_functions();

    batteryvoltage_current = batteryvoltage_last;
    powervoltage_current = powervoltage_last;
    read_voltage();                                                                 // Read_voltage function to control alarm.
    
    if (test_mode == 1) {
        usb_serial.printf("Voltage = %d   ,   %d\n", batteryvoltage_current, powervoltage_current);
    }
    
    uint32_t val = 0;
    colour_select_indicating_LED_wall(LED_colour);                                                      // Function to select colour.     
    indicator_LEDs.update(generate, NUMBER_LED_FRONT, val);                         // Function to set the LED's which shines to the wall (indicating change patient position).
    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_SingleEnded(0);         // 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.

    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 == 1) {                                                            // If statements for test purposal (untill * mark).
        usb_serial.printf("Loop time: %d ms\n",piezo_electric_sample_timer.read_ms());
    }
    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);
    }

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

int main() {                                                                        // Main function. inline function "Momo Init" bijvoorbeeld
    wait_ms(boot_delay_ms);                                                         // Wait to boot sensorplate first.
    i2c_sensorplate_adc.frequency(i2c_frequencyuencyuency);                                        // Set frequency for i2c connection to sensorplate (variable is declared in config part).
    i2c_power_adc.frequency(i2c_frequencyuencyuency);                                              // Same as line 695, but now for ADC to read battery- en powervoltage. 
    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_TWOTHIRDS);                                     
    adsAccu.setGain(GAIN_TWOTHIRDS);                                                // #) End of configuration ADC ranges.
    pi_serial.format(8, SerialBase::None, 1);                                       // Set serial communication line with PI. 

    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_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.
    delay_between_button_pressed.reset();                                           // Delaytimer reset en start.
    delay_between_button_pressed.start();

    set_intensity_LEDs();                                                                // Initialize intensity for user interface LED's and LED's shines to wall.  
    lock_feedback_LED = control_LED_intensity;                                      // Lock LED initialization.


    total_readout_cycle.attach_us(&read_adc, total_readout_cycle_time_us);          // Call function to start reading sensorplate and other functionalities.

    while (1) {
        wait_us(total_readout_cycle_time_us+1);                                     // Wait indefinitely.
    }
}