File content as of revision 2:e36213affbda:

#include "mbed.h"
#include "QEI.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);
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
const float Angle1_0 = 0.8411 ,Angle2_0 = -0.8411;    //initial angle of the motors
float XPosition = 0.0, YPosition = 0.0;          //real position of end point
float L2 = 300.0; 
float L3 = 300.0;           //length of the arms of the robot
const float pi = 3.14159265359;   //to convert pulses to radians 
const float PulsesPerRev = 16.0;    //        ||
const float GearRatio = 131.15;   //        ||
const float Time = 0.01;          //to control the tickers

float R = 160.0;              //radius of the work space


void positionxy(float Angle1, float Angle2,float &XPosition,float &YPosition)
{
    XPosition = L2*cos(Angle1)+L3*cos(Angle2);
    YPosition = L2*sin(Angle1)+L3*sin(Angle2);
}

void moveXpYp()
{ 
    Motor1Vel = emg1*2-1;
    Motor2Vel = emg2*2-1;
    Motor1Dir = 1;
    Motor2Dir = 1; 
}
void moveXpYn()
{ 
    Motor1Vel = emg1*2-1;
    Motor2Vel = 1-emg2*2;
    Motor1Dir = 1;
    Motor2Dir = 0; 
}
void moveXnYp()
{ 
    Motor1Vel = 1-emg1*2;
    Motor2Vel = emg2*2-1;
    Motor1Dir = 0;
    Motor2Dir = 1; 
}
void moveXnYn()
{ 
    Motor1Vel = 1-emg1*2;
    Motor2Vel = 1-emg2*2;
    Motor1Dir = 0;
    Motor2Dir = 0; 
}
void StopMotors()
{
    Motor1Vel = 0;
    Motor2Vel = 0;    
}
int choosecase(float cons1, float cons2)
{                       //decide which case has to be used
    Angle1 = (encoder1.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle1_0;   //angles of the motors in radians
    Angle2 = (encoder2.getPulses()/(2*PulsesPerRev*GearRatio))*2*pi + Angle2_0;
    positionxy(Angle1,Angle2,XPosition,YPosition);     //calculate the position of the end point
    int a = 0;
    if (Button == 1)
    {
        // not do anything
    }
    else if (pow((XPosition-400),2) + pow(YPosition,2) > pow(R,2))  // limit so that the robot does not break. Maybe try approximating with an elipse.
    {
        a = 1;       // don't move, the robot has reached the end of the workspace
    }
    else if (cons1*10 >= 5 && cons2*10 >= 5)
    {
        a = 2;
    }
    else if (cons1*10 >= 5 && cons2*10 < 5)
    {
        a = 3;    
    }
    else if (cons1*10 < 5 && cons2*10 >= 5)
    {
        a = 4;
    } 
    else if (cons1*10 < 5 && cons2*10 < 5)
    {
        a = 5;
    }
    pc.printf("case: %d \r\n",a);
    return a;

}
void selectcase()
{                                 //call function to move the motors
    int switchcons = choosecase(emg1.read(),emg2.read());
    switch(switchcons)
    {
        case 1:
            StopMotors();
            break;
        case 2:
            moveXpYp();
            break;
        case 3:
            moveXpYn();
            break;
        case 4:
            moveXnYp();
            break;
        case 5:
            moveXnYn();
            break;
        default:
            break;
    }
    
}
main()
{
    Motor1Dir = 1;
    Motor2Dir = 1;
    pc.baud(115200);
    switch_tick.attach(&selectcase,Time);
    while (true)
    {
        pc.printf("XP:  %f  YP:  %f  EMG1:  %f  EMG2:  %f\r\n", XPosition, YPosition,emg1.read(),emg2.read());
        wait(0.5f);
    }
}