File content as of revision 11:b46a4c48c08f:

//libaries
#include "mbed.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include "encoder.h"
#include "MODSERIAL.h"

//globalvariables Motor
Ticker Treecko;             //We make a awesome ticker for our control system
Ticker printer;
//PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
//DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control

//Encoder motor1(D13,D12,true);
MODSERIAL pc(USBTX,USBRX);

//double PwmPeriod = 1.0/5000.0;           //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
const double Ts = 0.1;                   // tickettijd/ sample time
//double e_prev = 0; 
//double e_int = 0;
double tijdstap = 0.002;
volatile double LBF;
volatile double RBF;
volatile double LTF;
volatile double RTF;

//buttons  en leds voor calibration
DigitalIn button1(PTA4);

DigitalOut ledred(LED_RED); 
DigitalOut ledblue(LED_BLUE);
DigitalOut ledgreen(LED_GREEN);

//MVC for calibration
double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
//MEAN for calibration - rest
double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;

double emgMEANSUBLB;double emgMEANSUBRB ;double emgMEANSUBLT ;double emgMEANSUBRT ;
double emgSUMLB;double emgSUMRB;double emgSUMLT;double emgSUMRT;


bool caldone = false; 
int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample

int Timescalibration = 0;
int TimescalibrationREST = 0;


// Biquad filters voor Left Bicep (LB)
// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
BiQuadChain NFLB;
BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
BiQuadChain HPFLB;
BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
BiQuadChain LPFLB;

// Biquad filters voor Right Bicep (RB)
// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
BiQuadChain NFRB;
BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
BiQuadChain HPFRB;
BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
BiQuadChain LPFRB;

// Biquad filters voor Left Tricep (LT)
// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
BiQuadChain NFLT;
BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
BiQuadChain HPFLT;
BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
BiQuadChain LPFLT;

// Biquad filters voor Left Tricep (RT)
// Biquad filters van respectievelijk Notch, High-pass en Low-pass filter
BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
BiQuadChain NFRT;
BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
BiQuadChain HPFRT;
BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
BiQuadChain LPFRT;



Timer looptime; //moetuiteindelijk weg

//filters
double emgNotchLB;
double emgHPLB;
double emgAbsHPLB;
double emgLPLB;

double emgNotchRB;
double emgHPRB;
double emgAbsHPRB;
double emgLPRB;

double emgNotchLT;
double emgHPLT;
double emgAbsHPLT;
double emgLPLT;

double emgNotchRT;
double emgHPRT;
double emgAbsHPRT;
double emgLPRT;

double f = 500;       // frequency
double dt = 1/f;      // sample frequency

AnalogIn emgLB(A0);   // EMG lezen
AnalogIn emgRB(A1);
AnalogIn emgLT(A2);
AnalogIn emgRT(A3);

//float MVCLB = 0.3;
//float MVCRB = 0.3;
//float MVCLT = 0.3;
//float MVCRT = 0.3;

// variabelen changePosition
int goalx, goaly;

void Filteren() 
{
    looptime.reset();
    looptime.start();
    
    //EMG 1
    
    emgNotchLB = NFLB.step(emgLB.read() );  // Notch filter
    emgHPLB = HPFLB.step(emgNotchLB);         // High-pass filter: also normalises around 0.
    emgAbsHPLB = abs(emgHPLB);            // Take absolute value
    emgLPLB = LPFLB.step(emgAbsHPLB);       // Low-pass filter: creates envelope
    emgMEANSUBLB = emgLPLB - RESTMEANLB;    //substract the restmean value
    LBF = emgLPLB/MVCLB;       // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
    
    emgNotchRB = NFRB.step(emgRB.read());  // Notch filter
    emgHPRB = HPFRB.step(emgNotchRB);       // High-pass filter: also normalises around 0.
    emgAbsHPRB = abs(emgHPRB);            // Take absolute value
    emgLPRB = LPFRB.step(emgAbsHPRB);       // Low-pass filter: creates envelope
    emgMEANSUBLB = emgLPLB - RESTMEANLB;
    RBF = emgLPRB/MVCRB;       // Scale to maximum signal: useful for motor
    
    emgNotchLT = NFLT.step(emgLT.read() );  // Notch filter
    emgHPLT = HPFLT.step(emgNotchLT);       // High-pass filter: also normalises around 0.
    emgAbsHPLT = abs(emgHPLT);            // Take absolute value
    emgLPLT = LPFLT.step(emgAbsHPLT);       // Low-pass filter: creates envelope
    emgMEANSUBLT = emgLPLT - RESTMEANLT;    //substract the restmean value
    LTF = emgLPLT/MVCLT;       // Scale to maximum signal: useful for motor
    
    emgNotchRT = NFRT.step(emgRT.read() );  // Notch filter
    emgHPRT = HPFRT.step(emgNotchRT);       // High-pass filter: also normalises around 0.
    emgAbsHPRT = abs(emgHPRT);            // Take absolute value
    emgLPRT = LPFRT.step(emgAbsHPRT);       // Low-pass filter: creates envelope
    emgMEANSUBRT = emgLPRT - RESTMEANRT;    //substract the restmean value
    RTF = emgLPRT/MVCRT;       // Scale to maximum signal: useful for motor
        
   //if (emgFiltered >1)
    //{
    //    emgFiltered=1.00;
    //}
//pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, loop = %f \r\n, emgreadRB = %f , emgFiltered = %f, maxi = %f \r\n, emgreadLT = %f , emgFiltered = %f, maxi = %f \r\n, emgreadRT = %f , emgFiltered = %f, maxi = %f \r\n",emgLB.read(), LBF, maxiLB,looptime.read(),emgRB.read(), RBF, maxiRB,emgLT.read(), LTF, maxiLT, emgRT.read(), RTF, maxiRT);
    //int maxwaarde = 4096;                   // = 64x64
    //double refP = emgFiltered*maxwaarde;
    //return refP;                            // value between 0 and 4096 
   
}

void CalibrationEMG()
{
    Timescalibration++;
    
    if(Timescalibration<2000)
    {
        
    emgNotchLB = NFLB.step(emgLB.read() );
    emgHPLB = HPFLB.step(emgNotchLB);   
    emgAbsHPLB = abs(emgHPLB);
    emgLPLB = LPFLB.step(emgAbsHPLB);
    emgSUMLB += emgLPLB;                         //SUM all rest values LB
    
    emgNotchRB = NFRB.step(emgRB.read()); 
    emgHPRB = HPFRB.step(emgNotchRB);
    emgAbsHPRB = abs(emgHPRB);
    emgLPRB = LPFRB.step(emgAbsHPRB);
    emgSUMRB += emgLPRB;                        //SUM all rest values RB
    
    emgNotchLT = NFLT.step(emgLT.read() );
    emgHPLT = HPFLT.step(emgNotchLT);
    emgAbsHPLT = abs(emgHPLT);
    emgLPLT = LPFLT.step(emgAbsHPLT);
    emgSUMLT += emgLPLT;                         //SUM all rest values LT
    
    emgNotchRT = NFRT.step(emgRT.read() );  
    emgHPRT = HPFRT.step(emgNotchRT);       
    emgAbsHPRT = abs(emgHPRT);           
    emgLPRT = LPFRT.step(emgAbsHPRT);
    emgSUMRT += emgLPRT;                         //SUM all rest values RT
    }
    if(Timescalibration==1999)
    {
        RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
        RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
        RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
        RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
    }
    if(Timescalibration>2000 && Timescalibration<6000)
    {
    emgNotchLB = NFLB.step(emgLB.read() ); 
    emgHPLB = HPFLB.step(emgNotchLB);        
    emgAbsHPLB = abs(emgHPLB);          
    emgLPLB = LPFLB.step(emgAbsHPLB);       
    double emgfinalLB = emgLPLB;
    if (emgfinalLB > MVCLB)
    {                                       //determine what the highest reachable emg signal is
          MVCLB = emgfinalLB;
    }    
    }
    
    if(Timescalibration>6000 && Timescalibration<10000)
    {
    emgNotchRB = NFRB.step(emgRB.read()); 
    emgHPRB = HPFRB.step(emgNotchRB); 
    emgAbsHPRB = abs(emgHPRB);           
    emgLPRB = LPFRB.step(emgAbsHPRB);      
    double emgfinalRB = emgLPRB;
    if (emgfinalRB > MVCRB)
    {                                       //determine what the highest reachable emg signal is
          MVCRB = emgfinalRB;
    }    
    }
    
    if(Timescalibration>10000 && Timescalibration<14000)
    {
    emgNotchLT = NFLT.step(emgLT.read() );
    emgHPLT = HPFLT.step(emgNotchLT);
    emgAbsHPLT = abs(emgHPLT);
    emgLPLT = LPFLT.step(emgAbsHPLT);
    double emgfinalLT = emgLPLT;
    if (emgfinalLT > MVCLT)
    {                                       //determine what the highest reachable emg signal is
          MVCLT = emgfinalLT;
    }    
    }
    
    if(Timescalibration>14000 && Timescalibration<18000)
    {
    emgNotchRT = NFRT.step(emgRT.read() );  
    emgHPRT = HPFRT.step(emgNotchRT);       
    emgAbsHPRT = abs(emgHPRT);           
    emgLPRT = LPFRT.step(emgAbsHPRT);
    double emgfinalRT = emgLPRT;
    if (emgfinalRT > MVCRT)
    {                                       //determine what the highest reachable emg signal is
          MVCRT = emgfinalRT;
    }    
    }
    
    if(Timescalibration>18000)
    {
         caldone=true; 
    }
   // pc.printf("maxi waarde = %f emgfinal = %f \r\n",maxi,emgfinal);
   //}
                                         //PAS ALS DEZE TRUE IS, MOET DE MOTOR PAS BEWEGEN!!!
  //return maxi;
}
       
double changePosition ()
{
    if (RBF>0.3) {
        goalx++;    // hoe veel verder gaat hij? 1 cm? 10 cm?
    }
    if (RTF>0.3) {
        goalx--;
    }
    if (LBF>0.3) {
        goaly++;
    }
    if (LTF>0.3) {
        goaly--;
    }
    pc.printf("goalx = %i, goaly = %i \n",goalx, goaly);
} 

/*
double Encoder ()
{
    double Huidigepositie = motor1.getPosition ();
    return Huidigepositie;             // huidige positie = current position
}

double FeedBackControl(double error, double &e_prev, double &e_int)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
{
    double kp = 0.001;                           // has jet to be scaled
    double Proportional= kp*error;
    
    double kd = 0.0004;                            // has jet to be scaled
    double VelocityError = (error - e_prev)/Ts; 
    double Derivative = kd*VelocityError;
    e_prev = error;
    
    double ki = 0.00005;                           // has jet to be scaled 
    e_int = e_int+Ts*error;
    double Integrator = ki*e_int;
    
    
    double motorValue = Proportional + Integrator + Derivative;
    return motorValue;
}

void SetMotor1(double motorValue)
{
    if (motorValue >= 0)
    {
        M1D = 0;
    }
    else 
    {
        M1D = 1;
    }

    if  (fabs(motorValue) > 1)    
    {
        M1E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
    }
    else
    {    
        M1E = fabs(motorValue);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
    }
}
*/
void MeasureAndControl ()
{
        // hier the control of the control system
     
    if(caldone==false)
    {
       if(button1.read()==false)  
        {
            CalibrationEMG();
       }
    }
    if (caldone==true)
    
    {
       Filteren();
       changePosition();
        //rest
    }
    
    //double Huidigepositie = Encoder(); 
    //double error = (refP - Huidigepositie);// make an error
    //double motorValue = FeedBackControl(error, e_prev, e_int);
    //double motorValue = refP;
    //SetMotor1(motorValue);
}

void Tickerfunctie()
{
    /*pc.printf("emgreadRB = %f , emgFiltered = %f, maxi = %f meanrest = %f\r\n",emgRB.read(), RBF, MVCRB, RESTMEANLB);
    pc.printf("emgreadLB = %f , emgFiltered = %f, maxi = %f, meanrest = %f emgSUMLB %f ,Timescalibration %i\r\n",emgLB.read(), LBF, MVCLB,RESTMEANRB,emgSUMLB, Timescalibration);
    pc.printf("emgreadRT = %f , emgFilteredRT = %f, maxiRT = %f meanrest = %f \r\n",emgRT.read(), RTF, MVCRT,RESTMEANRT);
    pc.printf("emgreadLT = %f , emgFilteredLT = %f, maxiLT = %f meanrest = %f \r\n",emgLT.read(), LTF, MVCLT,RESTMEANLT);
*/
}

int main()
{
    //voor EMG filteren
    //Left Bicep
    NFLB.add( &N1LB );
    HPFLB.add( &HP1LB ).add( &HP2LB );
    LPFLB.add( &LP1LB ).add( &LP2LB );
    
    //Right Bicep
    NFRB.add( &N1RB );
    HPFRB.add( &HP1RB ).add( &HP2RB );
    LPFRB.add( &LP1RB ).add( &LP2RB );  
    
    //Left Tricep
    NFLT.add( &N1LT );
    HPFLT.add( &HP1LT ).add( &HP2LT );
    LPFLT.add( &LP1LT ).add( &LP2LT ); 
    
    //Right Tricep
    NFRT.add( &N1RT );
    HPFRT.add( &HP1RT ).add( &HP2RT );
    LPFRT.add( &LP1RT ).add( &LP2RT ); 
    
    //voor serial
    pc.baud(115200);
    
    //motor
   // M1E.period(PwmPeriod); //set PWMposition at 5000hz
    //Ticker
    Treecko.attach(MeasureAndControl, tijdstap);   //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende 
                                            //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
    printer.attach(Tickerfunctie,0.4);
    while(true)
    {
        }
    
    
}