Jorick Leferink
emg eind code
Fork of
EMGStdevV3
by 
Dan August
Diff: main.cpp
- Revision:
- 2:56230ff9fc8f
- Parent:
- 1:1ffb9e3ae00f
diff -r 1ffb9e3ae00f -r 56230ff9fc8f main.cpp
--- a/main.cpp Mon Nov 04 13:26:18 2013 +0000
+++ b/main.cpp Tue Nov 05 07:53:04 2013 +0000
@@ -38,13 +38,12 @@
#define DEN3_3 -3.4119
#define DEN4_3 0.8104
-float std_dev (float input, int sig_number){
- // define variables
- static int startcount=0;
- float sum;
- int size = 20;
- float sig_out;
- float mean;
+double std_dev(float value, int number) {
+ int n;
+ float sum;
+ float sq_sum;
+ float mean;
+ float variance;
static int count_biceps=0;
static int count_triceps=0;
static int count_flexoren=0;
@@ -54,99 +53,76 @@
static float keeper_flexoren[20];
static float keeper_extensoren[20];
- if (startcount >= size)
- {//ja, dan gewoon std dev nemen, want keeper is al vol
- switch(sig_number){
+ switch (number){
case 1:
- keeper_biceps[count_biceps]=input;
+ int n = sizeof(keeper_biceps)/sizeof(float);
+ keeper_biceps[count_biceps]=value;
count_biceps++;
- if(count_biceps >= size)
- count_biceps=0;
- //sizeof(keeper_biceps)/sizeof(float);
- for(int i=0; i < size; i++)
- {sum+=keeper_biceps[i];
- }
- mean=sum/size;
- sum=0;
- for(int i=0; i < size; i++)
- {sum+=(keeper_biceps[i]-mean)*(keeper_biceps[i]-mean);
+ if ( count_biceps >= n)
+ count_biceps = 0;
+ double sum = 0;
+ double sq_sum = 0;
+ for(int i = 0; i < n; ++i) {
+ sum += keeper_biceps[i];
+ sq_sum += keeper_biceps[i] * keeper_biceps[i];
}
- sig_out=sqrt(sum/size);
- sum=0;
+ double mean = sum / n;
+ double variance = sq_sum / n - mean * mean;
+ return sqrt(variance);
break;
case 2:
- keeper_triceps[count_triceps]=input;
+ n = sizeof(keeper_triceps)/sizeof(float);
+ keeper_triceps[count_triceps]=value;
count_triceps++;
- if(count_triceps==size)
- count_triceps=0;
- //sizeof(keeper_triceps) / sizeof(float);
- for(int i=0; i < size; i++){
- sum+=keeper_triceps[i];
+ if ( count_triceps >= n)
+ count_triceps = 0;
+ sum = 0;
+ sq_sum = 0;
+ for(int i = 0; i < n; ++i) {
+ sum += keeper_triceps[i];
+ sq_sum += keeper_triceps[i] * keeper_triceps[i];
}
- mean=sum/size;
- sum=0;
- for(int i=0; i < size; i++){
- sum+=(keeper_triceps[i]-mean)*(keeper_triceps[i]-mean);
- }
- sig_out=sqrt(sum/size);
- sum=0;
+ mean = sum / n;
+ variance = sq_sum / n - mean * mean;
+ return sqrt(variance);
break;
- /*case 3:
- keeper_flexoren[count_flexoren]=input;
+ case 3:
+ n = sizeof(keeper_flexoren)/sizeof(float);
+ keeper_flexoren[count_flexoren]=value;
count_flexoren++;
- if(count_flexoren==size) count_flexoren=0;
-
- size=sizeof(keeper_flexoren)/sizeof(float);
- for(int i; i < size; i++){
- sum+=keeper_flexoren[i];
+ if ( count_flexoren >= n)
+ count_flexoren = 0;
+ sum = 0;
+ sq_sum = 0;
+ for(int i = 0; i < n; ++i) {
+ sum += keeper_flexoren[i];
+ sq_sum += keeper_flexoren[i] * keeper_flexoren[i];
}
- mean=sum/size;
- sum=0;
- for(int i; i < size; i++){
- sum+=(keeper_flexoren[i]-mean)*(keeper_flexoren[i]-mean);
- }
- sig_out=sqrt(sum/size);
- sum=0;
+ mean = sum / n;
+ variance = sq_sum / n - mean * mean;
+ return sqrt(variance);
break;
case 4:
- keeper_extensoren[count_extensoren]=input;
+ n = sizeof(keeper_extensoren)/sizeof(float);
+ keeper_triceps[count_extensoren]=value;
count_extensoren++;
- if(count_extensoren==size) count_extensoren=0;
-
- size=sizeof(keeper_extensoren)/sizeof(float);
- for(int i; i < size; i++){
- sum+=keeper_extensoren[i];
- }
- mean=sum/size;
- sum=0;
- for(int i; i < size; i++){
- sum+=(keeper_extensoren[i]-mean)*(keeper_extensoren[i]-mean);
+ if ( count_extensoren >= n)
+ count_extensoren = 0;
+ sum = 0;
+ sq_sum = 0;
+ for(int i = 0; i < n; ++i) {
+ sum += keeper_extensoren[i];
+ sq_sum += keeper_extensoren[i] * keeper_extensoren[i];
}
- sig_out=sqrt(sum/size);
- sum=0;
- break;*/
- } // einde switch
- return sig_out;
- } // einde if startcount ...
- else
- {startcount+=1;
- switch(sig_number){
- case 1:
- keeper_biceps[count_biceps]=input;
- count_biceps++;
- if(count_biceps >= size)
- count_biceps=0;
- break;
- case 2:
- keeper_triceps[count_triceps]=input;
- count_triceps++;
- if(count_triceps>=size)
- count_triceps=0;
- break;
- } // einde switch
- } // einde else
-} // einde std_dev
-
+ mean = sum / n;
+ variance = sq_sum / n - mean * mean;
+ return sqrt(variance);
+ break;
+ }
+
+
+}
+
float filter(int sig_number){
float sig_out;
// eerst variabelen definieren
@@ -207,15 +183,15 @@
in0_biceps = emg_biceps.read() - offset_biceps;
out4_biceps = out3_biceps; out3_biceps = out2_biceps; out2_biceps = out1_biceps; out1_biceps = out0_biceps;
out0_biceps = (NUM0*in0_biceps + NUM1*in1_biceps + NUM2*in2_biceps + NUM3*in3_biceps + NUM4*in4_biceps - DEN1*out1_biceps - DEN2*out2_biceps - DEN3*out3_biceps - DEN4*out4_biceps ) / DEN0;
- /*
+
//signaal filteren op 5Hz LOWPASS
in4_3_biceps = in3_3_biceps; in3_3_biceps = in2_3_biceps; in2_3_biceps = in1_3_biceps; in1_3_biceps = in0_3_biceps;
in0_3_biceps = abs(out0_biceps); // ruw - offset -> filter 1 -> stdev (-> filter 3)
out4_3_biceps = out3_3_biceps; out3_3_biceps = out2_3_biceps; out2_3_biceps = out1_3_biceps; out1_3_biceps = out0_3_biceps;
out0_3_biceps = (NUM0_3*in0_3_biceps + NUM1_3*in1_3_biceps + NUM2_3*in2_3_biceps + NUM3_3*in3_3_biceps + NUM4_3*in4_3_biceps - DEN1_3*out1_3_biceps - DEN2_3*out2_3_biceps - DEN3_3*out3_3_biceps - DEN4_3*out4_3_biceps ) / DEN0_3;
- */
+
- sig_out = out0_biceps;
+ sig_out = out0_3_biceps;
break;
case 2:
// signaal filteren op 15 Hz HIGHPASS
@@ -226,13 +202,13 @@
//signaal filteren op 5Hz LOWPASS
- /*in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps;
+ in4_3_triceps = in3_3_triceps; in3_3_triceps = in2_3_triceps; in2_3_triceps = in1_3_triceps; in1_3_triceps = in0_3_triceps;
in0_3_triceps = abs(out0_triceps);
out4_3_triceps = out3_3_triceps; out3_3_triceps = out2_3_triceps; out2_3_triceps = out1_3_triceps; out1_3_triceps = out0_3_triceps;
out0_3_triceps = (NUM0_3*in0_3_triceps + NUM1_3*in1_3_triceps + NUM2_3*in2_3_triceps + NUM3_3*in3_3_triceps + NUM4_3*in4_3_triceps - DEN1_3*out1_3_triceps - DEN2_3*out2_3_triceps - DEN3_3*out3_3_triceps - DEN4_3*out4_3_triceps ) / DEN0_3;
- */
+
- sig_out = out0_triceps;
+ sig_out = out0_3_triceps;
break;
case 3:
// signaal filteren op 15 Hz HIGHPASS
@@ -240,17 +216,17 @@
in0_flexoren = emg_flexoren.read();
out4_flexoren = out3_flexoren; out3_flexoren = out2_flexoren; out2_flexoren = out1_flexoren; out1_flexoren = out0_flexoren;
out0_flexoren = (NUM0*in0_flexoren + NUM1*in1_flexoren + NUM2*in2_flexoren + NUM3*in3_flexoren + NUM4*in4_flexoren - DEN1*out1_flexoren - DEN2*out2_flexoren - DEN3*out3_flexoren - DEN4*out4_flexoren ) / DEN0;
- /*
+
//signaal filteren op 5Hz LOWPASS
in4_3_flexoren = in3_3_flexoren; in3_3_flexoren = in2_3_flexoren; in2_3_flexoren = in1_3_flexoren; in1_3_flexoren = in0_3_flexoren;
- in0_3_flexoren = abs(out0_2_flexoren);
+ in0_3_flexoren = abs(out0_flexoren);
out4_3_flexoren = out3_3_flexoren; out3_3_flexoren = out2_3_flexoren; out2_3_flexoren = out1_3_flexoren; out1_3_flexoren = out0_3_flexoren;
out0_3_flexoren = (NUM0_3*in0_3_flexoren + NUM1_3*in1_3_flexoren + NUM2_3*in2_3_flexoren + NUM3_3*in3_3_flexoren + NUM4_3*in4_3_flexoren - DEN1_3*out1_3_flexoren - DEN2_3*out2_3_flexoren - DEN3_3*out3_3_flexoren - DEN4_3*out4_3_flexoren ) / DEN0_3;
- */
- sig_out = out0_flexoren;
+
+ sig_out = out0_3_flexoren;
break;
case 4:
// signaal filteren op 15 Hz HIGHPASS
@@ -258,17 +234,17 @@
in0_extensoren = emg_extensoren.read();
out4_extensoren = out3_extensoren; out3_extensoren = out2_extensoren; out2_extensoren = out1_extensoren; out1_extensoren = out0_extensoren;
out0_extensoren = (NUM0*in0_extensoren + NUM1*in1_extensoren + NUM2*in2_extensoren + NUM3*in3_extensoren + NUM4*in4_extensoren - DEN1*out1_extensoren - DEN2*out2_extensoren - DEN3*out3_extensoren - DEN4*out4_extensoren ) / DEN0;
- /*
+
//signaal filteren op 5Hz LOWPASS
in4_3_extensoren = in3_3_extensoren; in3_3_extensoren = in2_3_extensoren; in2_3_extensoren = in1_3_extensoren; in1_3_extensoren = in0_3_extensoren;
- in0_3_extensoren = abs(out0_2_extensoren);
+ in0_3_extensoren = abs(out0_extensoren);
out4_3_extensoren = out3_3_extensoren; out3_3_extensoren = out2_3_extensoren; out2_3_extensoren = out1_3_extensoren; out1_3_extensoren = out0_3_extensoren;
out0_3_extensoren = (NUM0_3*in0_3_extensoren + NUM1_3*in1_3_extensoren + NUM2_3*in2_3_extensoren + NUM3_3*in3_3_extensoren + NUM4_3*in4_3_extensoren - DEN1_3*out1_3_extensoren - DEN2_3*out2_3_extensoren - DEN3_3*out3_3_extensoren - DEN4_3*out4_3_extensoren ) / DEN0_3;
- */
- sig_out = out0_extensoren;
+
+ sig_out = out0_3_extensoren;
break;
}
return sig_out;
@@ -280,11 +256,20 @@
float emg_value_flexoren;
float emg_value_extensoren;
float dy;
+
//static int sig_count = 1;
-emg_value_biceps=std_dev(filter(1),1);
-emg_value_triceps=std_dev(filter(2),2);
+
+ emg_value_biceps = filter(1);
+ emg_value_triceps = filter(2);
+ emg_value_extensoren = filter(3);
+ emg_value_flexoren = filter(4);
+
+
+
+float dx;
+
dy = emg_value_biceps-emg_value_triceps;
-
+dx = emg_value_extensoren - emg_value_flexoren;
/*emg_value_flexoren = (100*filter(3)-44);
emg_value_extensoren = (100*filter(4)-44);*/
@@ -304,7 +289,7 @@
*/
//dy = emg_value_biceps-emg_value_triceps;
if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
- pc.printf("%.6f, %.6f, %.6f\n",emg_value_biceps, emg_value_triceps, dy);
+ pc.printf("%.6f\n",dy);
/**When not using the LED, the above could also have been done this way:
* pc.printf("%.6\n", emg0.read());
*/