File content as of revision 3:2ffbee47fb38:

 /**
 * 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
 */
#include "mbed.h"
#include "HIDScope.h"
#include <stdlib.h>
#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(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( 4 );
DigitalOut  led(LED1);
DigitalOut  led2(LED_GREEN);                  
const int size = 100;
vector<double> S(size,0);
double meanSum = 0;

double maxsignal = 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
BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 );
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.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.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 );

//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, 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];    
        }
    */
    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
    
    if (button1.read() == false){
        led2 = !led2;
        for (int n = 1; n < 5000; n++){ //calibrate for 5000 samples or 10 seconds
            
                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 );
    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.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);

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

}