codevoor esther
Dependencies: Encoder MODSERIAL mbed
Diff: main.cpp
- Revision:
- 1:0db79ea80741
- Parent:
- 0:155294201f36
- Child:
- 2:3987ed9570c8
diff -r 155294201f36 -r 0db79ea80741 main.cpp --- 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));