File content as of revision 0:3e98322ac0e2:

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

DigitalOut led(LED_RED);
DigitalOut ledd(LED_GREEN);
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; 
    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>5){
        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;
    led=1;
    wait(0.5);
    ledd=1;
    led=0;
}  

int tellerY(){
    t.reset();
    ledd=1; 
    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>5){
        led=1;    
        return 0;      // ga door naar het volgende programma
        }
    }
}

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 Lou;
float Lbu;
float Lru;
float roto;
float rotb;
float rotr;
float counto;
float countb;
float countr; 
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(){
    float omtrekklosje=diamtrklosje*pi;             // omzetten van touwlengte naar rotaties
    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)){
        pc.printf("i'm done bitches"); // hier moet komen te staan dat die door moet naar het volgende programma (zakken)
    }
}

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

int zakken(){
    pc.printf("zak zak zak zak zak maar lekker door door door door");
    return 0;
}

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