File content as of revision 4:833948d12d78:

#include "mbed.h"
#include "math.h"
#include "HIDScope.h"
#include "complex.h"

HIDScope scope(4);
// Analoge input signalen defineren
AnalogIn    EMG_in(A0);                 // EMG_in.read kan je nu gebruiken om het analoge signaal A0 uit te lezen
AnalogIn    EMG_int(A2);                // deze leest A2 uit
// ticker met de naam tick om je control functie uit te lezen.
Ticker      tick;



// x & y zijn inputs L is lengte van de arm r is de vector naar de end effector

const double L = 36;
const double pi = 3.1415926535897;
double theta_one;
double theta_two;

// tweede orde notch filter 50 Hz
// biquad 1 coefficienten
const double numnotch50biq1_1 = 1;
const double numnotch50biq1_2 = -1.61816178466632;
const double numnotch50biq1_3 = 1.00000006127058;
const double dennotch50biq1_2 = -1.59325742941798;
const double dennotch50biq1_3 = 0.982171881701431;
// biquad 2 coefficienten
const double numnotch50biq2_1 = 1;
const double numnotch50biq2_2 = -1.61816171933244;
const double numnotch50biq2_3 = 0.999999938729428;
const double dennotch50biq2_2 = -1.61431180968071;
const double dennotch50biq2_3 = 0.982599066293075;

// highpass filter 20 Hz coefficienten
const double numhigh20_1 = 0.837089190566345;
const double numhigh20_2 = -1.67417838113269;
const double numhigh20_3 = 0.837089190566345;
const double denhigh20_2 = -1.64745998107698;
const double denhigh20_3 = 0.700896781188403;

// lowpass 5 Hz coefficienten
const double numlow5_1 =0.000944691843840162;
const double numlow5_2 =0.00188938368768032;
const double numlow5_3 =0.000944691843840162;
const double denlow5_2 =-1.91119706742607;
const double denlow5_3 =0.914975834801434;

// storage variables voor 8 filters aanmaken. Voor een signaal zijn dit 4 biquads, namelijk notch(2), highpass(1) & lowpass(1)
double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0,f4_v1 = 0, f4_v2 = 0;
double f1_v1t = 0, f1_v2t = 0, f2_v1t = 0, f2_v2t = 0, f3_v1t = 0, f3_v2t = 0,f4_v1t = 0, f4_v2t = 0;

// biquadfilters die bij het filteren van signaal 1 horen
double biquadfilter(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;
}

// biquadfilters die bij het filteren van signaal 2 horen, copy paste, alle waardes zijn veranderd naar +t (t van two of twee)
double biquadfiltert(double ut, double &v1t, double &v2t, const double a1t, const double a2t, const double b0t, const double b1t, const double b2t)
{
    double vt = ut- a1t*v1t-a2t*v2t;
    double yt = b0t*vt+b1t*v1t+b2t*v2t;
    v2t = v1t;
    v1t = vt;
    return yt;
}



// myController vraagt de analoge waardes op (A0 en A2, d.m.v. AnalogIn[t].read())
// deze waardes worden gefilterd en naar HIDScope gestuurd
void myController()
{
// filteren van EMG signaal 1 (A0) eerst notch(2 biquads), dan highpass, rectify(abs()), lowpass
    double u1 = EMG_in.read();
    double y1 = biquadfilter( u1, f1_v1, f1_v2,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
    double y2 = biquadfilter( y1, f2_v1, f2_v2,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
    double y3 = biquadfilter( y2, f3_v1, f3_v2, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
    double y4 = abs(y3);
    double y5 = biquadfilter( y4, f4_v1, f4_v2, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
// versturen van het input signaal u1 en het gefilterde signaal y5 naar HIDScope channel 0 en 1
    //scope.set(0,u1);
    //scope.set(1,y5);
// filteren van EMG signaal 2 (A2), zelfde proces als signaal 1
    double u1t = EMG_int.read();
    double y1t = biquadfiltert( u1t, f1_v1t, f1_v2t,dennotch50biq1_2, dennotch50biq1_3,numnotch50biq1_1,numnotch50biq1_2,numnotch50biq1_3);
    double y2t = biquadfiltert( y1t, f2_v1t, f2_v2t,dennotch50biq2_2, dennotch50biq2_3,numnotch50biq2_1,numnotch50biq2_2,numnotch50biq2_3);
    double y3t = biquadfiltert( y2t, f3_v1t, f3_v2t, denhigh20_2,denhigh20_3,numhigh20_1, numhigh20_2, numhigh20_3);
    double y4t = abs(y3t);
    double y5t = biquadfiltert( y4t, f4_v1t, f4_v2t, denlow5_2,denlow5_3,numlow5_1, numlow5_2, numlow5_3);
    // versturen van input signaal2; u1t en het gefilterde signaal; y5t naar HIDScope channel 2 en 3
    //scope.set(2,u1t);
    //scope.set(3,y5t);
    // verzenden van de verstuurde signalen
    //scope.send();

    // xx en yy zijn de gewenste x en y waardes, de hoeken theta 1 en 2 worden berekend
    double xx = 7*50*y5;
    double yy = 7*50*y5t;
    double r = sqrt(pow(xx,2)+pow(yy,2)); // vector naar end effector
    double alfa = acos((2*pow(L,2)-pow(r,2))/(2*pow(L,2))); // alfa is de hoek tussen upper en lower arm
    double beta = acos((pow(r,2))/(2*L*r)); // beta is de hoek tussen upper arm en r
    // hoeken berekenen
    theta_one = atan2(yy,xx)+beta;
    theta_two = pi + alfa;
    
    // testen van de functie
    scope.set(0,xx);
    scope.set(1,yy);
    double xt = L*cos(theta_one)+L*cos(theta_one+theta_two);
    double yt = L*sin(theta_one)+L*sin(theta_one+theta_two);
    scope.set(2,xt);
    scope.set(3,yt);
    scope.send();
}

int main()
{

    tick.attach(&myController,0.01f);
    // roep elke 0.01 seconde de functie myController aan, dus elke 0.01 seconde wordt er gesamplet en dit sample gefilterd.

    
}