File content as of revision 34:87b96cc11c97:

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

// Variables
//State Machine and calibration
enum States {CalEMG, SelectDevice, EMG, Rest, Demonstration};
States State;
DigitalIn button (D1);

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

//Encoder and motor
double Huidigepositie1;
double Huidigepositie2;
double motorValue1;
double motorValue2;
Ticker Treecko;             //We make an awesome ticker for our control system
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);
double PwmPeriod = 1.0/5000.0;

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

MODSERIAL pc(USBTX,USBRX);

//PID
const double Ts = 0.002f;                   // tickertime/ sample time
double e_prev = 0; 
double e_int = 0;
double e_prev2 = 0;
double e_int2 = 0;

// EMG and 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;

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

volatile double LBF;
volatile double RBF;
volatile double LTF;
volatile double RTF;

// RKI
double pi = 3.14159265359;
double q1 = (pi/2);                     //Reference position angle 1 in radiance
double q2 = -(pi/2);                    //Reference position angle 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;

// Functions
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()); 
    emgHPRB = HPFRB.step(emgNotchRB); 
    emgAbsHPRB = abs(emgHPRB);
    emgLPRB = LPFRB.step(emgAbsHPRB);
    emgMEANSUBLB = emgLPLB - RESTMEANLB;
    RBF = emgLPRB/MVCRB;
    
    emgNotchLT = NFLT.step(emgLT.read() );
    emgHPLT = HPFLT.step(emgNotchLT);
    emgAbsHPLT = abs(emgHPLT);
    emgLPLT = LPFLT.step(emgAbsHPLT);
    emgMEANSUBLT = emgLPLT - RESTMEANLT;
    LTF = emgLPLT/MVCLT;
    
    emgNotchRT = NFRT.step(emgRT.read() );
    emgHPRT = HPFRT.step(emgNotchRT);
    emgAbsHPRT = abs(emgHPRT);
    emgLPRT = LPFRT.step(emgAbsHPRT);
    emgMEANSUBRT = emgLPRT - RESTMEANRT;
    RTF = emgLPRT/MVCRT;
}

void CalibrationEMG()
{
    pc.printf("Timescalibration = %i \r\n",Timescalibration);
    Timescalibration++;
    
    if(Timescalibration<2000)
    {
        pc.printf("calibration rest EMG, 12 seconds \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 left biceps, 6 seconds \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 right biceps, 6 seconds \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 left triceps, 6 seconds \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 right triceps, 6 seconds \r\n");
        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("calibration finished");
         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;
    
    refP = (((0.5*pi) - q1)/(2*pi));
    refP2 = (( q1 + q2)/(2*pi));    //Get reference positions
}

void SetpointRobot()                    // Get position from Potmeters
{    
    double Potmeterwaarde2 = potMeter2.read();
    double Potmeterwaarde1 = potMeter1.read();
    
    if  (Potmeterwaarde2>0.6) {
        Rsx += 0.001;                    //increases 1 mm when potmetervalue above 0.6
    }
    else if  (Potmeterwaarde2<0.4) {
        Rsx -= 0.001;                    //decreases 1 mm when potmetervalue below 0.4
    }
    else {                               //x value of setpoint doesn't change
    }

    if (Potmeterwaarde1>0.6) {           //increases 1 mm when potmetervalue above 0.6
        Rsy += 0.001;
    }
    else if (Potmeterwaarde1<0.4) {      //decreases 1 mm when potmetervalue below 0.4
        Rsy -= 0.001;                       
    }
    else {                               //y value of setpoint doesn't change
    }
}

void changePosition ()  // Get position from EMG signals
{
    if (RBF>0.5) {
        Rsx +=0.001;    // increases 1 mm
    }
    else {}
    if (RTF>0.5) {
        Rsx -=0.001;
    }
    else {}
    if (LBF>0.5) {
        Rsy +=0.001;
    }
    else {}
    if (LTF>0.5) {
        Rsy -=0.001;
    }
    else {}
}

double FeedBackControl(double error, double &e_prev, double &e_int)   // PID controller motor 1
// The values are not correctly tuned.
{
    double kp = 3.36;                           // kind of scaled.
    double Proportional= kp*error;
    
    double kd = 3.36;                           // kind of scaled. 
    double VelocityError = (error - e_prev)/Ts; 
    double Derivative = kd*VelocityError;
    e_prev = error;
    
    double ki = 4.2;                            // kind of scaled.
    e_int = e_int+Ts*error;
    double Integrator = ki*e_int;
    
    double motorValue = (Proportional + Integrator + Derivative)/4200;
    return motorValue;
}

double FeedBackControl2(double error2, double &e_prev2, double &e_int2)   // PID controller motor 2
// The values are not correctly tuned.
{
    double kp2 = 3.36                           // kind of scaled.
    double Proportional2= kp2*error2;
    
    double kd2 = 3.36;                          // kind of scaled. 
    double VelocityError2 = (error2 - e_prev2)/Ts; 
    double Derivative2 = kd2*VelocityError2;
    e_prev2 = error2;
    
    double ki2 = 2.1;                           // 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;                    //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
    }
    else {    
        M1E = fabs(motorValue);      //absolute velocity determined, motor is "off" at value of 0
    }
}

void SetMotor2(double motorValue2)
{
    if (motorValue2 >= 0) {
        M2D = 1;
    }
    else {
        M2D = 0;
    }
    if  (fabs(motorValue2) > 1) {
        M2E = 1;                    //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
    }
    else {    
        M2E = fabs(motorValue2);      //absolute velocity determined, motor is "off" at value of 0
    }
}

void MeasureAndControl(void)
{
    RKI();
    // control of 1st motor
    double Huidigepositie = motor1.getPosition()/4200; 
    double error = (refP - Huidigepositie);// make an error
    double motorValue = FeedBackControl(error, e_prev, e_int);
    SetMotor1(motorValue);
    // control of 2nd motor
    double Huidigepositie2 = motor2.getPosition()/4200; 
    double error2 = (refP2 - Huidigepositie2);// make an error
    double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
    SetMotor2(motorValue2);
}

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 {           // button==0
                State=Demonstration;
            }
            break;
        case EMG: //Control by EMG
            Filteren();
            changePosition();
            MeasureAndControl();
            if (button==0) {
                State=Rest;
            }
            else {}
            break;
        case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions.
            refP=refP;
            refP2=refP2;
            double Huidigepositie = motor1.getPosition()/4200; 
            double error = (refP - Huidigepositie);// make an error
            double motorValue = FeedBackControl(error, e_prev, e_int);
            SetMotor1(motorValue);
            // control of 2nd motor
            double Huidigepositie2 = motor2.getPosition()/4200; 
            double error2 = (refP2 - Huidigepositie2);// make an error
            double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
            SetMotor2(motorValue2);
            
            if ( button==1) {
                State=EMG;
            }
            else {}
            break;
        case Demonstration: // Control with Potmeters
            SetpointRobot();
            MeasureAndControl();
            break;
        }
} 

int main()
{
    //for filtering EMG
    //Left Biceps
    NFLB.add( &N1LB );
    HPFLB.add( &HP1LB ).add( &HP2LB );
    LPFLB.add( &LP1LB ).add( &LP2LB );
    
    //Right Biceps
    NFRB.add( &N1RB );
    HPFRB.add( &HP1RB ).add( &HP2RB );
    LPFRB.add( &LP1RB ).add( &LP2RB );  
    
    //Left Triceps
    NFLT.add( &N1LT );
    HPFLT.add( &HP1LT ).add( &HP2LT );
    LPFLT.add( &LP1LT ).add( &LP2LT ); 
    
    //Right Triceps
    NFRT.add( &N1RT );
    HPFRT.add( &HP1RT ).add( &HP2RT );
    LPFRT.add( &LP1RT ).add( &LP2RT ); 
    
    // serial
    pc.baud(115200);
    
    //motor
    M1E.period(PwmPeriod); //set PWMposition at 5000hz
   
    //State Machine
    State = CalEMG;
    Treecko.attach(&Loop_funtion, Ts);
    while(true)
    {}

 }