Bouke Scheltinga
Werkcollege opgave 23 september BMT K9
Dependencies: Encoder HIDScope MODSERIAL mbed QEI biquadFilter
Diff: main.cpp
- Revision:
- 36:f29a36683b1a
- Parent:
- 32:97cf6cb8d054
- Child:
- 37:6c04c15d9bbe
--- a/main.cpp Wed Oct 14 14:13:52 2015 +0000
+++ b/main.cpp Mon Oct 19 20:49:18 2015 +0000
@@ -4,87 +4,240 @@
#include "biquadFilter.h" //Filter direct form II
// [DEFINE INPUTS] //
-//AnalogIn emgL(A0); //Analog input left arm
-//AnalogIn emgR(PTB1); //Analog input right arm
-DigitalOut led1(LED_GREEN);
+
// [DEFINE CONSTANTS] //
-float emgL_L, emgL_LH, emgLeft, emgL_H, emgL_Notch;
-double B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG;
HIDScope scope(3); // Aantal HIDScope kanalen
MODSERIAL pc(USBTX,USBRX);
-volatile bool control_go = false;
Ticker control_tick;
Ticker T1;
// [BIQUAD FILTERS] //
int Fs = 512; // sampling frequency
-const double low_b0 = 0.05892937945281792
-const double low_b1 = 0.11785875890563584
-const double low_b2 = 0.05892937945281792
-const double low_a1 = -1.205716572226748
-const double low_a2 = 0.44143409003801976 // VIA online biquad calculator Lowpas 520-48-0.7071-6
+const double low_b0 = 0.05892937945281792;
+const double low_b1 = 0.11785875890563584;
+const double low_b2 = 0.05892937945281792;
+const double low_a1 = -1.205716572226748;
+const double low_a2 = 0.44143409003801976; // VIA online biquad calculator Lowpas 520-48-0.7071-6
-const double high_b0 = 0.6389437261127494
-const double high_b1 = -1.2778874522254988
-const double high_b2 = 0.6389437261127494
-const double high_a1 = -1.1429772843080923
-const double high_a2 = 0.41279762014290533 // VIA online biquad calculator Highpas 520-52-0.7071-6
-
-biquadFilter highpass1(high_a1, high_a2, high_b0, high_b1, high_b2);
-biquadFilter lowpass1(low_a1, low_a2, low_b0, low_b1, low_b2);
-
-AnalogIn input1(A0); // declaring the 4 inputs
-
-double u1; double y1; // declaring the input variables
+const double high_b0 = 0.6389437261127494;
+const double high_b1 = -1.2778874522254988;
+const double high_b2 = 0.6389437261127494;
+const double high_a1 = -1.1429772843080923;
+const double high_a2 = 0.41279762014290533; // VIA online biquad calculator Highpas 520-52-0.7071-6
-// [FUNCTIONS] //
-void ControlGo() //Control flag
-{
- control_go = true;
- led1 = 0;
-}
+//Left bicep
+biquadFilter highpassfilter_1(high_a1, high_a2, high_b0, high_b1, high_b2);// moeten nog waardes voor gemaakt worden? (>25Hz doorlaten)
+biquadFilter notchL1(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden (>52Hz doorlaten)
+biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden (<48Hz doorlaten)
+biquadFilter lowpassfilter_1(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden? (<5-10 Hz ???) // (<200 Hz doorlaten) => wat is het verschil wat is bruikbaar
-void sample_filter()
-{
- u1 = input1;
- y1 = highpass1.step(u1); // filter order is: high-pass --> rectify --> low-pass
- y1 = fabs(y1);
- y1 = lowpass1.step(y1);// roughly normalize to a scale of 0 - 1, where 0 is minimum and 1 is roughly the maximum output of dennis.
-
- B0 = y1;
- MOVAVG=B0*0.1+B1*0.1+B2*0.1+B3*0.1+B4*0.1+B5*0.1+B6*0.1+B7*0.1+B8*0.1+B9*0.1;
- B9=B8;
- B8=B7;
- B7=B6;
- B6=B5;
- B5=B4;
- B4=B3;
- B3=B2;
- B2=B1;
- B1=B0;
-
- led1 = 1; //De led gaat flikkeren wanneer deze loop uitgevoerd wordt
-}
+// Right bicep
+biquadFilter highpassfilter_2(high_a1, high_a2, high_b0, high_b1, high_b2);// moeten nog waardes voor gemaakt worden?
+biquadFilter notchR1(low_a1, low_a2, low_b0, low_b1, low_b2); // moeten nog waardes voor gemaakt worden
+biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // moeten nog waardes voor gemaakt worden
+biquadFilter lowpassfilter_2(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden?
+AnalogIn input1(A0); // EMG: Two AnalogIn EMG inputs
+AnalogIn input2(A1); // EMG: Two AnalogIn EMG inputs
+
+// __________________________
+// : [EMG variables] :
+// :__________________________:
+//
+
+volatile bool control_go = false; // EMG:
+volatile bool sample = false;
+volatile bool sample_error = false;
+volatile bool sample_error_strike = false;
+
+double Sample_EMG_L_1, Sample_EMG_L_2, Sample_EMG_L_3, Sample_EMG_L_4, Sample_EMG_L_5, Sample_EMG_L_6, Sample_EMG_L_7, Sample_EMG_L_8, Sample_EMG_L_9, Sample_EMG_L_10, moving_average_left;
+double Sample_EMG_R_1, Sample_EMG_R_2, Sample_EMG_R_3, Sample_EMG_R_4, Sample_EMG_R_5, Sample_EMG_R_6, Sample_EMG_R_7, Sample_EMG_R_8, Sample_EMG_R_9, Sample_EMG_R_10, moving_average_right;
+
+double minimum_L; double maximum_L;
+double EMG_L_min; double EMG_L_max;
+double minimum_R; double maximum_R;
+double EMG_R_min; double EMG_R_max;
+double EMG_left_Bicep; double EMG_Right_Bicep; // input variables
+double EMG_Left_Bicep_filtered_notch_1;double EMG_Right_Bicep_filtered_notch_1;
+double EMG_Left_Bicep_filtered_notch_2;double EMG_Right_Bicep_filtered_notch_2;
+double EMG_Left_Bicep_filtered; double EMG_Right_Bicep_filtered; // output variables
+
+
+double n=0; double c=0; double k=0; double p=0; double er=0;
+
+// FUNCTIONS
+void ControlGo();
+void take_sample();
+void Filter();
+void sample_filter();
+void countdown_from_5();
+void calibration();
+
+//==========================//
// [MAIN FUNCTION] //
+//==========================//
int main()
{
control_tick.attach(&ControlGo, (float)1/Fs);
pc.baud(9600);
+ // calibration(); // calibreer min en max positie
while(1)
{
- led1=0;
if(control_go)
{
sample_filter();
- scope.set(0,u1);
- scope.set(1,y1);
- scope.set(2,MOVAVG);
+ scope.set(0,EMG_left_Bicep); //left bicep unfiltered
+ scope.set(1,EMG_Left_Bicep_filtered); //Filtered signal
+ scope.set(2,moving_average_left); //
scope.send();
control_go = 0;
}
} // while end
} // main end
-
\ No newline at end of file
+
+// --------------------------------------------------------------------------------------------------------
+// [FUNCTIONS] //
+void ControlGo() //Control flag
+{
+ control_go = true;
+}
+
+void sample_filter()
+{
+ Filter();
+ take_sample();
+ if(sample)
+ {
+ sample=false;
+ Sample_EMG_L_1 = EMG_Left_Bicep_filtered; Sample_EMG_R_1 = EMG_Right_Bicep_filtered;
+
+ Sample_EMG_L_10= Sample_EMG_L_9; Sample_EMG_R_10= Sample_EMG_R_9;
+ Sample_EMG_L_9 = Sample_EMG_L_8; Sample_EMG_R_9 = Sample_EMG_R_8;
+ Sample_EMG_L_8 = Sample_EMG_L_7; Sample_EMG_R_8 = Sample_EMG_R_7;
+ Sample_EMG_L_7 = Sample_EMG_L_6; Sample_EMG_R_7 = Sample_EMG_R_6;
+ Sample_EMG_L_6 = Sample_EMG_L_5; Sample_EMG_R_6 = Sample_EMG_R_5;
+ Sample_EMG_L_5 = Sample_EMG_L_4; Sample_EMG_R_5 = Sample_EMG_R_4;
+ Sample_EMG_L_4 = Sample_EMG_L_3; Sample_EMG_R_4 = Sample_EMG_R_3;
+ Sample_EMG_L_3 = Sample_EMG_L_2; Sample_EMG_R_3 = Sample_EMG_R_2;
+ Sample_EMG_L_2 = Sample_EMG_L_1; Sample_EMG_R_2 = Sample_EMG_R_1;
+ }
+ moving_average_left=Sample_EMG_L_1*0.1+Sample_EMG_L_2*0.1+Sample_EMG_L_3*0.1+Sample_EMG_L_4*0.1+Sample_EMG_L_5*0.1+Sample_EMG_L_6*0.1+Sample_EMG_L_7*0.1+Sample_EMG_L_8*0.1+Sample_EMG_L_9*0.1+Sample_EMG_L_10*0.1;
+ moving_average_right=Sample_EMG_R_1*0.1+Sample_EMG_R_2*0.1+Sample_EMG_R_3*0.1+Sample_EMG_R_4*0.1+Sample_EMG_R_5*0.1+Sample_EMG_R_6*0.1+Sample_EMG_R_7*0.1+Sample_EMG_R_8*0.1+Sample_EMG_R_9*0.1+Sample_EMG_R_10*0.1;
+ n++;
+}
+
+void take_sample() // Take a sample every 25th sample
+{
+ if(n==25)
+ {
+ sample = true; n=0;
+ }
+
+ if(er==5)
+ {
+ sample_error = true; er=0;
+ }
+
+ sample_error_strike = true;
+}
+
+ // [FILTER FUNCTIONS] //
+ // [EMG] //
+
+void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
+ {
+ EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
+
+ EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
+ EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
+
+ EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered);
+ EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1);
+
+ EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2);
+ }
+
+void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
+ {
+ wait(1); pc.printf("5 \n\r"); wait(1); pc.printf("4 \n\r"); wait(1); pc.printf("3 \n\r"); wait(1); pc.printf("2 Ready \n\r");
+ wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
+ }
+
+void calibration()
+{
+
+
+ // [MINIMUM VALUE BICEPS CALIBRATION] //
+
+ pc.printf("Start minimum calibration in 5 seconds \n\r");
+ pc.printf("Keep your biceps as relaxed as possible \n\r");
+
+ countdown_from_5();
+ c=0;
+
+ while(c<2560) // 512Hz -> 2560 is equal to five seconds
+ {
+ Filter(); // Filter EMG signal
+ minimum_L=EMG_Left_Bicep_filtered+minimum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
+ minimum_R=EMG_Right_Bicep_filtered+minimum_R;
+// scope.set(0,EMG_left_Bicep);
+// scope.set(1,EMG_Left_Bicep_filtered);
+// scope.set(2,minimum_L);
+// scope.send();
+ c++; // Every sample c is increased by one until the statement c<2560 is false
+ wait(0.001953125); // wait one sample
+ }
+
+ pc.printf("Finished minimum calibration \n\r");
+
+ EMG_L_min=minimum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
+ EMG_R_min=minimum_R/2560;
+
+ pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);
+
+ wait (3); //cooldown
+
+
+ // [MAXIMUM VALUE BICEPS CALIBRATION] //
+
+
+ pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
+
+ countdown_from_5();
+ c=0;
+
+ while(c<2560) // 512Hz -> 2560 is equal to five seconds
+ {
+ Filter(); // Filter EMG signal
+ maximum_L=EMG_Left_Bicep_filtered+maximum_L; // Take previous sample EMG_Left_Bicep_filtered and add the new value
+ maximum_R=EMG_Right_Bicep_filtered+maximum_R;
+ c++; // Every sample c is increased by one until the statement c<2560 is false
+ wait(0.001953125);
+ }
+
+ pc.printf("Finished minimum calibration \n\r");
+
+ EMG_L_max=maximum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
+ EMG_R_max=maximum_R/2560;
+
+ pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f \n\r", EMG_L_max, EMG_R_max);
+
+ wait (3); //cooldown
+
+
+ // [MAXIMUM VALUE BICEPS CALIBRATION] //
+ // Calculate threshold percentages //
+
+ const float Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2)+EMG_L_min;; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
+ const float Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6)+EMG_L_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
+ const float Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
+ const float Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6)+EMG_R_min; //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);
+
+ pc.printf("left 1: %f left 2: %f right 1: %f right 2: %f \n\r", Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2);
+
+}
+
+
+
Repository toolbox
| Export to desktop IDE |
| Build repository |
Repository details
| Type: | Program |
|---|---|
| Mbed OS support: | Mbed 2 deprecated |
| Created: | 23 Sep 2015 |
| Imports: | 4 |
| Forks: | 0 |
| Commits: | 53 |
| Dependents: | 0 |
| Dependencies: | 6 |
| Followers: | 4 |
