File content as of revision 3:03db694efdbe:

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

//there is something wrong with the code, the motor turns twice when the potmeters are at the minimum
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);
DigitalOut Button(D3);
//DigitalOut ledb(LED1);

QEI encoder1(D8,D9,NC,16);
QEI encoder2(D10,D11,NC,16);

Ticker switch_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 X0 = 14.5, X1 = 21.85, X2 = 31.94, slope = -17.34/6.6;  //values of special points in the workspace
const float KV = 1;               //velocity constant, to scale the desired velocity of the end-effector
float X,Y;                        //emg input for the motors

const float pi = 3.14159265359,PulsesPerRev = 16.0,GearRatio = 131.15;   //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 bool ReturnIP;
int counter;                      //to count the time of inactivity

// 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 = 1;
    
    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);
    switch_tick.attach(&CallFuncMovement,Ts);
    while (true)
    {
    }
}