File content as of revision 0:fc3d2482b2f7:

#include "mbed.h"
#include "TSISensor.h"
#define ADE 0.7
#define ATR 0.3


DigitalOut LR(LED1);
DigitalOut LG(LED2);
DigitalOut LA(LED3);
PwmOut O1 (PTB0);
PwmOut O2 (PTB1);
AnalogIn AnalogicoXY(PTB2);
Ticker servo1;


char DIRE=0;
char HABO1=0;
char HABO2=0;


void servo1f (void);

void ME_servocontrol(void);

enum {PM,LEV_PATA,ADE_PATA,BAJAR_PATA,ATR_PATA};

char ME_servocontrol_estado=0;

TSISensor tsi;

int main()
{

    servo1.attach(&servo1f,0.05);
    //  servo1.attach(&ME_servocontrol,1);
//    myled.period(200);
//    myled=0.05;
    O1=0.025;
    O2=0.025;
    while(1) {
        ME_servocontrol();
    }
}

void servo1f ()
{
    if(HABO1==1 && DIRE==1)
        O1=O1+(0.005);
    if(HABO2==1 && DIRE==1)
        O2=O2+(0.005);
    if(HABO1==1 && DIRE==0)
        O1=O1-(0.005);
    if(HABO2==1 && DIRE==0)
        O2=O2-(0.005);
}

void ME_servocontrol()
{
    switch(ME_servocontrol_estado) {
        case PM:
            O1=0.04625;
            O2=0.0025;
            HABO1=0;
            HABO2=0;
            LR=0;
            LG=1;
            LA=1;
            if(/*AnalogicoXY>ADE &&*/tsi.readPercentage()>0.65 || /*AnalogicoXY<ATR*/ tsi.readPercentage()<0.35 && tsi.readPercentage()>0) ME_servocontrol_estado=LEV_PATA;
            break;
        case LEV_PATA:
            HABO1=0;
            HABO2=1;
            DIRE=1;
            LR=1;
            LG=0;
            LA=1;
            if(/*AnalogicoXY>ADE &&*/tsi.readPercentage()>0.65 && O2>=0.0625) {
                ME_servocontrol_estado=ADE_PATA;
            }
            if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O2>=0.0625) {
                ME_servocontrol_estado=ATR_PATA;
            }
            if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
                ME_servocontrol_estado=PM;
            }
            break;
        case ADE_PATA:
            HABO1=1;
            HABO2=0;
            DIRE=1;
            LR=1;
            LG=1;
            LA=0;
            if(/*AnalogicoXY>ADE &&*/ tsi.readPercentage()>0.65 && O1>=0.0625) {
                ME_servocontrol_estado=BAJAR_PATA;
            }
            if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O1>=0.0625) {
                ME_servocontrol_estado=LEV_PATA;
            }
            if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
                ME_servocontrol_estado=PM;
            }
            break;
        case BAJAR_PATA:
            HABO1=0;
            HABO2=1;
            DIRE=0;
            LR=1;
            LG=0;
            LA=0;
            if(/*AnalogicoXY>ADE &&*/ tsi.readPercentage()>0.65 && O2<=0.025) {
                ME_servocontrol_estado=ATR_PATA;
            }
            if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O2<=0.025) {
                ME_servocontrol_estado=ADE_PATA;
            }
            if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
                ME_servocontrol_estado=PM;
            }
            break;
        case ATR_PATA:
            HABO1=1;
            HABO2=0;
            DIRE=0;
            LR=0;
            LG=1;
            LA=0;
            if(/*AnalogicoXY>ADE &&*/ tsi.readPercentage()>0.65 && O1<=0.025) {
                ME_servocontrol_estado=LEV_PATA;
            }
            if(/*AnalogicoXY<ATR*/tsi.readPercentage()<0.35 && O1<=0.025) {
                ME_servocontrol_estado=BAJAR_PATA;
            }
            if(/*AnalogicoXY<ADE && AnalogicoXY>ATR*/tsi.readPercentage()<=0.65 && tsi.readPercentage()>=0.35 || tsi.readPercentage()==0) {
                ME_servocontrol_estado=PM;
            }
            break;
    }
}