Bouke Scheltinga
Werkcollege opgave 23 september BMT K9
Dependencies: Encoder HIDScope MODSERIAL mbed QEI biquadFilter
Diff: main.cpp
- Revision:
- 31:4ddf83ff4295
- Parent:
- 30:4d3c2113cd93
- Child:
- 33:61696d5fa735
--- a/main.cpp Tue Oct 13 15:27:28 2015 +0000
+++ b/main.cpp Tue Oct 13 20:38:23 2015 +0000
@@ -4,30 +4,41 @@
#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);
+DigitalOut debug_led_red(LED_RED); // Debug LED
+DigitalOut debug_led_green(LED_GREEN); // Debug LED
+DigitalOut debug_led_blue(LED_BLUE); // Debug LED
// [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);
+
+Ticker control_tick;
+Ticker cal_tick;
+
volatile bool control_go = false;
volatile bool sample = false;
volatile bool control_go_cal= false;
-Ticker control_tick;
-Ticker cal_tick;
-double n=0;
+
+const double off=1; //Led off
+const double on=0; //Led on
+
+double B0, B1, B2, B3, B4, B5, B6, B7, B8, B9, MOVAVG;
double minimum_L;
+double maximum_L;
double EMG_L_min;
-double maximum_L;
double EMG_L_max;
+double minimum_R;
+double maximum_R;
+double EMG_R_min;
+double EMG_R_max;
+double n=0;
double c=0;
// [BIQUAD FILTERS] //
const int Fs = 512; // sampling frequency
-const double wait_sample=1/Fs;
+
+const double sample_duration=1/Fs;
+
const double low_b1 = 1.480219865318266e-04; //filter coefficients
const double low_b2 = 2.960439730636533e-04;
const double low_b3 = 1.480219865318266e-04;
@@ -39,119 +50,207 @@
const double high_a2 = -1.571102440190402e+00; // a1 is normalized to 1
const double high_a3 = 6.480567348620491e-01;
-biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3);
-biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
+biquadFilter highpassfilter_1(high_a2, high_a3, high_b1, high_b2, high_b3);
+biquadFilter highpassfilter_2(high_a2, high_a3, high_b1, high_b2, high_b3);
+biquadFilter lowpassfilter_1(low_a2, low_a3, low_b1, low_b2, low_b3);
+biquadFilter lowpassfilter_2(low_a2, low_a3, low_b1, low_b2, low_b3);
-AnalogIn input1(A0); // declaring the 4 inputs
+AnalogIn input1(A0); // Two AnalogIn EMG inputs
+AnalogIn input2(A1);
-double u1; double y1; // declaring the input variables
+double EMG_left_Bicep; double EMG_Right_Bicep; // input variables
+double EMG_Left_Bicep_filtered; double EMG_Right_Bicep_filtered; // output variables
// [FUNCTIONS] //
+void calibration();
+void countdown_from_5();
+void Filter();
+void sample_filter();
+void take_sample();
+void ControlGo();
+
+
+ ///////////////////////////////
+ // //
+/////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
+ // // //
+ /////////////////////////////// //
+ //
+int main() //
+{ //
+ debug_led_red=off; //
+ debug_led_green=off; //
+ debug_led_blue=off; //
+ //
+ pc.baud(115200); //
+ pc.printf("Start Calibration \n\r"); //
+ calibration(); // Blue led while active //
+ //
+ control_tick.attach(&ControlGo, (float)1/Fs);
+ //
+ while(1) //
+ { //
+ if(control_go) //
+ {
+ debug_led_green = on; // //
+ sample_filter(); //
+ scope.set(0,EMG_left_Bicep); //
+ scope.set(1,EMG_Left_Bicep_filtered); //
+ scope.set(2,MOVAVG); //
+ scope.send(); //
+ control_go = 0; //
+ } //
+ } // while end //
+} // main end //
+ //
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+ ////////////////////////////////////
+ // //
+ // [Functions Described] //
+ // //
+ ////////////////////////////////////
+
+
+ // [CALIBRATION] // (blue LED)
+void calibration()
+{
+
+
+ // [MINIMUM VALUE BICEPS CALIBRATION] //
+
+
+ debug_led_blue=on;
+ 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;
+// scope.set(0,EMG_left_Bicep);
+// scope.set(1,EMG_Left_Bicep_filtered);
+// scope.set(2,maximum_R);
+// scope.send();
+ c++; // Every sample c is increased by one until the statement c<2560 is false
+ wait(0.002);
+ }
+
+ 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
+
+ debug_led_blue=off;
+
+
+ // [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);
+
+}
+
+
+ // [COUNTDOWN FUNCTION] //
+
+
+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");
+ }
+
+
+ // [FLOW CONTROL FUNCTIONS] //
+
+
void ControlGo() //Control flag
{
control_go = true;
- led1 = 0;
}
-void ControlGo_cal() //Control flag
-{
- control_go_cal = true;
- led1 = 0;
-}
-
-void take_sample() //Control flag
+void take_sample() // Take a sample every 25th sample
{
if(n==25)
{
sample = true;
- led1 = 0;
n=0;
+ debug_led_green = off;
}
}
-void countdown_from_5()
+
+ // [FILTER FUNCTIONS] //
+ // [EMG] //
+
+void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
{
- 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 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.
- y1 = 5*y1;
+ 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 = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
+
+ EMG_Left_Bicep_filtered = 5*EMG_Left_Bicep_filtered; EMG_Right_Bicep_filtered = 5*EMG_Right_Bicep_filtered; // NODIG??
}
-void calibratie()
-{
- // MINIMUM VALUE
- pc.printf("start minimum calibration in 5 seconds \n\r");
- countdown_from_5();
- c=0;
-
- while(c<2560)
- {
- Filter();
- minimum_L=y1+minimum_L;
- scope.set(0,u1);
- scope.set(1,y1);
- scope.set(2,minimum_L);
- scope.send();
- c++;
- wait((0.002));
- }
- pc.printf("Finished minimum calibration \n\r");
- EMG_L_min=minimum_L/2560;
- pc.printf("EMG_L_min = %f \n\r", EMG_L_min);
-
- // MAXIMUM VALUE
- pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
- countdown_from_5();
- c=0;
- next_measure_L_max:
- if(c<2560)
- {
- while(1)
- {
- if(control_go_cal);
- {
- control_go=false;
- Filter();
- maximum_L=y1+maximum_L;
- scope.set(0,u1);
- scope.set(1,y1);
- scope.set(2,maximum_L);
- scope.send();
- goto next_measure_L_max;
- }
- }
- }
- pc.printf("Finished minimum calibration \n\r");
- EMG_L_max=maximum_L/2560;
- pc.printf("EMG_L_max = %f \n\r", EMG_L_max);
-}
+ // [FILTER FUNCTIONS] //
+ // [Include Moving Average] //
+
void sample_filter()
{
Filter();
take_sample();
- led1=1;
if(sample)
{
sample=false;
- B0 = y1;
+ B0 = EMG_Left_Bicep_filtered;
B9=B8;
B8=B7;
B7=B6;
@@ -166,30 +265,5 @@
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;
n++;
-
- led1 = 1; //De led gaat flikkeren wanneer deze loop uitgevoerd wordt
+
}
-
-
-// [MAIN FUNCTION] //
-int main()
-{
- control_tick.attach(&ControlGo, (float)1/Fs);
- pc.baud(115200);
- pc.printf("Start \n\r");
- calibratie();
- while(1)
- {
- led1=1;
- if(control_go)
- {
- sample_filter();
- scope.set(0,u1);
- scope.set(1,y1);
- scope.set(2,MOVAVG);
- scope.send();
- control_go = 0;
- }
- } // while end
-} // main end
-
\ No newline at end of file
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 |
