Diff: main.cpp

Revision:

0:3e98322ac0e2

Child:

1:224bbe370e19

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 23 08:06:44 2017 +0000
@@ -0,0 +1,175 @@
+#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;
+}
\ No newline at end of file