File content as of revision 3:08bf605844eb:

#include "mbed.h"
#include "math.h"

DigitalOut led(LED_RED);
DigitalOut ledd(LED_GREEN);
DigitalOut leddd(LED_BLUE);
InterruptIn button(SW2);
Timer t;
Serial pc(USBTX, USBRX);
int Xin=0;
float huidigetijdX;
void ledtX(){
    t.reset();
    Xin++;
    pc.printf("Xin is %i\n",Xin);
    ledd=0;
    led=1;
    wait(0.5);
    ledd=1;
    led=0;
}  

int tellerX(){
    ledd=1; 
    leddd=1;
    pc.baud(115200);
    while(true){
    button.fall(ledtX);      
    /*if (EMG>threshold){
        Piek();                 // dit is wat je uiteindelijk wil dat er staat
    }*/
    t.start();
    huidigetijdX=t.read();
    if (huidigetijdX>2){
        led=1;          // ga door naar het volgende programma
        return 0;
        }
    }  
}   
int Yin=0;
float huidigetijdY;
void ledtY(){
    t.reset();
    Yin++;
    pc.printf("Yin is %i\n",Yin);
    ledd=0;
    leddd=1;
    wait(0.5);
    ledd=1;
    leddd=0;
}  

int tellerY(){
    t.reset();
    ledd=1;
    leddd=0; 
    pc.baud(115200);
    while(true){
    button.fall(ledtY);      
    /*if (EMG>threshold){
        Piek();                 // dit is wat je uiteindelijk wil dat er staat
    }*/
    t.start();
    huidigetijdY=t.read();
    if (huidigetijdY>2){
        leddd=1; 
        button.fall(0);   
        return 0;      // ga door naar het volgende programma 
        }
    }
}
int B;
float Pox = 0;
float Poy = 0;
float Pbx = 0;
float Pby = 887;
float Prx = 768;
float Pry = 443;
float Pex=121;
float Pey=308;
float diamtrklosje=20;
float pi=3.14159265359;
float omtrekklosje=diamtrklosje*pi;
float Lou;
float Lbu;
float Lru;
float dLod;
float dLbd;
float dLrd;
float roto;
float rotb;
float rotr;
float rotzo;
float rotzb;
float rotzr;
float counto;
float countb;
float countr;
float countzo;
float countzb;
float countzr; 
float Psx;
float Psy;
float Vex;
float Vey;
float Kz=1; // nadersnelheid instellen
float modVe;
float Vmax=20;
float Pstx;
float Psty;
float T=0.02;//seconds

Ticker loop;
float touwlengtes(){
    Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));     // rekent touwlengtes uit die nodig zijn voor de nieuwe positie aan de hand van de ingegeven coördinaten en de posities van de palen
    Lbu=sqrt(pow((Pstx-Pox),2)+pow((Psty-Pby),2));
    Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
    return 0;
}
float turns(){
    
    roto=Lou/omtrekklosje; 
    rotb=Lbu/omtrekklosje; 
    rotr=Lru/omtrekklosje; 
    counto=roto*4200;                               // omzetten van rotaties naar counts
    countb=rotb*4200;
    countr=rotr*4200;
    return 0;
}

    
float Pst(){
    Pstx=Pex+Vex*T;
    Psty=Pey+Vey*T;
    touwlengtes();
    Pex=Pstx;                   //Dit is nog een aanname. Moet zijn: read the encoder and put the real position as the Pe for the next step
    Pey=Psty;
    //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
    //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
    turns();
    //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); 
    pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
    /*float R;
    R=Vex/Vey;                  // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constand is en de end efector dus een rechte lijn maakt
    pc.printf("\n\r R=%f",R);*/
    return 0;
} 
 
float Ps(){
    Psx=(Xin)*30+121;
    Psy=(Yin)*30+308;     
    pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
    return 0;
}
void Ve(){
    Vex=Kz*(Psx-Pex);
    Vey=Kz*(Psy-Pey);
    modVe=sqrt(pow(Vex,2)+pow(Vey,2));
    if(modVe>Vmax){
        Vex=(Vex/modVe)*Vmax;
        Vey=(Vey/modVe)*Vmax;
    }
    Pst();
    pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
    if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
        B=5;
        loop.detach();
        }
}

int calculator(){
    pc.baud(115200);
    Ps();
    loop.attach(&Ve,0.02);
    return 0;
}

void zakker(){
    while(1){
        wait(1);
    if(B==5){                       // hierdoor wacht dit programma totdat de beweging klaar is  
    dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken
    dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
    dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
    rotzo=dLod/omtrekklosje;
    rotzb=dLbd/omtrekklosje;
    rotzr=dLrd/omtrekklosje;
    countzo=rotzo*4200;
    countzb=rotzb*4200;
    countzr=rotzr*4200;
    
    pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr);       // hier moet komen te staan hoe het zakken gaat
    }
    }
}

int main()
{
    tellerX();
    tellerY();
    
    calculator();
    zakker();
    return 0;
}