File content as of revision 27:fa493551be99:

#include "mbed.h"
#include "MODSERIAL.h"
#include "BiQuad.h"
#include "HIDScope.h"
#include <math.h>

//ATTENTION:    set mBed to version 151
//              set QEI to version 0, (gebruiken wij (nog) niet, is voor encoder)
//              set MODSERIAL to version 44
//              set HIDScope to version 7
//              set biquadFilter to version 7

AnalogIn emg0_in            (A0);                   //First raw EMG signal input
AnalogIn emg1_in            (A1);                   //Second raw EMG signal input
AnalogIn emg2_in            (A2);                   //Third raw EMG signal input

InterruptIn encoderA1       (D9);
InterruptIn encoderB1       (D8);
InterruptIn encoderA2       (D12);
InterruptIn encoderB2       (D13);

InterruptIn button1         (D10);                  
InterruptIn button2         (D11);

DigitalOut directionpin1    (D7);
DigitalOut directionpin2    (D4);

PwmOut pwmpin1              (D6);
PwmOut pwmpin2              (D5);

DigitalOut ledr             (LED_RED);
DigitalOut ledb             (LED_BLUE);
DigitalOut ledg             (LED_GREEN);


MODSERIAL pc(USBTX, USBRX);                       //Serial communication to see if the code works step by step, turn on if hidscope is off

//HIDScope    scope( 6 );                             //HIDScope set to 3x2 channels for 3 muscles, raw data + filtered

//Tickers
Ticker      ticker;                          

//Global variables
const float T   = 0.002f;                           //Ticker period             Deze wordt ook gebruikt in de PID, moet die niet anders???

//EMG filter
double emg0_filt, emg1_filt, emg2_filt;                                                          //Variables for filtered EMG data channel 0, 1 and 2
double emg0_raw, emg1_raw, emg2_raw;
double emg0_filt_x, emg1_filt_x, emg2_filt_x;
const int windowsize = 150;                                                                      //Size of the array over which the moving average (MovAg) is calculated. (random number)
double sum, sum1, sum2, sum3;                                                                    //variables used to sum elements in array
double StoreArray0[windowsize], StoreArray1[windowsize], StoreArray2[windowsize];                //Empty arrays to calculate MoveAg
double movAg0, movAg1, movAg2;                                                                   //outcome of MovAg (moet dit een array zijn??)

//Calibration variables
int x = -1;                                                                                      //Start switch, colour LED is blue.
int emg_cal = 0;                                                                                 //if emg_cal is set to 1, motors can begin to work in this code (!!)
const int sizeCal = 1500;                                                                        //size of the dataset used for calibration, eerst 2000
double StoreCal0[sizeCal], StoreCal1[sizeCal], StoreCal2[sizeCal];                               //arrays to put the dataset of the calibration in
double Mean0,Mean1,Mean2;                                                                        //average of maximum tightening
double Threshold0, Threshold1, Threshold2; 

//Biquad                                                                                         //Variables for the biquad band filters (alle 3 dezelfde maar je kan niet 3x 'emg0band' aanroepen ofzo)
BiQuadChain emg0filter;
BiQuad emg0band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg0band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg0band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
BiQuad notch1( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter biquad coefficients

BiQuadChain emg1filter;
BiQuad emg1band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg1band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg1band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
BiQuad notch2( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter

BiQuadChain emg2filter;
BiQuad emg2band1( 7.29441e-01, -1.89276e-08, -7.29450e-01, -1.64507e-01, -7.26543e-01 );
BiQuad emg2band2( 1.00000e+00, 1.99999e+00, 9.99994e-01, 1.72349e+00, 7.79616e-01 );
BiQuad emg2band3( 1.00000e+00, -1.99999e+00, 9.99994e-01, -1.93552e+00, 9.39358e-01 );
BiQuad notch3( 9.91104e-01, -1.60364e+00, 9.91104e-01, -1.60364e+00, 9.82207e-01 );                //Notch filter

// Inverse Kinematica variables
const double L1 = 0.208;                                  // Hoogte van tafel tot joint 1
const double L2 = 0.288;                                  // Hoogte van tafel tot joint 2
const double L3 = 0.212;                                  // Lengte van de arm
const double L4 = 0.089;                                  // Afstand van achterkant base tot joint 1
const double L5 = 0.030;                                  // Afstand van joint 1 naar joint 2
const double r_trans = 0.035;                             // Kan gebruikt worden om om te rekenen van translation naar shaft rotation 

// Variërende variabelen inverse kinematics: 
double q1ref = 0;                                   // Huidige motorhoek van joint 1 zoals bepaald uit referentiesignaal --> checken of het goede type is
double q2ref = 0;                                   // Huidige motorhoek van joint 2 zoals bepaald uit referentiesignaal --> checken of het goede type is
double v_x;                                         // Desired velocity end effector in x direction --> Determined by EMG signals
double v_y;                                         // Desired velocity end effector in y direction --> Determined by EMG signals

double Lq1;                                         // Translatieafstand als gevolg van motor rotation joint 1
double Cq2;                                         // Joint angle of the system (corrected for gear ratio 1:5)

double q1_dot;                                      // Benodigde hoeksnelheid van motor 1 om v_des te bereiken
double q2_dot;                                      // Benodigde hoeksnelheid van motor 2 om v_des te bereiken 

double q1_ii;                                       // Reference signal for motorangle q1ref 
double q2_ii;                                       // Reference signal for motorangle q2ref

//Variables PID controller
double PI = 3.14159;
double Kp1 = 17.5;                                  //Motor 1
double Ki1 = 1.02;
double Kd1 = 23.2;
double encoder1 = 0;
double encoder_radians1=0;

double Kp2 = 17.5;                                  //Motor 2
double Ki2 = 1.02;
double Kd2 = 23.2;
double encoder2 = 0;
double encoder_radians2=0;

double start_control = 0;


//--------------Functions----------------------------------------------------------------------------------------------------------------------------//


//------------------ Encoder motor 1 --------------------------------//

void encoderA1_rise()       
{
    if(encoderB1==false)
    {
        encoder1++;
    }
    else
    {
        encoder1--;
    }
}

void encoderA1_fall()      
{
    if(encoderB1==true)
    {
        encoder1++;
    }
    else
    {
        encoder1--;
    }
}

void encoderB1_rise()       
{
    if(encoderA1==true)
    {
        encoder1++;
    }
    else
    {
        encoder1--;
    }
}

void encoderB1_fall()      
{
    if(encoderA1==false)
    {
        encoder1++;
    }
    else
    {
        encoder1--;
    }
}

void encoder_count1()
{
    encoderA1.rise(&encoderA1_rise);
    encoderA1.fall(&encoderA1_fall);
    encoderB1.rise(&encoderB1_rise);
    encoderB1.fall(&encoderB1_fall);
}

//------------------ Encoder motor 2 --------------------------------//

void encoderA2_rise()       
{
    if(encoderB2==false)
    {
        encoder2++;
    }
    else
    {
        encoder2--;
    }
}

void encoderA2_fall()      
{
    if(encoderB2==true)
    {
        encoder2++;
    }
    else
    {
        encoder2--;
    }
}

void encoderB2_rise()       
{
    if(encoderA2==true)
    {
        encoder2++;
    }
    else
    {
        encoder2--;
    }
}

void encoderB2_fall()      
{
    if(encoderA2==false)
    {
        encoder2++;
    }
    else
    {
        encoder2--;
    }
}

void encoder_count2()
{
    encoderA2.rise(&encoderA2_rise);
    encoderA2.fall(&encoderA2_fall);
    encoderB2.rise(&encoderB2_rise);
    encoderB2.fall(&encoderB2_fall);
}

//------------------ Filter EMG + Calibration EMG --------------------------------//

void EMGFilter0()
{   
    emg0_raw      = emg0_in.read();                      //give name to raw EMG0 data calve
    emg0_filt_x   = emg0filter.step(emg0_raw);           //Use biquad chain to filter raw EMG data
    emg0_filt     = abs(emg0_filt_x);                    //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch, stel er komt iets raars uit, dan Notch uit de biquad chain halen en aparte chain voor aanmaken.
}

void EMGFilter1()
{
    emg1_raw      = emg1_in.read();                      //give name to raw EMG1 data bicep 1
    emg1_filt_x   = emg1filter.step(emg1_raw);           //Use biquad chain to filter raw EMG data
    emg1_filt     = abs(emg1_filt_x);                    //rectifier. LET OP: volgorde filter: band-notch-rectifier. Eerst band-rect-notch.
}

void EMGFilter2()
{
    emg2_raw      = emg2_in.read();                      //Give name to raw EMG1 data bicep 2
    emg2_filt_x   = emg2filter.step(emg2_raw);           //Use biquad chain to filter raw EMG data
    emg2_filt     = abs(emg2_filt_x);                    //Rectifier. LET OP: volgorde filter: band-notch-rectifier.
}
 
void MovAg()                                         //Calculate moving average (MovAg)
{
    for (int i = windowsize-1; i>=0; i--)            //Make arrays for the last datapoints of the filtered signals
    {
        StoreArray0[i] = StoreArray0[i-1];           //Shifts the i'th element one place to the right, this makes it "rolling or moving" average.
        StoreArray1[i] = StoreArray1[i-1];
        StoreArray2[i] = StoreArray2[i-1];
    }
    
    StoreArray0[0] = emg0_filt;                      //Stores the latest datapoint of the filtered signal in the first element of the array
    StoreArray1[0] = emg1_filt;
    StoreArray2[0] = emg2_filt;
    
    sum1 = 0.0;
    sum2 = 0.0;
    sum3 = 0.0;
    
    for(int a = 0; a<= windowsize-1; a++)            //Sums the elements in the arrays
    {
        sum1 += StoreArray0[a];
        sum2 += StoreArray1[a];
        sum3 += StoreArray2[a];
    }
    
    movAg0 = sum1/windowsize;                        //calculates an average in the array
    movAg1 = sum2/windowsize;
    movAg2 = sum3/windowsize;
}

void emg_filtered()             //Call all filter functions
{
    EMGFilter0();
    EMGFilter1();
    EMGFilter2();
    MovAg();
}
void switch_to_calibrate()
{
    x++;                        //Every time function gets called, x increases. Every button press --> new calibration state.
                                //Starts with x = -1. So when function gets called 1 time, x = 0.  In the end, x = 4 will reset to -1.

    if(x==0)                    //If x = 0, led is red
    {
        ledr = 0;
        ledb = 1;
        ledg = 1;
    }
    else if (x==1)              //If x = 1, led is blue
    {
        ledr = 1;
        ledb = 0;
        ledg = 1;
    }
    else if (x==2)            //If x = 2, led is green
    {
        ledr = 1;
        ledb = 1;
        ledg = 0;
    }
    else                        //If x = 3 or 4, led is white
    {
        ledr = 0;
        ledb = 0;
        ledg = 0;
    }
   
    if(x==4)                    //Reset back to x = -1
    {
        x = -1;
        emg_cal=0;              //reset, motors off
    }
}
    
        
void calibrate(void)
{
    switch(x)
    {
        case 0:                                         //If calibration state 0:
        {
            sum = 0.0;
            for(int j = 0; j<=sizeCal-1; j++)           //Array filled with datapoints from the EMGfilter signal of muscle 0
            {
                StoreCal0[j] = emg0_filt;
                sum+=StoreCal0[j];
                wait(0.001f);                           //Does there need to be a wait?
            }
            Mean0       = sum/sizeCal;                  //Calculate mean of the datapoints in the calibration set (2000 samples)
            Threshold0  = Mean0/2;                      //Threshold calculation = 0.5*mean                                            
            break;                                      //Stop. Threshold is calculated, we will use this further in the code
        }
        case 1:                                         //If calibration state 1:
        {
            sum = 0.0;                                  
            for(int j = 0; j<=sizeCal-1; j++)           //Array filled with datapoints from the EMGfilter signal of muscle 1
            {
                StoreCal1[j] = emg1_filt;
                sum+=StoreCal1[j];
                wait(0.001f);
            }
            Mean1       = sum/sizeCal;
            Threshold1  = Mean1/2;                      
            break;
        }
        case 2:                                         //If calibration state 2:
        {
            sum = 0.0;
            for(int j = 0; j<=sizeCal-1; j++)           //Array filled with datapoints from the EMGfilter signal of muscle 2
            {
                StoreCal2[j] = emg2_filt;
                sum+=StoreCal2[j];
                wait(0.001f);
            }
            Mean2       = sum/sizeCal;
            Threshold2  = Mean2/2;                    
            break;
        }
        case 3:                                         //EMG is calibrated, robot can be set to Home position.
        {
            emg_cal = 1;                                //This is the setting for which the motors can begin turning in this code (!!)
            
            wait(0.001f);
            break;
        }
        default:                                        //Ensures nothing happens if x is not 0,1 or 2.
        {
            break;
        }
    }
}
/*
void HIDScope_sample()
{    
    scope.set(0,emg0_raw);
    scope.set(1,emg0_filt);
    scope.set(1,movAg0);          //als moving average werkt
    scope.set(2,emg1_raw);
    scope.set(3,emg1_filt);
    scope.set(3,movAg1);          //als moving average werkt
    scope.set(4,emg2_raw);
    scope.set(5,emg2_filt);
    scope.set(5,movAg2);          //als moving average werkt

    scope.send();                   //Send data to HIDScope server
}
*/

//------------------ Inversed Kinematics --------------------------------//

void inverse_kinematics()
{
    Lq1 = q1ref*r_trans;                            
    Cq2 = q2ref/5.0;                               

    q1_dot = v_x + (v_y*(L1 + L3*sin(Cq2)))/(L4 + Lq1 + L3*cos(Cq2));     
    q2_dot = v_y/(L4 + Lq1 + L3*cos(Cq2));                                       

    q1_ii = q1ref + q1_dot*T;                       
    q2_ii = q2ref + q2_dot*T; 
        
    q1ref = q1_ii;
    q2ref = q2_ii; 
    
    start_control = 1;
}

void v_des_calculate_qref()
{
    if(emg_cal==1)                                   //After calibration is finished, emg_cal will be 1. Otherwise 0. 
    { 
                if(movAg1>Threshold1)                   //If the filtered EMG signal of muscle 1 is higher than the threshold, motor 1 will turn
                {
                    v_x = 1.0;                          //beweging in +x direction
                    ledr = 0;                           //red
                    ledb = 1;
                    ledg = 1;
                }
                else if(movAg2>Threshold2)              //If the filtered EMG signal of muscle 2 is higher than the threshold, motor 1 and 2 will turn
                {
                    v_y = 1.0;                          //beweging in +y direction
                    ledr = 1;                           //green
                    ledb = 1;
                    ledg = 0;
                }
               
                else if(movAg0>Threshold0)              //If the filtered EMG signal of muscle 0 is higher than the threshold, motor1 will turn in 1 direction
                {
                    v_x = -v_x;
                    v_y = -v_y;
                    ledr = 1;                           //Blue
                    ledb = 0;
                    ledg = 1;  
                }             
                else                                    //If not higher than the threshold, motors will not turn at all
                {                    
                    v_x = 0;
                    v_y = 0;
                    ledr = 0;                           //white
                    ledb = 0;
                    ledg = 0;
                }
                
        inverse_kinematics();                           //Call inverse kinematics function
        
        }
}

//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
double PID_controller1(double err1)
{
  static double err_integral1 = 0;
  static double err_prev1 = err1; // initialization with this value only done once!
  
  static BiQuad LowPassFilter1(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

  // Proportional part:
  double u_k1 = Kp1 * err1;

  // Integral part  
  err_integral1 = err_integral1 + err1 * T;
  double u_i1 = Ki1 * err_integral1;

  // Derivative part
  double err_derivative1 = (err1 - err_prev1)/T;
  double filtered_err_derivative1 = LowPassFilter1.step(err_derivative1);
  double u_d1 = Kd1 * filtered_err_derivative1;
  err_prev1 = err1;

  // Sum all parts and return it
  return u_k1 + u_i1 + u_d1;  
}

void start_your_engines1(double u1)
{
    if(encoder1<5250 && encoder1>-5250)                              //limits rotation, in counts                
    {
         pwmpin1 = fabs(u1);                                         //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
         directionpin1.write(u1 < 0.0f);
    }
    else
    {
        pwmpin1 = 0;
    }
}  

void engine_control1()                                           //Engine 1 is rotational engine, connected with left side pins
{
    while(start_control == 1)
    {
        encoder_radians1 = encoder1*(2*PI)/8400;
        double err1 = q1ref - encoder_radians1;
        double u1 = PID_controller1(err1);                               //PID controller function call
        start_your_engines1(u1);                                         //Call start_your_engines function
        
        break;
    }
}



//---------PID controller motor 1 + start motor 1 -----------------------------------------------------------//
double PID_controller2(double err2)
{
  static double err_integral2 = 0;
  static double err_prev2 = err2; // initialization with this value only done once!
  
  static BiQuad LowPassFilter2(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);

  // Proportional part:
  double u_k2 = Kp2 * err2;

  // Integral part
  err_integral2 = err_integral2 + err2 * T;
  double u_i2 = Ki2 * err_integral2;

  // Derivative part
  double err_derivative2 = (err2 - err_prev2)/T;
  double filtered_err_derivative2 = LowPassFilter2.step(err_derivative2);
  double u_d2 = Kd2 * filtered_err_derivative2;
  err_prev2 = err2;

  // Sum all parts and return it
  return u_k2 + u_i2 + u_d2;  
}

void start_your_engines2(double u2)
{
     if(encoder2<12600 && encoder2>-1)                              //limits translation in counts
     {
        pwmpin2 = fabs(u2);                                       //u_total moet nog geschaald worden om in de motor gevoerd te worden!!!
        directionpin2.write(u2 < 0.0f);
     }
    else
     {
        pwmpin2 = 0;
     }
    
}  

void engine_control2()                                             //Engine 2 is translational engine, connected with right side wires
{
    while(start_control == 1)
    {
        encoder_radians2 = encoder2*(2*PI)/8400;
        double err2 = q2ref - encoder_radians2;
        double u2 = PID_controller2(err2);                             //PID controller function call
        start_your_engines2(u2);                                       //Call start_your_engines function
        
        break;
    }
}

//------------------ Start main function --------------------------//


int main()
{         
        pc.baud(115200);
        pc.printf("Hello World!\r\n");                            //Serial communication only works if hidscope is turned off.
        pwmpin1.period_us(60);                                    //60 microseconds PWM period, 16.7 kHz 

        emg0filter.add( &emg0band1 ).add( &emg0band2 ).add( &emg0band3 ).add( &notch1 );        //attach biquad elements to chain
        emg1filter.add( &emg1band1 ).add( &emg1band2 ).add( &emg1band3 ).add( &notch2 );
        emg2filter.add( &emg2band1 ).add( &emg2band2 ).add( &emg2band3 ).add( &notch3 );
        
    while(true)
    {
        ticker.attach(&emg_filtered,T);                         //EMG signals filtered + moving average every T sec.
        ticker.attach(&v_des_calculate_qref,T);                 //v_des determined every T
              
       // HIDScope_tick.attach(&HIDScope_sample,T);             //EMG signals raw + filtered to HIDScope every T sec.
        
        button1.rise(switch_to_calibrate);                      //Switch state of calibration (which muscle)
        wait(0.2f);                                             //Wait to avoid bouncing of button
        button2.rise(calibrate);                                //Calibrate threshold for 3 muscles
        wait(0.2f);                                             //Wait to avoid bouncing of button
    
        pc.printf("x is %i\n\r",x);
        pc.printf("Movag0 = %f , Movag1 = %f, Movag2 = %f \n\r",movAg0, movAg1, movAg2);
        pc.printf("Thresh0 = %f , Thresh1 = %f, Thresh2 = %f \n\r",Threshold0, Threshold1, Threshold2);
        //wait(2.0f);    
    }      
}