Momo-Medical
Nucleo-transfer
Dependencies: ADS1015 MPU6050 PixelArray PixelArray-Nucleo mbed WS2813
Fork of
Nucleo-transfer
by 
Momo Medical
Diff: Sensorplate/main.cpp
- Revision:
- 68:1663f305ac33
- Parent:
- 67:1b300aa30923
- Child:
- 69:98db4df7278f
diff -r 1b300aa30923 -r 1663f305ac33 Sensorplate/main.cpp
--- a/Sensorplate/main.cpp Tue May 29 09:38:01 2018 +0000
+++ b/Sensorplate/main.cpp Tue Jun 05 12:17:57 2018 +0000
@@ -34,8 +34,8 @@
#define ALARMBUF 16
#define NUM_COLORS 8
-#define YELLOW_TRANSITION 12
-#define RED_TRANSITION 15
+#define YELLOW_TRANSITION 13
+#define RED_TRANSITION 16
#define NUM_LEDS_PER_COLOR 3
#define NUMBER_LED_FRONT (3)
#define ONE_COLOR
@@ -44,9 +44,9 @@
#define FADE_STEPS 20
InterruptIn button_lock(PC_0); // Input on intterupt base decleration.
-InterruptIn button_reposition(PC_3);
+InterruptIn button_reposition(PC_1);
InterruptIn button_mute(PC_2);
-InterruptIn button_new_patient(PC_1);
+InterruptIn button_new_patient(PC_3);
DigitalIn intensity_code(PA_12);
DigitalIn colour_code_1(PA_11);
@@ -85,7 +85,7 @@
*/
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.
+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.
@@ -99,6 +99,8 @@
int total_comet_cycle_time_ms = 750/16;
int i2c__frequency = 400000; // 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.
@@ -124,7 +126,8 @@
int LED_colour_wheel_percentage=0;
int ring_colour_old,mixer=0;
int colour_wheel_filler = 5;
-bool patient_present=true;
+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;
@@ -135,10 +138,11 @@
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.
+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;
@@ -150,7 +154,7 @@
bool test_belt = 0;
// Set test mode on (log functions to pc serial: interrupts, LED intensity and serial messages):
-bool test_mode = 1;
+bool test_mode = 0;
// Variable for connection test (should be changed):
bool connection_test_sensorplate;
@@ -174,7 +178,12 @@
void serial_read() // Function for serial read for select LED intensity and colour.
{
- LED_colour_wheel_percentage = pi_serial.getc();
+ 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.
@@ -352,10 +361,10 @@
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
+ 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,%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
+ 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);
@@ -366,6 +375,24 @@
}
+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;
@@ -396,44 +423,56 @@
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) {
+ 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]) {
+ 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(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]) {
+ 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);
+ //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));
+ 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){
+ 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\n");
+ //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) {
@@ -452,7 +491,7 @@
}
if(new_patient_button_hold_timer.read_ms()) {
- usb_serial.printf("filling circle\n");
+ //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) {
@@ -471,7 +510,7 @@
}
if(!reposition_button_hold_timer.read_ms()&&circle_filling_reposition&&!colour_wheel_drain_reposition) {
- usb_serial.printf("Short hold\n");
+ //usb_serial.printf("Short hold\n");
px.SetAll(colourbuf[6]);
for(int k =0; k<=colour_wheel_filler; k++) {
px.Set(k,colourbuf[5]);
@@ -486,7 +525,7 @@
}
if(!new_patient_button_hold_timer.read_ms()&&circle_filling_new_patient&&!colour_wheel_drain_new_patient) {
- usb_serial.printf("Short hold\n");
+ //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]);
@@ -501,7 +540,7 @@
}
if(colour_wheel_drain_reposition) {
- usb_serial.printf("drain\n");
+ //usb_serial.printf("drain\n");
px.SetAll(colourbuf[6]);
for(int k =0; k<=colour_wheel_filler; k++) {
px.Set(k,colourbuf[5]);
@@ -513,7 +552,7 @@
}
if(colour_wheel_drain_new_patient) {
- usb_serial.printf("drain\n");
+ //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]);
@@ -525,7 +564,7 @@
}
if(circle_filled_reposition) {
- usb_serial.printf("circle_filled_repo\n");
+ //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]);
@@ -537,14 +576,18 @@
}
if(circle_filled_new_patient) {
- usb_serial.printf("circle_filled_new_patient\n");
+ //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) {
+ if(!connection_test_sensorplate||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;
@@ -897,57 +940,71 @@
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;
- }
- if(testpin_sensorplate.read()) {
- px.Set(6,colourbuf[0]);
- px.SetI(6,255);
- }
- 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()) {
- px.Set(6,colourbuf[0]);
- px.SetI(6,255);
- }
- while(comet_timer.read_ms()<total_comet_cycle_time_ms) {}
- ws.write_offsets(px.getBuf(),0,0,0);
+ colour_code = (colour_code_1 << 1 | colour_code_0);
+ if(colour_code != 0b00 && pi_active == false) {
+ pi_active = true;
}
- __enable_irq();
- wait(1);
+ if(!pi_active) {
+ __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;
+ }
+ 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();
@@ -962,6 +1019,7 @@
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("Loop\n");
if (test_mode == 1) {
// usb_serial.printf("Connection test sensorplate = %d\n", connection_test_sensorplate);
@@ -1011,8 +1069,12 @@
// 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.
- angle_device_reference_belt.getAccelero(accelerometer_reference_belt); // Get accelerometer data from belt.
}
if (connection_test_sensorplate == 1) {
@@ -1021,6 +1083,7 @@
} 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 {
@@ -1035,6 +1098,16 @@
// usb_serial.printf("Loop time after timer_functions: %d ms\n",piezo_electric_sample_timer.read_ms());
}
+ if(buffer_counter>4) {
+ for(int i=0; i<4; 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]);
+ }
+ patient_present_old=patient_present;
+ buffer_counter=0;
+ }
colour_wheel(LED_colour_wheel_percentage); // Function to select colour.
percentage_tester+=0.2;
//LED_colour_wheel_percentage=percentage_tester;
@@ -1044,10 +1117,6 @@
// 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) {
@@ -1059,9 +1128,10 @@
}
//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());
+ //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.