File content as of revision 27:2d9f172c66ad:

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

//State Machine and calibration
enum States {CalEMG, SelectDevice, EMG, Rest, Demonstration};
States State;
int Counter;
bool Position_controller_on;
double looptime = 0.002f;
DigitalIn button (D1);

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

//global variables Motor
Ticker Treecko;             //We make an 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);

//Demonstration
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.002f;                   // 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;

// Filters
// Biquad filters voor Left Biceps (LB): Notch, High-pass and 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 Biceps (RB): Notch, High-pass and 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 Triceps (LT): Notch, High-pass and 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 for Right Triceps (RT): Notch, High-pass and 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;

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

// variables RKI
double pi = 3.14159265359;
double q1 = (pi/2);    //Reference position hoek 1 in radiance
double q2 = -(pi/2);   //Reference position hoek 2 in radiance
const double L1 = 0.30;        //Length arm 1 in mm
const double L2 = 0.38;        //Length arm 2 in mm
double B1 = 1;          //Friction constant motor 1
double B2 = 1;          //Friction constant motor 2
double K = 1;           //Spring constant movement from end-effector position to setpoint position
double Tijd = 1;           //Timestep value
double Rsx = 0.38;       //Reference x-component of the setpoint radius
double Rsy = 0.30;       //Reference y-component of the setpoint radius
double refP = 0;      //Reference position motor 1
double refP2 = 0; //Reference position motor 2
double Rex = cos(q1)*L1 - sin(q2)*L2;   //The x-component of the end-effector radius 
double Rey = sin(q1)*L1 + cos(q2)*L2;   //The y-component of the end-effector radius
double R1x = 0;                         //The x-component of the joint 1 radius
double R1y = 0;                         //The y-component of the joint 1 radius
double R2x = cos(q1)*L1;                //The x-component of the joint 2 radius
double R2y = sin(q1)*L1;                //The y-component of the joint 2 radius   
double Fx = 0;
double Fy = 0;
double Tor1 = 0;
double Tor2 = 0;
double w1= 0;
double w2= 0;


void Filteren() 
{  
    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;
    //}
}

void CalibrationEMG()
{
    pc.printf("Timescalibration = %i \r\n",Timescalibration);
    Timescalibration++;
    
    if(Timescalibration<2000)
    {
        pc.printf("calibration rest EMG \r\n");
        led = 1;
        
        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 = 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<3000)
    {
        pc.printf("maximum linker biceps \r\n");
        led = 1;
        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>3000 && Timescalibration<4000)
    {
        pc.printf(" maximum rechter biceps \r\n");
        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>4000 && Timescalibration<5000)
    {
        pc.printf("maximum linker triceps \r\n");
        led = 1;
        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>5000 && Timescalibration<6000)
    {
        pc.printf("maximum rechter triceps");
        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>6000)
    {
         pc.printf("calibratie afgelopen");
         State = SelectDevice; 
    }

}

void RKI()
{
    Rex = cos(q1)*L1 - sin(q2)*L2;
    Rey = sin(q1)*L1 + cos(q2)*L2;
    R2x = cos(q1)*L1;
    R2y = sin(q1)*L1;
    Fx = (Rsx-Rex)*K;
    Fy = (Rsy-Rey)*K;
    Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx;
    Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx;
    w1 = Tor1/B1;
    w2 = Tor2/B2;
    q1 = q1 + w1*Tijd;
    q2 = q2 + w2*Tijd;
    
    int maxwaarde = 4096;                   // = 64x64
    refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
    refP2 = (( q1 + q2)/(2*pi))*maxwaarde;    //Get reference positions was eerst 0.5*pi
}

void SetpointRobot()
{    
    double Potmeterwaarde2 = potMeter2.read();
    double Potmeterwaarde1 = potMeter1.read();
    
    if  (Potmeterwaarde2>0.6) {
        Rsx += 0.001;                    //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
    }
    else if  (Potmeterwaarde2<0.4) {
        Rsx -= 0.001;                    //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
    }
    else {                               //de x-waarde van de setpoint verandert niet
    }

    if (Potmeterwaarde1>0.6) {           //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
        Rsy += 0.001;
    }
    else if (Potmeterwaarde1<0.4) {      //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
        Rsy -= 0.001;                       
    }
    else {                               //de y-waarde van de setpoint verandert niet
    }
}

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.0015;                               // kind of scaled.
    double Proportional= kp*error;
    
    double kd = 0.000008;                           // kind of scaled. 
    double VelocityError = (error - e_prev)/Ts; 
    double Derivative = kd*VelocityError;
    e_prev = error;
    
    double ki = 0.0001;                           // 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.002;                             // kind of scaled.
    double Proportional2= kp2*error2;
    
    double kd2 = 0.000008;                           // 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 = 1;
    }
    else {
        M2D = 0;
    }
    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)
{
    //SetpointRobot(); 
    // RKI aanroepen
    RKI();
    // control of 1st motor
    //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);
    // control of 2nd motor
    //double refP2 = GetReferencePosition2(); 
    double Huidigepositie2 = motor2.getPosition(); 
    double error2 = (refP2 - Huidigepositie2);// make an error
    double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
    SetMotor2(motorValue2);
    pc.printf("refP = %f, huidigepos = %f, motorvalue = %f, refP2 = %f, huidigepos2 = %f, motorvalue2 = %f \r\n", refP, Huidigepositie, motorValue, refP2, Huidigepositie2, Huidigepositie2);
}
     
void changePosition ()    // DIT MOET NOG HEEL ERG GETUNED WORDEN !!!
{
    if (RBF>0.6) {
        Rsx +=0.001;    // hoe veel verder gaat hij? 1 cm? 10 cm?
    }
    else {}
    if (RTF>0.6) {
        Rsx -=0.001;
    }
    else {}
    if (LBF>0.6) {
        Rsy +=0.001;
    }
    else {}
    if (LTF>0.6) {
        Rsy -=0.001;
    }
    else {}
    pc.printf("goalx = %i, goaly = %i\r\n",goalx, goaly);
} 

void Loop_funtion()
{   
    pc.printf("state machine begint \r\n");
    switch(State)
    {    
        case CalEMG: // Calibration EMG
            CalibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
            break;    
        case SelectDevice: //Looks at the difference between current position and home. Select aansturen EMG or buttons
            State = EMG;
            if (button==1) {
                State=EMG;
            }
            else {           // if (button==0) {
                State=Demonstration;
            }
            break;
        case EMG: //Control by EMG
            Filteren();
            changePosition();
            MeasureAndControl();
            break;
        case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions.
        case Demonstration: // COntrol with potmeters
            break;
        }
} 

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);
    pc.printf("begint met programma \r\n");
    
    //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 = CalEMG;
    Position_controller_on = false;
    Treecko.attach(&Loop_funtion, Ts);
    while(true)
    {   }
    
 //is deze wel nodig?  
 }