Esther Keulers / Mbed 2 deprecated Begintotaalscript

g

Dependencies:   MODSERIAL mbed Encoder

begintotaalscript.cpp@3:a3e4580af271, 2013-11-05 (annotated)

Committer:
gerjan
Date:
Tue Nov 05 08:42:42 2013 +0000
Revision:
3:a3e4580af271
Parent:
2:103de31c8803
Child:
4:c79922420b70
wat maandag avond werkte;

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;
gerjan 3:a3e4580af271 52
Esther 2:103de31c8803 53 // Variabelen benoemen voor regelaar motor.
Esther 2:103de31c8803 54 double theta, theta_pen, up_theta, kp_theta, kd_theta, dtheta, ei_theta, ui_theta, ki_theta, ed_theta, u_theta, ud_theta, theta_pwm,pi;
Esther 2:103de31c8803 55 double r, r_pen, up_r, kp_r, kd_r, dr, ei_r, ui_r, ki_r, ed_r, u_r, ud_r, r_pwm, dri, dri_1, utot_r, inputsinus;
Esther 2:103de31c8803 56 double motor1_maxu, motor2_maxu;
gerjan 3:a3e4580af271 57
Esther 2:103de31c8803 58
Esther 1:8a0a39740897 59 pi=3.14159265359;
Esther 1:8a0a39740897 60
Esther 1:8a0a39740897 61 e1= 0.855930601814815;
Esther 1:8a0a39740897 62 e2= -1.711861203629630;
Esther 1:8a0a39740897 63 e3= 0.855930601814815;
Esther 1:8a0a39740897 64 f1=-1.690996376887443;
Esther 1:8a0a39740897 65 f2= 0.732726030371816;
Esther 1:8a0a39740897 66
Esther 1:8a0a39740897 67 g1= 0.098259168204717*pow(10.0,-4.0);
Esther 1:8a0a39740897 68 g2= 0.196518336409435*pow(10.0,-4.0);
Esther 1:8a0a39740897 69 g3=0.098259168204717*pow(10.0,-4.0);
Esther 1:8a0a39740897 70 h1= -1.991114292201654;
Esther 1:8a0a39740897 71 h2=0.991153595868935;
Esther 1:8a0a39740897 72
Esther 1:8a0a39740897 73 be1=0.914969144113083;
Esther 1:8a0a39740897 74 be2= -1.829938288226165;
Esther 1:8a0a39740897 75 be3= 0.914969144113083;
Esther 1:8a0a39740897 76 bf1=-1.822694925196308;
Esther 1:8a0a39740897 77 bf2=0.837181651256022;
Esther 1:8a0a39740897 78
Esther 1:8a0a39740897 79 bg1= 0.246193004641015*pow(10.0,-5.0);
Esther 1:8a0a39740897 80 bg2= 0.492386009282031 *pow(10.0,-5.0);
Esther 1:8a0a39740897 81 bg3= 0.246193004641015*pow(10.0,-5.0);
Esther 1:8a0a39740897 82 bh1= -1.995557124345789;
Esther 1:8a0a39740897 83 bh2= 0.995566972065975;
Esther 1:8a0a39740897 84
Esther 1:8a0a39740897 85
Esther 1:8a0a39740897 86 y1 = 0; /*Initialize y(n-1) = 0*/
Esther 1:8a0a39740897 87 y2 = 0; /*Initialize y(n-2) = 0*/
Esther 1:8a0a39740897 88 z1 = 0; /*Initialize z(n-1) = 0*/
Esther 1:8a0a39740897 89 z2 = 0; /*Initialize z(n-2) = 0*/
Esther 1:8a0a39740897 90 zabs1 = 0; /*Initialize zabs(n-1) = 0*/
Esther 1:8a0a39740897 91 zabs2 = 0; /*Initialize zabs(n-2) = 0*/
Esther 1:8a0a39740897 92 w1 = 0; /*Initialize w(n-1) = 0*/
Esther 1:8a0a39740897 93 w2 = 0; /*Initialize w(n-2) = 0*/
Esther 0:e9e50c0a6502 94
Esther 1:8a0a39740897 95 by1 = 0; /*Initialize by(n-1) = 0*/
Esther 1:8a0a39740897 96 by2 = 0; /*Initialize by(n-2) = 0*/
Esther 1:8a0a39740897 97 bz1 = 0; /*Initialize bz(n-1) = 0*/
Esther 1:8a0a39740897 98 bz2 = 0; /*Initialize bz(n-2) = 0*/
Esther 1:8a0a39740897 99 bzabs1 = 0; /*Initialize bzabs(n-1) = 0*/
Esther 1:8a0a39740897 100 bzabs2 = 0; /*Initialize bzabs(n-2) = 0*/
Esther 1:8a0a39740897 101 bw1 = 0; /*Initialize bw(n-1) = 0*/
Esther 1:8a0a39740897 102 bw2 = 0; /*Initialize bw(n-2) = 0*/
Esther 1:8a0a39740897 103 const float Ts = 0.001; /*Sample Time, const(ant)*/
Esther 1:8a0a39740897 104
Esther 1:8a0a39740897 105 //Constanten voor tekening: EMGdeel
Esther 1:8a0a39740897 106 xbegin= 0; //beginpositie
Esther 1:8a0a39740897 107 ybegin=0; //beginpositie
gerjan 3:a3e4580af271 108 qbegin=5.50; //beginhoek (in rad) vanaf de y-as, rechtsom gedraaid is positief
gerjan 3:a3e4580af271 109 MaxsnelheidV=150.0; //in mm/s
gerjan 3:a3e4580af271 110 MaxsnelheidD=0.26; //in rad/s
Esther 2:103de31c8803 111 //constanten regelaar
Esther 2:103de31c8803 112 kp_r = 0.006;
Esther 2:103de31c8803 113 ki_r = 0.005;
gerjan 3:a3e4580af271 114
Esther 1:8a0a39740897 115 Ticker looptimer;
Esther 1:8a0a39740897 116 looptimer.attach(setlooptimerflag,Ts);
Esther 1:8a0a39740897 117
Esther 1:8a0a39740897 118 while (true) { //oneindige while loop
Esther 1:8a0a39740897 119
Esther 1:8a0a39740897 120 while(looptimerflag != true);
Esther 1:8a0a39740897 121 looptimerflag = false;
Esther 1:8a0a39740897 122
Esther 1:8a0a39740897 123
Esther 1:8a0a39740897 124 // state 1 RUST
Esther 1:8a0a39740897 125 if (state == 1) {
Esther 1:8a0a39740897 126
Esther 1:8a0a39740897 127 pc.printf("state 1 RUST | knop1 = cal_motor knop2 = MENU EMG knop3 = Tekenen \n\r");
Esther 1:8a0a39740897 128
Esther 0:e9e50c0a6502 129
Esther 1:8a0a39740897 130 if (knop1 == false ) { //als knop 1 is ingedrukt
Esther 1:8a0a39740897 131 state=2;
Esther 1:8a0a39740897 132 wait(0.05); //wacht goed contact knopje
Esther 1:8a0a39740897 133
Esther 1:8a0a39740897 134 while(knop1 == false) {} //wacht tot knop is losgelaten
Esther 1:8a0a39740897 135 wait(0.05); // wacht goed contact knopje
Esther 1:8a0a39740897 136
Esther 1:8a0a39740897 137 }
Esther 1:8a0a39740897 138
Esther 1:8a0a39740897 139 if (knop2 == false ) { //als knop 2 wordt ingedrukt: state is 3
Esther 1:8a0a39740897 140 state=3;
Esther 1:8a0a39740897 141 wait(0.05);
Esther 1:8a0a39740897 142
Esther 1:8a0a39740897 143 while(knop2 == false) {}
Esther 1:8a0a39740897 144 wait(0.05);
Esther 1:8a0a39740897 145 }
Esther 1:8a0a39740897 146 if (knop3 == false && calibratie_rust == true && calibratie_max == true && calibratie_motor == true) {
Esther 2:103de31c8803 147 state=6; //state 6 kan alleen uitgevoerd worden wanneer alle calibraties gedaan zijn.
Esther 1:8a0a39740897 148 wait(0.05);
Esther 1:8a0a39740897 149
Esther 1:8a0a39740897 150 while(knop3 == false) {}
Esther 1:8a0a39740897 151 wait(0.05);
Esther 1:8a0a39740897 152 meting=true;
Esther 1:8a0a39740897 153
Esther 1:8a0a39740897 154 }
Esther 1:8a0a39740897 155 if (knop3 == false && state !=6) {
Esther 1:8a0a39740897 156 wait(0.05);
Esther 1:8a0a39740897 157 pc.printf("Niet alle calibraties uitgevoerd! \n");
Esther 1:8a0a39740897 158 wait(1);
Esther 1:8a0a39740897 159 }
Esther 1:8a0a39740897 160
Esther 1:8a0a39740897 161
Esther 0:e9e50c0a6502 162 }
Esther 0:e9e50c0a6502 163
Esther 1:8a0a39740897 164
Esther 1:8a0a39740897 165 //state2 CAL MOTOR
Esther 1:8a0a39740897 166 if (state == 2) {
Esther 1:8a0a39740897 167 pc.printf("state 2 cal_motor | knop1 = terug naar rust \n\r");
Esther 1:8a0a39740897 168
Esther 1:8a0a39740897 169
Esther 1:8a0a39740897 170 if (knop1 == false ) {
Esther 1:8a0a39740897 171 state=1;
Esther 1:8a0a39740897 172 wait(0.05);
Esther 1:8a0a39740897 173
Esther 1:8a0a39740897 174 while(knop1 == false) {}
Esther 1:8a0a39740897 175 wait(0.05);
Esther 1:8a0a39740897 176
Esther 2:103de31c8803 177 motor1.setPosition(200.0);
Esther 2:103de31c8803 178 motor2.setPosition(597.15);
Esther 2:103de31c8803 179
Esther 2:103de31c8803 180 x=0;
Esther 2:103de31c8803 181 y=0;
Esther 2:103de31c8803 182
gerjan 3:a3e4580af271 183 dri_1=0;
Esther 2:103de31c8803 184
Esther 2:103de31c8803 185 calibratie_motor = true;
Esther 1:8a0a39740897 186
Esther 1:8a0a39740897 187
Esther 1:8a0a39740897 188
Esther 1:8a0a39740897 189
Esther 1:8a0a39740897 190 }
Esther 0:e9e50c0a6502 191 }
Esther 1:8a0a39740897 192 // state 3 MENU EMG
Esther 1:8a0a39740897 193 if (state == 3) {
Esther 1:8a0a39740897 194 pc.printf("state 3 MENU EMG | knop1= cal EMG rust knop2= rust knop3= cal EMG max \n\r");
Esther 1:8a0a39740897 195
Esther 1:8a0a39740897 196
Esther 1:8a0a39740897 197 if (knop2 == false ) { // terug naar state 1
Esther 1:8a0a39740897 198 state=1;
Esther 1:8a0a39740897 199 wait(0.05);
Esther 1:8a0a39740897 200
Esther 1:8a0a39740897 201 while(knop2 == false) {}
Esther 1:8a0a39740897 202 wait(0.05);
Esther 1:8a0a39740897 203 }
Esther 1:8a0a39740897 204
Esther 1:8a0a39740897 205 if (knop1 == false ) { // naar state 4
Esther 1:8a0a39740897 206 state=4;
Esther 1:8a0a39740897 207 wait(0.05);
Esther 1:8a0a39740897 208
Esther 1:8a0a39740897 209 while(knop1 == false) {}
Esther 1:8a0a39740897 210 wait(0.05);
Esther 1:8a0a39740897 211
Esther 1:8a0a39740897 212 // begingrenswaarden stellen:
Esther 1:8a0a39740897 213 GrenswaardeV=0;
Esther 1:8a0a39740897 214 GrenswaardeD=0;
Esther 1:8a0a39740897 215 meting=true;
Esther 1:8a0a39740897 216 }
Esther 1:8a0a39740897 217
Esther 1:8a0a39740897 218 if (knop3 == false ) { // naar state 5
Esther 1:8a0a39740897 219 state=5;
Esther 1:8a0a39740897 220 wait(0.05);
Esther 1:8a0a39740897 221
Esther 1:8a0a39740897 222 while(knop3 == false) {}
Esther 1:8a0a39740897 223 wait(0.05);
Esther 1:8a0a39740897 224
Esther 1:8a0a39740897 225 // begingrenswaarden stellen:
Esther 1:8a0a39740897 226 MaxwaardeV=0;
Esther 1:8a0a39740897 227 MaxwaardeD=0;
Esther 1:8a0a39740897 228 meting=true;
Esther 1:8a0a39740897 229
Esther 1:8a0a39740897 230 }
Esther 1:8a0a39740897 231
Esther 1:8a0a39740897 232
Esther 0:e9e50c0a6502 233 }
Esther 1:8a0a39740897 234
Esther 1:8a0a39740897 235
Esther 1:8a0a39740897 236 //METEN VAN EMG
Esther 1:8a0a39740897 237 if (meting == true) {
Esther 1:8a0a39740897 238 // pc.printf("TEST CODE: Meting in gang\n");
Esther 1:8a0a39740897 239 yy = emg1.read(); //<measurement function, read from pin>
Esther 1:8a0a39740897 240 byy= emg2.read();
Esther 1:8a0a39740897 241
Esther 1:8a0a39740897 242 z=-f1*z1-f2*z2+e1*yy+e2*y1+e3*y2; /*hoogdoorlaatfilteroverdacht*/
Esther 1:8a0a39740897 243 bz=-bf1*bz1-bf2*bz2+be1*byy+be2*by1+be3*by2; /*hoogdoorlaatfilteroverdacht van biceps*/
Esther 1:8a0a39740897 244
Esther 1:8a0a39740897 245 y2 = y1; /*y(n-1) will be y(n-2) on next loop*/
Esther 1:8a0a39740897 246 y1= yy; /*y(n) will be y(n-1) on next loop*/
Esther 1:8a0a39740897 247 z2 = z1; /*z(n-1) will be z(n-2) on next loop*/
Esther 1:8a0a39740897 248 z1 = z; /*z(n) will be z(n-1) on next loop*/
Esther 1:8a0a39740897 249
Esther 1:8a0a39740897 250 by2 = by1; /*y(n-1) will be y(n-2) on next loop*/
Esther 1:8a0a39740897 251 by1= byy; /*y(n) will be y(n-1) on next loop*/
Esther 1:8a0a39740897 252 bz2 = bz1; /*z(n-1) will be z(n-2) on next loop*/
Esther 1:8a0a39740897 253 bz1 = bz; /*z(n) will be z(n-1) on next loop*/
Esther 1:8a0a39740897 254
Esther 1:8a0a39740897 255 zabs=abs(z);/*rectify*/
Esther 1:8a0a39740897 256 bzabs=abs(bz);/*rectify*/
Esther 1:8a0a39740897 257
Esther 1:8a0a39740897 258 w=-h1*w1-h2*w2+g1*zabs+g2*zabs1+g3*zabs2; /*laagdoorlaatfilteroverdracht*/
Esther 1:8a0a39740897 259 bw=-bh1*bw1-bh2*bw2+bg1*bzabs+bg2*bzabs1+bg3*bzabs2; /*laagdoorlaatfilteroverdracht van biceps*/
Esther 1:8a0a39740897 260
Esther 1:8a0a39740897 261 zabs2 = zabs1; /*z(n-1) will be z(n-2) on next loop*/
Esther 1:8a0a39740897 262 zabs1 = zabs; /*z(n) will be z(n-1) on next loop*/
Esther 1:8a0a39740897 263 w2 = w1; /*w(n-1) will be w(n-2) on next loop*/
Esther 1:8a0a39740897 264 w1 = w; /*w(n) will be w(n-1) on next loop*/
Esther 1:8a0a39740897 265
Esther 1:8a0a39740897 266 bzabs2 = bzabs1; /*z(n-1) will be z(n-2) on next loop*/
Esther 1:8a0a39740897 267 bzabs1 = bzabs; /*z(n) will be z(n-1) on next loop*/
Esther 1:8a0a39740897 268 bw2 = bw1; /*w(n-1) will be w(n-2) on next loop*/
Esther 1:8a0a39740897 269 bw1 = bw; /*w(n) will be w(n-1) on next loop*/
Esther 1:8a0a39740897 270
Esther 1:8a0a39740897 271 //pc.printf("%f,%f\n",w,bw);
Esther 1:8a0a39740897 272
Esther 1:8a0a39740897 273 wait(Ts); /*When finished wait (maybe a bit shorter?)*/
Esther 1:8a0a39740897 274
Esther 1:8a0a39740897 275 if (w>1.0) {
Esther 1:8a0a39740897 276 w=1.0;
Esther 1:8a0a39740897 277 }
Esther 1:8a0a39740897 278 if (bw>1.0) {
Esther 1:8a0a39740897 279 bw=1.0;
Esther 1:8a0a39740897 280 }
Esther 1:8a0a39740897 281
Esther 1:8a0a39740897 282 }
Esther 0:e9e50c0a6502 283
Esther 0:e9e50c0a6502 284
Esther 0:e9e50c0a6502 285
Esther 1:8a0a39740897 286 // state 4 EMG CAL IN RUST
Esther 1:8a0a39740897 287 if (state == 4) {
Esther 1:8a0a39740897 288 pc.printf("state 4 cal EMG RUST | knop1= MEMNU EMG \n\r");
Esther 1:8a0a39740897 289 pc.printf("%f,%f\n",GrenswaardeV,GrenswaardeD);
Esther 1:8a0a39740897 290 // Nieuwe grenswaarden bepalen
Esther 1:8a0a39740897 291 if (w > GrenswaardeV) {
Esther 1:8a0a39740897 292 GrenswaardeV = w;
Esther 1:8a0a39740897 293 }
Esther 1:8a0a39740897 294 if (bw > GrenswaardeD) {
Esther 1:8a0a39740897 295 GrenswaardeD = bw;
Esther 1:8a0a39740897 296 }
Esther 1:8a0a39740897 297 if (knop1 == false ) { // terug naar state 1
Esther 1:8a0a39740897 298 state=3;
Esther 1:8a0a39740897 299 wait(0.05);
Esther 1:8a0a39740897 300
Esther 1:8a0a39740897 301 while(knop1 == false) {}
Esther 1:8a0a39740897 302 wait(0.05);
Esther 1:8a0a39740897 303 calibratie_rust = true;
Esther 1:8a0a39740897 304 meting=false;
Esther 1:8a0a39740897 305
Esther 1:8a0a39740897 306 }
Esther 1:8a0a39740897 307 }
Esther 1:8a0a39740897 308 // state 5 EMG CAL MAX
Esther 1:8a0a39740897 309 if (state == 5) {
Esther 1:8a0a39740897 310 pc.printf("state 5 cal EMG MAX | knop3= EMG MENU \n\r");
Esther 1:8a0a39740897 311 pc.printf("%f,%f\n",MaxwaardeV,MaxwaardeD);
Esther 1:8a0a39740897 312 // Nieuwe maxwaarden bepalen
Esther 1:8a0a39740897 313 if (w > MaxwaardeV) {
Esther 1:8a0a39740897 314 MaxwaardeV = w;
Esther 1:8a0a39740897 315 }
Esther 1:8a0a39740897 316 if (bw > MaxwaardeD) {
Esther 1:8a0a39740897 317 MaxwaardeD = bw;
Esther 1:8a0a39740897 318 }
Esther 1:8a0a39740897 319
Esther 1:8a0a39740897 320 // einde maxwaarde bepalen:
Esther 1:8a0a39740897 321 if (knop3 == false ) { // terug naar state 3
Esther 1:8a0a39740897 322 state=3;
Esther 1:8a0a39740897 323 wait(0.05);
Esther 1:8a0a39740897 324
Esther 1:8a0a39740897 325 while(knop3 == false) {}
Esther 1:8a0a39740897 326 wait(0.05);
Esther 1:8a0a39740897 327 calibratie_max = true;
Esther 1:8a0a39740897 328 meting=false;
Esther 1:8a0a39740897 329
Esther 1:8a0a39740897 330 }
Esther 1:8a0a39740897 331
Esther 1:8a0a39740897 332 }
Esther 1:8a0a39740897 333
Esther 1:8a0a39740897 334 // state 6 TEKENEN
Esther 1:8a0a39740897 335
Esther 1:8a0a39740897 336 if (state == 6) {
Esther 1:8a0a39740897 337 // pc.printf("state 6 tekenen | knop3= rust \n\r");
Esther 1:8a0a39740897 338
Esther 1:8a0a39740897 339 // EMG
Esther 1:8a0a39740897 340 BereikD = MaxwaardeD-GrenswaardeD; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
Esther 1:8a0a39740897 341 BereikV = MaxwaardeV-GrenswaardeV; //Bereik van grafiek EMG (Voltage t.o.v. tijd)
gerjan 3:a3e4580af271 342
Esther 1:8a0a39740897 343 SnelheidV = (MaxsnelheidV/BereikV)*(w-GrenswaardeV); // Snelheid van pen (in cm/sec), lineair verband met het Bereik
Esther 1:8a0a39740897 344 SnelheidD = (MaxsnelheidD/BereikD)*(bw-GrenswaardeD); //Draaisnelheid pen (in graden/sec), lineair verband met het Bereik
Esther 1:8a0a39740897 345
Esther 1:8a0a39740897 346 if (SnelheidV<MaxsnelheidD) {
Esther 1:8a0a39740897 347 SnelheidV=MaxsnelheidD;
Esther 1:8a0a39740897 348 }
Esther 1:8a0a39740897 349
Esther 1:8a0a39740897 350 if (SnelheidD<MaxsnelheidD) {
Esther 1:8a0a39740897 351 SnelheidD=MaxsnelheidD;
Esther 1:8a0a39740897 352 }
Esther 1:8a0a39740897 353
Esther 1:8a0a39740897 354 if (SnelheidV>MaxsnelheidV) {
Esther 1:8a0a39740897 355 SnelheidV=MaxsnelheidV;
Esther 1:8a0a39740897 356 }
Esther 1:8a0a39740897 357
Esther 1:8a0a39740897 358 if (SnelheidD>MaxsnelheidV) {
Esther 1:8a0a39740897 359 SnelheidD=MaxsnelheidV;
Esther 1:8a0a39740897 360 }
Esther 1:8a0a39740897 361
Esther 1:8a0a39740897 362 deltas = SnelheidV*Ts; //afstand tussen twee meetpunten
Esther 1:8a0a39740897 363 deltaq = SnelheidD*Ts; //verandering hoek tov. de rechtdoor
Esther 1:8a0a39740897 364 q = deltaq+qbegin; // hoekverandering tov. het assenstelsel
Esther 1:8a0a39740897 365
Esther 1:8a0a39740897 366 deltay = deltas*cos(q); //veranding van pen in de y-richting
Esther 1:8a0a39740897 367 deltax = deltas*sin(q); // verandering van pen in de x-richting
Esther 1:8a0a39740897 368
Esther 1:8a0a39740897 369 x = xbegin+deltax; // nieuwe postie pen, x-coördinaat
Esther 1:8a0a39740897 370 y = ybegin+deltay; // nieuwe positie pen, y-coöridnaat
Esther 1:8a0a39740897 371
gerjan 3:a3e4580af271 372 if (x>297) { //grenzen A4
gerjan 3:a3e4580af271 373 x=297;
Esther 1:8a0a39740897 374 }
gerjan 3:a3e4580af271 375 if (y>210) {
gerjan 3:a3e4580af271 376 y=210;
Esther 1:8a0a39740897 377 }
Esther 1:8a0a39740897 378
Esther 1:8a0a39740897 379 if (x<0) { //grenzen A4
Esther 1:8a0a39740897 380 x=0;
Esther 1:8a0a39740897 381 }
Esther 1:8a0a39740897 382 if (y<0) {
Esther 1:8a0a39740897 383 y=0;
Esther 1:8a0a39740897 384 }
Esther 1:8a0a39740897 385
Esther 1:8a0a39740897 386 xbegin=x; // x zal xbegin worden in de volgende loop oftewel:(x(n) will be x(n-1) on next loop)
Esther 1:8a0a39740897 387 ybegin=y; // y zal ybgin worden in de volgende loop oftewel:(y(n) will be y(n-1) on next loop)
Esther 1:8a0a39740897 388 qbegin=q; // q zal qbegin worden in de volgende loop oftewel:(q(n) will be q(n-1) on next loop)
Esther 1:8a0a39740897 389
gerjan 3:a3e4580af271 390 pc.printf("bV=%f, bD=%f, sV=%f, sD=%f, x=%f, y=%f, hoek=%f\n",BereikV,BereikD,SnelheidV,SnelheidD,x,y,(d*57.3));
Esther 1:8a0a39740897 391
Esther 1:8a0a39740897 392
Esther 1:8a0a39740897 393 // motoren
Esther 1:8a0a39740897 394
Esther 2:103de31c8803 395 if (y>210.0) {
Esther 2:103de31c8803 396 y=210.0;
Esther 2:103de31c8803 397 }
Esther 2:103de31c8803 398 if(y<=0.0) {
Esther 2:103de31c8803 399 y=0.0;
Esther 2:103de31c8803 400 }
Esther 2:103de31c8803 401 if(x>297.0) {
Esther 2:103de31c8803 402 x=297.0;
Esther 2:103de31c8803 403 }
Esther 2:103de31c8803 404 if(x<=0.0) {
Esther 2:103de31c8803 405 x=0.0;
Esther 2:103de31c8803 406 }
Esther 2:103de31c8803 407
Esther 2:103de31c8803 408 // Omzetten hoek en straal
Esther 2:103de31c8803 409 //theta = atan(y/x)+0.25*pi;
Esther 2:103de31c8803 410 //r = (sqrt(x*x+y*y)) ;// * (2577/461.335);
Esther 2:103de31c8803 411
Esther 2:103de31c8803 412 theta = atan((x+69.80)/(y+69.80));
Esther 2:103de31c8803 413 r = sqrt( ((x + 69.8)*(x + 69.8))+ ((y + 69.8)*(y + 69.8)) );
Esther 2:103de31c8803 414
Esther 2:103de31c8803 415
Esther 2:103de31c8803 416 theta_pen = motor1.getPosition() * ((.5*pi)/400.0);
Esther 2:103de31c8803 417 r_pen = motor2.getPosition() * (461.34/2790.13);
Esther 1:8a0a39740897 418
Esther 1:8a0a39740897 419
Esther 1:8a0a39740897 420 dtheta = (theta - theta_pen);
Esther 1:8a0a39740897 421 dr = (r - r_pen);
Esther 2:103de31c8803 422
Esther 2:103de31c8803 423 //REGELAAR
Esther 2:103de31c8803 424 up_r = kp_r * dr; //P-actie
Esther 2:103de31c8803 425 dri = dri_1 + dr*Ts;
Esther 2:103de31c8803 426 ui_r = dri_1 * ki_r; //I-actie
Esther 2:103de31c8803 427 utot_r = up_r + ui_r; //
Esther 2:103de31c8803 428
Esther 2:103de31c8803 429 dri_1 = dri; //nieuwe waardes oud maken.
Esther 2:103de31c8803 430
Esther 2:103de31c8803 431 theta_pwm = (dtheta)*3.0;
Esther 2:103de31c8803 432 r_pwm = (utot_r/1.0);
Esther 2:103de31c8803 433
Esther 1:8a0a39740897 434 //NAAR MOTOR
Esther 1:8a0a39740897 435
Esther 1:8a0a39740897 436 //Zorgen dat pwm tussen -1 en 1 blijft.
Esther 1:8a0a39740897 437 if(theta_pwm > 1) {
Esther 1:8a0a39740897 438 theta_pwm=1;
Esther 1:8a0a39740897 439 }
Esther 1:8a0a39740897 440 if(theta_pwm < -1) {
Esther 1:8a0a39740897 441 theta_pwm=-1;
Esther 1:8a0a39740897 442 }
Esther 1:8a0a39740897 443 if(r_pwm > 1) {
Esther 1:8a0a39740897 444 r_pwm=1;
Esther 1:8a0a39740897 445 }
Esther 1:8a0a39740897 446 if(r_pwm < -1) {
Esther 1:8a0a39740897 447 r_pwm=-1;
Esther 1:8a0a39740897 448 }
Esther 2:103de31c8803 449
Esther 1:8a0a39740897 450 // Bepaal richting waarin motoren moeten draaien
Esther 1:8a0a39740897 451 if(theta_pwm > 0)
Esther 1:8a0a39740897 452 motor1dir.write(1);
Esther 1:8a0a39740897 453 else
Esther 1:8a0a39740897 454 motor1dir.write(0);
Esther 1:8a0a39740897 455 if(r_pwm > 0)
Esther 1:8a0a39740897 456 motor2dir.write(1);
Esther 1:8a0a39740897 457 else
Esther 1:8a0a39740897 458 motor2dir.write(0);
Esther 1:8a0a39740897 459
Esther 1:8a0a39740897 460 // print naar pc
gerjan 3:a3e4580af271 461 //pc.printf("t=%.3f dt=%.3f tpwm=%.3f | r=%.3f dr=%.3f rpwm=%.3f inputsin=%0.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm, inputsinus);
Esther 2:103de31c8803 462
Esther 1:8a0a39740897 463 //schrijf PWM naar motor
Esther 1:8a0a39740897 464 pwm_motor1.write(abs(theta_pwm));
Esther 1:8a0a39740897 465 pwm_motor2.write(abs(r_pwm));
Esther 1:8a0a39740897 466
Esther 1:8a0a39740897 467 // terug naar state 1 knop
Esther 1:8a0a39740897 468 if (knop3 == false ) { // terug naar state 1
Esther 1:8a0a39740897 469 state=1;
gerjan 3:a3e4580af271 470
Esther 1:8a0a39740897 471 wait(0.05);
Esther 1:8a0a39740897 472
Esther 1:8a0a39740897 473 while(knop3 == false) {}
Esther 1:8a0a39740897 474 wait(0.05);
Esther 1:8a0a39740897 475 }
Esther 1:8a0a39740897 476
Esther 1:8a0a39740897 477 }
Esther 0:e9e50c0a6502 478
Esther 0:e9e50c0a6502 479 } //sluiten oneindige while loop
Esther 0:e9e50c0a6502 480
Esther 1:8a0a39740897 481 } //afsluiten main