Roy van Zijl / Mbed 2 deprecated frdm_gpio1

Program to read out fiterd emg signals

Dependencies:   HIDScope biquad-master mbed Encoder MODSERIAL Matrix MatrixMath

Files at this revision

API Documentation at this revision

Revision 3:2ffbee47fb38, committed 2017-10-31

Comitter:
RoyvZ
Date:
Tue Oct 31 21:50:46 2017 +0000
Parent:
2:293665548183
Commit message:
Added Matrices

Changed in this revision

Encoder.lib Show annotated file Show diff for this revision Revisions of this file
MODSERIAL.lib Show annotated file Show diff for this revision Revisions of this file
Matrix.lib Show annotated file Show diff for this revision Revisions of this file
MatrixMath.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.lib	Tue Oct 31 21:50:46 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/vsluiter/code/Encoder/#18b000b443af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MODSERIAL.lib	Tue Oct 31 21:50:46 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/MODSERIAL/#a3b2bc878529

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Matrix.lib	Tue Oct 31 21:50:46 2017 +0000
@@ -0,0 +1,1 @@
+http://os.mbed.com/users/DiondeGreef/code/Matrix/#92115bd8c705

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MatrixMath.lib	Tue Oct 31 21:50:46 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Yo_Robot/code/MatrixMath/#93948a9bbde2

--- a/main.cpp	Tue Oct 24 11:58:29 2017 +0000
+++ b/main.cpp	Tue Oct 31 21:50:46 2017 +0000
@@ -1,4 +1,4 @@
-/**
+ /**
  * Demo program for BiQuad and BiQuadChain classes
  * author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion
  */
@@ -8,24 +8,48 @@
 #include <iostream>
 #include <iomanip>
 #include <complex>
+#include <vector>
 #include "BiQuad.h"
-
+#include "Matrix.h"
+#include "MatrixMath.h"
+#include "MODSERIAL.h"
+#include "encoder.h"
 
 AnalogIn    emg0( A0 );
 AnalogIn    emg1( A1 );
-DigitalIn   button1(PTA4);
+DigitalIn   button1(D11);
+Encoder     encoder1(D13,D12);
+Encoder     encoder2(D13,D12); //moet nog even aangepast worden
+
+
+DigitalOut motorDirection(D4);
+PwmOut motorSpeed(D5);
+
+MODSERIAL pc(USBTX, USBRX);
 
 Ticker      sample_timer;
 Ticker      cal_timer;
-HIDScope    scope( 2 );
+HIDScope    scope( 4 );
 DigitalOut  led(LED1);
-//DigitalOut  led2(LED_GREEN);                  
+DigitalOut  led2(LED_GREEN);                  
+const int size = 100;
+vector<double> S(size,0);
+double meanSum = 0;
+
+double maxsignal = 0;
 
-double S[50];
-double meanSum = 0;
+double L0 = 0.123;
+double L1 = 0.37;
+double L2 = 0.41;
+double q1 = encoder1.getPosition()/131; //Calibreren nog toevoegen
+double q2 = encoder2.getPosition()/131; //Calibreren mist nog
+double Periode = 1/1000;   //1000 is het aantal Hz
 
 //Filter toevoegen, bestaande uit notch, high- en lowpass filter
 BiQuadChain Notch;
+BiQuadChain Notch50;
+BiQuadChain bqcLP;
+BiQuadChain bqc;
 //50 Hz Notch filter, for radiation from surroundings
 
 //Notch filter chain for 100 Hz
@@ -33,73 +57,155 @@
 BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 );
 BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 );
 
+//BiQuad Notch(0.9179504645111498,-1.4852750515677946,  0.9179504645111498, -1.4852750515677946, 0.8359009290222995);
+
 //Notch filter chain for 50 Hz
-BiQuad bq1( 9.87512e-01, -1.95079e+00, 9.87563e-01, -1.96308e+00, 9.87512e-01 );
-BiQuad bq2( 1.00000e+00, -1.97533e+00, 9.99919e-01, -1.96726e+00, 9.93522e-01 );
-BiQuad bq3( 1.00000e+00, -1.97546e+00, 1.00003e+00, -1.97108e+00, 9.93951e-01 );
+BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 );
+BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 );
+BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 );
 
 //15 Hz Highpass filter, as recommended by multiple studies regarding EMG signals
 //BiQuad Highpass15(9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01);
 
 //2Hz Highpass
-BiQuad Highpass1(9.91154e-01, -1.98231e+00, 9.91154e-01, -1.98223e+00, 9.82385e-01);
+BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01);
 
 
 //20 Hz Lowpass
-BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 );
+//BiQuad Lowpass100( 3.62168e-03, 7.24336e-03, 3.62168e-03, -1.82269e+00, 8.37182e-01 );
+
+//250 Hz Lowpass
+//BiQuad Lowpass100 (2.92893e-01, 5.85786e-01, 2.92893e-01, -3.60822e-16, 1.71573e-01 );
+
+//80 Hz Lowpass
+BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 );
+BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 );
+
+
 
 //450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...)
 //BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01);
-
+     
+double findRMS(vector<double> array)
+{
+     int i;
+     double sumsquared = 0.000;
+     double RMS;
+     //sumsquared = 0;
+     for (i = 0; i < size; i++)
+     {
+        sumsquared = sumsquared + array.at(i)*array.at(i);
+     }
+     RMS = sqrt((double(1)/size)*(sumsquared)); 
+     return RMS;
+}
+     
+     
+     
 void sample()
 {
     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
     //scope.set(0, emg0.read() );
     //scope.set(1, fabs(Notch.step(emg0.read())) );
-    scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) ); 
-
-    for (int i=1;i < 50; i++) {
+    scope.set(0, emg0.read()-0.4542);
+    //scope.set(1, bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
+    //scope.set(2, fabs(Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)))));
+    //scope.set(3, fabs(bqc.step(emg0.read()-0.4542)));
+    //scope.set(3, Notch.step(Notch50.step(bqcLP.step(Highpass1.step(fabs(emg0.read()-0.4542))))));
+    
+    
+    //scope.set(0, fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))) ); 
+    /*
+    for (int i = size-1; i > 1; i--) {
             S[i] = S[i-1];    
-            meanSum = meanSum + S[i];    
         }
-    S[0] = fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read()))));
-    double mean = meanSum / 50;
-    scope.set(1, fabs(4*Highpass1.step(Lowpass100.step(Notch.step(emg0.read())))));
-    meanSum = 0;
+    */
+    S.erase(S.begin());
+    double b = bqc.step(emg0.read()-0.4542);
+    S.push_back(b);
+    //S[0] = bqc.step(emg0.read()-0.4542)));
+    //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
+    double a = findRMS(S);
+    scope.set(1, a);
+    scope.set(2, S[0]);
+    //meanSum = 0; */
     /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
     *  Ensure that enough channels are available (HIDScope scope( 2 ))
     *  Finally, send all channels to the PC at once */
     scope.send();
     /* To indicate that the function is working, the LED is toggled */
     led = !led;
+    
+    motorSpeed.write(0.38080*(a/maxsignal));
+    motorDirection.write(1);
 }
 
 void calibration()
 {
     //function to determine maximal EMG input in order to allow motorcontrol to be activated
-    double maxsignal = 0;
+    
     if (button1.read() == false){
+        led2 = !led2;
         for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
-            double signalfinal = fabs(Highpass1.step(Lowpass100.step(Notch.step(emg0.read()))));
+            
+                S.erase(S.begin());
+                double b = bqc.step(emg0.read()-0.4542);
+                S.push_back(b);
+                //S[0] = bqc.step(emg0.read()-0.4542)));
+                //Notch50.step(bqcLP.step(Highpass1.step(emg0.read()-0.4542)));
+                double signalfinal = findRMS(S);
             if (signalfinal >= maxsignal){
                 maxsignal = signalfinal; //keep changing the maximal signal
+                pc.baud(9600);
+                pc.printf("%d \n",maxsignal);
             }
         }
+        led2 = 1;       
     }
 }
 
 int main()
 {   
     //Constructing the notch filter chain
-    Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 );
-   
+    Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 );
+    Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 );
+    bqcLP.add( &bqLP1 ).add( &bqLP2 );
+    bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 );
     /**Attach the 'sample' function to the timer 'sample_timer'.
     * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz
     */
-    sample_timer.attach(&sample, 0.001);
+    sample_timer.attach(&sample, 0.002);
     cal_timer.attach(&calibration, 0.002);//ticker to check if motor is being calibrated
+    led2 = 1;
+    /*empty loop, sample() is executed periodically*/
+    
+    pc.baud(9600);
+    DigitalOut myled(LED1);
+ 
+//---
+    Matrix JAPPAPP(2,2);
+    Matrix qdot(2,1);
+    Matrix Vdes(2,1);
+    Matrix qsetpoint(2,1);
 
-    /*empty loop, sample() is executed periodically*/
+    // Fill Matrix with data.
+    JAPPAPP     << -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2))  << -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2))
+                << (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2))                     << (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2));
+             
+             
+    // Fill Matrix with data.
+    Vdes     << emg0.read        //Goede code nog toevoegen, Vx
+             << emg1.read;      //goede code nog toevoegen, Vy
+    
+    
+    qdot = JAPPAPP*Vdes;
+    pc.printf("\r\nJAPPAPPP: ");     //Alleen visualisatie
+    qdot.print();                    //Alleen visualisatie
+    
+    qsetpoint = q1 + qdot*Periode;
+    
+    
+    
     while(true) {