Diff: main.cpp

Revision:

3:2ffbee47fb38

Parent:

2:293665548183

Child:

4:afa85283eb18

--- 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) {