Momo-Medical
Nucleo-transfer
Dependencies: ADS1015 MPU6050 PixelArray PixelArray-Nucleo mbed WS2813
Fork of
Nucleo-transfer
by 
Momo Medical
Sensorplate/main.cpp
- Committer:
- deldering95
- Date:
- 2018-07-03
- Revision:
- 72:9d9846baf590
- Parent:
- 71:040674ed2ce1
File content as of revision 72:9d9846baf590:
/********************* 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"
#include "Watchdog.h"
#define ALARMBUF 16
#define NUM_COLORS 8
#define YELLOW_TRANSITION 13
#define RED_TRANSITION 16
#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
Watchdog woofwoof;
InterruptIn button_lock(PC_0); // Input on intterupt base decleration.
InterruptIn button_reposition(PC_1);
InterruptIn button_mute(PC_2);
InterruptIn button_new_patient(PC_3);
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 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 = 100000; // I2C Frequency.
int baud_rate = 115200; // Baud rate.
int uart_input_buffer[120];
int buffer_counter = 0;
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;
int patient_present=4;
int patient_present_old=4;
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 = 30, intensity_night = 15; // 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;
bool i2c_error=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);
float percentage_tester=0;
// Variable to set if belt is used to test algorithm:
bool test_belt = 1;
// 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):
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.
{
mute_feedback_LED=1;
if(pi_serial.readable()) {
uart_input_buffer[buffer_counter] = pi_serial.getc();
buffer_counter++;
}
mute_feedback_LED=0;
}
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 (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[3], piezo_resistive_array[1], piezo_resistive_array[4], piezo_resistive_array[2], 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\n%d,%d\n%d,%d\n", piezo_electric_array[0], piezo_electric_array[3], piezo_electric_array[1], piezo_electric_array[4], piezo_electric_array[2],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
}
}
}
bool i2c_error_detect()
{
int error_counter=0;
for(int i=0; i<4; i++) {
if(piezo_resistive_array[i]==193+16*i)error_counter++;
if(piezo_resistive_array[i+4]==193+16*i)error_counter++;
}
if(error_counter==8)return 1;
error_counter=0;
for(int i=0; i<3; i++) {
if (piezo_electric_array[i]==149)error_counter++;
if (piezo_electric_array[i+3]==165)error_counter++;
}
if (error_counter==6)return 1;
return 0;
}
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*15)/100)+1;
float led_partly = ((percentage_in*15)%100);
if(leds_on>=YELLOW_TRANSITION&&leds_on<RED_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_old==colourbuf[4])) {
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_old==colourbuf[4])) {
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)>0) {
px.Set(((16-leds_on)%16),ring_colour);
px.SetI(((16-leds_on)%16),int(0.01*led_partly*intensity*2.55));
}
//usb_serial.printf("percentage in %d,%d\n",percentage_in,leds_on);
if(patient_present==4) {
//if(mixer>20)mixer=20;
//ring_colour = colour_fade(ring_colour,colourbuf[4],mixer);
px.SetAll(colourbuf[4]);
px.SetAllI(int(2.55*intensity));
//mixer++;
//usb_serial.putc(0xee);
}
/*else{
if(mixer>20)mixer=20;
ring_colour = colour_fade(colourbuf[4],ring_colour,mixer);
px.SetAll(ring_colour);
px.SetAllI(int(2.55*intensity));
mixer++;
usb_serial.putc(0xdd);
}*/
if(reposition_button_hold_timer.read_ms()) {
usb_serial.printf("filling circle repo\n");
circle_filling_reposition = true;
colour_wheel_filler = reposition_button_hold_timer.read_ms()/250+1;
if(colour_wheel_filler>=9) {
colour_wheel_filler=9;
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 patient\n");
circle_filling_new_patient = true;
colour_wheel_filler = new_patient_button_hold_timer.read_ms()/250+1;
usb_serial.printf("Hold %d\n",colour_wheel_filler);
if(colour_wheel_filler>=9) {
colour_wheel_filler=9;
circle_filled_new_patient = 10;
circle_filling_new_patient = false;
colour_wheel_drain_new_patient = false;
new_patient_flag = 1;
//usb_serial.printf("patient filled");
}
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]);
//usb_serial.printf("counter %d\n",k);
}
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 repo\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));
if(colour_wheel_filler>=5) {
colour_wheel_drain_reposition=true;
circle_filling_reposition=false;
} else colour_wheel_filler++;
}
if(!new_patient_button_hold_timer.read_ms()&&circle_filling_new_patient&&!colour_wheel_drain_new_patient) {
usb_serial.printf("Short hold patient\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));
if(colour_wheel_filler>=5) {
colour_wheel_drain_new_patient=true;
circle_filling_new_patient=false;
} else colour_wheel_filler++;
}
//usb_serial.printf("pre drain %d\n",colour_wheel_filler);
if(colour_wheel_drain_reposition&&!circle_filling_new_patient) {
usb_serial.printf("drain_repo\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<0)colour_wheel_filler=0;
if(!colour_wheel_filler)colour_wheel_drain_reposition=false;
}
//usb_serial.printf("post repo %d\n",colour_wheel_filler);
if(colour_wheel_drain_new_patient&&!circle_filling_reposition) {
usb_serial.printf("drain_patient\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<0)colour_wheel_filler=0;
if(!colour_wheel_filler)colour_wheel_drain_new_patient=false;
}
//usb_serial.printf("post drain %d\n",colour_wheel_filler);
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--;
percentage_tester=0;
}
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--;
}
if(!connection_test_sensorplate||i2c_error) {
//usb_serial.printf("i2c error = %d\n",i2c_error);
px.Set(5,colourbuf[0]);
px.Set(6,colourbuf[0]);
px.Set(7,colourbuf[0]);
px.SetI(5,int(intensity*0.5));
px.SetI(6,int(intensity*2.55));
px.SetI(7,int(intensity*0.5));
}
ws.write_offsets(px.getBuf(),0,0,0);
ring_colour_old=ring_colour;
}
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_new_patient == 1) {
new_patient_button_hold_timer.stop();
new_patient_button_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()
{
//NVIC_SystemReset();
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 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.
}
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);
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.rise(&sensorplate_detached);
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.
colour_code = (colour_code_1 << 1 | colour_code_0);
if(colour_code != 0b00 && pi_active == false) {
pi_active = true;
}
if(!pi_active) {
speaker1 = 1;
wait_ms(boot_delay_ms); // Wait to boot sensorplate first.
speaker1 = 0;
__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;
}//else if(comet>160)pi_active=true;
if(testpin_sensorplate.read()||i2c_error) {
px.Set(5,colourbuf[0]);
px.Set(6,colourbuf[0]);
px.Set(7,colourbuf[0]);
px.SetI(5,int(intensity*0.5));
px.SetI(6,int(intensity*2.55));
px.SetI(7,int(intensity*0.5));
}
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);
if(testpin_sensorplate.read()||i2c_error) {
px.Set(5,colourbuf[0]);
px.Set(6,colourbuf[0]);
px.Set(7,colourbuf[0]);
px.SetI(5,int(intensity*0.5));
px.SetI(6,int(intensity*2.55));
px.SetI(7,int(intensity*0.5));
}
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.
woofwoof.Configure(10);
while (1) {
piezo_electric_sample_timer.reset(); // Clock gebruiken o.i.d.?
piezo_electric_sample_timer.start();
connection_test_sensorplate = !testpin_sensorplate && pi_active;
//usb_serial.printf("start %d\n",piezo_electric_sample_timer.read_us());
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 (!testpin_sensorplate) {
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());
}
//usb_serial.printf("eenderde %d\n",piezo_electric_sample_timer.read_us());
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());
}
//usb_serial.printf("tweederde %d\n",piezo_electric_sample_timer.read_us());
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.getAccelero(accelerometer_reference_belt); // Get accelerometer data from belt.
if(accelerometer_reference_belt[0]==0) {
MPU6050 angle_device_reference_belt(PB_9, PB_8);
angle_device_reference_belt.getAccelero(accelerometer_reference_belt); // Get accelerometer data from belt.
}
angle_device_reference_belt.getGyro(gyroscope_reference_belt); // Get gyroscope 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]);
//usb_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());
}
if(buffer_counter>4&& buffer_counter%2==0) {
for(int i=0; i<buffer_counter-1; i++) {
if(uart_input_buffer[i]=='='&&(uart_input_buffer[(i+2)%4]=='{'||uart_input_buffer[(i+2)%4]=='='))patient_present=uart_input_buffer[(i+1)];
if(uart_input_buffer[i]=='{'&&(uart_input_buffer[(i+2)%4]=='='||uart_input_buffer[(i+2)%4]=='{'))LED_colour_wheel_percentage=uart_input_buffer[(i+1)];
if((patient_present_old==4&&patient_present!=4)||(patient_present_old!=4&&patient_present==4))mixer=0;
//usb_serial.putc(uart_input_buffer[i]);
}
//usb_serial.putc(0x20);
patient_present_old=patient_present;
buffer_counter=0;
}
if(1==0){
percentage_tester+=0.25;
patient_present=1;
LED_colour_wheel_percentage=percentage_tester;
}
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());
}
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(connection_test_sensorplate)i2c_error=i2c_error_detect();
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());
}
woofwoof.Service();
//usb_serial.printf("einde %d\n",piezo_electric_sample_timer.read_us());
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);
// }
}
}