File content as of revision 18:7fb73aa6dbc0:

/*           ___________     ___
            /  |    |   \   /  o\
           /___|____|____\_/   _ >
          /   /   |  \     _____/
          \_ / ___|__ \ __/ 
           /  /      \  \
          /__/        \__\                */

#include "mbed.h"
#include "math.h"
#include "QEI.h"
#include "BiQuad.h"
#include "MODSERIAL.h"

MODSERIAL       pc(USBTX, USBRX);

//Defining all in- and outputs
//EMG input
AnalogIn        emgBR( A0 );    //Right Biceps
AnalogIn        emgBL( A1 );    //Left Biceps

//Output motor 1 and reading Encoder motor 1
DigitalOut      motor1DirectionPin(D4);
PwmOut          motor1MagnitudePin(D5);
QEI             Encoder1(D12,D13,NC,32);

//Output motor 2 and reading Encoder motor 2
DigitalOut      motor2DirectionPin(D7);
PwmOut          motor2MagnitudePin(D6);
QEI             Encoder2(D10,D11,NC,32);

//Output motor 3 and reading Encoder motor 3
DigitalOut      motor3DirectionPin(D8);
PwmOut          motor3MagnitudePin(D9);
QEI             Encoder3(D2,D3,NC,32);

//LED output, needed for feedback
DigitalOut      led_R(LED_RED);
DigitalOut      led_G(LED_GREEN);
DigitalOut      led_B(LED_BLUE);

//Setting Tickers for sampling EMG and determing if the threshold is met
Ticker          sample_timer;
Ticker          threshold_timerR;
Ticker          threshold_timerL;

//Timer needed to determine the threshold for a pre-set time period
Timer           t_thresholdR;
Timer           t_thresholdL;

//Variables to store the current time in
float           currentTimeTR;
float           currentTimeTL;

//This is used when testing without EMG
InterruptIn     button(SW2); 
InterruptIn     button2(SW3);

//Timer needed for timing EMG input for the X and Y coördinates
Timer           t;

// Boolean needed to know if new input coordinates have to be given
bool            Move_done = false;
bool            Input_done = true;

/* Defining all the different BiQuad filters, which contain a Notch filter,
High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
and the Low-pass filter cancels out all frequencies below 4 Hz. The filters are
declared four times, so that they can be used for sampling of right and left
biceps, during measurements and calibration. */

/* Defining all the normalized values of b and a in the Notch filter for the
creation of the Notch BiQuad */

BiQuad          bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
BiQuad          bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );

BiQuad          bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
BiQuad          bqNotchTL( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );

/* Defining all the normalized values of b and a in the High-pass filter for the
creation of the High-pass BiQuad */

BiQuad          bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
BiQuad          bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );

BiQuad          bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
BiQuad          bqHighTL( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );

/* Defining all the normalized values of b and a in the Low-pass filter for the
creation of the Low-pass BiQuad */

BiQuad          bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
BiQuad          bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );

BiQuad          bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
BiQuad          bqLowTL( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );

// Creating a variable needed for the creation of the BiQuadChain
BiQuadChain     bqChain1;
BiQuadChain     bqChain2;

BiQuadChain     bqChainTR;
BiQuadChain     bqChainTL;

//Declaring all floats needed in the filtering process
float          emgBRfiltered;   //Right biceps Notch+High pass filter
float          emgBRrectified;  //Right biceps rectified
float          emgBRcomplete;   //Right biceps low-pass filter, filtering complete

float          emgBLfiltered;   //Left biceps Notch+High pass filter
float          emgBLrectified;  //Left biceps rectified
float          emgBLcomplete;   //Left biceps low-pass filter, filtering complete

// Declaring all variables needed for getting the Threshold value
float          numsamples = 500;
float          emgBRsum = 0;
float          emgBRmeanMVC;
float          thresholdBR;

float          emgBLsum = 0;
float          emgBLmeanMVC;
float          thresholdBL;

/* Function to sample the EMG of the Right Biceps and get a Threshold value
from it, which can be used throughout the process */

void Threshold_samplingBR()
{
    t_thresholdR.start();
    currentTimeTR = t_thresholdR.read();

    if (currentTimeTR <= 1) {

        emgBRfiltered = bqChainTR.step( emgBR.read() );     //Notch+High-pass
        emgBRrectified = fabs(emgBRfiltered);               //Rectification
        emgBRcomplete = bqLowTR.step(emgBRrectified);       //Low-pass

        emgBRsum = emgBRsum + emgBRcomplete;
    }
    emgBRmeanMVC = emgBRsum/numsamples;
    thresholdBR = emgBRmeanMVC * 0.20;

}
/* Function to sample the EMG of the Left Biceps and get a Threshold value
from it, which can be used throughout the process */

void Threshold_samplingBL()
{
    t_thresholdL.start();
    currentTimeTL = t_thresholdL.read();

    if (currentTimeTL <= 1) {

        emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
        emgBLrectified = fabs( emgBLfiltered );           //Rectification
        emgBLcomplete = bqLow2.step( emgBLrectified );    //Low-pass

        emgBLsum = emgBLsum + emgBLcomplete;
    }

    emgBLmeanMVC = emgBLsum/numsamples;
    thresholdBL = emgBLmeanMVC * 0.20;
    
}

// EMG sampling and filtering

void EMG_sample()
{
    //Filtering steps for the Right Biceps EMG
    emgBRfiltered = bqChain1.step( emgBR.read() );   //Notch+High-pass
    emgBRrectified = fabs(emgBRfiltered);            //Rectification
    emgBRcomplete = bqLow1.step(emgBRrectified);     //Low-pass

    //Filtering steps for the Left Biceps EMG
    emgBLfiltered = bqChain2.step( emgBL.read() );    //Notch+High-pass
    emgBLrectified = fabs( emgBLfiltered );           //Rectification
    emgBLcomplete = bqLow2.step( emgBLrectified );    //Low-pass
}

//Function to make the BiQuadChain for the Notch and High pass filter for all three filters
void getbqChain()
{
    bqChain1.add(&bqNotch1).add(&bqHigh1);                 //Making the BiQuadChain
    bqChain2.add(&bqNotch2).add(&bqHigh2);

    bqChainTR.add(&bqNotchTR).add(&bqHighTR);
    bqChainTL.add(&bqNotchTR).add(&bqHighTL);
}

//Initial input value for couting the X-values
int Xin=0   ;           //set X to zero for the first input sequence
int Xin_new;
float huidigetijdX;

/*Feedback system for counting values of X:
The user has 2 secondes to give input before the program jumps to the next 
section. If input is regesered the timer is reset so the user has 2 secondes 
again for the next input.*/
void ledtX()
{
    t.reset();  //Reset (restart) the timer
    Xin++;
    pc.printf("Xin is %i\n",Xin);
    led_G=0;    //Feedback for user to ensure his input is regestered
    led_R=1;    
    wait(0.2);  //time led green on
    led_G=1;
    led_R=0;
    wait(0.5);  /*prevent multiple counts for one muscle contraction. This way
only one contraction can be regestered per half second*/
}

// Couting system for values of X
int tellerX()
{
    if (Move_done == true) {
        t.reset();
        led_G=1;
        led_B=1;
        led_R=0;
        while(true) {
            //button.fall(ledtX);   // this can be used for testing without EMG
            if (emgBRcomplete > thresholdBR) {
                ledtX();
            }
            t.start();                  //Start timer 
            huidigetijdX=t.read();
            if (huidigetijdX>2) {       //After 2 seconds without input
                led_R=1;                
                Xin_new = Xin;
                Xin = 0;        //Reset X to zero for the next input sequence

                return Xin_new;         //Go to the next program 
            }

        }

    }
    return 0;
}

// Initial values needed for Y (see comments at X function)
int Yin=0;
int Yin_new;
float huidigetijdY;

/*Feedback system for counting values of Y:
The user has 2 secondes to give input before the program jumps to the next 
section. If input is regesered the timer is reset so the user has 2 secondes 
again for the next input.*/
void ledtY()
{
    t.reset();  //Reset (restart) the timer
    Yin++;
    pc.printf("Yin is %i\n",Yin);
    led_G=0;    //Feedback for user to ensure his input is regestered
    led_B=1;
    wait(0.2);  //time led green on
    led_G=1;
    led_B=0;
    wait(0.5);  /*prevent multiple counts for one muscle contraction. This way
only one contraction can be regestered per half second*/
}

// Couting system for values of Y
int tellerY()
{
    if (Move_done == true) {
        t.reset();
        led_G=1;
        led_B=0;
        led_R=1;
        while(true) {
            //button.fall(ledtY);   // this can be used for testing without EMG
            if (emgBRcomplete > thresholdBR) {
                ledtY();
            }
            t.start();              //Start timer
            huidigetijdY=t.read();
            if (huidigetijdY>2) {   //After 2 seconds without input
                led_B=1;
                Yin_new = Yin;
                Yin = 0;
                Input_done = true;  //Set input done to True
                Move_done = false;  //Next section is moving so move done is false
                return Yin_new;     //Go to the next program 

            }
        }
    }
    return 0;      //Go to the next program 
}

// Declaring all variables needed for calculating rope lengths,
/* The following six floats were found using our SOLIDWORKS model*/
float          Pox = 0;
float          Poy = 0;
float          Pbx = 0;
float          Pby = 887;
float          Prx = 768;
float          Pry = 443;
/* The end-effector is manually placed in this (see beneath) position*/
float          Pex=91;          //initial value choosen for calibration
float          Pey=278;         //initial value choosen for calibration
float          diamtrklosje=20;
float          pi=3.14159265359;    //M_PI didn't work for some reason
float          omtrekklosje=diamtrklosje*pi;
float          Lou;
float          Lbu;
float          Lru;
float          dLod;
float          dLbd;
float          dLrd;

// Declaring variables needed for calculating motor counts
float          roto;
float          rotb;
float          rotr;
float          rotzo;
float          rotzb;
float          rotzr;
float          counto;
float          countb;
float          countr;
float          countzo;
float          countzb;
float          countzr;

// Declaring variables needed for calculating motor movements to get to a certain point
float          hcounto;
float          hcountb;
float          hcountr;
int            reference_o;
int            reference_b;
int            reference_r;
int            position_o;
int            position_b;
int            position_r;
int            error_o;
int            error_b;
int            error_r;
float          motorValue1;
float          motorValue2;
float          motorValue3;
float          Psx;
float          Psy;
float          Vex;
float          Vey;
float          Pstx;
float          Psty;
float          T=0.02;//seconds
float          kpo = 21;
float          kpb = 21;
float          kpr = 21;
Ticker          controlmotor1; 
Ticker          controlmotor2; 
Ticker          controlmotor3; 

//Deel om motor(en) aan te sturen--------------------------------------------
// start Motor 1 ------------------------------------------------------------
float P1(int error_o, float kpo)    //Virtual spring with springconstant kpo
{
    return error_o*kpo;
}

void MotorController1()
{
/*The reference is the place you want to go to but you have to subtract the 
initial set position (hcounts) since the encoders 'think' they are at 0 when 
starting*/
    reference_o = (int) (counto-hcounto);
    position_o = Encoder1.getPulses();
    error_o = reference_o - position_o;

    if (-20<error_o && error_o<20) {    /*within this bandwith we are satisfied
    with the error thus the motor should not move anymore*/
        motorValue1 = 0;
    } else {
        motorValue1 = P1(error_o, kpo)/4200;
    }
/*differs from the other to due to the motor being on the opposite side of the pillar*/
    if (motorValue1 >=0) motor1DirectionPin=0;       
    else motor1DirectionPin=1;
    if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
    else motor1MagnitudePin = fabs(motorValue1);
}
// end Motor 1 --------------------------------------------------------------
// start Motor 2 ------------------------------------------------------------
float P2(int error_b, float kpb)    //Virtual spring with springconstant kpb
{
    return error_b*kpb;
}

void MotorController2()
{
/*The reference is the place you want to go to but you have to subtract the 
initial set position (hcounts) since the encoders 'think' they are at 0 when 
starting*/
    reference_b = (int) (-(countb-hcountb));
    position_b = Encoder2.getPulses();
    error_b = reference_b - position_b;
    
    if (-20<error_b && error_b<20) {    /*within this bandwith we are satisfied
    with the error thus the motor should not move anymore*/
        motorValue2 = 0;
    } else {
        motorValue2 = P2(error_b, kpb)/4200;
    }

    if (motorValue2 <=0) motor2DirectionPin=0;
    else motor2DirectionPin=1;
    if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
    else motor2MagnitudePin = fabs(motorValue2);
}
// end Motor 2 --------------------------------------------------------------
// start Motor 3 ------------------------------------------------------------
float P3(int error_r, float kpr)    //Virtual spring with springconstant kpr
{
    return error_r*kpr;
}

void MotorController3()
{
/*The reference is the place you want to go to but you have to subtract the 
initial set position (hcounts) since the encoders 'think' they are at 0 when 
starting*/
    reference_r = (int) (-(countr-hcountr));
    position_r = Encoder3.getPulses();
    error_r = reference_r - position_r;

    if (-20<error_r && error_r<20) {    /*within this bandwith we are satisfied
    with the error thus the motor should not move anymore*/
        motorValue3 = 0;
    } else {
        motorValue3 = P3(error_r, kpr)/4200;
    }

    if (motorValue3 <=0) motor3DirectionPin=0;
    else motor3DirectionPin=1;
    if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
    else motor3MagnitudePin = fabs(motorValue3);
}
// end Motor 3 --------------------------------------------------------------
// einde deel motor----------------------------------------------------------

Ticker loop;

/*Calculates ropelengths that are needed to get to new positions for each time 
step, based on the set coordinates and the position of the poles */
/*We think a lot of float with return zero could have been voids*/
float touwlengtes()
{
    Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
    Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
    Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
    return 0;
}

/* Calculates rotations (and associated counts) of the motor to get to the
desired new position for each time step*/
float turns()
{
    roto=Lou/omtrekklosje;
    rotb=Lbu/omtrekklosje;
    rotr=Lru/omtrekklosje;
    counto=roto*4200;
    countb=rotb*4200;
    countr=rotr*4200;
    return 0;
}

//calculate the setpoint for each time step in coördinates, ropelenghts and counts
float Pst()
{
    Pstx=Pex+Vex*T;                     
    Psty=Pey+Vey*T;
    touwlengtes();
    Pex=Pstx;
    Pey=Psty;
    turns();
    return 0;
}

//Calculating desired end position based on the EMG input                 
float Ps()
{
    Psx=(Xin_new)*30+91;
    Psy=(Yin_new)*30+278;
    return 0;
}

//Calculates the vector pointing from position to setpoint
void Ve()
{
    Vex=(Psx-Pex);
    Vey=(Psy-Pey);
    Pst();      //calculates the new position for the next time step
    if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)) {  /*If the velocities are lower
    than 0.01 m/s the move is done and the loop can be detached*/
        Move_done=true;
        loop.detach();
    }
}

// Calculating the desired position
int calculator()
{
    Ps();
    if (Move_done == false) {   /*While the move is being executed the new 
    vector and new position (Pst) have to be calculated continiously*/
        loop.attach(&Ve,0.02);
    }
    return 0;
}

// Function which makes it possible to lower the end-effector to pick up a piece
void zakker()
{
    /*277.85 is the distance between the board and the bottom of the magnet */
    dLod=sqrt(pow(Lou,2)+pow((277.85),2))-Lou;
    dLbd=sqrt(pow(Lbu,2)+pow((277.85),2))-Lbu;    
    dLrd=sqrt(pow(Lru,2)+pow((277.85),2))-Lru;
    rotzo=dLod/omtrekklosje;
    rotzb=dLbd/omtrekklosje;
    rotzr=dLrd/omtrekklosje;
    countzo=rotzo*4200;
    countzb=rotzb*4200;
    countzr=rotzr*4200;
    /*first one (counto)differs from the other to due to the motor being on the 
    opposite side of the pillar*/
    counto=countzo+hcounto+reference_o;        
    countb=-(reference_b-countzb-hcountb);
    countr=-(reference_r-countzr-hcountr);
}

//Checks if the threshold for the left biceps is reached 
void zakken_threshold()
{
    if (Move_done == true) {    //should only be executed when the move is done
        if (emgBLcomplete > thresholdBL) {
            zakker();
        }
    }
}
/*Calculates the counts corrosponding with the set position (which is (0,0))*/
void setcurrentposition()
{
    if(Input_done==true) {
        hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
        hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
        hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
        Input_done=false;
    }
}

int main()
{
    pc.baud(115200);
    wait(1.0f);//Gives you one second between starting te program and calibrating
    getbqChain();
    threshold_timerR.attach(&Threshold_samplingBR, 0.002);
    threshold_timerL.attach(&Threshold_samplingBL, 0.002);
    setcurrentposition();
    while(true) {
        sample_timer.attach(&EMG_sample, 0.002);
        zakken_threshold();
        wait(2.5f); /*To give the user time between calibration and input, and 
        allow the lowering to take place before new input is asked*/ 
        tellerX();
        tellerY();
        calculator();
        controlmotor1.attach(&MotorController1, 0.01);
        controlmotor2.attach(&MotorController2, 0.01);
        controlmotor3.attach(&MotorController3, 0.01);
        wait(4.0f); /*To allow the move in the XY-plane to finish before 
        lowering can start*/
    }
}