For BIOROBOTICS PROJECT
Dependencies: HIDScope MODSERIAL QEI mbed
main.cpp
- Committer:
- dbayuadi
- Date:
- 2015-10-28
- Revision:
- 2:ac98d055a6cd
- Parent:
- 1:3bae5ab25e20
File content as of revision 2:ac98d055a6cd:
#include "mbed.h" #include "HIDScope.h" #include "QEI.h" AnalogIn emg_left(A0); AnalogIn emg_right(A1); DigitalOut led(LED_BLUE); HIDScope scope(6); Ticker get; AnalogIn pot(A1); Serial pc(USBTX, USBRX); // tx, rx volatile bool fn_go = false; double y_notch_1; double y_notch_2; double y_high; double y_low; double y_rect; const double Fs = 500*2; const double Ts = 1/Fs; double a; const double gain_notch_1 = 1.000000; const double b0_notch_1 = 1.0*gain_notch_1; const double b1_notch_1 = -1.90215058158*gain_notch_1; const double b2_notch_1 = 1.0*gain_notch_1; const double a1_notch_1 = -1.76851639902; const double a2_notch_1 = 0.89458928499; double v1_notch_1 = 0, v2_notch_1 = 0; const double gain_notch_2 = 1.000000; const double b0_notch_2 = 1.0*gain_notch_2; const double b1_notch_2 = -1.90215058158*gain_notch_2; const double b2_notch_2 = 1.0*gain_notch_2; const double a1_notch_2 = -1.85196790295; const double a2_notch_2 = 0.92089778695; double v1_notch_2 = 0, v2_notch_2 = 0; //high pass of 5 Hz Wc const double gain_high = 1; const double b0_high = 0.978030479206560*gain_high; const double b1_high = -1.956060958413119*gain_high; const double b2_high = 0.978030479206560*gain_high; const double a1_high = -1.955578240315036; const double a2_high = 0.956543676511203; double v1_high = 0, v2_high = 0; double v1_high_1 = 0, v2_high_1=0; //low pass of 0.3 Hz Wc const double gain_low = 0.00001; const double b0_low = 0.088708177364838*gain_low; const double b1_low = 0.177416354729676*gain_low; const double b2_low = 0.088708177364838*gain_low; const double a1_low = -1.997334271812535; const double a2_low = 0.997337820139629; double v1_low = 0, v2_low = 0; double v1_low_1 = 0,v2_low_1=0; double biquad(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } double biquad1(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } double biquad2(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } double biquad3(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } double biquad4(double u, double&v1,double&v2,const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u - a1*v1 - a2*v2; double y = b0*v + b1*v1 + b2*v2; v2 = v1; v1 = v; return y; } void fn_activate() { fn_go = true; } double z1; double y_high_1; double y_notch_1_1; double y_notch_2_1; double y_rect_1; double y_low_1; double t = 0; double value = 0; double max() // calibration { if (t <= 10) { value = value + 0.0; } else if(t>10 && t<=20) { if(value<=y_low) { value = y_low; } else { value = value +0.0; } } else { value = value + 0.0; } return value*0.80; } double val; double gain() { if(max()==0) { val = 0; } else if(max()>=0) { val = 1/max(); } return val; } const double a1 = 11.1111; const double b1 = -0.1111; const double a2 = 4.7619; const double b2 = -0.047619; void scopeSend() { y_high = biquad1(emg_left.read(), v1_high, v2_high, a1_high, a2_high, b0_high, b1_high, b2_high); y_rect = abs(y_high); y_low = biquad2(y_rect, v1_low, v2_low, a1_low, a2_low, b0_low, b1_low, b2_low); t = t+Ts; y_high_1 = biquad3(emg_right.read(), v1_high_1, v2_high_1, a1_high, a2_high, b0_high, b1_high, b2_high); y_rect_1 = abs(y_high_1); y_low_1 = biquad4(y_rect_1, v1_low_1, v2_low_1, a1_low, a2_low, b0_low, b1_low, b2_low); scope.set(0,emg_left.read()); scope.set(1,emg_right.read()); scope.set(2,y_low); scope.set(3,y_low_1); scope.set(4,a1*y_low + b1); scope.set(5,a2*y_low_1 + b2); scope.send(); } int main() { get.attach(&fn_activate,Ts); while (true) { if(t<=10) { led = 0; } if(t>10 && t<=20) { led = 1; } if(t>20) { led = 0; } if(fn_go == true) { scopeSend(); fn_go = false; } } }