codevoor esther
Dependencies: Encoder MODSERIAL mbed
Diff: main.cpp
- Revision:
- 5:8f3530006712
- Parent:
- 4:863a52425322
- Child:
- 6:bea0424b407c
diff -r 863a52425322 -r 8f3530006712 main.cpp --- 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