Revision 5:8f3530006712, committed 2013-11-04

Comitter:

gerjan

Date:

Mon Nov 04 18:52:10 2013 +0000

Parent:

4:863a52425322

Child:

6:bea0424b407c

Commit message:

prepare i;

Changed in this revision

--- a/main.cpp	Mon Nov 04 16:29:51 2013 +0000
+++ b/main.cpp	Mon Nov 04 18:52:10 2013 +0000
@@ -26,7 +26,7 @@
 
 // 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, 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, inputsinus;
     double motor1_maxu, motor2_maxu;
     double Ts;
     
@@ -58,6 +58,9 @@
     motor2.setPosition(0);
     x=0;
     y=0;
+    
+    kp_r = 
+    ki_r =
 
 
 
@@ -73,7 +76,8 @@
         y   =   0;
         
         inputsinus  =   inputsinus + Ts*pi;
-        theta   = 0.25*pi;       
+        
+        theta   = atan(y/x)+0.25*pi;       
         r       = (sqrt(x*x+y*y)) ;// *   (2577/461.335);
                 
 
@@ -83,9 +87,14 @@
         
         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;
 
-        theta_pwm   = (dtheta)*3;   
-        r_pwm       = (dr)*0.0015;
+        theta_pwm   = (dtheta)*3.0;   
+        r_pwm       = (utot_r/1.0);
 
         //NAAR MOTOR