Diff: main.cpp

Revision:

1:0db79ea80741

Parent:

0:155294201f36

Child:

2:3987ed9570c8

--- a/main.cpp	Fri Nov 01 15:34:03 2013 +0000
+++ b/main.cpp	Mon Nov 04 14:21:07 2013 +0000
@@ -28,7 +28,6 @@
     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 motor1_maxu, motor2_maxu;
-    
     double Ts;
     
 // Sample tijd
@@ -61,24 +60,24 @@
         while(looptimerflag != true);
         looptimerflag = false;
 
-        x = (potmeter1.read()*297.0+69.8);
+        x = (potmeter1.read()*297.0+69.8)*-1.0;
         y = (potmeter2.read()*210.0+69.8);
 
+        //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/2577);
+        r_pen       = motor2.getPosition()  *   (363.0/2577.0);
         
         
         dtheta  = (theta - theta_pen);
         dr      = (r - r_pen);
 
-        dtheta  =  
-
-        theta_pwm   = (dtheta)*0.001;   
+        theta_pwm   = (dtheta)*0.5;   
         r_pwm       = (dr)*0.001;
 
         //NAAR MOTOR
@@ -108,7 +107,7 @@
             motor2dir.write(0);
         
         // print naar pc
-        pc.printf("t=%.3f   dt=%.3f  tpwm=%.3f\n", theta, dtheta, theta_pwm);
+        pc.printf("t=%.3f   dt=%.3f  tpwm=%.3f  |  r=%.3f   dr=%.3f  rpwm=%.3f\n", theta, dtheta, theta_pwm, r, dr, r_pwm);
 
         //schrijf PWM naar motor
         pwm_motor1.write(abs(theta_pwm));