Jitse Giesen
/
Koppeling
Hier Roodmans
main.cpp
- Committer:
- Jitse_Giesen
- Date:
- 2017-10-23
- Revision:
- 0:3e98322ac0e2
- Child:
- 1:224bbe370e19
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; }