File content as of revision 1:14b685c3abbd:

#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);
DigitalOut led2(D3);
AnalogIn emgx(A0);
AnalogIn emgy(A1);
//DigitalOut Button(D10);
//DigitalOut ledb(LED1);

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

Ticker switch_tick;
const float pi = 3.14159265359;   //to convert pulses to radians 
const float PulsesPerRev = 16;    //        ||
const float GearRatio = 131.15;   //        ||
const float Time = 0.01;          //to control the tickers

float limitx =3, limity = 3, positionx = 1, positiony = 1;

int i = 0;                         //counter to go back to initial position if there is no activity

void movex()
{                            //move motor in the positive x axis
    led1 = 1;
}
void movey()
{                            //move motor in the y axis
    led2 = 1;    
}
void movexy()
{                            //move motor in the x and y axis
    led2 = 1;
    led1 = 1;
}
void initialPosition()
{                             //move the motor back to the initial position
    led2 = 0;
    led1 = 0;
}
int choosecase(float emgx, float emgy)
{                       //decide which case has to be used
    int a = 0;
    if (positionx >= limitx || positiony >= limity)  // limit so that the robot does not break. Maybe try approximating with an elipse.
    {
        a = 4;
        i = 0;
    }
    else if (emgx > 0 && emgy == 0)
    {
        a = 1;
        i = 0;
    }
    else if (emgx == 0 && emgy > 0)
    {
        a = 2;
        i = 0;
    }
    else if (emgx > 0 && emgy > 0)
    {
        a = 3;
        i = 0;
    }
    else if (i > 199)          // after some time of inactivity (2s) the motor goes to initial position
    {
        a = 4;
        i = 0;
    }
    else 
    {
        i++;
    }
    //pc.printf("emgx: %f  emgy: %f \r\n",emgx,emgy);
    return a;

}
void selectcase()
{                                 //call function to move the motors
    int switchcons = choosecase(emgx,emgy);
    switch(switchcons)
    {
        case 1:
            movex();
            break;
        case 2:
            movey();
            break;
        case 3:
            movexy();
            break;
        case 4:
            initialPosition();
            break;
        default:
            break;
    }
    pc.printf("Chosen case: %d \r\n",switchcons);
}
main()
{
    pc.baud(115200);
    switch_tick.attach(&selectcase,0.01);
    while (true) {

    }
}