g
Dependencies: MODSERIAL mbed Encoder
Diff: begintotaalscript.cpp
- Revision:
- 2:103de31c8803
- Parent:
- 1:8a0a39740897
- Child:
- 3:a3e4580af271
diff -r 8a0a39740897 -r 103de31c8803 begintotaalscript.cpp --- a/begintotaalscript.cpp Mon Nov 04 19:30:07 2013 +0000 +++ b/begintotaalscript.cpp Mon Nov 04 21:51:21 2013 +0000 @@ -49,8 +49,13 @@ //Variabelen EMG/motoren 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; double MaxsnelheidV,MaxsnelheidD,GrenswaardeD,GrenswaardeV,MaxwaardeD,MaxwaardeV,BereikD,BereikV,SnelheidV,SnelheidD,xbegin,ybegin,qbegin,x,y,q,deltas,deltaq,deltax,deltay; - double theta, r, dtheta, dr, theta_pen, r_pen, theta_pwm, r_pwm, pi; +// Variabelen benoemen voor regelaar motor. + 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; + 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; + double motor1_maxu, motor2_maxu; + + pi=3.14159265359; e1= 0.855930601814815; @@ -103,6 +108,9 @@ qbegin=315; //beginhoek (in graden) vanaf de y-as, rechtsom gedraaid is positief MaxsnelheidV=15; //in cm/s MaxsnelheidD=15; //in graden/s +//constanten regelaar + kp_r = 0.006; + ki_r = 0.005; Ticker looptimer; looptimer.attach(setlooptimerflag,Ts); @@ -136,7 +144,7 @@ wait(0.05); } if (knop3 == false && calibratie_rust == true && calibratie_max == true && calibratie_motor == true) { - state=6; //state 6 kan alleen uitgevoerd worden wanneer alle calibraties gedaan zijn. + state=6; //state 6 kan alleen uitgevoerd worden wanneer alle calibraties gedaan zijn. wait(0.05); while(knop3 == false) {} @@ -166,11 +174,17 @@ while(knop1 == false) {} wait(0.05); - motor1.setPosition(0); - motor2.setPosition(0); + motor1.setPosition(200.0); + motor2.setPosition(597.15); + + x=0; + y=0; + + dri=0; + + calibratie_motor = true; - calibratie_motor = true; } @@ -377,19 +391,46 @@ // motoren - x = x*10.0 + 69.8; - y = y*10.0 + 69.8; - theta = atan(y/x) ;// * (400.0/(.5*pi)); - r = (sqrt(x*x+y*y)) ;// * (2577/461.335); - theta_pen = motor1.getPosition() * ((.5*pi)/400); - r_pen = motor2.getPosition() * (363.0/2577.0); + if (y>210.0) { + y=210.0; + } + if(y<=0.0) { + y=0.0; + } + if(x>297.0) { + x=297.0; + } + if(x<=0.0) { + x=0.0; + } + + // Omzetten hoek en straal + //theta = atan(y/x)+0.25*pi; + //r = (sqrt(x*x+y*y)) ;// * (2577/461.335); + + theta = atan((x+69.80)/(y+69.80)); + r = sqrt( ((x + 69.8)*(x + 69.8))+ ((y + 69.8)*(y + 69.8)) ); + + + theta_pen = motor1.getPosition() * ((.5*pi)/400.0); + r_pen = motor2.getPosition() * (461.34/2790.13); dtheta = (theta - theta_pen); dr = (r - r_pen); - theta_pwm = (dtheta)*0.5; - r_pwm = (dr)*0.001; + + //REGELAAR + up_r = kp_r * dr; //P-actie + dri = dri_1 + dr*Ts; + ui_r = dri_1 * ki_r; //I-actie + utot_r = up_r + ui_r; // + + dri_1 = dri; //nieuwe waardes oud maken. + + theta_pwm = (dtheta)*3.0; + r_pwm = (utot_r/1.0); + //NAAR MOTOR //Zorgen dat pwm tussen -1 en 1 blijft. @@ -405,6 +446,7 @@ if(r_pwm < -1) { r_pwm=-1; } + // Bepaal richting waarin motoren moeten draaien if(theta_pwm > 0) motor1dir.write(1); @@ -416,12 +458,12 @@ motor2dir.write(0); // print naar pc - pc.printf("t=%.3f dt=%.3f tpwm=%.3f | r=%.3f dr=%.3f rpwm=%.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm); + 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); + //schrijf PWM naar motor pwm_motor1.write(abs(theta_pwm)); pwm_motor2.write(abs(r_pwm)); - // terug naar state 1 knop if (knop3 == false ) { // terug naar state 1 state=1;