Diff: Sensorplate/main.cpp

Revision:

33:df21cb8dc5c7

Parent:

32:0944efc47e46

Child:

34:1614f4f2b841

--- a/Sensorplate/main.cpp	Wed Oct 11 07:39:14 2017 +0000
+++ b/Sensorplate/main.cpp	Wed Oct 11 12:56:17 2017 +0000
@@ -1,10 +1,16 @@
 /********************* CODE INFORMATIE ******************************
-Author          :   Danny Eldering & Ricardo Molenaar
-Company         :   Momo Medical
-Source          :   developer.mbed.org
-File            :   main.cpp
-Version | -date :   1.0 | 28-9-2017
-Flowchart       :   https://drive.google.com/open?id=0B_6KzOyPBBX-NExwRFc1VEtEdUk
+Date of creation:           20-12-1908
+Author:                     Elke Salzmann
+co-authors:                 Menno Gravemaker
+(c) Copyright by Momo Medical BV. 
+ 
+Current version name:                   2.1.2
+Date of creation:                       14-01-2017
+Purpose of this file                    Oneliner of purpose 
+Update ‘what’s new in this version?’:   short meaningful description (no more than 3 lines) 
+Todo:                                   short meaningful description of next actions that could be worked on(less than 3 lines) 
+Source file:                            http://mbed.com 
+
 */
 
 /************************ CONFIG ***********************************/
@@ -12,6 +18,7 @@
 #include "mbed.h"
 #include "Adafruit_ADS1015.h"
 #include "MPU6050.h"
+#include "MPU6050_belt.h"
 #include "neopixel.h"
 #define NLED (3)
 #define ONE_COLOR
@@ -20,7 +27,6 @@
 InterruptIn reposition(p17);
 InterruptIn mute(p16);
 InterruptIn new_patient(p18);
-DigitalIn   supplyvoltage(p20);                                             // Analog input between 0 and 1 for reading supplyvoltage from measuringpoint before power supply.
 
 DigitalOut  LED_intern1(LED1);
 DigitalOut  LED_intern2(LED2);
@@ -44,6 +50,7 @@
 I2C i2c(p28, p27);                  // I2C
 I2C i2cAccu(p9, p10);               // I2C for accupack
 MPU6050 agu(p28,p27);               // Accelerometer/Gyroscope Unit
+MPU6050_belt agu_belt(p28,p27);     // Accelerometer/Gyroscope Unit Belt
 Adafruit_ADS1115 pr1(&i2c, 0x48);   // first PiëzoResistive ADC
 Adafruit_ADS1115 pr2(&i2c, 0x49);   // second PiëzoResistive ADC
 Adafruit_ADS1115 pel(&i2c, 0x4B);   // PiëzoElectric ADC
@@ -63,6 +70,8 @@
 int k = 0;
 float acce[3];                      // Raw accelerometer data
 float gyro[3];                      // Raw gyroscope data
+float acce_belt[3];                 // Raw accelerometer data from belt
+float gyro_belt[3];                 // Raw gyroscope data from belt
 char LED_colour = 'g';                                                            // Variable to set LED colour.
 bool lock_state = 0, lock_flag = 0, mute_state = 0, alarm = 0, calibration_flag = 0, intensity_select = 1;            // Boolean variables for states logging.
 bool mute_flag = 0, new_patient_flag = 0, reposition_flag = 0;
@@ -81,6 +90,16 @@
 double intensity, control_LED_intensity = 0;                                // Variable between 0 and 1 to set the intensity of the LED's above the buttons.
 int a; // Test
 
+/*************************** TEST ********************************/
+// Verify algoritm function: for belt activation, set test_belt 1 (connect pin p20 to 3.3V)
+DigitalIn test_pin(p20, PullDown);
+bool test_belt = 1; //test_pin;
+
+// Verify if interrupts are working properly
+
+// Verify if parameters are 
+bool test_mode = 1;
+
 /*************************** CODE ********************************/
 
 void set_intensity()                                                        // Function to set the intensity for the LED's.
@@ -91,34 +110,24 @@
         intensity = intensity_night;
     }
     control_LED_intensity = (intensity/100);
-    pc.printf("Intensity LED's shines to wall = %f\n", intensity);
-    pc.printf("Intensity LED above buttons = %f\n", control_LED_intensity);
+    
+    if (test_mode == 1) {
+        pc.printf("Intensity LED's shines to wall = %f\n", intensity);
+        pc.printf("Intensity LED's above buttons = %f\n", control_LED_intensity);
+    }
 }
 
 void serial_read()                                                          // Serial read for select LED intensity and colour.
 {
     if (pi.readable()) {
-        char message[10];
+        char message[4];
         pi.scanf("%s", message);
-
-        if (message[0] == '0') {
-            intensity_select = 0;
-        }
-
-        if (message[0] == '1') {
-            intensity_select = 1;
-        }
-
-        if (message[1] == 'g') {
-            LED_colour = 'g';
-        }
-
-        if (message[1] == 'y') {
-            LED_colour = 'y';
-        }
-
-        if (message[1] == 'r') {
-            LED_colour = 'r';
+        
+        intensity_select = message[0];
+        LED_colour = message[1];
+        
+        if (test_mode == 1) {
+            pc.printf("%s", message);   
         }
     }
 }
@@ -127,24 +136,41 @@
 {
     if (mute_flag == 1) {
         pi.printf(">01\n");
-        pc.printf(">01\n");
+        
+        if (test_mode == 1) { 
+            pc.printf(">01\n");
+        }
+        
         mute_flag = 0;
     }
 
     if (new_patient_flag == 1) {
         pi.printf(">03\n");
-        pc.printf(">03\n");
+        
+        if (test_mode == 1) { 
+            pc.printf(">03\n");
+        }
+        
         new_patient_flag = 0;
     }
 
     if (reposition_flag == 1) {
         pi.printf(">02\n");
-        pc.printf(">02\n");
+        
+        if (test_mode == 1) { 
+            pc.printf(">02\n");
+        }
+        
         reposition_flag = 0;
     }
 
     if (batteryvoltage_current != batteryvoltage_last) {
         pi.printf("%%d\n", batteryvoltage_current);
+        
+        if (test_mode == 1) {    
+            pc.printf("%%d\n", batteryvoltage_current);
+        }
+        
         batteryvoltage_last = batteryvoltage_current;
     }
 
@@ -204,10 +230,13 @@
             power_plug_logged = power_plug_state;
         }
     }
-    if(a == 1) {
-        pi.printf("!,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,\n", res[4], res[7], res[6], res[5], res[1], res[0], res[2], res[3], elec[0], elec[1], elec[2], elec[3], elec[4], acce[0]*100, acce[1]*100, acce[2]*100, gyro[0]*100, gyro[1]*100, gyro[2]*100); // print all to serial port
+    
+    if (a == 1) {
+        pi.printf("!,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", res[4], res[7], res[6], res[5], res[1], res[0], res[2], res[3], elec[0], elec[1], elec[2], elec[3], elec[4], acce[0], acce[1], acce[2], gyro[0], gyro[1], gyro[2], acce_belt[0], acce_belt[1], acce_belt[2], gyro_belt[0], gyro_belt[1], gyro_belt[2]); // print all to serial port
+        pc.printf("Belt accelerometer values: %f,%f,%f,%f,%f,%f\n", acce_belt[0], acce_belt[1], acce_belt[2], gyro_belt[0], gyro_belt[1], gyro_belt[2]);
     }
     //receiving order: 8 resistive sensors, 5 electric readings, 3 accelerometer axes, 3 gyroscope axes
+    //Belt values print to pc
 }
 
 void colour_select(char LED_colour)                                         // Function to select the colour.
@@ -372,7 +401,7 @@
 
 void timer_functions()
 {
-    pc.printf("locktime= %d\n",lock_hold_timer.read_ms());
+    pc.printf("Locktime = %d\n",lock_hold_timer.read_ms());
     if ((lock_hold_timer.read_ms() > locktime_ms) && lock_flag == 0 && lock == 0) { // If statement for lock function.
         lock_flag = 1;
         LED_intern2 = !LED_intern2;
@@ -503,7 +532,12 @@
             angle = acce[2]*100;
         }
         agu.getGyro(gyro);                                                  // Get gyroscope data
-
+        
+        if (test_belt == 1) {
+            agu_belt.getGyro(gyro_belt);                                        // Get gyroscope data from Belt
+            agu_belt.getAccelero(acce_belt);                                    // Get accelerometer data from belt
+        }
+        
         while(t.read_us()<(3*(cycle_time/5))) {}                            // Wait untill 60% of cycle
 
         elec[3] = pel.readADC_SingleEnded(0);                               // Fourth PE readout