Esther Keulers / Mbed 2 deprecated Begintotaalscript

g

Dependencies:   MODSERIAL mbed Encoder

begintotaalscript.cpp@1:8a0a39740897, 2013-11-04 (annotated)

Committer:
Esther
Date:
Mon Nov 04 19:30:07 2013 +0000
Revision:
1:8a0a39740897
Parent:
0:e9e50c0a6502
Child:
2:103de31c8803
begintotaalscriptbegin

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Esther 0:e9e50c0a6502 1 #include "mbed.h"
Esther 0:e9e50c0a6502 2 #include "MODSERIAL.h"
Esther 1:8a0a39740897 3 #include "encoder.h"
Esther 1:8a0a39740897 4
Esther 1:8a0a39740897 5 volatile bool looptimerflag;
Esther 1:8a0a39740897 6
Esther 1:8a0a39740897 7 void setlooptimerflag(void)
Esther 1:8a0a39740897 8 {
Esther 1:8a0a39740897 9 looptimerflag=true;
Esther 1:8a0a39740897 10 }
Esther 0:e9e50c0a6502 11
Esther 0:e9e50c0a6502 12
Esther 0:e9e50c0a6502 13 int main()
Esther 0:e9e50c0a6502 14 {
Esther 1:8a0a39740897 15 //Communicatie met de pc
Esther 0:e9e50c0a6502 16 MODSERIAL pc(USBTX,USBRX,64,1024);
Esther 0:e9e50c0a6502 17 pc.baud(115200);
Esther 0:e9e50c0a6502 18
Esther 1:8a0a39740897 19 //Benoemen inputs
Esther 0:e9e50c0a6502 20 DigitalIn knop1(PTC2);
Esther 0:e9e50c0a6502 21 DigitalIn knop2(PTB3);
Esther 0:e9e50c0a6502 22 DigitalIn knop3(PTB2);
Esther 0:e9e50c0a6502 23
Esther 1:8a0a39740897 24 AnalogIn emg1(PTB0); //Analog input emg1
Esther 1:8a0a39740897 25 AnalogIn emg2(PTB1); //Analog input emg2
Esther 1:8a0a39740897 26
Esther 1:8a0a39740897 27 Encoder motor1(PTD0,PTC9);
Esther 1:8a0a39740897 28 Encoder motor2(PTD2,PTC8);
Esther 1:8a0a39740897 29
Esther 1:8a0a39740897 30 /* PWM control to motor */
Esther 1:8a0a39740897 31 PwmOut pwm_motor1(PTA12);
Esther 1:8a0a39740897 32 PwmOut pwm_motor2(PTA5);
Esther 1:8a0a39740897 33 /* Direction pin */
Esther 1:8a0a39740897 34 DigitalOut motor1dir(PTD3);
Esther 1:8a0a39740897 35 DigitalOut motor2dir(PTD1);
Esther 1:8a0a39740897 36
Esther 0:e9e50c0a6502 37 knop1.mode(PullUp);
Esther 0:e9e50c0a6502 38 knop2.mode(PullUp);
Esther 0:e9e50c0a6502 39 knop3.mode(PullUp);
Esther 0:e9e50c0a6502 40
Esther 1:8a0a39740897 41 //Variabelen voor menu
Esther 0:e9e50c0a6502 42 int state = 1;
Esther 0:e9e50c0a6502 43
Esther 1:8a0a39740897 44 bool calibratie_rust = false;
Esther 1:8a0a39740897 45 bool calibratie_max = false;
Esther 1:8a0a39740897 46 bool calibratie_motor = false;
Esther 1:8a0a39740897 47 bool meting = false;
Esther 1:8a0a39740897 48
Esther 1:8a0a39740897 49 //Variabelen EMG/motoren
Esther 1:8a0a39740897 50 double yy,z,zabs,w,y1,z1,zabs1,w1,y2,z2,zabs2,w2,e1,e2,e3,f1,f2,g1,g2,g3,h1,h2,byy,bz,bzabs,bw,by1,bz1,bzabs1,bw1,by2,bz2,bzabs2,bw2,be1,be2,be3,bf1,bf2,bg1,bg2,bg3,bh1,bh2;
Esther 1:8a0a39740897 51 double MaxsnelheidV,MaxsnelheidD,GrenswaardeD,GrenswaardeV,MaxwaardeD,MaxwaardeV,BereikD,BereikV,SnelheidV,SnelheidD,xbegin,ybegin,qbegin,x,y,q,deltas,deltaq,deltax,deltay;
Esther 1:8a0a39740897 52 double theta, r, dtheta, dr, theta_pen, r_pen, theta_pwm, r_pwm, pi;
Esther 1:8a0a39740897 53
Esther 1:8a0a39740897 54 pi=3.14159265359;
Esther 1:8a0a39740897 55
Esther 1:8a0a39740897 56 e1= 0.855930601814815;
Esther 1:8a0a39740897 57 e2= -1.711861203629630;
Esther 1:8a0a39740897 58 e3= 0.855930601814815;
Esther 1:8a0a39740897 59 f1=-1.690996376887443;
Esther 1:8a0a39740897 60 f2= 0.732726030371816;
Esther 1:8a0a39740897 61
Esther 1:8a0a39740897 62 g1= 0.098259168204717*pow(10.0,-4.0);
Esther 1:8a0a39740897 63 g2= 0.196518336409435*pow(10.0,-4.0);
Esther 1:8a0a39740897 64 g3=0.098259168204717*pow(10.0,-4.0);
Esther 1:8a0a39740897 65 h1= -1.991114292201654;
Esther 1:8a0a39740897 66 h2=0.991153595868935;
Esther 1:8a0a39740897 67
Esther 1:8a0a39740897 68 be1=0.914969144113083;
Esther 1:8a0a39740897 69 be2= -1.829938288226165;
Esther 1:8a0a39740897 70 be3= 0.914969144113083;
Esther 1:8a0a39740897 71 bf1=-1.822694925196308;
Esther 1:8a0a39740897 72 bf2=0.837181651256022;
Esther 1:8a0a39740897 73
Esther 1:8a0a39740897 74 bg1= 0.246193004641015*pow(10.0,-5.0);
Esther 1:8a0a39740897 75 bg2= 0.492386009282031 *pow(10.0,-5.0);
Esther 1:8a0a39740897 76 bg3= 0.246193004641015*pow(10.0,-5.0);
Esther 1:8a0a39740897 77 bh1= -1.995557124345789;
Esther 1:8a0a39740897 78 bh2= 0.995566972065975;
Esther 1:8a0a39740897 79
Esther 1:8a0a39740897 80
Esther 1:8a0a39740897 81 y1 = 0; /*Initialize y(n-1) = 0*/
Esther 1:8a0a39740897 82 y2 = 0; /*Initialize y(n-2) = 0*/
Esther 1:8a0a39740897 83 z1 = 0; /*Initialize z(n-1) = 0*/
Esther 1:8a0a39740897 84 z2 = 0; /*Initialize z(n-2) = 0*/
Esther 1:8a0a39740897 85 zabs1 = 0; /*Initialize zabs(n-1) = 0*/
Esther 1:8a0a39740897 86 zabs2 = 0; /*Initialize zabs(n-2) = 0*/
Esther 1:8a0a39740897 87 w1 = 0; /*Initialize w(n-1) = 0*/
Esther 1:8a0a39740897 88 w2 = 0; /*Initialize w(n-2) = 0*/
Esther 0:e9e50c0a6502 89
Esther 1:8a0a39740897 90 by1 = 0; /*Initialize by(n-1) = 0*/
Esther 1:8a0a39740897 91 by2 = 0; /*Initialize by(n-2) = 0*/
Esther 1:8a0a39740897 92 bz1 = 0; /*Initialize bz(n-1) = 0*/
Esther 1:8a0a39740897 93 bz2 = 0; /*Initialize bz(n-2) = 0*/
Esther 1:8a0a39740897 94 bzabs1 = 0; /*Initialize bzabs(n-1) = 0*/
Esther 1:8a0a39740897 95 bzabs2 = 0; /*Initialize bzabs(n-2) = 0*/
Esther 1:8a0a39740897 96 bw1 = 0; /*Initialize bw(n-1) = 0*/
Esther 1:8a0a39740897 97 bw2 = 0; /*Initialize bw(n-2) = 0*/
Esther 1:8a0a39740897 98 const float Ts = 0.001; /*Sample Time, const(ant)*/
Esther 1:8a0a39740897 99
Esther 1:8a0a39740897 100 //Constanten voor tekening: EMGdeel
Esther 1:8a0a39740897 101 xbegin= 0; //beginpositie
Esther 1:8a0a39740897 102 ybegin=0; //beginpositie
Esther 1:8a0a39740897 103 qbegin=315; //beginhoek (in graden) vanaf de y-as, rechtsom gedraaid is positief
Esther 1:8a0a39740897 104 MaxsnelheidV=15; //in cm/s
Esther 1:8a0a39740897 105 MaxsnelheidD=15; //in graden/s
Esther 1:8a0a39740897 106
Esther 1:8a0a39740897 107 Ticker looptimer;
Esther 1:8a0a39740897 108 looptimer.attach(setlooptimerflag,Ts);
Esther 1:8a0a39740897 109
Esther 1:8a0a39740897 110 while (true) { //oneindige while loop
Esther 1:8a0a39740897 111
Esther 1:8a0a39740897 112 while(looptimerflag != true);
Esther 1:8a0a39740897 113 looptimerflag = false;
Esther 1:8a0a39740897 114
Esther 1:8a0a39740897 115
Esther 1:8a0a39740897 116 // state 1 RUST
Esther 1:8a0a39740897 117 if (state == 1) {
Esther 1:8a0a39740897 118
Esther 1:8a0a39740897 119 pc.printf("state 1 RUST | knop1 = cal_motor knop2 = MENU EMG knop3 = Tekenen \n\r");
Esther 1:8a0a39740897 120
Esther 0:e9e50c0a6502 121
Esther 1:8a0a39740897 122 if (knop1 == false ) { //als knop 1 is ingedrukt
Esther 1:8a0a39740897 123 state=2;
Esther 1:8a0a39740897 124 wait(0.05); //wacht goed contact knopje
Esther 1:8a0a39740897 125
Esther 1:8a0a39740897 126 while(knop1 == false) {} //wacht tot knop is losgelaten
Esther 1:8a0a39740897 127 wait(0.05); // wacht goed contact knopje
Esther 1:8a0a39740897 128
Esther 1:8a0a39740897 129 }
Esther 1:8a0a39740897 130
Esther 1:8a0a39740897 131 if (knop2 == false ) { //als knop 2 wordt ingedrukt: state is 3
Esther 1:8a0a39740897 132 state=3;
Esther 1:8a0a39740897 133 wait(0.05);
Esther 1:8a0a39740897 134
Esther 1:8a0a39740897 135 while(knop2 == false) {}
Esther 1:8a0a39740897 136 wait(0.05);
Esther 1:8a0a39740897 137 }
Esther 1:8a0a39740897 138 if (knop3 == false && calibratie_rust == true && calibratie_max == true && calibratie_motor == true) {
Esther 1:8a0a39740897 139 state=6; //state 6 kan alleen uitgevoerd worden wanneer alle calibraties gedaan zijn.
Esther 1:8a0a39740897 140 wait(0.05);
Esther 1:8a0a39740897 141
Esther 1:8a0a39740897 142 while(knop3 == false) {}
Esther 1:8a0a39740897 143 wait(0.05);
Esther 1:8a0a39740897 144 meting=true;
Esther 1:8a0a39740897 145
Esther 1:8a0a39740897 146 }
Esther 1:8a0a39740897 147 if (knop3 == false && state !=6) {
Esther 1:8a0a39740897 148 wait(0.05);
Esther 1:8a0a39740897 149 pc.printf("Niet alle calibraties uitgevoerd! \n");
Esther 1:8a0a39740897 150 wait(1);
Esther 1:8a0a39740897 151 }
Esther 1:8a0a39740897 152
Esther 1:8a0a39740897 153
Esther 0:e9e50c0a6502 154 }
Esther 0:e9e50c0a6502 155
Esther 1:8a0a39740897 156
Esther 1:8a0a39740897 157 //state2 CAL MOTOR
Esther 1:8a0a39740897 158 if (state == 2) {
Esther 1:8a0a39740897 159 pc.printf("state 2 cal_motor | knop1 = terug naar rust \n\r");
Esther 1:8a0a39740897 160
Esther 1:8a0a39740897 161
Esther 1:8a0a39740897 162 if (knop1 == false ) {
Esther 1:8a0a39740897 163 state=1;
Esther 1:8a0a39740897 164 wait(0.05);
Esther 1:8a0a39740897 165
Esther 1:8a0a39740897 166 while(knop1 == false) {}
Esther 1:8a0a39740897 167 wait(0.05);
Esther 1:8a0a39740897 168
Esther 1:8a0a39740897 169 motor1.setPosition(0);
Esther 1:8a0a39740897 170 motor2.setPosition(0);
Esther 1:8a0a39740897 171
Esther 1:8a0a39740897 172
Esther 1:8a0a39740897 173 calibratie_motor = true;
Esther 1:8a0a39740897 174
Esther 1:8a0a39740897 175
Esther 1:8a0a39740897 176 }
Esther 0:e9e50c0a6502 177 }
Esther 1:8a0a39740897 178 // state 3 MENU EMG
Esther 1:8a0a39740897 179 if (state == 3) {
Esther 1:8a0a39740897 180 pc.printf("state 3 MENU EMG | knop1= cal EMG rust knop2= rust knop3= cal EMG max \n\r");
Esther 1:8a0a39740897 181
Esther 1:8a0a39740897 182
Esther 1:8a0a39740897 183 if (knop2 == false ) { // terug naar state 1
Esther 1:8a0a39740897 184 state=1;
Esther 1:8a0a39740897 185 wait(0.05);
Esther 1:8a0a39740897 186
Esther 1:8a0a39740897 187 while(knop2 == false) {}
Esther 1:8a0a39740897 188 wait(0.05);
Esther 1:8a0a39740897 189 }
Esther 1:8a0a39740897 190
Esther 1:8a0a39740897 191 if (knop1 == false ) { // naar state 4
Esther 1:8a0a39740897 192 state=4;
Esther 1:8a0a39740897 193 wait(0.05);
Esther 1:8a0a39740897 194
Esther 1:8a0a39740897 195 while(knop1 == false) {}
Esther 1:8a0a39740897 196 wait(0.05);
Esther 1:8a0a39740897 197
Esther 1:8a0a39740897 198 // begingrenswaarden stellen:
Esther 1:8a0a39740897 199 GrenswaardeV=0;
Esther 1:8a0a39740897 200 GrenswaardeD=0;
Esther 1:8a0a39740897 201 meting=true;
Esther 1:8a0a39740897 202 }
Esther 1:8a0a39740897 203
Esther 1:8a0a39740897 204 if (knop3 == false ) { // naar state 5
Esther 1:8a0a39740897 205 state=5;
Esther 1:8a0a39740897 206 wait(0.05);
Esther 1:8a0a39740897 207
Esther 1:8a0a39740897 208 while(knop3 == false) {}
Esther 1:8a0a39740897 209 wait(0.05);
Esther 1:8a0a39740897 210
Esther 1:8a0a39740897 211 // begingrenswaarden stellen:
Esther 1:8a0a39740897 212 MaxwaardeV=0;
Esther 1:8a0a39740897 213 MaxwaardeD=0;
Esther 1:8a0a39740897 214 meting=true;
Esther 1:8a0a39740897 215
Esther 1:8a0a39740897 216 }
Esther 1:8a0a39740897 217
Esther 1:8a0a39740897 218
Esther 0:e9e50c0a6502 219 }
Esther 1:8a0a39740897 220
Esther 1:8a0a39740897 221
Esther 1:8a0a39740897 222 //METEN VAN EMG
Esther 1:8a0a39740897 223 if (meting == true) {
Esther 1:8a0a39740897 224 // pc.printf("TEST CODE: Meting in gang\n");
Esther 1:8a0a39740897 225 yy = emg1.read(); //<measurement function, read from pin>
Esther 1:8a0a39740897 226 byy= emg2.read();
Esther 1:8a0a39740897 227
Esther 1:8a0a39740897 228 z=-f1*z1-f2*z2+e1*yy+e2*y1+e3*y2; /*hoogdoorlaatfilteroverdacht*/
Esther 1:8a0a39740897 229 bz=-bf1*bz1-bf2*bz2+be1*byy+be2*by1+be3*by2; /*hoogdoorlaatfilteroverdacht van biceps*/
Esther 1:8a0a39740897 230
Esther 1:8a0a39740897 231 y2 = y1; /*y(n-1) will be y(n-2) on next loop*/
Esther 1:8a0a39740897 232 y1= yy; /*y(n) will be y(n-1) on next loop*/
Esther 1:8a0a39740897 233 z2 = z1; /*z(n-1) will be z(n-2) on next loop*/
Esther 1:8a0a39740897 234 z1 = z; /*z(n) will be z(n-1) on next loop*/
Esther 1:8a0a39740897 235
Esther 1:8a0a39740897 236 by2 = by1; /*y(n-1) will be y(n-2) on next loop*/
Esther 1:8a0a39740897 237 by1= byy; /*y(n) will be y(n-1) on next loop*/
Esther 1:8a0a39740897 238 bz2 = bz1; /*z(n-1) will be z(n-2) on next loop*/
Esther 1:8a0a39740897 239 bz1 = bz; /*z(n) will be z(n-1) on next loop*/
Esther 1:8a0a39740897 240
Esther 1:8a0a39740897 241 zabs=abs(z);/*rectify*/
Esther 1:8a0a39740897 242 bzabs=abs(bz);/*rectify*/
Esther 1:8a0a39740897 243
Esther 1:8a0a39740897 244 w=-h1*w1-h2*w2+g1*zabs+g2*zabs1+g3*zabs2; /*laagdoorlaatfilteroverdracht*/
Esther 1:8a0a39740897 245 bw=-bh1*bw1-bh2*bw2+bg1*bzabs+bg2*bzabs1+bg3*bzabs2; /*laagdoorlaatfilteroverdracht van biceps*/
Esther 1:8a0a39740897 246
Esther 1:8a0a39740897 247 zabs2 = zabs1; /*z(n-1) will be z(n-2) on next loop*/
Esther 1:8a0a39740897 248 zabs1 = zabs; /*z(n) will be z(n-1) on next loop*/
Esther 1:8a0a39740897 249 w2 = w1; /*w(n-1) will be w(n-2) on next loop*/
Esther 1:8a0a39740897 250 w1 = w; /*w(n) will be w(n-1) on next loop*/
Esther 1:8a0a39740897 251
Esther 1:8a0a39740897 252 bzabs2 = bzabs1; /*z(n-1) will be z(n-2) on next loop*/
Esther 1:8a0a39740897 253 bzabs1 = bzabs; /*z(n) will be z(n-1) on next loop*/
Esther 1:8a0a39740897 254 bw2 = bw1; /*w(n-1) will be w(n-2) on next loop*/
Esther 1:8a0a39740897 255 bw1 = bw; /*w(n) will be w(n-1) on next loop*/
Esther 1:8a0a39740897 256
Esther 1:8a0a39740897 257 //pc.printf("%f,%f\n",w,bw);
Esther 1:8a0a39740897 258
Esther 1:8a0a39740897 259 wait(Ts); /*When finished wait (maybe a bit shorter?)*/
Esther 1:8a0a39740897 260
Esther 1:8a0a39740897 261 if (w>1.0) {
Esther 1:8a0a39740897 262 w=1.0;
Esther 1:8a0a39740897 263 }
Esther 1:8a0a39740897 264 if (bw>1.0) {
Esther 1:8a0a39740897 265 bw=1.0;
Esther 1:8a0a39740897 266 }
Esther 1:8a0a39740897 267
Esther 1:8a0a39740897 268 }
Esther 0:e9e50c0a6502 269
Esther 0:e9e50c0a6502 270
Esther 0:e9e50c0a6502 271
Esther 1:8a0a39740897 272 // state 4 EMG CAL IN RUST
Esther 1:8a0a39740897 273 if (state == 4) {
Esther 1:8a0a39740897 274 pc.printf("state 4 cal EMG RUST | knop1= MEMNU EMG \n\r");
Esther 1:8a0a39740897 275 pc.printf("%f,%f\n",GrenswaardeV,GrenswaardeD);
Esther 1:8a0a39740897 276 // Nieuwe grenswaarden bepalen
Esther 1:8a0a39740897 277 if (w > GrenswaardeV) {
Esther 1:8a0a39740897 278 GrenswaardeV = w;
Esther 1:8a0a39740897 279 }
Esther 1:8a0a39740897 280 if (bw > GrenswaardeD) {
Esther 1:8a0a39740897 281 GrenswaardeD = bw;
Esther 1:8a0a39740897 282 }
Esther 1:8a0a39740897 283 if (knop1 == false ) { // terug naar state 1
Esther 1:8a0a39740897 284 state=3;
Esther 1:8a0a39740897 285 wait(0.05);
Esther 1:8a0a39740897 286
Esther 1:8a0a39740897 287 while(knop1 == false) {}
Esther 1:8a0a39740897 288 wait(0.05);
Esther 1:8a0a39740897 289 calibratie_rust = true;
Esther 1:8a0a39740897 290 meting=false;
Esther 1:8a0a39740897 291
Esther 1:8a0a39740897 292 }
Esther 1:8a0a39740897 293 }
Esther 1:8a0a39740897 294 // state 5 EMG CAL MAX
Esther 1:8a0a39740897 295 if (state == 5) {
Esther 1:8a0a39740897 296 pc.printf("state 5 cal EMG MAX | knop3= EMG MENU \n\r");
Esther 1:8a0a39740897 297 pc.printf("%f,%f\n",MaxwaardeV,MaxwaardeD);
Esther 1:8a0a39740897 298 // Nieuwe maxwaarden bepalen
Esther 1:8a0a39740897 299 if (w > MaxwaardeV) {
Esther 1:8a0a39740897 300 MaxwaardeV = w;
Esther 1:8a0a39740897 301 }
Esther 1:8a0a39740897 302 if (bw > MaxwaardeD) {
Esther 1:8a0a39740897 303 MaxwaardeD = bw;
Esther 1:8a0a39740897 304 }
Esther 1:8a0a39740897 305
Esther 1:8a0a39740897 306 // einde maxwaarde bepalen:
Esther 1:8a0a39740897 307 if (knop3 == false ) { // terug naar state 3
Esther 1:8a0a39740897 308 state=3;
Esther 1:8a0a39740897 309 wait(0.05);
Esther 1:8a0a39740897 310
Esther 1:8a0a39740897 311 while(knop3 == false) {}
Esther 1:8a0a39740897 312 wait(0.05);
Esther 1:8a0a39740897 313 calibratie_max = true;
Esther 1:8a0a39740897 314 meting=false;
Esther 1:8a0a39740897 315
Esther 1:8a0a39740897 316 }
Esther 1:8a0a39740897 317
Esther 1:8a0a39740897 318 }
Esther 1:8a0a39740897 319
Esther 1:8a0a39740897 320 // state 6 TEKENEN
Esther 1:8a0a39740897 321
Esther 1:8a0a39740897 322 if (state == 6) {
Esther 1:8a0a39740897 323 // pc.printf("state 6 tekenen | knop3= rust \n\r");
Esther 1:8a0a39740897 324
Esther 1:8a0a39740897 325 // EMG
Esther 1:8a0a39740897 326 BereikD = MaxwaardeD-GrenswaardeD; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
Esther 1:8a0a39740897 327 BereikV = MaxwaardeV-GrenswaardeV; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
Esther 1:8a0a39740897 328
Esther 1:8a0a39740897 329 SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV); // Snelheid van pen (in cm/sec), lineair verband met het Bereik
Esther 1:8a0a39740897 330 SnelheidD = (MaxsnelheidD/BereikD)*(bw-GrenswaardeD); //Draaisnelheid pen (in graden/sec), lineair verband met het Bereik
Esther 1:8a0a39740897 331
Esther 1:8a0a39740897 332 if (SnelheidV<MaxsnelheidD) {
Esther 1:8a0a39740897 333 SnelheidV=MaxsnelheidD;
Esther 1:8a0a39740897 334 }
Esther 1:8a0a39740897 335
Esther 1:8a0a39740897 336 if (SnelheidD<MaxsnelheidD) {
Esther 1:8a0a39740897 337 SnelheidD=MaxsnelheidD;
Esther 1:8a0a39740897 338 }
Esther 1:8a0a39740897 339
Esther 1:8a0a39740897 340 if (SnelheidV>MaxsnelheidV) {
Esther 1:8a0a39740897 341 SnelheidV=MaxsnelheidV;
Esther 1:8a0a39740897 342 }
Esther 1:8a0a39740897 343
Esther 1:8a0a39740897 344 if (SnelheidD>MaxsnelheidV) {
Esther 1:8a0a39740897 345 SnelheidD=MaxsnelheidV;
Esther 1:8a0a39740897 346 }
Esther 1:8a0a39740897 347
Esther 1:8a0a39740897 348 deltas = SnelheidV*Ts; //afstand tussen twee meetpunten
Esther 1:8a0a39740897 349 deltaq = SnelheidD*Ts; //verandering hoek tov. de rechtdoor
Esther 1:8a0a39740897 350 q = deltaq+qbegin; // hoekverandering tov. het assenstelsel
Esther 1:8a0a39740897 351
Esther 1:8a0a39740897 352 deltay = deltas*cos(q); //veranding van pen in de y-richting
Esther 1:8a0a39740897 353 deltax = deltas*sin(q); // verandering van pen in de x-richting
Esther 1:8a0a39740897 354
Esther 1:8a0a39740897 355 x = xbegin+deltax; // nieuwe postie pen, x-coördinaat
Esther 1:8a0a39740897 356 y = ybegin+deltay; // nieuwe positie pen, y-coöridnaat
Esther 1:8a0a39740897 357
Esther 1:8a0a39740897 358 if (x>29.7) { //grenzen A4
Esther 1:8a0a39740897 359 x=29.7;
Esther 1:8a0a39740897 360 }
Esther 1:8a0a39740897 361 if (y>21.0) {
Esther 1:8a0a39740897 362 y=21.0;
Esther 1:8a0a39740897 363 }
Esther 1:8a0a39740897 364
Esther 1:8a0a39740897 365 if (x<0) { //grenzen A4
Esther 1:8a0a39740897 366 x=0;
Esther 1:8a0a39740897 367 }
Esther 1:8a0a39740897 368 if (y<0) {
Esther 1:8a0a39740897 369 y=0;
Esther 1:8a0a39740897 370 }
Esther 1:8a0a39740897 371
Esther 1:8a0a39740897 372 xbegin=x; // x zal xbegin worden in de volgende loop oftewel:(x(n) will be x(n-1) on next loop)
Esther 1:8a0a39740897 373 ybegin=y; // y zal ybgin worden in de volgende loop oftewel:(y(n) will be y(n-1) on next loop)
Esther 1:8a0a39740897 374 qbegin=q; // q zal qbegin worden in de volgende loop oftewel:(q(n) will be q(n-1) on next loop)
Esther 1:8a0a39740897 375
Esther 1:8a0a39740897 376 pc.printf("%f,%f,%f,%f\n",SnelheidV,SnelheidD,x,y);
Esther 1:8a0a39740897 377
Esther 1:8a0a39740897 378
Esther 1:8a0a39740897 379 // motoren
Esther 1:8a0a39740897 380 x = x*10.0 + 69.8;
Esther 1:8a0a39740897 381 y = y*10.0 + 69.8;
Esther 1:8a0a39740897 382 theta = atan(y/x) ;// * (400.0/(.5*pi));
Esther 1:8a0a39740897 383 r = (sqrt(x*x+y*y)) ;// * (2577/461.335);
Esther 1:8a0a39740897 384
Esther 1:8a0a39740897 385 theta_pen = motor1.getPosition() * ((.5*pi)/400);
Esther 1:8a0a39740897 386 r_pen = motor2.getPosition() * (363.0/2577.0);
Esther 1:8a0a39740897 387
Esther 1:8a0a39740897 388
Esther 1:8a0a39740897 389 dtheta = (theta - theta_pen);
Esther 1:8a0a39740897 390 dr = (r - r_pen);
Esther 1:8a0a39740897 391 theta_pwm = (dtheta)*0.5;
Esther 1:8a0a39740897 392 r_pwm = (dr)*0.001;
Esther 1:8a0a39740897 393 //NAAR MOTOR
Esther 1:8a0a39740897 394
Esther 1:8a0a39740897 395 //Zorgen dat pwm tussen -1 en 1 blijft.
Esther 1:8a0a39740897 396 if(theta_pwm > 1) {
Esther 1:8a0a39740897 397 theta_pwm=1;
Esther 1:8a0a39740897 398 }
Esther 1:8a0a39740897 399 if(theta_pwm < -1) {
Esther 1:8a0a39740897 400 theta_pwm=-1;
Esther 1:8a0a39740897 401 }
Esther 1:8a0a39740897 402 if(r_pwm > 1) {
Esther 1:8a0a39740897 403 r_pwm=1;
Esther 1:8a0a39740897 404 }
Esther 1:8a0a39740897 405 if(r_pwm < -1) {
Esther 1:8a0a39740897 406 r_pwm=-1;
Esther 1:8a0a39740897 407 }
Esther 1:8a0a39740897 408 // Bepaal richting waarin motoren moeten draaien
Esther 1:8a0a39740897 409 if(theta_pwm > 0)
Esther 1:8a0a39740897 410 motor1dir.write(1);
Esther 1:8a0a39740897 411 else
Esther 1:8a0a39740897 412 motor1dir.write(0);
Esther 1:8a0a39740897 413 if(r_pwm > 0)
Esther 1:8a0a39740897 414 motor2dir.write(1);
Esther 1:8a0a39740897 415 else
Esther 1:8a0a39740897 416 motor2dir.write(0);
Esther 1:8a0a39740897 417
Esther 1:8a0a39740897 418 // print naar pc
Esther 1:8a0a39740897 419 pc.printf("t=%.3f dt=%.3f tpwm=%.3f | r=%.3f dr=%.3f rpwm=%.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm);
Esther 1:8a0a39740897 420 //schrijf PWM naar motor
Esther 1:8a0a39740897 421 pwm_motor1.write(abs(theta_pwm));
Esther 1:8a0a39740897 422 pwm_motor2.write(abs(r_pwm));
Esther 1:8a0a39740897 423
Esther 1:8a0a39740897 424
Esther 1:8a0a39740897 425 // terug naar state 1 knop
Esther 1:8a0a39740897 426 if (knop3 == false ) { // terug naar state 1
Esther 1:8a0a39740897 427 state=1;
Esther 1:8a0a39740897 428 wait(0.05);
Esther 1:8a0a39740897 429
Esther 1:8a0a39740897 430 while(knop3 == false) {}
Esther 1:8a0a39740897 431 wait(0.05);
Esther 1:8a0a39740897 432 }
Esther 1:8a0a39740897 433
Esther 1:8a0a39740897 434 }
Esther 0:e9e50c0a6502 435
Esther 0:e9e50c0a6502 436 } //sluiten oneindige while loop
Esther 0:e9e50c0a6502 437
Esther 1:8a0a39740897 438 } //afsluiten main