Jitse Giesen
/
Koppeling
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp@0:3e98322ac0e2, 2017-10-23 (annotated)
- Committer:
- Jitse_Giesen
- Date:
- Mon Oct 23 08:06:44 2017 +0000
- Revision:
- 0:3e98322ac0e2
- Child:
- 1:224bbe370e19
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Jitse_Giesen | 0:3e98322ac0e2 | 1 | #include "mbed.h" |
| Jitse_Giesen | 0:3e98322ac0e2 | 2 | #include "math.h" |
| Jitse_Giesen | 0:3e98322ac0e2 | 3 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 4 | DigitalOut led(LED_RED); |
| Jitse_Giesen | 0:3e98322ac0e2 | 5 | DigitalOut ledd(LED_GREEN); |
| Jitse_Giesen | 0:3e98322ac0e2 | 6 | InterruptIn button(SW2); |
| Jitse_Giesen | 0:3e98322ac0e2 | 7 | Timer t; |
| Jitse_Giesen | 0:3e98322ac0e2 | 8 | Serial pc(USBTX, USBRX); |
| Jitse_Giesen | 0:3e98322ac0e2 | 9 | int Xin=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 10 | float huidigetijdX; |
| Jitse_Giesen | 0:3e98322ac0e2 | 11 | void ledtX(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 12 | t.reset(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 13 | Xin++; |
| Jitse_Giesen | 0:3e98322ac0e2 | 14 | pc.printf("Xin is %i\n",Xin); |
| Jitse_Giesen | 0:3e98322ac0e2 | 15 | ledd=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 16 | led=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 17 | wait(0.5); |
| Jitse_Giesen | 0:3e98322ac0e2 | 18 | ledd=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 19 | led=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 20 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 21 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 22 | int tellerX(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 23 | ledd=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 24 | pc.baud(115200); |
| Jitse_Giesen | 0:3e98322ac0e2 | 25 | while(true){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 26 | button.fall(ledtX); |
| Jitse_Giesen | 0:3e98322ac0e2 | 27 | /*if (EMG>threshold){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 28 | Piek(); // dit is wat je uiteindelijk wil dat er staat |
| Jitse_Giesen | 0:3e98322ac0e2 | 29 | }*/ |
| Jitse_Giesen | 0:3e98322ac0e2 | 30 | t.start(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 31 | huidigetijdX=t.read(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 32 | if (huidigetijdX>5){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 33 | led=1; // ga door naar het volgende programma |
| Jitse_Giesen | 0:3e98322ac0e2 | 34 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 35 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 36 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 37 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 38 | int Yin=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 39 | float huidigetijdY; |
| Jitse_Giesen | 0:3e98322ac0e2 | 40 | void ledtY(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 41 | t.reset(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 42 | Yin++; |
| Jitse_Giesen | 0:3e98322ac0e2 | 43 | pc.printf("Yin is %i\n",Yin); |
| Jitse_Giesen | 0:3e98322ac0e2 | 44 | ledd=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 45 | led=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 46 | wait(0.5); |
| Jitse_Giesen | 0:3e98322ac0e2 | 47 | ledd=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 48 | led=0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 49 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 50 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 51 | int tellerY(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 52 | t.reset(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 53 | ledd=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 54 | pc.baud(115200); |
| Jitse_Giesen | 0:3e98322ac0e2 | 55 | while(true){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 56 | button.fall(ledtY); |
| Jitse_Giesen | 0:3e98322ac0e2 | 57 | /*if (EMG>threshold){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 58 | Piek(); // dit is wat je uiteindelijk wil dat er staat |
| Jitse_Giesen | 0:3e98322ac0e2 | 59 | }*/ |
| Jitse_Giesen | 0:3e98322ac0e2 | 60 | t.start(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 61 | huidigetijdY=t.read(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 62 | if (huidigetijdY>5){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 63 | led=1; |
| Jitse_Giesen | 0:3e98322ac0e2 | 64 | return 0; // ga door naar het volgende programma |
| Jitse_Giesen | 0:3e98322ac0e2 | 65 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 66 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 67 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 68 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 69 | float Pox = 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 70 | float Poy = 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 71 | float Pbx = 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 72 | float Pby = 887; |
| Jitse_Giesen | 0:3e98322ac0e2 | 73 | float Prx = 768; |
| Jitse_Giesen | 0:3e98322ac0e2 | 74 | float Pry = 443; |
| Jitse_Giesen | 0:3e98322ac0e2 | 75 | float Pex=121; |
| Jitse_Giesen | 0:3e98322ac0e2 | 76 | float Pey=308; |
| Jitse_Giesen | 0:3e98322ac0e2 | 77 | float diamtrklosje=20; |
| Jitse_Giesen | 0:3e98322ac0e2 | 78 | float pi=3.14159265359; |
| Jitse_Giesen | 0:3e98322ac0e2 | 79 | float Lou; |
| Jitse_Giesen | 0:3e98322ac0e2 | 80 | float Lbu; |
| Jitse_Giesen | 0:3e98322ac0e2 | 81 | float Lru; |
| Jitse_Giesen | 0:3e98322ac0e2 | 82 | float roto; |
| Jitse_Giesen | 0:3e98322ac0e2 | 83 | float rotb; |
| Jitse_Giesen | 0:3e98322ac0e2 | 84 | float rotr; |
| Jitse_Giesen | 0:3e98322ac0e2 | 85 | float counto; |
| Jitse_Giesen | 0:3e98322ac0e2 | 86 | float countb; |
| Jitse_Giesen | 0:3e98322ac0e2 | 87 | float countr; |
| Jitse_Giesen | 0:3e98322ac0e2 | 88 | float Psx; |
| Jitse_Giesen | 0:3e98322ac0e2 | 89 | float Psy; |
| Jitse_Giesen | 0:3e98322ac0e2 | 90 | float Vex; |
| Jitse_Giesen | 0:3e98322ac0e2 | 91 | float Vey; |
| Jitse_Giesen | 0:3e98322ac0e2 | 92 | float Kz=1; // nadersnelheid instellen |
| Jitse_Giesen | 0:3e98322ac0e2 | 93 | float modVe; |
| Jitse_Giesen | 0:3e98322ac0e2 | 94 | float Vmax=20; |
| Jitse_Giesen | 0:3e98322ac0e2 | 95 | float Pstx; |
| Jitse_Giesen | 0:3e98322ac0e2 | 96 | float Psty; |
| Jitse_Giesen | 0:3e98322ac0e2 | 97 | float T=0.02;//seconds |
| Jitse_Giesen | 0:3e98322ac0e2 | 98 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 99 | Ticker loop; |
| Jitse_Giesen | 0:3e98322ac0e2 | 100 | float touwlengtes(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 101 | 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 |
| Jitse_Giesen | 0:3e98322ac0e2 | 102 | Lbu=sqrt(pow((Pstx-Pox),2)+pow((Psty-Pby),2)); |
| Jitse_Giesen | 0:3e98322ac0e2 | 103 | Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2)); |
| Jitse_Giesen | 0:3e98322ac0e2 | 104 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 105 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 106 | float turns(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 107 | float omtrekklosje=diamtrklosje*pi; // omzetten van touwlengte naar rotaties |
| Jitse_Giesen | 0:3e98322ac0e2 | 108 | roto=Lou/omtrekklosje; |
| Jitse_Giesen | 0:3e98322ac0e2 | 109 | rotb=Lbu/omtrekklosje; |
| Jitse_Giesen | 0:3e98322ac0e2 | 110 | rotr=Lru/omtrekklosje; |
| Jitse_Giesen | 0:3e98322ac0e2 | 111 | counto=roto*4200; // omzetten van rotaties naar counts |
| Jitse_Giesen | 0:3e98322ac0e2 | 112 | countb=rotb*4200; |
| Jitse_Giesen | 0:3e98322ac0e2 | 113 | countr=rotr*4200; |
| Jitse_Giesen | 0:3e98322ac0e2 | 114 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 115 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 116 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 117 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 118 | float Pst(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 119 | Pstx=Pex+Vex*T; |
| Jitse_Giesen | 0:3e98322ac0e2 | 120 | Psty=Pey+Vey*T; |
| Jitse_Giesen | 0:3e98322ac0e2 | 121 | touwlengtes(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 122 | Pex=Pstx; //Dit is nog een aanname. Moet zijn: read the encoder and put the real position as the Pe for the next step |
| Jitse_Giesen | 0:3e98322ac0e2 | 123 | Pey=Psty; |
| Jitse_Giesen | 0:3e98322ac0e2 | 124 | //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty); |
| Jitse_Giesen | 0:3e98322ac0e2 | 125 | //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru); |
| Jitse_Giesen | 0:3e98322ac0e2 | 126 | turns(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 127 | //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr); |
| Jitse_Giesen | 0:3e98322ac0e2 | 128 | pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr); |
| Jitse_Giesen | 0:3e98322ac0e2 | 129 | /*float R; |
| Jitse_Giesen | 0:3e98322ac0e2 | 130 | 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 |
| Jitse_Giesen | 0:3e98322ac0e2 | 131 | pc.printf("\n\r R=%f",R);*/ |
| Jitse_Giesen | 0:3e98322ac0e2 | 132 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 133 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 134 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 135 | float Ps(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 136 | Psx=(Xin)*30+121; |
| Jitse_Giesen | 0:3e98322ac0e2 | 137 | Psy=(Yin)*30+308; |
| Jitse_Giesen | 0:3e98322ac0e2 | 138 | pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy); |
| Jitse_Giesen | 0:3e98322ac0e2 | 139 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 140 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 141 | void Ve(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 142 | Vex=Kz*(Psx-Pex); |
| Jitse_Giesen | 0:3e98322ac0e2 | 143 | Vey=Kz*(Psy-Pey); |
| Jitse_Giesen | 0:3e98322ac0e2 | 144 | modVe=sqrt(pow(Vex,2)+pow(Vey,2)); |
| Jitse_Giesen | 0:3e98322ac0e2 | 145 | if(modVe>Vmax){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 146 | Vex=(Vex/modVe)*Vmax; |
| Jitse_Giesen | 0:3e98322ac0e2 | 147 | Vey=(Vey/modVe)*Vmax; |
| Jitse_Giesen | 0:3e98322ac0e2 | 148 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 149 | Pst(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 150 | pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey); |
| Jitse_Giesen | 0:3e98322ac0e2 | 151 | if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 152 | pc.printf("i'm done bitches"); // hier moet komen te staan dat die door moet naar het volgende programma (zakken) |
| Jitse_Giesen | 0:3e98322ac0e2 | 153 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 154 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 155 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 156 | int calculator(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 157 | pc.baud(115200); |
| Jitse_Giesen | 0:3e98322ac0e2 | 158 | Ps(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 159 | loop.attach(&Ve,0.02); |
| Jitse_Giesen | 0:3e98322ac0e2 | 160 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 161 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 162 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 163 | int zakken(){ |
| Jitse_Giesen | 0:3e98322ac0e2 | 164 | pc.printf("zak zak zak zak zak maar lekker door door door door"); |
| Jitse_Giesen | 0:3e98322ac0e2 | 165 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 166 | } |
| Jitse_Giesen | 0:3e98322ac0e2 | 167 | |
| Jitse_Giesen | 0:3e98322ac0e2 | 168 | int main() |
| Jitse_Giesen | 0:3e98322ac0e2 | 169 | { |
| Jitse_Giesen | 0:3e98322ac0e2 | 170 | tellerX(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 171 | tellerY(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 172 | calculator(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 173 | //zakker(); |
| Jitse_Giesen | 0:3e98322ac0e2 | 174 | return 0; |
| Jitse_Giesen | 0:3e98322ac0e2 | 175 | } |