File content as of revision 17:dbdbd1edc260:

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

//State Machine en calibratie
enum States {Cal1, Cal2, CalEMG, SelectDevice, EMG, Rest, Demo};
States State;
int Counter;
bool Position_controller_on;
double looptime = 0.002f;
double value;
double home1;
DigitalIn button (D4);

//Encoder/motor
double Huidigepositie1;
double Huidigepositie2;
double motorValue1;
double motorValue2;

//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 
PwmOut M2E(D5);
DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
Encoder motor1(D13,D12,true);
Encoder motor2(D9,D8,true);
DigitalOut M2D(D4);

//DEMO
AnalogIn potMeter2(A1);
AnalogIn potMeter1(A2);



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 e_prev2 = 0;
double e_int2 = 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 led(D2)

//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 LooptimeEMG; //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);

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

// variabelen changePosition
int goalx, goaly;

void Filteren() 
{
   // LooptimeEMG.reset();
   // LooptimeEMG.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)
    {
        led = 1;
        wait(0.2);
        led = 0;
        
    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)
    {
        led = 1;
        wait(0.2);
        led = 0; 
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        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)
    {
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
    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)
    {
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
    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)
    {
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
        wait(0.2);
        led = 1;
        wait(0.2);
        led = 0;
    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 GetReferencePosition() 
{
    double Potmeterwaarde = potMeter2.read(); //naam moet universeel worden
    int maxwaarde = 4096;                   // = 64x64
    double refP = Potmeterwaarde*maxwaarde;
    return refP;                            // value between 0 and 4096 
}

double GetReferencePosition2() 
{
    double potmeterwaarde2 = potMeter1.read();
    int maxwaarde2 = 4096;                   // = 64x64
    double refP2 = potmeterwaarde2*maxwaarde2;
    return refP2;                            // value between 0 and 4096 
}
    
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;                             // kind of scaled.
    double Proportional= kp*error;
    
    double kd = 0.0004;                           // kind of scaled. 
    double VelocityError = (error - e_prev)/Ts; 
    double Derivative = kd*VelocityError;
    e_prev = error;
    
    double ki = 0.00005;                           // kind of scaled.
    e_int = e_int+Ts*error;
    double Integrator = ki*e_int;
    
    double motorValue = Proportional + Integrator + Derivative;
    return motorValue;
}

double FeedBackControl2(double error2, double &e_prev2, double &e_int2)   // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking)
{
    double kp2 = 0.001;                             // kind of scaled.
    double Proportional2= kp2*error2;
    
    double kd2 = 0.0004;                           // kind of scaled. 
    double VelocityError2 = (error2 - e_prev2)/Ts; 
    double Derivative2 = kd2*VelocityError2;
    e_prev2 = error2;
    
    double ki2 = 0.00005;                           // kind of scaled.
    e_int2 = e_int2+Ts*error2;
    double Integrator2 = ki2*e_int2;
    
    double motorValue2 = Proportional2 + Integrator2 + Derivative2;
    return motorValue2;
}


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 SetMotor2(double motorValue2)
{
    if (motorValue2 >= 0) {
        M2D = 0;
    }
    else {
        M2D = 1;
    }

    if  (fabs(motorValue2) > 1) {
        M2E = 1;                    //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
    }
    else {    
        M2E = fabs(motorValue2);      //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
    }
}

void MeasureAndControl(void)
{
    // hier the control of the 1st control system
    double refP = GetReferencePosition();                    //KOMT UIT RKI
    double Huidigepositie = motor1.getPosition(); 
    double error = (refP - Huidigepositie);// make an error
    double motorValue = FeedBackControl(error, e_prev, e_int);
    SetMotor1(motorValue);
    // hier the control of the 2nd control system
    double refP2 = GetReferencePosition2(); 
    double Huidigepositie2 = motor2.getPosition(); 
    double error2 = (refP2 - Huidigepositie2);// make an error
    double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
    SetMotor2(motorValue2);
}
     
void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
{
    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\r\n",goalx, goaly);
} 

void Loop_funtion()
{   
    switch(State){
        case Cal1: //Calibration motor 1
        // naar achteren bewegen( als voorbeeld Arvid), daarna deze waarde opslaan als offset. Dan bewegen naar home middels PID en verschil encodervalue uiterste stand en home1.
            motorValue1 = 0.1f; motorValue2=0;
            M2E = fabs(motorValue2);
            M1E = fabs(motorValue1);
            if         
               if (Huidigepositie1== 0)
               {
                                        SetMotor1(value); //value is waarde encoder voor loodrechte hoeken,.
                    if (fabs(Huidigepositie1-)<0.01) {
                        State=Cal2;
                    }
                }
                else {
                    SetMotor1(0);
                    Loop_funtion();
                }
            break;
            
        case Cal2: //Calibration motor 2
                 if (Huidigepositie2== 0)
                 {
                       if (Huidigepositie2<0.01){
                       State=CalEMG;
                 }
                 else {
                     SetMotor2(0);
                     Loop_funtion();
                 }
            break;
        case CalEMG: // Calibration EMG
            CalibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
            State=SelectDevice;
            break;    
        case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
            if (button==1) {
                State=EMG;
            }
            if (button==0) {
                State=Demo;
            }
            break;
        case EMG: //Aansturen met EMG
            Filteren();
            changePosition();
            //RKI --> output refP van motor
            MeasureAndControl();
            break;
         case Demo: // Aansturen met toetsenbord
            break;
        }
} 

/*void Tickerfunctie()
{
    //if(caldone == false)
    //{
    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();//deze moet ooit nog weg --> pas op voor errors
{
    //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);
    
    //State Machine
    State = Cal1;
    Position_controller_on = false;
    Treecko.attach(&Loop_funtion, looptime);
    while(true)
    {   }
    
 //is deze wel nodig?  
 }