Diff: main.cpp

Revision:

6:bea0424b407c

Parent:

5:8f3530006712

Child:

7:422b8ec97278

--- a/main.cpp	Mon Nov 04 18:52:10 2013 +0000
+++ b/main.cpp	Mon Nov 04 19:58:59 2013 +0000
@@ -26,10 +26,10 @@
 
 // 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;
-    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, dr_1, inputsinus;
+    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, dr_1, utot_r, inputsinus;
     double motor1_maxu, motor2_maxu;
     double Ts;
-    
+
 // Sample tijd
     Ts = 0.001;
 
@@ -53,16 +53,16 @@
     //Tijd looptimer instellen.
     Ticker looptimer;
     looptimer.attach(setlooptimerflag,Ts);
-    
+
     motor1.setPosition(200.0);
     motor2.setPosition(0);
     x=0;
     y=0;
-    
-    kp_r = 
-    ki_r =
 
+    kp_r = 0.006;
+    ki_r = 0.015;
 
+    inputsinus=0;
 
 // Oneidige loop...
     while(true) {
@@ -74,30 +74,48 @@
 
         x   =   sin(inputsinus)*297;
         y   =   0;
-        
-        inputsinus  =   inputsinus + Ts*pi;
-        
-        theta   = atan(y/x)+0.25*pi;       
+
+        inputsinus  =   inputsinus + (Ts*1)*pi;
+
+        //Binnen berijk blijven
+
+        if (y>210) {
+            y=210
+        }
+        if(y<=0) {
+            y=0
+        }
+        if(x>297) {
+            x=297
+        }
+        if(x<=0) {
+            x=0
+        }
+
+          // Omzetten hoek en straal
+        theta   = atan(y/x)+0.25*pi;
         r       = (sqrt(x*x+y*y)) ;// *   (2577/461.335);
-                
+
 
         theta_pen   = motor1.getPosition()  *   ((.5*pi)/400.0);
         r_pen       = motor2.getPosition()  *   (363.0/2196.0);
-        
-        
+
+
         dtheta  = (theta - theta_pen);
         dr      = (r - r_pen);
-        
+
         //REGELAAR
-        up_r = kp_r * dr;
-        ui_r = (dr_1 + (dr * Ts)) * ki_r;
-        utot_r = up_r + ui_r;
+        up_r = kp_r * dr;                   //P-actie
+        ui_r = (dr_1 + (dr * Ts)) * ki_r;   //I-actie
+        utot_r = up_r + ui_r;               //
 
-        theta_pwm   = (dtheta)*3.0;   
+        dr_1 =  dr;                         //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.
         if(theta_pwm > 1) {
             theta_pwm=1;
@@ -121,11 +139,11 @@
             motor2dir.write(1);
         else
             motor2dir.write(0);
-        
+
         // print naar pc
         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       
+        //schrijf PWM naar motor
         pwm_motor1.write(abs(theta_pwm));
         pwm_motor2.write(abs(r_pwm));