Dan August
nu met gains, threshold en eerst opvullen
Fork of
EMGStdevV3
by 
Dan August
Diff: main.cpp
- Revision:
- 0:59e2ea255892
- Child:
- 1:1ffb9e3ae00f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun Nov 03 20:18:17 2013 +0000
@@ -0,0 +1,329 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+
+//Define objects
+AnalogIn emg_biceps(PTB0);
+AnalogIn emg_triceps(PTB1);
+AnalogIn emg_flexoren(PTB2);
+AnalogIn emg_extensoren(PTB3); //Analog input
+PwmOut red(LED_RED); //PWM output
+Ticker timer;
+MODSERIAL pc(USBTX,USBRX,64,1024);
+
+#define offset_biceps 0 // offset ruwe invoer met adapter motoren, waar toepassen?
+
+//high pass filter constantes 15Hz cutoff 4e orde, Fs = 312,5Hz (geeft een wat mooiere waarde voor periode en is geen veelvoud van 50Hz)
+#define NUM0 0.6731 // constante
+#define NUM1 -2.6925 // z^-1
+#define NUM2 4.0388 // z^-2etc.
+#define NUM3 -2.6925
+#define NUM4 0.6731
+
+#define DEN0 1 // constante
+#define DEN1 -3.2133
+#define DEN2 3.9348
+#define DEN3 -2.1689
+#define DEN4 0.4531
+
+//lowpass filter constantes 4Hz 4e orde, Fs = 312,5 z
+#define NUM0_3 0.00000236 // constante
+#define NUM1_3 0.00000944 // z^-1
+#define NUM2_3 0.00001415 // z^-2etc.
+#define NUM3_3 0.00000944
+#define NUM4_3 0.00000236
+
+#define DEN0_3 1 // constante
+#define DEN1_3 -3.7899
+#define DEN2_3 5.3914
+#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;
+ static int count_biceps=0;
+ static int count_triceps=0;
+ static int count_flexoren=0;
+ static int count_extensoren=0;
+ static float keeper_biceps[20];
+ static float keeper_triceps[20];
+ 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){
+ case 1:
+ keeper_biceps[count_biceps]=input;
+ count_biceps++;
+ if(count_biceps >= size)
+ count_biceps=0;
+ //sizeof(keeper_biceps)/sizeof(float);
+ for(int i=0; i < (size-1); i++)
+ {sum+=keeper_biceps[i];
+ }
+ mean=sum/size;
+ sum=0;
+ for(int i=0; i < size-1; i++)
+ {sum+=(keeper_biceps[i]-mean)*(keeper_biceps[i]-mean);
+ }
+ sig_out=sqrt(sum/size);
+ sum=0;
+ break;
+ case 2:
+ keeper_triceps[count_triceps]=input;
+ count_triceps++;
+ if(count_triceps==size)
+ count_triceps=0;
+ //sizeof(keeper_triceps) / sizeof(float);
+ for(int i=0; i < size-1; i++){
+ sum+=keeper_triceps[i];
+ }
+ mean=sum/size;
+ sum=0;
+ for(int i=0; i < size-1; i++){
+ sum+=(keeper_triceps[i]-mean)*(keeper_triceps[i]-mean);
+ }
+ sig_out=sqrt(sum/size);
+ sum=0;
+ break;
+ /*case 3:
+ keeper_flexoren[count_flexoren]=input;
+ 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];
+ }
+ 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;
+ break;
+ case 4:
+ keeper_extensoren[count_extensoren]=input;
+ 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);
+ }
+ 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
+
+float filter(int sig_number){
+ float sig_out;
+ // eerst variabelen definieren
+
+ //biceps
+ //filter 1
+ float in0_biceps =0;
+ static float in1_biceps =0, in2_biceps = 0, in3_biceps = 0, in4_biceps = 0;
+ static float out0_biceps = 0, out1_biceps = 0 , out2_biceps = 0, out3_biceps = 0, out4_biceps = 0;
+
+ //filter 3
+ float in0_3_biceps =0;
+ static float in1_3_biceps =0, in2_3_biceps = 0, in3_3_biceps = 0, in4_3_biceps = 0;
+ static float out0_3_biceps = 0, out1_3_biceps = 0 , out2_3_biceps = 0, out3_3_biceps = 0, out4_3_biceps = 0;
+
+
+ //triceps
+ //filter 1
+ float in0_triceps =0;
+ static float in1_triceps =0, in2_triceps = 0, in3_triceps = 0, in4_triceps = 0;
+ static float out0_triceps = 0, out1_triceps = 0 , out2_triceps = 0, out3_triceps = 0, out4_triceps = 0;
+
+ //filter 3
+ float in0_3_triceps =0;
+ static float in1_3_triceps =0, in2_3_triceps = 0, in3_3_triceps = 0, in4_3_triceps = 0;
+ static float out0_3_triceps = 0, out1_3_triceps = 0 , out2_3_triceps = 0, out3_3_triceps = 0, out4_3_triceps = 0;
+
+
+ //flexoren
+ //filter 1
+ float in0_flexoren =0;
+ static float in1_flexoren =0, in2_flexoren = 0, in3_flexoren = 0, in4_flexoren = 0;
+ static float out0_flexoren = 0, out1_flexoren = 0 , out2_flexoren = 0, out3_flexoren = 0, out4_flexoren = 0;
+
+ //filter 3
+ float in0_3_flexoren =0;
+ static float in1_3_flexoren =0, in2_3_flexoren = 0, in3_3_flexoren = 0, in4_3_flexoren = 0;
+ static float out0_3_flexoren = 0, out1_3_flexoren = 0 , out2_3_flexoren = 0, out3_3_flexoren = 0, out4_3_flexoren = 0;
+
+
+ //extensoren
+ //filter 1
+ float in0_extensoren =0;
+ static float in1_extensoren =0, in2_extensoren = 0, in3_extensoren = 0, in4_extensoren = 0;
+ static float out0_extensoren = 0, out1_extensoren = 0 , out2_extensoren = 0, out3_extensoren = 0, out4_extensoren = 0;
+
+ //filter 3
+ float in0_3_extensoren =0;
+ static float in1_3_extensoren =0, in2_3_extensoren = 0, in3_3_extensoren = 0, in4_3_extensoren = 0;
+ static float out0_3_extensoren = 0, out1_3_extensoren = 0 , out2_3_extensoren = 0, out3_3_extensoren = 0, out4_3_extensoren = 0;
+
+
+
+ switch(sig_number){
+ case 1:
+ // signaal filteren op 15 Hz HIGHPASS
+ in4_biceps = in3_biceps; in3_biceps = in2_biceps; in2_biceps = in1_biceps; in1_biceps = in0_biceps;
+ 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;
+ break;
+ case 2:
+ // signaal filteren op 15 Hz HIGHPASS
+ in4_triceps = in3_triceps; in3_triceps = in2_triceps; in2_triceps = in1_triceps; in1_triceps = in0_triceps;
+ in0_triceps = emg_triceps.read() - offset_biceps;
+ out4_triceps = out3_triceps; out3_triceps = out2_triceps; out2_triceps = out1_triceps; out1_triceps = out0_triceps;
+ out0_triceps = (NUM0*in0_triceps + NUM1*in1_triceps + NUM2*in2_triceps + NUM3*in3_triceps + NUM4*in4_triceps - DEN1*out1_triceps - DEN2*out2_triceps - DEN3*out3_triceps - DEN4*out4_triceps ) / DEN0;
+
+
+ //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;
+ 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;
+ break;
+ case 3:
+ // signaal filteren op 15 Hz HIGHPASS
+ in4_flexoren = in3_flexoren; in3_flexoren = in2_flexoren; in2_flexoren = in1_flexoren; in1_flexoren = in0_flexoren;
+ 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);
+ 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;
+ break;
+ case 4:
+ // signaal filteren op 15 Hz HIGHPASS
+ in4_extensoren = in3_extensoren; in3_extensoren = in2_extensoren; in2_extensoren = in1_extensoren; in1_extensoren = in0_extensoren;
+ 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);
+ 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;
+ break;
+ }
+ return sig_out;
+}
+
+void looper()
+{ float emg_value_biceps;
+ float emg_value_triceps;
+ 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);
+//dy = emg_value_biceps-emg_value_triceps;
+
+
+ /*emg_value_flexoren = (100*filter(3)-44);
+ emg_value_extensoren = (100*filter(4)-44);*/
+
+ /*if(emg_value_biceps < 0.10){
+ emg_value_biceps=0;
+ }
+ else {
+ emg_value_biceps = emg_value_biceps;
+ }
+ if(emg_value_triceps < 0.10){
+ emg_value_triceps=0;
+ }
+ else {
+ emg_value_triceps=emg_value_triceps;
+ }
+ */
+ //dy = emg_value_biceps-emg_value_triceps;
+ if(pc.rxBufferGetSize(0)-pc.rxBufferGetCount() > 30)
+ pc.printf("%.6f, %.6f\n",emg_value_biceps, emg_value_triceps);
+ /**When not using the LED, the above could also have been done this way:
+ * pc.printf("%.6\n", emg0.read());
+ */
+}
+
+int main()
+{
+ /*setup baudrate. Choose the same in your program on PC side*/
+ pc.baud(115200);
+ /*set the period for the PWM to the red LED*/
+ red.period_ms(2);
+ /**Here you attach the 'void looper(void)' function to the Ticker object
+ * The looper() function will be called every 1/Fs seconds.
+ * Please mind that the parentheses after looper are omitted when using attach.
+ */
+ timer.attach(looper,0.0032); //invullen in seconde. .0032 is niet eens afgerond, dus vandaar die frequentie.
+ while(1) //Loop
+ {
+ /*Empty!*/
+ /*Everything is handled by the interrupt routine now!*/
+ }
+}
\ No newline at end of file