File content as of revision 66:88c910cd4d9e:

/********************* CODE INFORMATON ******************************
Date of creation:           30-09-2017
Authors:                    Danny Eldering & Ricardo Molenaar
co-authors:                 Menno Gravemaker & Ishvara Lalta
(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 ALARMBUF 16
#define NUM_COLORS 8
#define YELLOW_TRANSITION 12
#define RED_TRANSITION 15
#define NUM_LEDS_PER_COLOR 3
#define NUMBER_LED_FRONT (3)
#define ONE_COLOR
#define TAIL_LENGTH 6
#define COMET_TAIL_END_INTENSITY 90
#define FADE_STEPS 20

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

DigitalIn intensity_code(PA_12);
DigitalIn colour_code_1(PA_11);
DigitalIn colour_code_0(PB_12);
InterruptIn 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(ALARMBUF);
//WS2812 ws(PA_7, ALARMBUF, 3, 9, 9, 6);
WS2812 ws(PA_7, ALARMBUF, 3, 12, 8, 12);

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 comet_timer;
Timer reposition_button_hold_timer;
Timer new_patient_button_hold_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.
RawSerial usb_serial(SERIAL_TX, SERIAL_RX);                                            // tx, rx
RawSerial 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_comet_cycle_time_ms = 750/16;
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[6] = {0,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,0x555555,0x800080,0x000000,0x8B4513};         // 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.
int LED_colour_wheel_percentage=0;
int ring_colour_old,mixer=0;
int colour_wheel_filler = 5;
bool colour_wheel_drain_reposition = false;
bool colour_wheel_drain_new_patient = false;
bool circle_filling_reposition = false;
bool circle_filling_new_patient = false;
int circle_filled_reposition = 0;
int circle_filled_new_patient = 0;
int comet=0;
int tail,tail_step;
//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 = 80, intensity_night = 50;                                       // 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.
{
    LED_colour_wheel_percentage = pi_serial.getc();
}

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,%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], piezo_electric_array[5]); // 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,%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],piezo_electric_array[5]); // print all to serial port
        }
    } else {
        pi_serial.printf("!,%d,%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,0);
        if (test_mode == 1) {
            usb_serial.printf("!,%d,%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,0); // print all to serial port
        }
    }

}

int minimum(int a,int b)
{
    if(a>b)return b;
    else return a;
}

int colour_fade(int colour_old,int colour_new,int colour_mix)
{
    int old_r=(colour_old&0xff0000)>>16;
    int old_g=(colour_old&0x00ff00)>> 8;
    int old_b=(colour_old&0x0000ff)>> 0;
    int new_r=(colour_new&0xff0000)>>16;
    int new_g=(colour_new&0x00ff00)>> 8;
    int new_b=(colour_new&0x0000ff)>> 0;
    int mix = (100*colour_mix)/FADE_STEPS;
    int mix_r=((new_r-old_r)*mix/100)+old_r;
    int mix_g=((new_g-old_g)*mix/100)+old_g;
    int mix_b=((new_b-old_b)*mix/100)+old_b;
    return ((mix_r<<16)|(mix_g<<8)|(mix_b));

}



void colour_wheel(int percentage_in)
{
    px.SetAllI(int(intensity*2.55));
    if(percentage_in>100)percentage_in=100;
    int ring_colour = colourbuf[2];
    px.SetAll(colourbuf[6]);
    int leds_on =(percentage_in*16)/100;
    float led_partly = ((percentage_in*16)%100);
    if(leds_on==YELLOW_TRANSITION) {
        if(mixer>20)mixer=20;
        ring_colour = colour_fade(colourbuf[2],colourbuf[3],mixer);
        mixer++;
    }
    if(leds_on>YELLOW_TRANSITION&&leds_on<RED_TRANSITION&&ring_colour_old==colourbuf[3]) {
        ring_colour=colourbuf[3];
        mixer=0;
    }
    if(leds_on==RED_TRANSITION) {
        if(mixer>20)mixer=20;
        usb_serial.printf("mixer:%d\n",mixer);
        ring_colour = colour_fade(colourbuf[3],colourbuf[0],mixer);
        mixer++;
    }
    if(leds_on>RED_TRANSITION&&ring_colour_old==colourbuf[0]) {
        ring_colour = colourbuf[0];
        mixer=0;
    }
    usb_serial.printf("on:%d,part:%f\n",leds_on,led_partly);
    for(int j = 0; j<=(leds_on); j++)px.Set(((16-j)%16),ring_colour);
    px.SetAllI(int(intensity*2.55));
    if((16-leds_on-1)>0) {
        px.Set(((16-leds_on-1)%16),ring_colour);
        px.SetI(((16-leds_on-1)%16),int(0.01*led_partly*intensity*2.55));
    }
    usb_serial.printf("percentage in %d,%d\n",percentage_in,leds_on);

    if(reposition_button_hold_timer.read_ms()) {
        usb_serial.printf("filling circle\n");
        circle_filling_reposition = true;
        colour_wheel_filler = reposition_button_hold_timer.read_ms()/250;
        if(colour_wheel_filler>=8) {
            colour_wheel_filler=8;
            circle_filled_reposition = 15;
            circle_filling_reposition = false;
            colour_wheel_drain_reposition = false;
            reposition_flag = 1;
        }
        px.SetAll(colourbuf[6]);
        for(int k =0; k<=colour_wheel_filler; k++) {
            px.Set(k,colourbuf[5]);
            px.Set(16-k,colourbuf[5]);
        }
        px.SetAllI(int(intensity*2.55));
    }

    if(new_patient_button_hold_timer.read_ms()) {
        usb_serial.printf("filling circle\n");
        circle_filling_new_patient = true;
        colour_wheel_filler = new_patient_button_hold_timer.read_ms()/250;
        if(colour_wheel_filler>=8) {
            colour_wheel_filler=8;
            circle_filled_new_patient = 10;
            circle_filling_new_patient = false;
            colour_wheel_drain_new_patient = false;
            new_patient_flag = 1;
        }
        px.SetAll(colourbuf[6]);
        for(int k =0; k<=colour_wheel_filler; k++) {
            px.Set((k+8)%16,colourbuf[5]);
            px.Set((16-k+8)%16,colourbuf[5]);
        }
        px.SetAllI(int(intensity*2.55));
    }

    if(!reposition_button_hold_timer.read_ms()&&circle_filling_reposition&&!colour_wheel_drain_reposition) {
        usb_serial.printf("Short hold\n");
        px.SetAll(colourbuf[6]);
        for(int k =0; k<=colour_wheel_filler; k++) {
            px.Set(k,colourbuf[5]);
            px.Set(16-k,colourbuf[5]);
        }
        px.SetAllI(int(intensity*2.55));
        colour_wheel_filler++;
        if(colour_wheel_filler>=5) {
            colour_wheel_drain_reposition=true;
            circle_filling_reposition=false;
        }
    }

    if(!new_patient_button_hold_timer.read_ms()&&circle_filling_new_patient&&!colour_wheel_drain_new_patient) {
        usb_serial.printf("Short hold\n");
        px.SetAll(colourbuf[6]);
        for(int k =0; k<=colour_wheel_filler; k++) {
            px.Set((k+8)%16,colourbuf[5]);
            px.Set((16-k+8)%16,colourbuf[5]);
        }
        px.SetAllI(int(intensity*2.55));
        colour_wheel_filler++;
        if(colour_wheel_filler>=5) {
            colour_wheel_drain_new_patient=true;
            circle_filling_new_patient=false;
        }
    }

    if(colour_wheel_drain_reposition) {
        usb_serial.printf("drain\n");
        px.SetAll(colourbuf[6]);
        for(int k =0; k<=colour_wheel_filler; k++) {
            px.Set(k,colourbuf[5]);
            px.Set(16-k,colourbuf[5]);
        }
        px.SetAllI(int(intensity*2.55));
        colour_wheel_filler--;
        if(!colour_wheel_filler)colour_wheel_drain_reposition=false;
    }

    if(colour_wheel_drain_new_patient) {
        usb_serial.printf("drain\n");
        px.SetAll(colourbuf[6]);
        for(int k =0; k<=colour_wheel_filler; k++) {
            px.Set((k+8)%16,colourbuf[5]);
            px.Set((16-k+8)%16,colourbuf[5]);
        }
        px.SetAllI(int(intensity*2.55));
        colour_wheel_filler--;
        if(!colour_wheel_filler)colour_wheel_drain_new_patient=false;
    }

    if(circle_filled_reposition) {
        usb_serial.printf("circle_filled_repo\n");
        px.SetAll(colourbuf[6]);
        for(int k =0; k<=circle_filled_reposition; k++) {
            px.Set((16-k)%16,colourbuf[5]);
        }
        px.Set((16-circle_filled_reposition)%16,colourbuf[4]);
        px.SetAllI(int(intensity*2.55));
        circle_filled_reposition--;
    }

    if(circle_filled_new_patient) {
        usb_serial.printf("circle_filled_new_patient\n");
        px.SetAll(colourbuf[5]);
        px.SetAllI(int(intensity*2.55*(7-(circle_filled_new_patient%5))/7));
        circle_filled_new_patient--;
    }

    ws.write_offsets(px.getBuf(),0,0,0);
    ring_colour_old=ring_colour;
}

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||button_new_patient.read()==0) reposition_lock_flag = 1;
    else {
        reposition_button_hold_timer.reset();
        reposition_button_hold_timer.start();

        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_button_hold_timer.stop();
    reposition_button_hold_timer.reset();
    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||button_reposition.read()==0) new_patient_lock_flag = 1;
    else {
        new_patient_button_hold_timer.reset();
        new_patient_button_hold_timer.start();

        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");

    }
    new_patient_button_hold_timer.stop();
    new_patient_button_hold_timer.reset();


    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_reposition == 1) {
        reposition_button_hold_timer.stop();
        reposition_button_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;
    }
}

void sensorplate_detached()
{
    if(piezo_electric_sample_timer.read_us() > total_readout_cycle_time_us) {
        NVIC_SystemReset();
    }
}


int main()                                                                          // Main function. inline function "Momo Init" bijvoorbeeld
{
    pi_serial.attach(serial_read,Serial::RxIrq);
    ws.useII(WS2812::PER_PIXEL);
    ws.setII(255);
    px.SetAll(colourbuf[4]);                                                       // use global intensity scaling
    set_intensity_LEDs();                                                           // Initialize intensity for user interface LED's and LED's shines to wall.
    ws.write_offsets(px.getBuf(),0,0,0);

    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.

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

    testpin_sensorplate.fall(&sensorplate_detached);
    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) {
        comet_timer.reset();
        comet_timer.start();
        ws.useII(WS2812::PER_PIXEL);
        ws.setII(255);
        tail_step = (255-COMET_TAIL_END_INTENSITY)/TAIL_LENGTH;
        comet++;
        px.SetAll(colourbuf[6]);
        px.Set(comet%ALARMBUF,colourbuf[4]);
        px.SetI(comet%ALARMBUF,255);
        for(int i=1; i<=TAIL_LENGTH; i++) {
            tail=(comet-i);
            if(tail<0)tail=0;
            px.Set(tail%ALARMBUF,colourbuf[4]);
            px.SetI(tail%ALARMBUF,(255-(tail_step*i)));
        }
        colour_code = (colour_code_1 << 1 | colour_code_0);
        if(colour_code != 0b00 && pi_active == false && (comet%ALARMBUF)==(ALARMBUF-1)) {
            pi_active = true;
        }
        ws.write_offsets(px.getBuf(),0,0,0);
        while(comet_timer.read_ms()<total_comet_cycle_time_ms) {}
    }
    for(int i=ALARMBUF-1; i>=0; i--) {
        comet_timer.reset();
        comet_timer.start();
        for(int j=1; j<=TAIL_LENGTH; j++) {
            tail=(i-j);
            if(tail>=0) {
                px.Set(tail,colourbuf[4]);
                px.SetI(tail,(int(255-(tail_step*j))));
            }
        }
        px.Set(i,colourbuf[4]);
        px.SetI(i,255);
        while(comet_timer.read_ms()<total_comet_cycle_time_ms) {}
        ws.write_offsets(px.getBuf(),0,0,0);
    }

    __enable_irq();
    wait(1);
    delay_between_button_pressed.reset();                                           // Delaytimer reset en start.
    delay_between_button_pressed.start();

//    ws.useII(WS2812::GLOBAL);
    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_3();            // First PE readout.
            piezo_electric_array[3] = piezo_electric_adc.readADC_Differential_1_3();            // 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/3))) {} // 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_3();            // Second PE readout.
            piezo_electric_array[4] = piezo_electric_adc.readADC_Differential_1_3();            // First 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/3))) {} // 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_3();            // Third PE readout.
            piezo_electric_array[5] = piezo_electric_adc.readADC_Differential_1_3();            // First 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);
            }
        }
        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_wheel(LED_colour_wheel_percentage);                                  // Function to select colour.
        set_userinterface_LED();                                                        // Set LED's of user interface (LED's above buttons).

        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_3();         // 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);
//        }
    }
}