Diff: begintotaalscript.cpp

Revision:

2:103de31c8803

Parent:

1:8a0a39740897

Child:

3:a3e4580af271

--- 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;