Felix W
Workshop2
Diff: main.cpp
- Revision:
- 13:bc1df4256578
- Parent:
- 12:e5be1469bc76
--- a/main.cpp Wed Apr 14 14:08:47 2021 +0000
+++ b/main.cpp Wed Apr 21 15:07:50 2021 +0000
@@ -1,3 +1,16 @@
+//Buttons zum ein und ausschalten der Messung
+
+//Sensor data sheet https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Datasheets/Sensirion_Differential_Pressure_Datasheet_SdP8xx_Analog.pdf
+
+//Messung via printf auf CMD Zeil ausgeben, mit P5 serial connector verscuchen abzugreifen
+
+//Druck respektive die Konstruktion zum Sensor & Ventouri vorbereiten
+
+//Ventouri richtig rechnen, mit Luftvolumen von Lunge und der Range des Sensors
+
+//Baud Rate 115200
+
+
#include "mbed.h"
#include "platform/mbed_thread.h"
@@ -8,158 +21,89 @@
#include "FastPWM.h"
#include "RangeFinder.h"
+#include "math.h"
+
using namespace std::chrono;
InterruptIn user_button(USER_BUTTON);
-DigitalOut led(LED1);
+DigitalOut led(LED1);
+Timer runtimeTimer, user_button_timer;
-bool executeMainTask = false;
-Timer user_button_timer, loop_timer;
-int Ts_ms = 50;
+bool executeMainTask = false;
+int Ts_ms = 50;
-/* declaration of custom button functions */
+//initialize functions
void button_fall();
void button_rise();
+float readSensor(AnalogIn sensor);
+float map_pressure_sensor(float input);
+void printP5(int value);
+void printSerialPlotter(int v1 , int v2);
+float venturiSpeed(float DiaEntry, float DiaNarrow, float densAir, float dP);
+float venturiFlow(int timeStart, int timeStop);
+float getCirlceArea(float diameter);
+//float detectBreath(float aiflow, int start, int stop);
+bool detectBreath(float mappedSensorValue);
-// SHARP GP2Y0A21Y IR Sensor
-// https://www.digitec.ch/de/s1/product/sharp-distanz-sensor-1-st-gp2y0a21y-aktive-bauelemente-8425699?gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hMUr4sljdd8LfsdhBBlhxKY5gyDmZQ49ghgiIRZaKWdj85ISUw5r4oaAmM9EALw_wcB&gclsrc=aw.ds
-/* create analog input object */
-AnalogIn analogIn(PC_2);
-float dist_IRSensor = 0.0f;
+//Analog Port for sensor
+AnalogIn analogIn(PA_0);
+float pressureDifference = 0.0f;
+float VDD = 3.3f;
+
-// 78:1 Metal Gearmotor 20Dx43L mm 12V CB
-// https://www.pololu.com/product/3477
-/* create enable dc motor digital out object */
-DigitalOut enable_motors(PB_15);
-/* create pwm objects */
-FastPWM pwmOut_M1(PB_13);
-FastPWM pwmOut_M2(PA_9);
-FastPWM pwmOut_M3(PA_10);
-double Ts_pwm_s = 0.00005; // this needs to be a double value (no f at the end)
-/* create encoder read objects */
-EncoderCounter encoderCounter_M1(PA_6, PC_7);
-EncoderCounter encoderCounter_M2(PB_6, PB_7);
-EncoderCounter encoderCounter_M3(PA_0, PA_1);
-/* create speed controller objects, only M1 and M2, M3 is used open-loop */
-float counts_per_turn = 20.0f*78.125f; // counts/turn * gearratio
-float kn = 180.0f/12.0f; // (RPM/V)
-float max_voltage = 12.0f; // adjust this to 6.0f if only one batterypack is used
-SpeedController speedController_M1(counts_per_turn, kn, max_voltage, pwmOut_M1, encoderCounter_M1);
-SpeedController speedController_M2(counts_per_turn, kn, max_voltage, pwmOut_M2, encoderCounter_M2);
+//Venturi Pipe parameters
+float DiaEntry = 6 * 10e-3; //m
+float DiaNarrow = 2.2 * 10e-3; //m
+float densityAir = 1.2041; //kg/m^3
+float pi = 3.14159;
+int threshold = 5;
-// Futaba Servo S3001 20mm 3kg Analog
-// https://www.modellmarkt24.ch/pi/RC-Elektronik/Servos/Standard-Servo-20mm/futaba-servo-s3001-20mm-3kg-analog.html?gclid=CjwKCAjw3pWDBhB3EiwAV1c5rK_-x_Bt19_wIY-IcS2C-RULXKBtYfY0byxejkZLjASro-EMPBUhrxoCgaQQAvD_BwE
-/* create servo objects */
-Servo servo_S1(PB_2);
-Servo servo_S2(PC_8);
-Servo servo_S3(PC_6);
-// Servo servo_S3(PC_6); // PC_6 is used for ultra sonic sensor below
-int servoPeriod_mus = 20000;
-int servoOutput_mus_S1 = 0;
-int servoOutput_mus_S2 = 0;
-int servoOutput_mus_S3 = 0;
-int servo_counter = 0;
-int loops_per_second = static_cast<int>(ceilf(1.0f/(0.001f*(float)Ts_ms)));
+int nBreath = 0;
+float totalBreathVolume = 0;
+float currentBreathVolume = 0;
-// Groove Ultrasonic Ranger V2.0
-// https://ch.rs-online.com/web/p/entwicklungstools-sensorik/1743238/?cm_mmc=CH-PLA-DS3A-_-google-_-CSS_CH_DE_Raspberry_Pi_%26_Arduino_und_Entwicklungstools_Whoop-_-(CH:Whoop!)+Entwicklungstools+Sensorik-_-1743238&matchtype=&pla-306637898829&gclid=Cj0KCQjwpdqDBhCSARIsAEUJ0hOLQOOaw_2-Ob03u4YGwXthQPeSyjaazFqNuMkTIT8Ie18B1pD7P9AaAn18EALw_wcB&gclsrc=aw.ds
-/* create range finder object (ultra sonic distance sensor) */
-RangeFinder rangeFinder(PC_6, 5782.0f, 0.02f, 17500); // 1/Ts_ms = 20 Hz parametrization
-// RangeFinder rangefinder(PB_6, 5782.0f, 0.02f, 7000); // 1/Ts_ms = 50 Hz parametrization
-float dist_USSensor = 0.0f;
-
-int main()
-{
+int main(){
+
+ int i = 0;
+ runtimeTimer.start();
user_button.fall(&button_fall);
user_button.rise(&button_rise);
- loop_timer.start();
-
- /* enable hardwaredriver dc motors */
- enable_motors = 1;
- /* initialize pwm for motor M3*/
- pwmOut_M3.period(Ts_pwm_s);
- /* set pwm output zero at the beginning, range: 0...1 -> u_min...u_max */
- pwmOut_M3.write(0.5);
-
- /* enable servos, you can also disable them */
- servo_S1.Enable(servoOutput_mus_S1, servoPeriod_mus);
- servo_S2.Enable(servoOutput_mus_S2, servoPeriod_mus);
- servo_S3.Enable(servoOutput_mus_S3, servoPeriod_mus);
+ int last_print = -999999;
- while (true) {
-
- loop_timer.reset();
-
- /* ------------- start hacking ------------- -------------*/
-
- if (executeMainTask) {
-
- /* read analog input */
- dist_IRSensor = analogIn.read() * 3.3f;
-
- /* command a speed to dc motors M1 and M2*/
- speedController_M1.setDesiredSpeedRPS( 1.0f);
- speedController_M2.setDesiredSpeedRPS(-0.5f);
- /* write output voltage to motor M3 */
- pwmOut_M3.write(0.75);
+ // printf("System running %f\n\r", pressureDifference);
+ while(true){
+ if (executeMainTask){
+
+ i = 0;
- /* command servo position via output time, this needs to be calibrated */
- servo_S1.SetPosition(servoOutput_mus_S1);
- servo_S2.SetPosition(servoOutput_mus_S2);
- servo_S3.SetPosition(servoOutput_mus_S3);
- if (servoOutput_mus_S1 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
- servoOutput_mus_S1 += 100;
- }
- if (servoOutput_mus_S2 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
- servoOutput_mus_S2 += 100;
- }
- if (servoOutput_mus_S3 <= servoPeriod_mus & servo_counter%loops_per_second == 0 & servo_counter != 0) {
- servoOutput_mus_S3 += 100;
+ //pressureDifference = analogIn.read()*VDD;
+ pressureDifference = readSensor(analogIn);
+ float dP = map_pressure_sensor(pressureDifference);
+
+ int timestamp = runtimeTimer.elapsed_time().count();
+ float speed = venturiSpeed(DiaEntry, DiaNarrow, densityAir, dP);
+ float airVolume = venturiFlow(timestamp, last_print);
+ totalBreathVolume += airVolume;
+ if (detectBreath(dP)){
+ currentBreathVolume += airVolume;
+ } else if (!detectBreath(dP)){
+ currentBreathVolume *= 0.1;
}
- servo_counter++;
-
- /* read ultra sonic distance sensor */
- dist_USSensor = rangeFinder.read_cm();
-
- /* visual feedback that the main task is executed */
- led = !led;
-
- } else {
-
- dist_IRSensor = 0.0f;
-
- speedController_M1.setDesiredSpeedRPS(0.0f);
- speedController_M2.setDesiredSpeedRPS(0.0f);
- pwmOut_M3.write(0.5);
-
- servoOutput_mus_S1 = 0;
- servoOutput_mus_S2 = 0;
- servoOutput_mus_S3 = 0;
- servo_S1.SetPosition(servoOutput_mus_S1);
- servo_S2.SetPosition(servoOutput_mus_S2);
- servo_S3.SetPosition(servoOutput_mus_S3);
-
- dist_USSensor = 0.0f;
-
- led = 0;
+ if ((timestamp-last_print) > 50000){
+ //printf("%d, %d, %f, %f, %f, %f, %f, %f\n\r", timestamp,last_print,dP, pressureDifference,speed, airVolume, currentBreathVolume ,totalBreathVolume);
+ printf("%f, %f, %f, %f, %f, %f\n\r", dP, pressureDifference,speed, airVolume, currentBreathVolume ,totalBreathVolume);
+ last_print = timestamp;
+
+ }
+ led != led;
}
- /* do only output via serial what's really necessary (this makes your code slow)*/
- printf("%3.3f, %3d, %3d, %3d, %3d, %3.3f, %3.3f, %3.3f;\r\n",
- dist_IRSensor,
- servoOutput_mus_S1,
- servoOutput_mus_S2,
- servoOutput_mus_S3,
- encoderCounter_M3.read(),
- speedController_M1.getSpeedRPS(),
- speedController_M2.getSpeedRPS(),
- dist_USSensor);
-
- /* ------------- stop hacking ------------- -------------*/
-
- int T_loop_ms = duration_cast<milliseconds>(loop_timer.elapsed_time()).count();
- int dT_loop_ms = Ts_ms - T_loop_ms;
- thread_sleep_for(dT_loop_ms);
+ else {
+ if (i==0){
+ printf("Program paused, waiting until button gets pressed!\r\n");
+ i = 1;
+ }
+ }
}
}
@@ -168,12 +112,68 @@
user_button_timer.reset();
user_button_timer.start();
}
-
+
void button_rise()
{
- int t_button_ms = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
+ int t_button = duration_cast<milliseconds>(user_button_timer.elapsed_time()).count();
user_button_timer.stop();
- if (t_button_ms > 200) {
- executeMainTask = !executeMainTask;
+ if(t_button > 200) executeMainTask = !executeMainTask;
+}
+
+float readSensor(AnalogIn sensor){
+ return sensor.read() * VDD;
+}
+
+float map_pressure_sensor(float input){
+ float dP;
+ dP = ((750*input)/VDD) - 150;
+ if (abs(dP)< threshold){ //Filtering out noise
+ dP = 0;
+ }
+
+
+ return dP;
+}
+
+void printP5(int value){
+ printf("%d\r\n", value);
+}
+
+void printSerialPlotter(int v1 , int v2){
+ printf("%d %d",v1,v2);
+}
+
+float venturiSpeed(float DiaEntry, float DiaNarrow, float densAir, float dP){
+ //formula https://www.leifiphysik.de/mechanik/stroemungslehre/ausblick/venturi-rohr
+ float vEntry;
+ float A1 = getCirlceArea(DiaEntry);
+ float A2 = getCirlceArea(DiaNarrow);
+ float absdP = abs(dP);
+ vEntry = sqrt(abs((2*dP)/(densAir*((pow((A1-A2),2)-1)))));
+ if(dP < 0){
+ vEntry = (-1)*vEntry;
+ }
+ return vEntry;
+}
+
+float venturiFlow(int timeStart, int timeStop){
+ float volume;
+ float flowSpeed = venturiSpeed(DiaEntry, DiaNarrow, densityAir, map_pressure_sensor(pressureDifference));
+ float flowRate = flowSpeed * getCirlceArea(DiaEntry);
+ int timeDelate = timeStop - timeStart; //Microseconds
+ volume = timeDelate *10e-6 * flowRate; //to SI
+ return abs(volume);
+}
+
+float getCirlceArea(float diameter){
+ float area = (pow(diameter/2,2))*pi;
+ return area;
+}
+
+bool detectBreath(float mappedSensorValue){
+ if (mappedSensorValue > 6){
+ return true;
+ }else{
+ return false;
}
}
\ No newline at end of file