File content as of revision 4:2786719c73fd:

#include "mbed.h"
#include "QEI.h"
#include "BiQuad.h"
#include "MODSERIAL.h"
//#include "HIDScope.h"

Serial pc (USBTX,USBRX);

PwmOut Motor1Vel(D6);            //motor 1 velocity control
DigitalOut Motor1Dir(D7);        //motor 1 direction
PwmOut Motor2Vel(D5);            //motor 2 velocity control
DigitalOut Motor2Dir(D4);        //motor 2 direction
DigitalOut led1(D2);
AnalogIn emg1(A0);
AnalogIn emg2(A1);
AnalogIn emg3(A3);
DigitalOut Button(D3);
//DigitalOut ledb(LED1);

QEI encoder1(D8,D9,NC,16);      //define encoder pins
QEI encoder2(D10,D11,NC,16);

BiQuad bq1 ( 0.9150 , -1.8299 , 0.9150 , 1.0000 , -1.8227 , 0.8372); //Highpass
BiQuad bq2 ( 0.0001 ,   0.0002  ,  0.0001  ,  1.0000 ,  -1.9733  ,  0.9737); //Lowpass

Ticker motors_tick;
Ticker emg_tick;

float Angle1,Angle2;              //real angles of the motor
float XPos, YPos;                 //position of the end-effector
float XSet, YSet;                 //desired position of the end-effector
float L = 300.0;                  //length of the arms of the robot
const float Ts = 0.01;            //to control the tickers
const float KV = 1;               //velocity constant, to scale the desired velocity of the end-effector

volatile double y=0;
const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.15*3;   //to convert pulses to radians 
const float Angle1_0 = pi/2 ,Angle2_0 = 0;    //initial angle of the motors

//variables and constants for the PID
const float KP = 10, KI = 5, KD = 2, N = 100;
float M1_v1 = 0, M1_v2 = 0, M2_v1 = 0, M2_v2 = 0;

volatile double a = 0, b = 0, c = 0;      //variables to store the highest emg signal to normalize the data
double Y1, Y2;                // Y1 and Y2 are taken to conform Y to be used to change the set position X is also used to change the set position
volatile double Y, X;          

volatile bool ReturnIP;
int counter;                      //to count the time of inactivity

double emg1filt(double X)
{
    double xtemp=(bq2.step(fabs(bq1.step(emg1))));
    if (xtemp>a)
        a=xtemp;
         
    X = xtemp/a;
    return X;
}

double emg2filt(double Y1)
{
    double y1temp = (bq2.step(fabs(bq1.step(emg1))));
    if (y1temp>b)
        b=y1temp;
        
    Y1 = y1temp/b;
    return Y1;
}

int emg3filt(int Y2)
{
    double y2temp = (bq2.step(fabs(bq1.step(emg1))));
    if (y2temp>c)
    {
        c=y2temp;
    }    
    Y2 = y2temp/c;
    if(Y1 < 0.3)
        Y2 = 1;
    else
        Y2 = -1;
    return Y2;
}

void callemgfilt()
{
    X = emg1filt(X);
    Y = emg2filt(Y1)*emg3filt(Y2);  // emg2filt gives the amplitude of Y and emg3filt the sign

}

// PID controller (Tustin approximation)
float PID(float err, const float KP, const float KI, const float KD,const float Ts, const float N, float &v1, float &v2)
{
    const float a1 = -(N*Ts+2);
    const float a2 = -(N*Ts-2)/(N*Ts+2);
    const float b0 = (4*KP+4*KD*N+2*KI*Ts+2*KP*N*Ts+KI*N*pow(Ts,2))/(2*N*Ts+4);
    const float b1 = (KI*N*pow(Ts,2)-4*KP-4*KD*N)/(N*Ts+2);
    const float b2 = (4*KP+4*KD*N-2*KI*Ts-2*KP*N*pow(Ts,2))/(2*N*Ts+4);
    
    float v = err-a1*v1-a2*v2;
    float u = abs(b0*v+b1*v1+b2*v2);
    v2 = v1; v1 = v;
    return u;
}

void positionxy(float Angle1, float Angle2,float &XPos,float &YPos)
{ 
    Angle1 = (encoder1.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle1_0;   //angles of the arms driven by the motors in radians
    Angle2 = (encoder2.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle2_0;
    
    XPos = L*cos(Angle1)+L*cos(Angle2);    // calculate the position of the end-effector
    YPos = L*sin(Angle1)+L*sin(Angle2);
}

int Direction(float err)
{   //choose the direction of the motor, using the difference between the set angle and the actual angle.
    int a;
    if(err >= 0) 
        a = 1;
    else
        a = 0;
    return a;
}

void MoveMotors()    // move the motors using the inverse kinematic equations of the robot
{   
    float VX = KV*(XSet - XPos);     //set the desired velocity, proportional to the difference between the set point and the end-effector position.
    float VY = KV*(YSet-YPos);
    
    float W1 = (VX*(XPos-L*cos(Angle1))+VY*(YPos-L*sin(Angle1)))/(L*YPos*cos(Angle1)-L*XPos*sin(Angle1)); // calculate the needed angular velocity to achieve the desired velocity
    float W2 = (-VX*XPos-VY*YPos)/(L*YPos*cos(Angle1)-L*XPos*sin(Angle1));
    
    float Angle1Set = Angle1 + W1*Ts;   //calculate the set angle, angle at which the motor should be after 1 period with the calculated angular velocity
    float Angle2Set = Angle2 + W2*Ts;
    
    Motor1Vel = PID(Angle1Set-Angle1,KP,KI,KD,Ts,N,M1_v1,M1_v2);   //PID controller to choose the velocity of the motors
    Motor1Dir = Direction(Angle1Set-Angle1);
    Motor2Vel = PID(Angle2Set-Angle2,KP,KI,KD,Ts,N,M2_v1,M2_v2);
    Motor2Dir = Direction(Angle2Set-Angle2);
}

void GoToSet()
{   
    XSet += X;  // keep increasing the set point, depending on the input
    YSet += Y;
    positionxy(Angle1,Angle2,XPos,YPos);
    MoveMotors();    
}

void InitialPosition()  //Go back to the initial position
{  
    const float  AllowedError = 0.5;  
    const float X0 = 14.5, X1 = 21.85, X2 = 31.94, slope = -17.34/6.6;  //values of special points in the workspace
    
    positionxy(Angle1,Angle2,XPos,YPos);
    if(sqrt(pow(XPos-XSet,2)+pow(YPos-YSet,2))<AllowedError)  //to call the function until the end effector is at the initial position
    {
        ReturnIP = false;
    }
    else
    {
        ReturnIP = true; 
    }
    //The following if-else statement is to choose a set point so that the end-effector does not go out of the work space while returning 
    if(XPos<X1)
    {
        XSet = X0;
        YSet = 0;
    }
    else if(XPos < X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0 ))
    {
        XSet = XPos;
        YSet = 0;
    }
    else if(XPos >= X2 && (XPos < slope*YPos+X0 || XPos < slope*YPos+X0))
    {
        XSet = X2;
        YSet = 0;
    }
    else
    {
        XSet = X0;
        YSet = 0;
    }
    MoveMotors();
}

void CallFuncMovement()   //decide which case has to be used
{                       
    positionxy(Angle1,Angle2,XPos,YPos);     //calculate the position of the end point
    if (Button == 1)
    {  //stop motors
        Motor1Vel = 0;
        Motor2Vel = 0;
    }
    else if (ReturnIP)
    {   //when InitialPosition is called, keep calling until the end-effector gets there;
        InitialPosition();
        counter = 0;
    }
    else if (sqrt(pow(XPos-300,2) + pow(YPos,2)) > 280 || sqrt(pow(XPos,2) + pow(YPos,2)) > 570 || sqrt(pow(XPos,2) + pow(YPos-300,2)) > 320 || sqrt(pow(XPos,2) + pow(YPos+300,2)) > 320 ) 
    { // limit so that the robot does not break. 
        InitialPosition();
        counter = 0;       
    }
    else if (counter == 2/Ts)
    {  //after 2 seconds of inactivity the robot goes back to the initial position
        InitialPosition();
        counter = 0;
    }
    else if (X > 0 || Y != 0)
    {
        GoToSet(); 
        counter = 0;   
    }
    else 
    {
        counter++;
    } 
}

main()
{
    pc.baud(115200);
    emg_tick.attach(&callemgfilt,0.001);
    motors_tick.attach(&CallFuncMovement,Ts);
    while (true)
    { /*keep the program running*/ }
}