Revision 4:863a52425322, committed 2013-11-04

Comitter:

gerjan

Date:

Mon Nov 04 16:29:51 2013 +0000

Parent:

3:0edffb90e739

Child:

5:8f3530006712

Commit message:

kijken;

Changed in this revision

--- a/main.cpp	Mon Nov 04 16:08:46 2013 +0000
+++ b/main.cpp	Mon Nov 04 16:29:51 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;
+    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 motor1_maxu, motor2_maxu;
     double Ts;
     
@@ -66,24 +66,14 @@
         while(looptimerflag != true);
         looptimerflag = false;
 
-
-        x = (potmeter1.read()*297.0);
-        y = (potmeter2.read()*210.0);
-
-        //x = x*10.0 + 69.8;
-        //y = y*10.0 + 69.8;
+        //x = (potmeter1.read()*297.0);
+        //y = (potmeter2.read()*210.0);
 
-        if(x < 30){
-        theta = 0.25*pi;
-        }
-        if(y < 21){
-        theta = 0.25*pi;
-        }
+        x   =   sin(inputsinus)*297;
+        y   =   0;
         
-        if(x >= 30 && y >= 21){
-                theta   = atan(y/x)+(.25*pi);// *   (400.0/(.5*pi));
-        }
-        
+        inputsinus  =   inputsinus + Ts*pi;
+        theta   = 0.25*pi;       
         r       = (sqrt(x*x+y*y)) ;// *   (2577/461.335);
                 
 
@@ -94,8 +84,8 @@
         dtheta  = (theta - theta_pen);
         dr      = (r - r_pen);
 
-        theta_pwm   = (dtheta)*0.5;   
-        r_pwm       = (dr)*0.001;
+        theta_pwm   = (dtheta)*3;   
+        r_pwm       = (dr)*0.0015;
 
         //NAAR MOTOR
         
@@ -124,9 +114,9 @@
             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
+        //schrijf PWM naar motor       
         pwm_motor1.write(abs(theta_pwm));
         pwm_motor2.write(abs(r_pwm));